Updated cog for ur16 how I understand the documentation

[fmauch]

This PR spins out of #460, as it not only affects the UR16e.

Up until now, the robots' CoG calculation were centered inside the links they were referring to. This PR sets them to the place where they are supposed to be according to UR's documentation

The actual inertia matrix isn't changed by this PR, that might still have to be updated. UR provides 3x3 inertia matrices, but (at least in URSim) the ones from the ur3e and ur5e seem to be very sparse (the following files are from a URSim v5.8, so all robots are actually e-Series):

urcontrol.conf.UR3:inertia_matrix = [ [0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0.000144] ]
urcontrol.conf.UR16:inertia_matrix = [ [0.03351, 0.00002, -0.00001, 0.00002, 0.03374, 0.00374, -0.00001, 0.00374, 0.02100], [0.02796, -0.00010, -0.00720, -0.00010, 0.47558, 0.00003, -0.00720, -0.00001, 0.47635], [0.01091, 0.00006, 0.01012, 0.00006, 0.12060, 0.00001, 0.01012, 0.00001, 0.11714], [0.00609, -0.00001, -0.00000, -0.00001, 0.00245, 0.00083, 0.00000, 0.00083, 0.00579], [0.00389, -0.00001, 0.00000, -0.00001, 0.00219, -0.00045, 0.00000, -0.00045, 0.00363], [0.00117, 0.00000,  0.00000, 0.00000, 0.00118, 0.00000, 0.00000, 0.00000, 0.00084] ]
urcontrol.conf.UR10:inertia_matrix = [ [0.03408, 0.00002, -0.00425, 0.00002, 0.03529, 0.00008, 0.00425, 0.00008, 0.02156], [0.02814, 0.00005, -0.01561, 0.00005, 0.77068, 0.00002, -0.01561, 0.00002, 0.76943], [0.01014, 0.00008, 0.00916, 0.00008, 0.30928, 0.00000, 0.00916, 0.00000, 0.30646], [0.00296, -0.00001, -0.00000, -0.00001, 0.00222, -0.00024, -0.00000, -0.00024, 0.00258], [0.00296, -0.00001, -0.00000, -0.00001, 0.00222, -0.00024, -0.00000, -0.00024, 0.00258], [0.00040, 0.00000,  -0.00000, 0.00000, 0.00041, 0.00000, -0.00000, 0.00000, 0.00034] ]
urcontrol.conf.UR5:inertia_matrix = [ [0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0.0002] ]

Currently, this is a draft implementation for the UR16e. If somebody with more experience about inertias than me could have a look at this, I'd be happy do adapt the parameters for the other robots, as well.

ToDo:

• verify that I am actually doing the right thing -> help needed
• Update inertia cylinder dimensions -> How should we handle those?
• Update ur3
• Update ur5
• Update ur10
• Update ur3e
• Update ur5e
• Update ur10e

[gavanderhoorn]

This PR sets them to the place where they are supposed to be according to UR's documentation

So where are the origins supposed to be (I haven't read the document yet)?

[fmauch]

This PR sets them to the place where they are supposed to be according to UR's documentation

So where are the origins supposed to be (I haven't read the document yet)?

Roughly, where we have them right now, but they aren't exactly in the geometric center.

[gavanderhoorn]

Unfortunately I won't have time to investigate this/support you.

Perhaps one of the other maintainers (@ipa-nhg or @miguelprada) could lend a hand.

[gavanderhoorn]

@fmauch: would this be something you could potentially work on during WRID20?

I don't know whether you've already decided whether you'd join or not, so this is just a question/suggestion of course.

[gavanderhoorn]

In light of #521, having correct inertia elements (and mass) is becoming more-and-more important.

[fmauch]

I could definitely work on that, yes.

I've talked back to the guys at UR and it seems that the inertias are only set for the UR10 and UR16 as for the smaller ones they are negligible compared to the motor inertia used internally. That means that we should probably stick to the cylinder approximations using the masses and cog as listed in UR's documentation.

[ipa-nhg]

@fmauch: would this be something you could potentially work on during WRID20?

I was about to propose the same, I am unfortunately not an expert of the topic but I can help during the WRID reviewing and testing changes.

[matteolucchi]

I'll continue here the conversation started with @gavanderhoorn in #521 as the next steps regard this pull request.

To recap:

• we have the exact information of the cog for all the robot models from the UR document linked above, these have been added for the UR16 and need to be checked for the other models.
• from what I can see all the inertia matrices of the links are currently approximated to the ones of cylinders, following @fmauch comment:

I've talked back to the guys at UR and it seems that the inertias are only set for the UR10 and UR16 as for the smaller ones they are negligible compared to the motor inertia used internally.

the inertia matrices of UR3 and UR5 can be left as they are, but we should add the exact inertia matrices extracted from URSim to UR10, UR16.

I am not sure about what would be a good strategy to include the matrices for UR10 and UR16 without compromising the current modular file structure.

To @fmauch : given the big effort you put into this I would leave the lead to you but I would happy to help you out, please let me know how can I support you with this

[fmauch]

the inertia matrices of UR3 and UR5 can be left as they are, but we should add the exact inertia matrices extracted from URSim to UR10, UR16.

I'm not sure whether this is the correct conclusion to draw from this. I'd suggest trying the cylinder approximations here, as well. But we could use them to improve our approximation to match this as close as possible.

[matteolucchi]

@fmauch wrote:

I'm not sure whether this is the correct conclusion to draw from this. I'd suggest trying the cylinder approximations here, as well. But we could use them to improve our approximation to match this as close as possible.

Ok so should we start with fixing the cogs for all the models while keeping all the intertia matrices approximated to the ones of cylinders?

[gavanderhoorn]

@fmauch wrote:

@matteolucchi wrote:

the inertia matrices of UR3 and UR5 can be left as they are, but we should add the exact inertia matrices extracted from URSim to UR10, UR16.

I'm not sure whether this is the correct conclusion to draw from this. I'd suggest trying the cylinder approximations here, as well. But we could use them to improve our approximation to match this as close as possible.

@fmauch: should we not try to use the official nrs as much as possible?

If this leads to undesirable behaviour in Gazebo, then we should try to compensate, but as URDFs are supposed to be a faithful model of the real robot, using the 'real' values would seem to be desirable.

[fmauch]

@fmauch: should we not try to use the official nrs as much as possible?

I'm just not sure if they reflect the same physical parameters. As far as I understood, those values are used together with motor inertia to calculate the actual robot dynamics. As we don't separate that in gazebo AFAIK, it might make sense to stick with what's resulting from the geometric information that we have. But I don't know enough about physics dynamics simulation to give a fully qualified opinion on that.

Apart from that I do agree that sticking as close as possible to the official numbers does make sense.

[fmauch]

I've updated this PR to define the full inertia matrix. This allows us to either use the values coming from UR directly or inserting the ones from the cylinder approximation.

With the values coming from UR the inertia boxes are angled (some quite significantly). Using a position joint interface this seems to work properly. I would suggest to test this with a draft implementation of #521 before proceeding here.

@matteolucchi Are you going to test this?

Note: Currently, only the UR16e version will work.

Edit: I hacked up a draft version where the robot at least does not explode and performs motions using an effort trajectory controller.

[gavanderhoorn]

With the values coming from UR the inertia boxes are angled (some quite significantly).

@fmauch: could you perhaps attach a screenshot?

[fmauch]

It looks like this:

[gavanderhoorn]

@fmauch wrote:

It looks like this: [...]

Comparing it to the IRB 1200 over at abb_experimental (just to have something to compare with):

[fmauch]

Just for reference: With the cylinder approximations used it looks like this:

[fmauch]

I'll go ahead and implement the cog and full inertia matrix for all variants. We can decide on the inertia content later, as well.

[matteolucchi]

@fmauch Unfortunately I have no time to work on it today, I will check back tomorrow and see how far you got with this and #521 . If there is something specific you want me to test I would be happy to contribute. I didn't realize you were going to work on it today

[fmauch]

I've implemented the changes as recapped in #504 (comment)

CI should accept this again and we can test this by rebasing #525 ontop of this.

I am still not fully convinced that I got all the rotations correct, but the cog docs also are a bit confusing as I can't find a system how the cog is offset in wrist_1 and wrist_2 for example. E.g. the UR5 and UR10 model state the following cog offsets:

UR5

wrist_1: [0, -0.0018, 0.01634]
wrist_2: [0,  0.0018, 0.01634]

UR10

wrist_1: [0.000, 0.007, 0.018]
wrist_2: [0.000, 0.007, 0.018]

I can't follow why for the UR5 (and UR5e) the sign of m_y should flip between the two links while for the UR10 (and UR3 and UR3e and UR10e) it doesn't.

So, this should be reviewed thoroughly questioning all steps I took.