Home | Tutorials | Wiki | Issues
Ask Your Question
3

Set and Get position of gazebo model, using ROS plugin.

asked 2013-04-11 09:46:26 -0500

skhan gravatar image

updated 2013-04-11 09:50:55 -0500

Hi,

What is the right way to handle the joints position using given ROS plugin?

Best Regards,

Salman

edit retag flag offensive close merge delete

Comments

Hi. I am curretnly working on the same topic. would you like to share some of your idea?

gazer gravatar imagegazer ( 2013-05-23 16:05:14 -0500 )edit

2 Answers

Sort by ยป oldest newest most voted
8

answered 2013-04-12 07:20:06 -0500

AndreiHaidu gravatar image

Hi,

I will write a short example how to listen to a ROS topic, and control the joint regarding the position you get from the topic, the control will be via a PID controller. Here is some info about PID controllers.

You will change the value of the joint target position in the Callback function, and compute the effort and apply it on the joint in the OnUpdate() function.

Then you can just create a ROS subscriber and init the PID in your Load() function, something like:

void ROSPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
    {

      int argc = 0;
      char** argv = NULL;
      ros::init(argc,argv,"ROS plugin");
      this->rosnode = new ros::NodeHandle();

      // Store the pointer to the model
      this->model = _parent;

      // Store the joint you want to control
      this->joint = this->model->GetJoint("joint_name");

      // initialize the pid ( common::Pid ), 
      // you have to see for yourself which values works best for you
      this->jointPID (10, 0, 0, 10, -10);

      // subscribing to the rost topic, and calling the callback function
      this->ROS_subscriber = this->rosnode->subscribe("/ROS_topic", 10, &ROSPlugin::ROSCallback, this);

      // Listen to the update event. This event is broadcast every simulation iteration.
      this->updateConnection = event::Events::ConnectWorldUpdateStart(boost::bind(&DataGlovePlugin::OnUpdate, this));

    }

The callback function from the ROS topic:

void ROSPlugin::ROSCallback(const package_name::MsgType& msg)
{
     // you update the position of where you want the joint to move
     this->joint_target_pos = msg.pos;
}

The OnUpdate() function:

void ROSPlugin::OnUpdate()
{
    // compute the steptime for the PID
    common::Time currTime = this->model->GetWorld()->GetSimTime();
    common::Time stepTime = currTime - this->prevUpdateTime;
 this->prevUpdateTime = currTime;

   // set the current position of the joint, and the target position, 
    // and the maximum effort limit
    double pos_target = this->joint_target_pos;
    double pos_curr = this->joint->GetAngle(0).Radian();
    double max_cmd = this->joint_max_effort;

    // calculate the error between the current position and the target one
    double pos_err = pos_curr - pos_target;

    // compute the effort via the PID, which you will apply on the joint
    double effort_cmd = this->jointPID.Update(pos_err, stepTime);

    // check if the effort is larger than the maximum permitted one
    effort_cmd = effort_cmd > max_cmd ? max_cmd :
                (effort_cmd < -max_cmd ? -max_cmd : effort_cmd);

    // apply the force on the joint
    this->joint->SetForce(0, effort_cmd);

}

If you have problems implementing the idea let me know.

Cheers, Andrei

edit flag offensive delete link more

Comments

Thanks Andrei, m gona try it. I hope it will work for me =)

skhan gravatar imageskhan ( 2013-04-12 08:15:42 -0500 )edit

That's great, Andrei. Have you considered working this into a tutorial on the wiki?

Ben B gravatar imageBen B ( 2013-04-18 20:40:34 -0500 )edit

Thanks, I did not think about it, but if you think it's worth it, I could complete plugin and write it as a tutorial. I just need to know where to put it, and under which title.

AndreiHaidu gravatar imageAndreiHaidu ( 2013-04-19 10:42:02 -0500 )edit

It could expand this one: http://gazebosim.org/wiki/Tutorials/1.5/rosenabledmodel_plugin or be a second part to it -- under the "ROS Integration" section.

Ben B gravatar imageBen B ( 2013-04-19 13:07:33 -0500 )edit

is this working?

gazer gravatar imagegazer ( 2013-06-14 17:30:40 -0500 )edit
1

answered 2013-04-12 03:49:26 -0500

Boris gravatar image

Cannot tell for the v1.5, but in v1.4 you can use ROS plugins from gazebo and pr2_gazebo_plugins packages to control a kinematic chain. There are also GazeboRosDiffdrive plugin in gazebo_plugins package.

So, it really depends on complexity of your system and the control goals.

For instance, to control robotic arm with robot_mechanism_controllers you will need to do the following:

  1. Start gazebo with ROS API plugin:

    $ gazebo -s libgazebo_ros_api_plugin.so
    
  2. Add pr2_controller_manager to your SDF model file:

    <sdf>
        ...
        <model>
            ...
            <plugin name="controller_manager" filename="libgazebo_ros_controller_manager.so" />
        </model>
    </sdf>
    
  3. Now you can use ROS robot_mechanism_controllers with your model.

Of course you have to add the paths to these plugins to GAZEBO_PLUGIN_PATH.

Above approach suits my specific needs, though you may find it more appropriate to write your own ROS-enabled model plugin. Also see this tutorial.

I was unable to make it all work with v1.5, so it might be necessary to modify some sources.

edit flag offensive delete link more

Comments

Thanks Boris, I give it a try and also wait that someone post a precise solution.

skhan gravatar imageskhan ( 2013-04-12 04:09:49 -0500 )edit
Login/Signup to Answer

Question Tools

3 followers

Stats

Asked: 2013-04-11 09:46:26 -0500

Seen: 12,075 times

Last updated: Apr 12 '13