Feature request: Expose tool pose from RT connection in ROS

[sepjansen]

The standard URDF available in ROS Industrial (https://github.com/ros-industrial/universal_robot/blob/indigo-devel/ur_description/urdf/ur5.urdf.xacro) does not give a perfect transform between base_link and ee_link, because each UR is calibrated individually and the URDF assumes nominal values for link dimensions, etc. This leads to an error in the order of [mm].

Extracting the calibrated DH parameters from a UR robot and use that to build a more accurate URDF model is possible but rather cumbersome and still does not return an exact solution (mismatch is about 0.1 mm in our experiments).

I think it would be possible to expose the tool pose that UR sends over the RT connection in ROS either as a geomtry_msg/PoseStamped topic or directly via /tf. That way, the URDF model can still be used for collision checking and visualization, but the more accurate tool pose can be used when calculating pick positions, etc.

Is there interest to expand the ur_modern_driver with this?

[ThomasTimm]

I was not aware of this discrepancy, but it definitely sounds like a really useful feature.
The information is already available and should thus be easy to publish via tf.

The best way to implement it would obviously be to have some parameters specifying the root and child link names, as ee_link cannot be reused (otherwise the driver and robot_state_publisher would constantly publish opposing information). Do you have any good suggestions for a default name for the end effector link?

[miguelprada]

Extracting the calibrated DH parameters from a UR robot and use that to build a more accurate URDF model is possible but rather cumbersome and still does not return an exact solution (mismatch is about 0.1 mm in our experiments).

Do you have any idea of why this happens @sepjansen?

Having an accurate URDF is a better option, if feasible. Otherwise application code relying on the URDF to control the robot in joint space (e.g. MoveIt!, several ros_control controllers) will need to deal with additional correction terms to account for this discrepancy.

[ThomasTimm]

Obviously it would be better to have accurate URDFs, but if there truly is a small difference between each robot, that isn't really a possibility.
For now, I think the best approach would be to have the driver publish the true transform to the end effector and having people use that frame for MoveIt! and ros_control instead of ee_link or tool0 from the URDF (Which one is being used by MoveIt @miguelprada ?).
And then work on getting that joint transformation in the URDF changed/removed...
Alternatively (for better backwards compatibility) the driver could try and read the robot_description from the parameter server and then modify the string to add another link (ee_link_controller?) and then overwrite the robot_description paramteer? This requires that the plugins notices the new parameter value though, so it is not a very stable approach.

[sepjansen]

@miguelprada: We're not sure where the difference comes from. Perhaps UR does a correction based on joint temperature as well.

Another important drawback of trying to build a more accurate URDF is that the calibration procedure from UR does not seem to care about the positions of the intermediate joints, as long as the overall transform from base_link to ee_link is correct. On our robot for example, the distance between base_link and shoulder_link is 914 meter. Since distance between shoulder and elbow is around the same, the final transformation is correct.
This means the corrected URDF is impractical, because the link positions are incorrect, so collision checking would make no sense.

@ThomasTimm: default name for ee_link could be something like 'ee_link_calibrated'. Note that 'ur_ee_link' also already exists URDF.

[miguelprada]

@ThomasTimm Both MoveIt! and some ros_control controllers require having a model of the robot to compute either FK or IK, generally using KDL. Having just the current real pose of the end effector would not help much in these cases.

@sepjansen Is that really 914 meter!? I'm struggling to understand how a model which doesn't care about intermadiate links can actually work and compute accurate inverse kinematics in the controller itself.

In any case, I wouln't want to get in the way of a fix which works for you. I guess I'll come back to this issue whenever we receive our own URs, which we don't have yet.

[sepjansen]

@miguelprada : Yes, it really is 914 meter. I was baffled at first too. This will probably vary greatly between individual URs, so maybe ours is an exception.

[miguelprada]

@sepjansen The only reason I can see this working is if such distances are detected along the joint axes of rotation. This way one error would cancel with the error on the next link at any joint rotation value. Can you confirm this is the case?

It shouldn't be too difficult to correct the paremeters if it is, I think.

[ThomasTimm]

I just though about @sepjansen original idea about using a geomtry_msg/PoseStamped msg, it actually has an advantage: it supports publishing the TCP velocity, not just its current position... Could it be relevant to have both a tf a and a pose msg published? The data is being parsed anyway, its just a matter of serializing the messages and publishing it.

[sepjansen]

@miguelprada: I've included the joint transformations that were based on the DH parameters for our UR5 robot. Hopefully, it will give some insight in the relations.
ur5_calibrated_joints.urdf.txt

@ThomasTimm: I think having the actual TCP velocity can be relevant, yes. So that would involve publishing a tf and a twist msg, right? Would there be an advantage in publishing the pose msg next to the tf? They would basically contain the same information.

[ThomasTimm]

Ahh my bad, for reasons unknown I though the pose msg had velocity as well, just as the joint_state msg has both position and velocity... So the preferred solution would be to publish tf between base_link and ee_link_calibrated while simultaneously publishing a twistStamped with the header->frame_id set to ee_link_calibrated (and of course read the two strings "base_link" and "ee_link_calibrated" from the parameter server)?

[sepjansen]

Yes, that make sense I think. I actually already started implementing this. I'll create a pull request when done.

It think it's easiest to include these publishers in the publishRTMsg function, do you agree? Alternatively, we could put them in a separate thread.

[ThomasTimm]

yea, that was also my idea, but I'll happily leave it to you :-)

I guess you already know, but you can get TCP pose/vel with robot_.rt_interface_->robot_state_->getToolVectorActual() and robot_.rt_interface_->robot_state_->getTcpSpeedActual();
Just be sure to read the values before https://github.com/ThomasTimm/ur_modern_driver/blob/master/src/ur_ros_wrapper.cpp#L574 to avoid any races

[sepjansen]

Thnx for the tip :) I'll keep you posted on development

[shaun-edwards]

It would be great if we adopted the ROS-Industrial standard frame names. The standard already has a frame called base, which should match the controller base frame. We would need to add a new frame to represent the controller tool frame, named something appropriate. The new frame could be added to the URDF as a floating joint (now supported by robot_state_publisher here).

It's also worth noting, that the analytic IK does not use the URDF values, so creating a dynamic URDF would not solve possible incorrect IK, if the analytic solver is used.

@ThomasTimm, @ipa-fxm, I would like to see this issue addressed in the ROS-Industrial repo. Addressing it here and then merging into the ROS-I is going to be harder because of backwards incompatibilities.

[sepjansen]

Good point. I wasn't aware of this standard, but I see now that the UR urdf's conform to this (https://github.com/ros-industrial/universal_robot/blob/indigo-devel/ur_description/urdf/ur5.urdf.xacro).

I'll set the default frame names to base and tool0-calibrated.

[shaun-edwards]

Can we use tool0_controller or something similar. All controllers can report their tool pose, but not all will be using an internal calibration. Most industrial robots do no use a calibration (at least I'm not aware of them doing so), other than joint zero calibration which should be reflected in the joint states.

[ThomasTimm]

Thanks for chiming in Shaun.

My hope is that by handling this issue by publishing extra topics/frames, it won't lead to any forking problems or backwards incompatibilities once this driver is merged into ROS-I.

[ThomasTimm]

@sepjansen

Could you write a mini tutorial on how to use the tool0_controller frame?
Don't worry about formatting (although you are more than welcome to do that as well), even if it is only a small draft it will still be better than nothing.

[sepjansen]

Yeah, sure. I'm assuming you mean: How to use tool0_controller in a URDF model (using floating joint)?

I'm guessing a short example urdf will help most. Is there a good place to put this? In the README perhaps?

[ThomasTimm]

Yea, that would be great.
Once the transition to ROS-I is complete and this package is available on the ros wiki, I would like to turn it into a separate tutorial, but for now that would probably be the right place to put it.

[jeppewalther]

@sepjansen How do you extract DH parameters from the robot ?

[sepjansen]

@jeppewalther: The parameters are stored in a calibration file on the controller. You should probably ask your UR supplier how to access it. There's not really an easy way to access the parameters, which is one of the reason I prefer this solutions rather than manually retrieving the parameters.

[ThomasTimm]

As universal robots now have their support site open to everyone, I think we can safely disclose such information, as it is freely available on their homepage at http://www.universal-robots.com/support/
If people already know the username and password to access i.e. /root/.urcontrol/calibration.conf on the robot and thus have access to the filesystem, I don't see any point in asking them to contact their supplier... Especially since my experience with suppliers is that they hardly know what DH parameters are...

@sepjansen Can we close this issue?

[sepjansen]

Ah, I had no idea this information was already freely available on the support site 👍

You can find more details in this file, detailing how to restore calibration file s after replacing a flash card.

Yes, let's close the issue then.

[jeppewalther]

@sepjansen Thanks for the link thats very usefull.