Hi,
What is the right way to handle the joints position using given ROS plugin?
Best Regards,
Salman
Originally posted by skhan on Gazebo Answers with karma: 131 on 2013-04-11
Post score: 3
Comment by gazer on 2013-05-23:
Hi. I am curretnly working on the same topic. would you like to share some of your idea?
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
Originally posted by AndreiHaidu with karma: 2108 on 2013-04-12
This answer was ACCEPTED on the original site
Post score: 8
Comment by skhan on 2013-04-12:
Thanks Andrei, m gona try it. I hope it will work for me =)
Comment by Ben B on 2013-04-18:
That's great, Andrei. Have you considered working this into a tutorial on the wiki?
Comment by AndreiHaidu on 2013-04-19:
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.
Comment by Ben B on 2013-04-19:
It could expand this one: http://gazebosim.org/wiki/Tutorials/1.5/ros_enabled_model_plugin or be a second part to it -- under the "ROS Integration" section.
Comment by gazer on 2013-06-14:
is this working?
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:
Start gazebo with ROS API plugin:
$ gazebo -s libgazebo_ros_api_plugin.so
Add pr2_controller_manager to your SDF model file:
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.
Originally posted by Boris with karma: 330 on 2013-04-12
This answer was NOT ACCEPTED on the original site
Post score: 1
Comment by skhan on 2013-04-12:
Thanks Boris, I give it a try and also wait that someone post a precise solution.