Calibrated URDF

[fmauch]

As URs are factory calibrated and tcp positions can differ to those coming from the decription's FK, it would be nice to reflect this in the description, as well.

I went ahead, creating a model from the calibrated DH parameters, which results in a model, where the two long arm links are far off with one of our robots:

I next wrote a routine that draws in the segments on their rotation axis, which works reliably. This will be released as well, but first I wanted to state the question on how to integrate this. As I don't want to establish another ur_description package, I'd like to integrate it here.

As you can see in the screenshot, I established two links per robot segment to respect the DH parameters. From my calbration-"correction" I gain another set of parameters, where the shoulder and elbow offset are set to 0, so that a visually appealing robot can be modeled. These are of course no strict DH parameters anymore and I needed two additional parameters to counter a rotation introduced by a non-zero alpha parameter when setting the d-parameters to 0.

First of all: I am aware, that a yaml-based presentation as proposed in #371 might be better and I will probably adapt to this later. Also I am aware, that there are a lot of other things attached to this such as inertias, which I haven't covered so far. This is still at an early stage, I just wanted to clarify the direction before implementing something that will get rejected anyway.

Now to the real questions:

• Would a one-link-per-segment policy be better? This would need to actually save a full transformation parameter set (3dof translation, 3dof rotation) per link as there aren't that many zero-entries anymore. On the other hand, the DH parameters not being pure DH parameters anymore is also rather implicit...
  To be specific, imagine parameters alpha_1=pi/2 + 0.2 and theta_2=0.2. This results in a rotation

in Quaternion [0.770, -0.077, 0.063, 0.630]
in RPY (radian) [1.775, -0.196, -0.040]
in RPY (degree) [101.686, -11.228, -2.306]

between shoulder_link and upper_arm_link. Of course, these values are magnitudes higher than anything real, but the problem stays the same.

• I currently set the links of the two arm segments all in one line instead of using the shoulder and elbow offset as previously. This was first of all easier to implement (remember, we actually don't have any square angle anymore) and secondly doesn't have any kinematic effect to my interpretation. The visuals can (and are) still be offset outside this central axis. The inertias can be treated the same. Should I instead use the "old" kinematics, where the rotation origins sit central in the segment bases?

To complete things, I attached a screenshot of our robots using the calibrated URDF, where the error between tool0 and tool0_controller (used by ur_modern) is in the magnitude of 10e-12

I'm happy about any feedback to get this a really cool feature :)

[gavanderhoorn]

@fmauch: @cpt-yoshi commented in #357 he has performed a similar conversion.

Perhaps you can compare notes?

[fmauch]

I updated the parameter inclusion to use yaml files instead of the external xacro files.
Also, I now use the 6DOF parameters for each joint and got rid of the passive joints. @cpt-yoshi: This could fit your needs, as well, right? If no objections arise, I'll rebase this onto current kinetic-devel and finish it for all robot models.

[captain-yoshi]

I updated the parameter inclusion to use yaml files instead of the external xacro files.

The yaml approach makes the description a lot cleaner.

Also, I now use the 6DOF parameters for each joint and got rid of the passive joints

Great! I would wait for @gavanderhoorn before finsihing for all robot models, I think he'll conduct some tests to see if IKFast struggles with more then 1 non-zero rotation in one joint (r, p, y). If IKFast would fail and we wanted to support it, then we would have to add passive joints.

I just have some small interrogations:

• Shouldn't the inertial origin of the links have the same offsets origin of visual and collision?

• Shouldn't the axis of all joints match the original urdf? (I think you compensate for that in the links...)

• If I understand you algorithm correctly, an offset is added to the links so that the mesh is as close as possible from the original non calibrated urdf. If so, should these offsets change depending on the calibration file?

[fmauch]

• Shouldn't the inertial origin of the links have the same offsets origin of visual and collision?

I haven't looked at the inertias, yet. First I wanted to get the base concept final before doing everything multiple times. But yes, you are absolutely right.

• Shouldn't the axis of all joints match the original urdf? (I think you compensate for that in the links...)

I was thinking about this, yes. What kept me from doing this was the fact, that then the calibration algorithm would have to know which model to calibrate for. Currently this doesn't matter, as I drag all the joints to the centerline, as the uncalibrated dh-parameters do, as well, btw. Having the joints stick out to the center of the next link is as far as I understood pure aesthetics. Same goes for the joint orientation· btw. In my models all joints are rotating around their z axis, which is not the case in the current model.

• If I understand you algorithm correctly, an offset is added to the links so that the mesh is as close as possible from the original non calibrated urdf. If so, should these offsets change depending on the calibration file?

You are talking about this, right?

<origin xyz="0 0 ${shoulder_offset}" rpy="${pi/2} 0 ${-pi/2}"/>

To my knowledge, there's no such value inside the calibration. Also, in the current file, they are manually set, as the comment says:

    <!-- Arbitrary offsets for shoulder/elbow joints -->
    <xacro:property name="shoulder_offset" value="0.220941" />  <!-- measured from model -->
    <xacro:property name="elbow_offset" value="-0.1719" /> <!-- measured from model -->

This strongly relates to the second point. I just used the offsets, so the meshes show up roughly at the right place. To make this accurate, one would have to change the meshes depending on the real calibration (e.g. the upper arm being 0.3 mm too long or things like that). What I basically do is moving the meshes out of center on their respective rotation axis, which isn't completely wrong in my opinion. Visually, they look correct and for collision checking you probably won't consider letting something in proximity of half a millimeter to the robot pass, right?

Same goes for the inertias. As they are approximations only, anyway, it won't matter too much, if their center is slightly off.

[fmauch]

I took some time to fix the inertias to match the link structure. I was a bit surprised that on the kinetic-devel branch they seem to be wrong in a couple of metrics:

• The upper arm inertia is not centered in the visual arm segment, probably, because simply the shoulder offset is used for offsetting it.
• CoM in the wrist links don't sit inside the correct links. E.g. wrist1 has its CoM inside the end of the forearm.
• Because of the second point the inertia's geometry of wrist3 is matching wrist2 instead of the actual moving part of wrist3.

I went along and fixed those issues on the way. Please correct me, if I understood something completely wrong here, I usually don't work with gazebo or inertias.

[gavanderhoorn]

Thanks @fmauch. It could well be that the Gazebo model is incorrect -- I don't normally work with it either and it predates my involvement with these packages by a few years.

Overall: would it be possible for you to submit a separate PR with the inertia fixes? That way we don't mix things up and it becomes easier to review.

I've also added one in-line comment.

[gavanderhoorn]

What was your motivation for moving this here? base_link is the root of the ROS kinematic chain, and that typically is used to attach the first link of a robot model to.

The base frame is not meant to have any associated visual or collision geometry, but is a frame that is supposed to be coincident with the origin of the 'world' frame of the robot manufacturer.

The fact that the file for the first link is called base.dae here is just a coincidence.

For some more words on base_link, base and their use you could take a look at Coordinate Frames for Serial Industrial Manipulators.

[fmauch]

Thanks for pointing that out. That's the result of calibrating data is based on base, so I attached the chain to base. As gazebo was complaining about inertias, if the shoulder's parent didn't have inertias, I moved geometry and inertia to base.

I was aware, that base should represent the robot's cartesian frame, but I understand, that I didn't follow the REP199 completely. Will fix that :)

In general I think it makes sense to include it in this MR, as without adapting them, inertias resulting out of my changes might be even less correct than before. The CoMs laying in the correct segments automatically fall out of my chain_correction, as the coordinate frames changed, as mentioned earlier.

I can, however skip the correction of the inertia geometry for wrist3, as this will be quite independent.

[gavanderhoorn]

In general I think it makes sense to include it in this MR, as without adapting them, inertias resulting out of my changes might be even less correct than before. The CoMs laying in the correct segments automatically fall out of my chain_correction, as the coordinate frames changed, as mentioned earlier.

I would suggest to submit a separate PR. If that PR is deemed OK, we merge it. Then you rebase this PR on top of the new current state.

That would be cleaner I believe.

I was aware, that base should represent the robot's cartesian frame, but I understand, that I didn't follow the REP199 completely

well, the REP isn't an official REP yet, so there is no obligation to follow it. But it would be good to keep it in mind and only do things differently if there is a proper need for it (as the REP also suggests).

[fmauch]

I would suggest to submit a separate PR. If that PR is deemed OK, we merge it. Then you rebase this PR on top of the new current state.

Alright :)

I corrected the base issue and I will have a look over the REP once more.

[felixvd]

I was mistagged for @fmauch in the post above, but the notification surely went through anyway, as he's the thread author.

[gavanderhoorn]

I was mistagged for @fmauch in the post above, but the notification surely went through anyway, as he's the thread author.

Indeed. Apologies.

[fmauch]

I moved out the inertia correction, as proposed by @gavanderhoorn, however I would like to show what this currently leads to:

Which is worse compared to the current situation as shown in this post
ros-visualization/rviz#1204 (comment)

It wouldn't be much of an issue to make them correct (which is currently on another branch: https://github.com/fmauch/universal_robot/tree/inertia) which results in a more correct version:

Should I still leave it like that?

[gavanderhoorn]

The idea was to contribute the fixes to the inertias in a separate PR @fmauch :)

We'd still like to get those in, but if you submit a separate PR, we could review those changes in isolation and then get them merged.

at that point you can rebase your current calibration branch on-top of kinetic-devel (which would then include the fixes to the inertias) and we end up where I believe you'd like to be: fixed inertias as a starting point for this PR.

---

Edit: unless I misunderstood you. The fixes that you suggested are -- though related -- not from the calibration procedure, but just general fixes based on the structure of the robot, correct (ie: even uncalibrated it was using incorrect values)?

[fmauch]

The idea was to contribute the fixes to the inertias in a separate PR @fmauch :)

We'd still like to get those in, but if you submit a separate PR, we could review those changes in isolation and then get them merged.

at that point you can rebase your current calibration branch on-top of kinetic-devel (which would then include the fixes to the inertias) and we end up where I believe you'd like to be: fixed inertias as a starting point for this PR.

I didn't get you suggested to change the order of merging those features... Thank you for your patience with me :)

[fmauch]

Just for reference: The MR about inertias is up in #426

[hariharan82]

Hey Fellas, I am currently trying to include the calibration offsets for the UR10 into the meshes. I have a xml file that defines the meshes for the UR10. I don't exactly follow the discussion here but is there a way to account for the calibration offsets se when i use the meshes to perform the FK, the flange position matches exactly what the robot controller says. Also, I am currently using IKfast for IK but I am not sure if that would work if the meshes are modified. Any explanation/help is appreciated. These are my calibration offsets

delta_theta = [ -7.68578854641674555e-05, -0.0484988195154549828, 0.0773589887018100442, -0.0286841179499151347, -6.23968602647445076e-05, -1.36814605310694358e-06]
delta_a = [ -7.06689796862263305e-05, 0.000473292415404835687, 0.00142607850210341169, -9.47264317413558358e-06, 8.52074827089885432e-05, 0]
delta_d = [ 0.000945515566846549804, 5.43382695506826963, -8.0663748652800713, 2.63227402564373003, 9.0280898773970053e-05, 0.000204171581686854453]
delta_alpha = [ 0.000255797545358982248, -0.00546750558962930643, -0.00623286337061196728, 0.000170403695184617732, 0.000277199666766536623, 0]

[fmauch]

Hey Fellas, I am currently trying to include the calibration offsets for the UR10 into the meshes. I have a xml file that defines the meshes for the UR10. I don't exactly follow the discussion here but is there a way to account for the calibration offsets se when i use the meshes to perform the FK, the flange position matches exactly what the robot controller says.

I don't quite understand, why the meshes are part of the FK. Isn't usually the kinematics (in terms of TF) used for this? Anyway, with the currently pushed version all meshes and inertias should be at the right place. As long as correct kinematics are provided in the yaml file, everything should fall at its place.

We hope the new ur_rtde_driver will be release soon, which will include a routine to extract this yaml-file from the robot automatically.

[miguelprada]

What's the state on this one? Is it in conflict or complementary to #371 and #441?

I still haven't been able to test it much, but I think it's a great contribution.

[fmauch]

I have a branch rebased ontop of #371 ready only covering CB robots. We should probably think about the order of all of that. Merge #371, rebase #441 ontop of that and then #414 ontop of that? @ipa-led

I think, there are some issues regarding the link origins especially with ros-industrial-attic/ur_modern_driver/issues/320 in mind.

[miguelprada]

I have a branch rebased ontop of #371 ready only covering CB robots. We should probably think about the order of all of that. Merge #371, rebase #441 ontop of that and then #414 ontop of that? @ipa-led

Great, sounds very reasonable to me.

[nicolasCheron]

Hello, In order to get an accurate model of my UR10 in simulation, I moved the robot in hundreds of positions and get the position in Cartesian and joint space. Then I used scipy to find the DH parameters that minimized the error between the cartesian position and the position from the direct geometrical model. I limited the search around the theoretical DH parameters. I have now this parameters :
DeltaX : d = [7.07489864e-04,-1.79827174e-04,-1.76065257e-04,-2.05058836e-04,-2.49932350e-05,4.13236499e-07] a = [6.09568665e-05,-2.43622129e-04,1.15715437e-03,3.50628841e-05,2.02848136e-05,1.44781964e-06] alpha = [1.74431908e-04,-2.88575826e-04,-9.55831763e-04,4.34453688e-04,-1.55413778e-04,-4.40614317e-05] theta = [5.18360443e-03,-1.18625151e-03,2.53951697e-03,-3.99284086e-03,3.91415132e-02,1.98777510e-03]

The error is less than 1 mm and 0.001 rads on the three rotation angles.
These values can be used to get the inverse kinematic in closed form (it did not work with the delta_d in hundreds of meters).

But I don't get how to create the urdf from the DH parameters as the coordinates of the robot are not the same (all rotations are not on z axis and frames are not at the same position). Does someone know how to fix this ?

[simonschmeisser]

Sorry for the spam, but what's the current status of this? https://github.com/UniversalRobots/Universal_Robots_ROS_Driver recommends to use this branch from fmauch and since Universal_Robots_ROS_Driver has been released I wondered if there is actually a "official" branch of this package intended to be used or still the fork?

[gavanderhoorn]

As long as the official documentation states the branch should be used, the branch should be used.

[fmauch]

It's still the fork, as we plan to merge #371 first.

[fmauch]

I rebased my work on top of @ipa-led's work from #371. I also rebased my ur16e branch (see #460) ontop of that.

I would love if we could push things forward here, so we can get rid of my fork being required for using ur_robot_driver.

[dhled]

I support this ! I will try to have a look to your code @fmauch so maybe it can help the reviewer.

[fmessmer]

@fmauch @ipa-led @gavanderhoorn
any updates on getting the various feature branches merged/streamlined?
also, what's the roadmap for getting things released/tagged?

[fmauch]

I think getting everything merged into melodic-staging starting with #371 would be a good starting point.

[gavanderhoorn]

Continuing this in #495.

[gavanderhoorn]

I'll thank you for the work in #495 @fmauch :)