Retrieve end effector cartesian pose in MoveIt

[Flavian Hautbois]

Hi everyone,

I am creating a node that has to publish the pose (and if possible twist) of the end effector. My robot publishes a tf which I think could be used in this fashion. But I would also like to benefit from MoveIt's high-level API which allows to give names to the end effectors.

Additional question: if you have anything for the end effector twist, that would be very helpful.

F

[Simon Jansen]

Hey,

Assuming you're using the high-level movegroup interface, could you not do something like this:

moveit::planning_interface::MoveGroup *group;

group = new moveit::planning_interface::MoveGroup("arm");

group->setEndEffectorLink("hand_left");

geometry_msgs::PoseStamped current_pose = group->getCurrentPose();

regards,

Simon

[Flavian Hautbois]

I'm a bit confused here. Why do you have to specify an EndEffectorLink, when you already define end-effectors during the MoveIt configuration?

My robot publishes on robot_state, and is configured to have exactly one end-effector named "manipulator". 

When I want to move it through joint_trajectory_action, I call:

move_group_interface::MoveGroup group("manipulator");

group.setPoseTarget(pose);

group.move();

and this moves my robot.

Now I want to do the reverse operation on the feedback state, which is basically what is supposed to be the "scene_robot", in a way that I can pass only my "manipulator" end-effector as an argument and get the current pose.

Of course I tried: 

move_group_interface::MoveGroup group("manipulator");

group.getCurrentPose();

but this didn't work (for some reason...), meaning that I didn't get true values. So there must be a pointer issue, I have to bind this group to the right robot model.

How do I do that?

Thanks 

[Simon Jansen]

The setEndEffectorLink is indeed optional. By default moveit will use the end effector specified in your condfig. However, I added it to show that you can get the pose from any link of the robot in this manner.

The way you get the pose works for me normally. I did have some trouble sometimes that it returned zeros or other wrong values. I think it was a timing issue. Try adding a delay between the creation of the group and the getCurrentPose function. What also did the trick was calling the function twice. Not sure why, but the first call would return zeros and the second the true values.

regards,

Simon

[Flavian Hautbois]

To give closure to this thread, I managed to make it work by adding

ros::AsyncSpinner spinner(1);

spinner.start();

at the beginning of the file.

[Ioan Sucan]

Flavian,

The MoveGroup object requires ROS communication for monitoring the current robot state using the /joint_states topic. This is why the ROS spinner was needed. For the issue that Simon noticed, the first time you call the function a CurrentStateMonitor (http://moveit.ros.org/doxygen/classplanning__scene__monitor_1_1CurrentStateMonitor.html) object is internally constructed and only one second of delay is allowed until all values for all joints must have been received. This sometimes does not happen, which leads to incorrect values.

Ioan

[Simon Jansen]

Ah, that explains a lot :)

So better practice would be to start the state monitor first, by calling startStateMonitor?

[Ioan Sucan]

Simon,

Yes, that is the case. The catch is though, if you are using the move_group_interface::MoveGroup class, such a function is not exposed. I can add one though. Would that be useful?

Ioan

[Simon Jansen]

It would indeed be useful if there was some way to start the state monitor via the MoveGroup class interface, yes. It would be neater than having to call the getCurrentPose first.

Is there a drawback to just startingthe state monitor on construction of the MoveGroup object directly? Perhaps there is something I'm overlooking.

[Ioan Sucan]

On Fri, Oct 11, 2013 at 10:53 AM, Simon Jansen <seps...@gmail.com> wrote

It would indeed be useful if there was some way to start the state monitor via the MoveGroup class interface, yes. It would be neater than having to call the getCurrentPose first.

I will add that.
 

Is there a drawback to just startingthe state monitor on construction of the MoveGroup object directly? Perhaps there is something I'm overlooking.

This was just an efficiency consideration.

Ioan

[Ioan Sucan]

Function added.

Ioan

[Flavian Hautbois]

Good job!

By the way, I'd like to know if there is a way to retrieve the end effector pose, and know if it's an updated value or not?

The reason for that is that I am also calculating the twist between two poses in a while loop. However, since the loop is faster than the update rate of the robot state, I obviously end up with a velocity of 0 when the same pose is called twice (or more).

Is there a function "is_new()" or something like that? Or should I subscribe to a topic in particular to synchronize my pose requests with the robot_state update? 

2013/10/11 Ioan Sucan <isu...@gmail.com>

--
F. Hautbois

[Ioan Sucan]

Flavian,

In this case I recommend using a CurrentStateMonitor class directly and subscribing to the update callback, so you know when an update is received. Then you can read the pose of the end effector from the reported state.

Ioan

[Flavian Hautbois]

So, I get what you mean, but I would need a code example to see it more clearly, could you provide one?

(I searched the MoveIt! website but didn't find anything)

2013/10/11 Ioan Sucan <isu...@gmail.com>

--
F. Hautbois

[Ioan Sucan]

Something along these lines should do:

void myfun(planning_scene_monitor::CurrentStateMonitor*cs, const sensor_msgs::JointStateConstPtr &)
{

// called when there are updates;

moveit::core::RobotStatePtr rs = cs->getCurrentState();

const Eigen::Affine3d &tf = rs->getLinkTansform(...);
...

}
....

planning_scene_monitor::CurrentStateMonitor cs(robot_model);

cs.addUpdateCallback(boost::bind(myfun, &cs, _1));
cs.startStateMonitor();