Skip to content
This repository has been archived by the owner on Aug 3, 2020. It is now read-only.

Add recursive mimic joint #177

Merged
merged 15 commits into from
Mar 17, 2017
Merged

Add recursive mimic joint #177

Changes from 1 commit
Commits
Show all changes
15 commits
Select commit Hold shift + click to select a range
6f7cc85
Add recursive mimic joint
alextoind Jan 12, 2017
258de29
Fix crash on 0 free joints, opens empty window (#178)
bmagyar Jan 19, 2017
c9d0168
changelogs
wjwwood Jan 26, 2017
7563f00
1.12.7
wjwwood Jan 26, 2017
fc71f00
remove divide by 2 when writing boxes to collada format (#133)
jacquelinekay Feb 1, 2017
4d03177
Do a few cleanup tasks in collada_urdf (#183)
clalancette Feb 1, 2017
3c7933b
fix missed mandatory -std=c++11 flag (#181)
4eetah Feb 3, 2017
6218443
add Chris and Shane as maintainers (#184)
wjwwood Feb 8, 2017
3384a0a
Allow supplying NodeHandle for initParam (#168)
piyushk Feb 10, 2017
2755753
Remove old gazebo settings.
clalancette Feb 14, 2017
05b4a41
Fix recursive offset and add infinite loop check
alextoind Feb 17, 2017
a88c94e
Fix computation
alextoind Feb 20, 2017
139299d
Merge branch 'patch-1' of https://github.com/alextoind/robot_model in…
sloretz Mar 17, 2017
4c59e35
Added joint_state_publisher tests for mimic_chain and mimic_cycle
sloretz Mar 17, 2017
3e97af7
Make URDF valid
sloretz Mar 17, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 6 additions & 1 deletion joint_state_publisher/joint_state_publisher/joint_state_publisher
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -214,9 +214,14 @@ class JointStatePublisher():
elif name in self.dependent_joints:
param = self.dependent_joints[name]
parent = param['parent']
joint = self.free_joints[parent]
factor = param.get('factor', 1)
offset = param.get('offset', 0)
while parent in self.dependent_joints:
param = self.dependent_joints[parent]
parent = param['parent']
factor *= param.get('factor', 1)
# the offset is relative only to the first parent
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link

@sbarthelemy sbarthelemy Feb 9, 2017 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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).

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

joint = self.free_joints[parent]

if has_position and 'position' in joint:
msg.position[i] = joint['position'] * factor + offset
Expand Down