From implementation of RobotHW to implementation of PI control

I have implemented customized RobotHW object and used controller manage to publish/subscribe topics of which enables control joints in this article.

https://hellosagal.tistory.com/13

ros_control API를 이용한 하드웨어 제어프로그램 작성

 

In this time, I will implement position control of the joint using PI control makes torque input.

First of all, I will implement RobotHW likes the previous time but has some changes.

run.cpp: generates RobotHW object with some joint names

#include "./sagalbot.h"
#include "controller_manager/controller_manager.h"
#include "ros/ros.h"
#include <string>

int main(int argc, char **argv){
    /*.....No change...*/
    ros::Rate rate(static_cast<double>(dt_inv)); /*Simulation interval*/
 
    while (ros::ok())
    {
        ros::Duration d = ros::Time::now() - t;
        ros::Time t = ros::Time::now();
        sagal_robot->read(dt_inv);
        cm.update(t, d);
        sagal_robot->write();
        rate.sleep();
    }
    /*.....No change...*/
    return 0;
}

 

sagalbot.cpp: MyRobot::read() method has simulation of torque-speed-position dynamics. It assumes no friction and some damping.(without damping, the system cannot be stabled since the eigen value is not in [-1,1])

#include "./sagalbot.h"

using namespace sagalbot;

/*.....No change...*/

void MyRobot::read(const unsigned int _dt_inv){
    const size_t num_joints = joint_list.size();
    double eff_prev[num_joints];
    double vel_prev[num_joints];
    double pos_prev[num_joints];
    for(size_t i=0; i<num_joints;++i){
        eff_prev[i] = eff[i];
        vel_prev[i] = vel[i];
        pos_prev[i] = pos[i];
        eff[i] = cmd[i];
        vel[i] = eff_prev[i]/_dt_inv + 0.97*vel_prev[i];
        pos[i] = vel_prev[i]/_dt_inv + pos_prev[i];
    }
}

/*.....No change...*/

 

 

 

Well, the system get torque input so we should publish proper /command topic.

At this time, I made a action sever to update torque command and a action server to get reference position.

The action server should has these functions.

1)Subscribe reference position topic from the action client

2)Subscribe joint state topic from RobotHW

3)Calculate proper effort(torque)

1) and 3) are may be implemented just using actionlib package but 2) may need some subscriber class.

cmd_server.cpp: define action server class and create a instance of it

the action server calls execution callback invokes job() to implement feedback control.

To get data from the subscriber object, the action server shall be enabled to access subscriber object. So it receives shared pointer(To prevent dangling pointer bug) of the subscriber object argument.

#include <iostream>
#include "ros/ros.h"
#include "sensor_msgs/JointState.h"
#include "std_msgs/Float64.h"
#include <actionlib/server/simple_action_server.h>
#include <functional>
#include "pan_tilt_pi_ctrl/cmd_actionAction.h"
#include "jnt_sts_subscriber.h"

#define SEND_FEEDBACK_MSG 1

using JntMsgCallback = boost::function< void(const sensor_msgs::JointState &)>;

const size_t len = 2;
double e_sum[len];

class cmd_actionAction
{
    protected:
    /*What is name of the action?*/
    std::string action_name;
    ros::Rate cycle_freq;
    /*Declare the action server*/
    actionlib::SimpleActionServer<pan_tilt_pi_ctrl::cmd_actionAction> as_;
    ros::Publisher snake_publisher;
    ros::Publisher scorpion_publisher;
    pan_tilt_pi_ctrl::cmd_actionFeedback fb_;
    pan_tilt_pi_ctrl::cmd_actionResult result_;
    /*Obtain sensor info.*/
    std::shared_ptr<JntStsSubscriber> jnt_info_handle;
    double posi[len];
    /*PI control parameters*/
    const double upper_sat = 25.0;
    const double lower_sat = 25.0;
    const double Kp = 1.0;

    /*Callback fncs. for exec, preempt*/
    void ExecCallback(const pan_tilt_pi_ctrl::cmd_actionGoalConstPtr& _goal){
        if( (as_.isActive() == false) && (as_.isPreemptRequested() == true) ) return; /*Reject when it is not activated or preempted*/
        /*ROS_INFO("This action trying to reach goal %d", goal_tmp); */
        /*Iterates until the job is complete*/
        while(!isJobCplt(_goal)){
            /*When the user input Ctrl + C to shutdown this node*/
            if(!ros::ok()){
                /*Report progress to final count and set this action as aborted.*/
                result_.snake_posi = posi[0];
                result_.snake_posi = posi[1];
                as_.setAborted(result_, "I failed!");
                ROS_INFO("%s is shutted down.", action_name.c_str());
                break;
            }
            /*Reject when it is not activated or preempted*/
            if( (as_.isActive() == false) && (as_.isPreemptRequested() == true) ) return;
            /*Do job something*/
            job(_goal);
            /*Is the job complete?*/
            if(isJobCplt(_goal)){
                /*Report progress to final count and set this action as succeeded.*/
                result_.snake_posi = posi[0];
                result_.scorpion_posi = posi[1];
                as_.setSucceeded(result_, "I succeeded!");
                /*ROS_INFO("%s is reached the goal %d.", action_name.c_str(), goal_tmp);*/
            }
            else{
                #if SEND_FEEDBACK_MSG
                /*Set the feedback and publish a feedback.*/
                fb_.snake_posi_fb = posi[0];
                fb_.scorpion_posi_fb = posi[1];
                as_.publishFeedback(fb_);
                /*ROS_INFO("%s is %d of the goal %d.", action_name.c_str(), progress,  goal_tmp);*/
                #endif
            }
            cycle_freq.sleep();
        }
    }
    void PreemptCallback(){
        /*Report progress to final count and set this action as preempted.*/
	    result_.snake_posi = posi[0];
        result_.scorpion_posi = posi[1];
	    as_.setPreempted(result_,"I got Preempted"); 
	    ROS_WARN("%s got preempted!", action_name.c_str());
    }


    inline void job(const pan_tilt_pi_ctrl::cmd_actionGoalConstPtr& _goal){
        /*Do feedback ctrl*/
        /*Get sensor info*/
        for(size_t i=0; i<len;++i){
            posi[i] = jnt_info_handle->pos[i];
        }
        /*Get error*/
        double e[len];
        for(size_t i=0; i<len; ++i){
            if((e_sum[i] < 20) ||(e_sum[i] > -20) ){
                e_sum[i] += e[i];
            }
        }
        e[0] = _goal->snake_posi_dest - posi[0];
        /*Calculate output*/

        /*Define torque of "snake_controller"*/
        double u[2];
        u[0] = 0.5*e[0] + 0.01 * e_sum[0];
        std_msgs::Float64 snake_msg;
        snake_msg.data = u[0];
        snake_publisher.publish(snake_msg);

    }
    inline bool isJobCplt(const pan_tilt_pi_ctrl::cmd_actionGoalConstPtr& _goal){
        bool retval = false;
        if(_goal->snake_posi_dest == posi[0]){
            retval = true;
        }
        return retval;
    }

    public:
    cmd_actionAction(ros::NodeHandle& _nh, const std::string& _action_name, const std::shared_ptr<JntStsSubscriber>& _jnt_info_handle, const unsigned int& _cycle_freq = 5)
    /*Register execution callback fnc. with initialization list.*/
    :as_(_nh, _action_name, boost::bind(&cmd_actionAction::ExecCallback, this, _1), false)
    /*Set name and cycle frequency of this action.*/
    ,action_name(_action_name), jnt_info_handle(_jnt_info_handle),cycle_freq(_cycle_freq)
    {
        /*Register Preempt callback fnc..*/
        as_.registerPreemptCallback(boost::bind(&cmd_actionAction::PreemptCallback, this));
        /*Advertise /command topic to control joint */
        snake_publisher = _nh.advertise<std_msgs::Float64>("/pan_tilt/effort_controller_snake/command",1);
    }

    
    void StartAction(){as_.start();}
};

int main(int argc, char** argv){
    ros::init(argc, argv, "cmd_action_server");
    ros::NodeHandle nh_;
    ros::NodeHandle nh2_;
    
    /*Allocate same names in same order with the hardware_interface*/
    /*I will change it using service of hardware node*/
    std::vector<std::string> jnt_names;
    jnt_names.push_back("snake_joint");
    jnt_names.push_back("scorpion_joint");
    auto jnt_info_handle = std::make_shared<JntStsSubscriber>(jnt_names);

    ros::Subscriber jnt_status_subscriber = nh_.subscribe("/pan_tilt/joint_states", 1,&JntStsSubscriber::GetMsgCB,jnt_info_handle.get());

    e_sum[0] = 0;
    e_sum[1] = 0;
    ROS_INFO("Starting Feedback control Action Server");
    auto myaction = new cmd_actionAction(nh_, "cmd_action", jnt_info_handle, 10);
    myaction->StartAction();
    while(ros::ok()){
        ros::spinOnce();
    }
    delete myaction;
    return 0;
}

 

 

In this code, I have defined <JntStsSubscriber> class to get joint status from RobotHW.

jnt_sts_subscriber.cpp: 

#include "jnt_sts_subscriber.h"

JntStsSubscriber::JntStsSubscriber(const std::vector<std::string>& _jnt_names)
:num_jnts(_jnt_names.size()){
    id.reserve(num_jnts);
    pos.reserve(num_jnts);
    vel.reserve(num_jnts);
    eff.reserve(num_jnts);
    struct JointNameID{
        std::string Joint_name;
        size_t input_order;
    };
    std::vector<JointNameID> joint_id_container;
    joint_id_container.reserve(num_jnts);
    for(size_t i=0; i<num_jnts;++i){
        JointNameID element;
        element.Joint_name = _jnt_names[i];
        element.input_order = i;
        joint_id_container.push_back(std::move(element));
    }
    std::sort(joint_id_container.begin(), joint_id_container.end(), []
        (const JointNameID& _left, const JointNameID& _right)->bool{
            return _left.Joint_name < _right.Joint_name; /*Order by joint names to get message order*/
        });
    for(size_t i=0; i<num_jnts;++i){
        ROS_INFO("%s is was %d th and now %d th",joint_id_container[i].Joint_name.c_str(), static_cast<unsigned int>(joint_id_container[i].input_order), static_cast<unsigned int>(i));
        /*Store the sort information*/
        id.push_back(joint_id_container[i].input_order);
    }
}

void JntStsSubscriber::GetMsgCB(const boost::shared_ptr<sensor_msgs::JointState>& _jnt_state){
    for(size_t i=0; i<num_jnts;++i){
        /*Get input with inverse-sorting*/
        pos[i] = _jnt_state->position[id[i]];
        vel[i] = _jnt_state->velocity[id[i]];
        eff[i] = _jnt_state->effort[id[i]];
        //ROS_INFO("Get msg: %lf", pos[i]);
    }
}

See sorting in the constructor and subscriber.

joint_state_publisher publishes state of joints in order of name of joints. For example, when I register joints "snake_joint" and "scorpion_joint", Sensor_msgs::JointState is published in order of  "scorpion_joint" to "snake_joint".

(See this link https://github.com/ros-industrial/ur_modern_driver/issues/113)

So, it receives joint names to subscribe and sorting them to get sorting information.

Additionally, the subscriber shall be defined as a class to store subscribed data. Or not, you have to global variable and it makes hard to subscribe multiple topic and hard to understand.

 

 

 

cmd_cliend.cpp: It sends goal message(reference position) to the action server.

#include "ros/ros.h"
#include <iostream>
#include <functional>
#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/terminal_state.h>
#include "pan_tilt_pi_ctrl/cmd_actionAction.h"
/*Shorten long and dirty type definition*/
using GOAL_STATE = actionlib::SimpleClientGoalState::StateEnum;
/*do you wanna have a bad time?*/
#define USE_LAMBDA 1
#define VIEW_FEEDBACK 0

class MyActionClient{
    protected:
    /*Actual action client*/
    actionlib::SimpleActionClient<pan_tilt_pi_ctrl::cmd_actionAction> ac_;
    /*Goal input*/
    pan_tilt_pi_ctrl::cmd_actionGoal goal_;

    void ActiveCB(){
        ROS_INFO("Now the action has activated");
    }
    void FeedbackCB(const pan_tilt_pi_ctrl::cmd_actionFeedbackConstPtr& _feedback){
        /*Report when the server published feedback.*/
        #if VIEW_FEEDBACK
        ROS_INFO("Feedback: Now the state is %lf", _feedback->snake_posi_fb);
        #endif
    }
    /*Non-static member cannot be declared using "auto" but argu*/
    std::function<void(const actionlib::SimpleClientGoalState&,const pan_tilt_pi_ctrl::cmd_actionResultConstPtr&)> DoneCBThis 
    = [this](const auto& state, const auto& _result)->void{
        /*Report the status when it finished in such cases.*/
        auto state_now = ac_.getState();
        if(state_now == GOAL_STATE::SUCCEEDED){
            ROS_INFO("The action has finished successfully.: %lf", _result->snake_posi);
        }else if(state_now == GOAL_STATE::PREEMPTED){
            ROS_INFO("The action has Preempted.");
        }else{
            ROS_ERROR("The action has terminated unexpectly.");
        };
    };

    public:
    MyActionClient(ros::NodeHandle& _nh, const std::string& _to_subscribe = ""):
    ac_(_nh, _to_subscribe,true)    {
        ac_.waitForServer();
    };
    void SendGoal(const double& _goal){
        goal_.snake_posi_dest = _goal;
        ROS_INFO("Sending Goal [%lf]",goal_.snake_posi_dest);
        ac_.sendGoal(goal_,
        DoneCBThis
        ,boost::bind(&MyActionClient::ActiveCB, this)
        ,boost::bind(&MyActionClient::FeedbackCB, this, _1)    );
    }
    void CancelGoal(){
        ac_.cancelGoal();
    }
    const actionlib::SimpleClientGoalState GetState() const {
        auto state_now = ac_.getState();
        return state_now;
    }
};

int main (int argc, char **argv) {

    /*run this code with argument to control "snake" "snake controller"*/
    ros::init(argc, argv, "cmd_action_client");
    ros::NodeHandle nh_;


    /*Set a goal from argument 1*/
    pan_tilt_pi_ctrl::cmd_actionGoal goal;
    goal.snake_posi_dest = atof(argv[1]);


    ROS_INFO("Waiting for action server to start.");
    auto myActClient = new MyActionClient(nh_, "cmd_action");
    ROS_INFO("Action server started, sending goal.");

    myActClient->SendGoal(atoi(argv[1]));


    ros::Rate sleep_rate(1);
    while(ros::ok()){
        auto GOAL_STATE = myActClient->GetState();
        ROS_INFO("Goal status: %s", GOAL_STATE.toString().c_str());
        if((GOAL_STATE != GOAL_STATE::ACTIVE)&&(GOAL_STATE != GOAL_STATE::PENDING))
        {
            delete myActClient; /*Goal is deadvertised by delete*/
            break;/*E.N.D*/
        }
        sleep_rate.sleep();
        ros::spinOnce();
    }
    return 0;
}

 

I will skip detailed descriptions such as launch files and action_msg definition.

However, full source code is uploaded in my github.

https://github.com/SagalinNagoya/pan_tilt_pi_ctrl

GitHub - SagalinNagoya/pan_tilt_pi_ctrl: urdf model of pan/tilt robot and control joints with feedback control action to hardwar

 

Let's give reference position 5.0

rosrun pan_tilt_pi_ctrl pan_tilt_pi_ctrl_cmd_client 5.0

We can see snake_joint get command which changes and its position converges to 5.

Also, we can see the control structure using rqt_graph.