Transform from tool frame to end-effector frame

[Patrick Goebel]

Hello,

It seems MoveIt computes IK for a target pose of the end-effector link
which is usually the point of attachment of the gripper to the arm. But
it also seems that it might be more natural to plan in a "tool frame",
for example, a frame located in between the fingers of the gripper. If
I have this tool frame defined in my URDF and I have a desired pose for
the tool frame (say relative to the base_link frame), how do I "back
transform" this into a target pose for the end-effector link? Does
MoveIt have such a function itself, or does one have to use tf? And if
one has to use tf, has anyone already figured out the magic
incantation? I've been trying for hours to work it out using tf and
quaternions but my head is just going in circles.

Thanks!
patrick

[Martin Günther]

Hi Patrick,

you can set up MoveIt so it treats your URDF tool frame as the IK tip
link. I've been using that setup for a long time, and it works for me.
My URDF [1] is set up like this:

-> lf
/
b -> m1 -> m2 -> m3 -> m4 -> m5 => g => gtf
\
-> rf

-> = revolute joint
=> = fixed joint

b = katana_base_link
m1 = katana_motor1_pan_link
m2 = katana_motor2_lift_link
m3 = katana_motor3_lift_link
m4 = katana_motor4_lift_link
m5 = katana_motor5_wrist_roll_link

g = katana_gripper_link
gtf = katana_gripper_tool_frame

lf = katana_left_finger_link
rf = katana_right_finger_link

Now, in my SRDF [2] I've defined the groups like this:

<group name="arm">
<!-- tip_link is also IK tip link -->
<chain base_link="katana_base_link" tip_link="katana_gripper_tool_frame" />
</group>
<group name="gripper">
<link name="katana_gripper_link" />
<link name="katana_r_finger_link" />
<link name="katana_l_finger_link" />
</group>

Intuitively, the links {b...m5} and all the joints in between are
my "arm", and {g, lf, rf} and the two finger joints are my "gripper".
The way I've defined the groups however, the two links {g, gtf} are
now part of both groups, but since there are only fixed joints involved,
MoveIt is fine with that.

The important part is that the "tip_link" of your chain implicitly defines
the IK tip link. I've tested that printing out the `tip_name`
in the `initialize()` method of my IKFast plugin. If you define your chain
wrongly, the poses will be passed to the IKFast plugin in the wrong tf frame.
Since the IKFast plugin doesn't check that it can actually handle the `tip_name`,
it will misinterpret the request and fail to find any IK solutions.

Another thing: You don't need to define a collision geometry for your tool
frame, but you should (in order to work around [3]).

Cheers,
Martin

[1]: https://github.com/uos/katana_driver/blob/hydro/katana_description/urdf/katana_450_6m90a.urdf.xacro

[2]: https://github.com/uos/calvin_robot/blob/hydro/calvin_moveit_config/config/calvin.srdf#L14

[3]: https://github.com/ros-planning/moveit_core/issues/158

[Patrick Goebel]

Hi Martin,

This is absolutely brilliant! Many thanks for spelling it out so
nicely. Everything now works like a charm. If this info isn't already
on the Wiki somewhere, I'd vote for doing a copy-and-paste of your reply
into the setup instructions.

--patrick

P.S. I still don't have my IKFast plugin working but I'll try a few more
attempts before bugging the list.

[Zoss, Jeremy K.]

Patrick,

 

Here's my best understanding.  I'm sure someone will correct me if I'm too wrong.

 

1) You typically send a motion planning request to MoveIt using a MoveGroupAction or MotionPlanRequest.  The actual request syntax can take many forms (action, C++ call, etc), but contains the same core information.  This request includes the "group_name" (defined in the MoveIt Package SRDF) and a set of "goal_constraints" (typically the target cartesian or joint position).  At first glance, it is unclear how Cartesian goal positions are specified (what reference/base frame, what target/tool frame, etc).

 

2) Digging into the MotionPlanRequest reveals that Cartesian goals ultimately resolve into a combination of PositionConstraint and OrientationConstraint.  Both of these messages contain a “link_name” field that specifies which link (frame) the target position refers to.  Typically, this link_name gets filled in as the “tip frame” of your SRDF planning group.  Similarly, the “header.frame_id” field provides the name of the reference frame for the given position.  Typically, this frame is filled in with the MoveIt planning frame (the parent frame of the “Virtual Joint” defined in the MoveIt config).

 

3) There are methods provided to change these reference frames, but they’re specific to how you interface with MoveIt.  Here are some examples using the moveit::planning_interface::MoveGroup class:

 

  3a) Specify the frame you’re trying to control using the “end_effector_link” argument to most setPoseTarget() commands.  Or use the setEndEffectorLink() method to change the default link for all following commands.  Review the code in MoveGroup::MoveGroupImpl::constructGoal() and kinematic_constraints::constructGoalConstraints to see how those values populate the MotionPlanRequest fields. 

 

  3b) Specify the reference frame of the commanded cartesian goal using the setPoseReferenceFrame() method.  See one of the MoveGroup::setPoseTarget() methods for how this value populates into the MotionPlanRequest header.

 

4) So, to plan for a tool-frame that’s not part of your robot kinematic chain, I think you should be able to call moveGroup.setEndEffectorLink(“tcp_frame”)  (or your equivalent) to allow you to specify the target position of your TCP rather than the wrist flange.  I haven’t tested this, so I can’t be sure it actually works.  It’s entirely likely that your target frame must be defined in the URDF used to create your MoveIt config package, but that’s often not too difficult to do.

 

5) Alternatively, if you don’t trust all that black magic, you can do it manually:

  void move_tcp_to_pos( const geometry_msgs::Pose& world_to_tcp)

  {

    tf::Transform world_to_wrist_tf, world_to_tcp_tf, tcp_to_wrist_tf;

    geometry_msgs::Pose world_to_wrist;

 

    // lookup transform (this should be cached, since it’s probably static)

    tf_listener->lookupTransform(“tcp_frame”, “tool0”, ros::Time(0.0f), tcp_to_wrist_tf)

 

    // convert goal to TF data type, for easy math

    tf::poseMsgToTF(world_to_tcp, world_to_tcp_tf);

 

    // apply the transform from TCP->wrist

    world_to_wrist_tf = world_to_tcp_tf * tcp_to_wrist_tf

 

    // convert TF data type back to the type used in MoveIt Pose commands

    tf::poseTFToMsg(world_to_wrist_tf, world_to_wrist);

 

    // send command to moveIt

    move_group_ptr->setPoseTarget(world_to_wrist);

    move_group_ptr->move();

  }

 

Hope this helps!

  Jeremy Zoss

  Robotics & Automation Engineering

  Southwest Research Institute

[Zoss, Jeremy K.]

One caveat to this approach:

If you do include the end-effector into your MoveIt planning chain, such that the "tip_frame" is now your TCP rather than the robot wrist-flange, then you MUST make sure your kinematics plugin uses the same configuration. For the standard KDL solver, this is not an issue, but it *does* come into play when creating an analytic kinematics plugin (e.g. using IKFast).

It's not too big a deal to create an IKFast solution that includes the static end-effector link, but this makes your analytic kinematics plugin non-portable to other applications that use different end-effector geometry. Similarly, there are no double-checks to verify that the kinematics used by your solver are the same as those represented in your URDF. Bad things can happen if you change your end-effector geometry in the URDF but forget to update your IKFast kinematic plugin.

It is for these reasons that we typically create our ROS-Industrial moveit packages using a "basic" kinematic chain that only extends from the robot base to its wrist flange. This keeps the kinematics simple and portable. We then use other approaches (outlined in my previous email) to allow for planning TCP poses in arbitrary reference frames.

Both approaches work. Just keep in mind the tradeoffs.

Thanks for the discussion!

Jeremy Zoss
Robotics & Automation Engineering
Southwest Research Institute

-----Original Message-----
From: moveit...@googlegroups.com [mailto:moveit...@googlegroups.com] On Behalf Of Patrick Goebel

[Patrick Goebel]

Thanks Jeremy--this is immensely helpful to my understanding.  I'm working with the Python API and I'd swear I tried the set_end_effector_link() function without success but I might have had issues in my URDF model at the time.  So I'm going to go back and try again since (regarding your second message with the caveat) I would prefer to do it the way you suggest.

And thanks for the tf code--I'll try converting it to Python and seeing if I can make it work.

--patrick

[Patrick Goebel]

Hi Jeremy,

So I went back to a standard MoveIt configuration with the arm group chain being defined only up to the wrist flange and the gripper group including the tool frame link.  It turns out that using set_end_effector_link("tcp_frame") *does* work but only up to a point.  The reason I failed to see it before is that for some reason, I need to set the position and orientation tolerances higher than with Martin's solution for an IK solution to be found.

For example, suppose I set a target pose with the gripper held out horizontally 0.17 m directly in front of the robot and 0.85 cm off the ground.  This pose is well within the range of the arm.  With Martin's solution, I can dial the tolerances down almost as low as I like--for example 0.001 for position and 0.01 for orientation and the solution is found on the first try.  However, with the standard configuration and using set_end_effector_link("tcp_frame"), the best I can get on position tolerance is 0.04 with an orientation tolerance of 0.1.  If I go below that, I get a final failure message of the form:

[ INFO] [1394889030.613797484]: RRTConnect: Created 1 states (1 start + 0 goal)
[ INFO] [1394889030.613824461]: No solution found after 3.682868 seconds
[ INFO] [1394889030.613862985]: Unable to solve the planning problem
[ INFO] [1394889030.613910255]: Planning attempt 5 of at most 5
[ INFO] [1394889030.615964154]: RRTConnect: Starting with 1 states
[ WARN] [1394889032.709080083]: More than 80% of the sampled goal states fail to satisfy the constraints imposed on the goal sampler. Is the constrained sampler working correctly?
[ INFO] [1394889034.986685362]: Position constraint violated on link 'right_gripper_link'. Desired: 0.170000, 0.000000, 0.850000, current: 0.236250, -0.019024, 0.859036
[ INFO] [1394889034.986894588]: Differences -0.0662503 0.0190239 -0.00903588
[ INFO] [1394889034.987066630]: Orientation constraint satisfied for link 'right_gripper_link'. Quaternion desired: 0.000000 0.000000 0.000000 1.000000, quaternion actual: -0.011138 -0.020855 -0.009530 0.999675, error: x=0.022689, y=0.041497, z=0.019536, tolerance: x=0.050000, y=0.050000, z=0.050000
[ERROR] [1394889035.620520628]: RRTConnect: Unable to sample any valid states for goal tree

Increasing the planning time to 15 seconds does not help.  Furthermore, with the standard configuration, pick and place does not seem to work at all regardless of how high I make the tolerances or planning time so I'm wondering if the set_end_effector_link() is actually being propagated to the pick function.

So for now, Martin's solution seems to work best although it would be nice if the standard configuration worked as well.

--patrick

On 03/14/2014 01:12 PM, Zoss, Jeremy K. wrote:

[Bas de Bruijn]

Hi Guys,

I've come here thru a link in a thread on the ROS-i list and hope someone can nudge me in the right direction.

My robot is hardware driven by Machinekit, where Machinekit is taking in trajectories that are planned from Moveit. Right now I've got an application where we plan to move many small products, with differently placed holes to a fixed (world) tool. A video so you can see the mechanics (and the degrees of freedom (5) it has) https://youtu.be/h_oO0OhFvQY

I’ve taken a look, and updated myself w.r.t. to tf (the turtle tf tutorial) and I think option 4 or 5 as outlined above would be my way to go, w.r.t. processing data of many products with varying locations. Basically the product is fixed/mounted in an insert which is attached to the effector (my tool).

so my choice seems to be:

either option 4:

attach a frame (depicting the hole location transform) from the tool0 link of the arm, and then plan this frame to the required world location. If this doesn't work (because this frame needs to be in the URDF) can I then for different hole locations change that frame (on the fly) and replan?

or option 5:

My robot is a 5DOF robot, and I while i understand making a transform from the product to the required pose of the arm, I realise I fail to see the mechanism how this works if I don’t know the exact pose of the product. Because that pose is determined by the arm itself. 

For example, I only know the hole location (x,y,z) and it being “level” (no rot around x nor y) the rotation around the Z axis is determined by the arm because of the 5DOF situation.

or is there another option in which I can attach/change tool urdfs from different tools on the fly? Like a real tool change? That could be a (not preferred, but workable) solution as opposed to generate a configuration for each and every single hole location of each and every product. That would be a maintenance nightmare for somebody that has to work with that :)

I hope I got the right terminology.

Any pointers would be welcome.

Thanks in advance

Bas

[boooooo...@gmail.com]

Hi, Martin:
Sorry for digging up this thread almost three years later. I am using the ROS-I with the Fanuc M20-iA. I am facing the same problem as how to take the end effector's dimension into the account when doing the motion plan. inspired by your post, I created a virtual link(let's say, tip link) which connect to the tip of mt end effector(let's say, eff link), and claim the tip link as the end effector group while I count the eff link as part of the manipulator (since it is fixed to J6, so I see it as a extension of the J6). By doing this, I actually succeeded getting the end effector's offset for each plan but here comes another problem: When I start my own Move Group Interface, I can not find my end effector, even I use setEndEffector() function I will get a empty string when I call the getEndEffector(), I am not quite sure how to solve this, or maybe my approach of getting the offset of the end effector is wrong?

Many thanks!
Best,
Bosch

在 2014年3月14日星期五 UTC-7上午3:52:38，Martin Günther写道：

[boooooo...@gmail.com]

Alright, after sometime I found out why:
1. In the manipulator group you still have to account the end effector link as part of the manipulator group
2. An interesting point about SRDF is that for the name of the end_effector group, you have to specify the same name as the end_effector link or there will be an error saying that no link of such name is found.

Best,
Bosch

在 2014年3月14日星期五 UTC-7上午3:52:38，Martin Günther写道：

On Thu, 13 Mar 2014 21:22:26 -0700