Add recursive mimic joint

[alextoind]

Following the discussion on answer.ros.org I've added the possibility to create a chain of mimic joints; before, each dependent joint was only able to mime the motion of free joints.

[wjwwood]

The code changes lgtm, but I'll defer to @DLu to decide on whether or not it addresses the issue correctly.

[alextoind]

@wjwwood of course, thank you!

[sbarthelemy]

Instead of doing the recursion at each publish time, what about doing it at init?
ie. compute and store the proper (recursive) factors and offsets at init time, then do no recursion at publish time.

Also, with this change, I suspect an urdf with a mimic cycle may cause join_state_publisher to recurse infinitely ;)

[sbarthelemy]

I think you should take the parent offset into account too. Why would you not?

[alextoind]

I believed that too at first: I summed up all the offset, I tried it with a simple three-joint scheme and I made some tests with ad-hoc values. Results? Everything concerning the revolution motion was right. The offset was... Strange.

I thought about it for a while and I concluded that a user is expecting to set the speed of the joint w.r.t. the whole chain of recursive joints, but the initial placement is logical to be w.r.t. the immediate first parent from which the URDF is computing the distance.

Example: imagine to have three joints Ja mimics Jb which mimics Jc. And suppose that Jb has an offset of pi/2 w.r.t. Jc. If you sum up all the offset, even if Ja has no offset w.r.t. Jb, it ends to inherit the other offsets and to refer them to Jb.

[sbarthelemy]

Hi @alextoind, thank you for your answer, and sorry for my late reply

I thought about it for a while and I concluded that a user is expecting to set the speed of the joint w.r.t. the whole chain of recursive joints, but the initial placement is logical to be w.r.t. the immediate first parent from which the URDF is computing the distance.

I would suggest to implement exactly what the spec/doc states instead of guessing what users have in mind (they might all have a different thing in mind).

Here is the mimic doc:
this tag is used to specify that the defined joint mimics another existing joint. The value of this joint can be computed as value = multiplier * other_joint_value + offset.

Example: imagine to have three joints Ja mimics Jb which mimics Jc. And suppose that Jb has an offset of pi/2 w.r.t. Jc. If you sum up all the offset, even if Ja has no offset w.r.t. Jb, it ends to inherit the other offsets and to refer them to Jb.

I'm not sure to understand. Is this the example you imagined?

 <joint name="Jc" type="continuous">
 </joint>
 <joint name="Jb" type="continuous">
     <mimic joint="Jc" multiplier="x" offset="y"/>
 </joint>
 <joint name="Ja" type="continuous">
    <mimic joint="Jb" multiplier="x'" offset="y'"/>
 </joint>

Then I would expect the following behavior:

(1) Jb_value == x * Jc_value + y
(2) Ja_value == x' * Jb_value + y'
(3) Ja_value == (x * x') * Jc_value + (x' * y + y')

Do you agree?

[alextoind]

Yes, I agree with you about the last formula (which I've already computed one month ago before testing this PR). Expanded it becomes the following:

J_k = f_k * J_k-1 + o_k
    = f_k * f_k-1 * ... * f_2 * f_1 * J_0 +
      + f_k * (f_k-1 * (... * (f_2 * o_1 + o_2) + ...) + o_k-1) + o_k

Nonetheless I chose the other implementation because of some specific cases that I tested and which seem to me a nonsense.

With the URDF I'm saying something like:

• Jb has to move x time faster than Jc with an offset of y radians w.r.t. Jc itself.
• Ja has to move x' time faster than Jb with an offset of y' radians w.r.t. Jb itself.

Now:

1. Suppose that both x and x' are 2, i.e. twice the velocity of their parent.
2. Suppose also that y = 0.8' and y' = -1.6'.

Following the formula Ja will move four time as faster as Jc (pretty fine) and it will start in the same location of Jb (2 * 0.8 - 1.6 = 0). This seems to me non intuitive starting from the URDF, because it forces me to think of all the interactions within the whole recursive chain.

Now, I see that it could be a feature for someone, but I would like to be sure that you are aware of this behavior.

[alextoind]

A part from the specific example, with the above forumlation the starting position of Ja depends not only on its offset, but also from its velocity ratio: if I set my application and later I need to improve a bit the factor of Ja for a faster motion (x' = 3 in the example), I must also modify its offset to guarantee that the starting position remains unchanged (y' = -2.4 if I want that Ja and Jb start from the same location).

[sbarthelemy]

Following the formula Ja will move four time as faster as Jc (pretty fine) and it will start in the same location of Jb (2 * 0.8 - 1.6 = 0).

agreed

Now, I see that it could be a feature for someone, but I would like to be sure that you are aware of this behavior.

I'm aware of the behavior, and it seems sane to me.
I really think it is the only possible interpretation of the URDF spec.

This seems to me non intuitive starting from the URDF, because it forces me to think of all the interactions within the whole recursive chain.

I have the opposite impression ;)

equation (1) and (2) above are the direct translations of the URDF mimic tags. There is no need to consider the whole chain to understand them and they combine naturally into equation (3).

If we were instead to implement

(3') Ja_value == (x * x') * Jc_value + y'

Then, since it is incompatible with equation (2), when reading the urdf we would need to look at Jb definition to know if I need to compute Ja from equation (2) or equation (3').

A part from the specific example, with the above formulation the starting position of Ja depends not only on its offset, but also from its velocity ratio:

yes.

if I set my application and later I need to improve a bit the factor of Ja for a faster motion (x' = 3 in the example), I must also modify its offset to guarantee that the starting position remains unchanged (y' = -2.4 if I want that Ja and Jb start from the same location).

Yes, that seems normal to me too.

The usages of the mimic tags (and the URDF in general) I know of are not to describe behavior but hardware. For instance mimic tags are used for NAO and Pepper hands (a single motor drives all the finger joints) and for NAO HipPitch joints (again two joints driven by a single motor).

I don't know what you're trying to achieve, but it seems out of URDF's (mimic tag) scope.

Would you detail what you are trying to do?

It's probably possible to address your need in joint_state_publisher without changing the interpretation of the URDF files.

[alextoind]

I don't even use offset in my application, so it is definitely not a problem to me.

I just wanted to make a useful change for the community and I thank you for your time and experience in this field.

[alextoind]

Instead of doing the recursion at each publish time, what about doing it at init?
ie. compute and store the proper (recursive) factors and offsets at init time, then do no recursion at publish time.

@sbarthelemy this sounds good to me. I just followed the previous approach and inserted the code directly there. It would be a waste though.

Also, with this change, I suspect an urdf with a mimic cycle may cause join_state_publisher to recurse infinitely ;)

This is another good point. I started from the fact that URDF does not manage chain loops and I faced it head-on. Actually I have not considered that each joint can mimic whatever it wants, thus a mimic cycle can be easily built.

I will try to fix both in the following days, but how would you handle the infinite mimic loops? Maybe the best choice is a fatal error to advise the user that something strange is happening.

[wjwwood]

@DLu ping, can you +1/-1 this one when you have some spare time?

[DLu]

Apologies for the delay. I'm fine with the run-time calculation of the offset. However, I concur that the recursion should take each offset into account, or at least have it be flag configurable. Are there any URDFs out there that currently use the recursive mimic chain?

[alextoind]

@DLu I can change the code at anytime, but please have a quick look at the above comments. Maybe the suggested configurable flag would be the best solution?

[DLu]

@alextoind My top preference is just to have it use the offset at each stage of the calculation, because as @sbarthelemy says, that is the most straightforward definition of the spec. I think if you're going to start chaining together recursive mimic tags, having to consider the whole chain is par for the course.

[alextoind]

Perfect! I'm going to change the implementation, but what about the other two suggestions of @sbarthelemy?

1. Should I move the computation in the `init'?
2. Is there a standard way to exit in case of an infinite recursive mimic chain (e.g. throwing exceptions or something)?

[DLu]

1. Nah.
2. I would exit, print to ROS_ERROR

[alextoind]

I've tested it quickly but it seems to work.
Actually I'm a C++ programmer so have a look if I've made some trivial mistake.

[sbarthelemy]

Hi thank you for thew changes,

I think the correct math is:

offset += factor * param.get('offset', 0)
factor *= param.get('factor', 1)

(in that order)

[alextoind]

If the recursive formula is correct

J_k = f_k * J_k-1 + o_k
    = f_k * f_k-1 * ... * f_2 * f_1 * J_0 +
      + f_k * (f_k-1 * (... * (f_2 * o_1 + o_2) + ...) + o_k-1) + o_k

It should be:

• factor = factor_new * factor_old
• offset = factor_new * offset_old + offset_new

[sbarthelemy]

Your code does not match your math.

Here is your math:

factor = factor_new * factor_old
offset = factor_new * offset_old + offset_new

Here is what I think your code does:

factor = factor_new * factor_old
offset = factor_new * factor_old * offset_old + offset_new

I also think you're looking at the mimic chain from the wrong side. (hence the math is wrong).

Let consider the former example again:

 <joint name="Jc" type="continuous">
 </joint>
 <joint name="Jb" type="continuous">
     <mimic joint="Jc" multiplier="x" offset="y"/>
 </joint>
 <joint name="Ja" type="continuous">
    <mimic joint="Jb" multiplier="x'" offset="y'"/>
 </joint>

Let do the algorithm step by step, with my modification.
At first you have

name == Ja
factor = x'
offset = y'
parent = Jb

parent in self.dependent_joints == True
parent = Jc
offset += factor * y
offset == y' + x'*y
factor *= x
factor == x'*x

parent in self.dependent_joints == False

hence:
offset == y' + x'*y
factor == x'*x

we're back on equation (3).

[alextoind]

You are right I'm sorry, I completely forget about factor_old already computed inside factor_new when doing factor_new * offset_old.

[sbarthelemy]

LGTM, thank you!

[DLu]

LGTM. Everyone happy?

[wjwwood]

Thanks @DLu and everyone else for following up on it.

I'll leave it to @clalancette or @sloretz to merge and release this.

[sloretz]

I think a unit test to make sure it does catch a cycle of mimic joints is important to make sure this isn't broken by future code changes. Unless someone writes it first, I'll make time to write one at the end of this week.

[sbarthelemy]

@sloretz any update? I had a quick look and did not found any test for this project, so adding one (and hooking it to CI...) would be much more work. Maybe this could be merged as is?

[sloretz]

EDIT: Oops, it would help if I used a valid URDF. It works

@alextoind I expected the joint_state_publisher node to exit with a non-zero error code (line 225) when there is a cycle of mimic joints, but it continues publishing. Would you mind having a look at the issue?