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
9 changes: 8 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 @@ -216,11 +216,18 @@ class JointStatePublisher():
parent = param['parent']
factor = param.get('factor', 1)
offset = param.get('offset', 0)
# Handle recursive mimic chain
recursive_mimic_chain_joints = [name]
while parent in self.dependent_joints:
if parent in recursive_mimic_chain_joints:
error_message = "Found an infinite recursive mimic chain"
rospy.logerr("%s: [%s, %s]", error_message, ', '.join(recursive_mimic_chain_joints), parent)
sys.exit(-1)
recursive_mimic_chain_joints.append(parent)
param = self.dependent_joints[parent]
parent = param['parent']
factor *= param.get('factor', 1)
# the offset is relative only to the first parent
offset = factor*offset + param.get('offset', 0)
Copy link

Choose a reason for hiding this comment

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

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)

Copy link
Contributor Author

Choose a reason for hiding this comment

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

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

Copy link

Choose a reason for hiding this comment

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

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

Copy link
Contributor Author

Choose a reason for hiding this comment

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

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

joint = self.free_joints[parent]

if has_position and 'position' in joint:
Expand Down