bkinnightskytw
/
kdl_parser
mirror of https://github.com/existedinnettw/kdl_parser.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Add recursive mimic joint (#177) 

* Add recursive mimic joint
This commit is contained in:
Alessandro Tondo 2017-03-17 22:52:22 +01:00 committed by Shane Loretz
parent 2755753e43
commit 7de89ed9bd
6 changed files with 105 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
joint_state_publisher/CMakeLists.txt
View File

@ -6,3 +6,10 @@ find_package(catkin REQUIRED)
catkin_package()
install(PROGRAMS joint_state_publisher/joint_state_publisher DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
if(CATKIN_ENABLE_TESTING)
find_package(catkin REQUIRED COMPONENTS rostest)
add_rostest(test/test_mimic_chain.launch)
add_rostest(test/test_mimic_cycle.launch)
endif()

14
joint_state_publisher/joint_state_publisher/joint_state_publisher
View File

@ -214,9 +214,21 @@ 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)
# 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']
offset += factor * param.get('offset', 0)
factor *= param.get('factor', 1)
joint = self.free_joints[parent]
if has_position and 'position' in joint:
msg.position[i] = joint['position'] * factor + offset

27
joint_state_publisher/test/mimic_chain.urdf Normal file
View File

@ -0,0 +1,27 @@
<?xml version="1.0"?>
<urdf>
<robot name="mimic_chain_robot">
<link name="link1"/>
<link name="link2"/>
<link name="link3"/>
<link name="link4"/>
<joint name="j12" type="revolute">
<parent link="link1"/>
<child link="link2"/>
<limit effort="10" velocity="10" lower="-1" upper="1"/>
</joint>
<joint name="j23" type="revolute">
<parent link="link2"/>
<child link="link3"/>
<mimic joint="j12"/>
<limit effort="10" velocity="10" lower="-1" upper="1"/>
</joint>
<joint name="j34" type="revolute">
<parent link="link3"/>
<child link="link4"/>
<mimic joint="j23"/>
<limit effort="10" velocity="10" lower="-1" upper="1"/>
</joint>
</robot>
</urdf>

28
joint_state_publisher/test/mimic_cycle.urdf Normal file
View File

@ -0,0 +1,28 @@
<?xml version="1.0"?>
<urdf>
<robot name="mimic_cycle_robot">
<link name="link1"/>
<link name="link2"/>
<link name="link3"/>
<link name="link4"/>
<joint name="j12" type="revolute">
<parent link="link1"/>
<child link="link2"/>
<mimic joint="j34"/>
<limit effort="10" velocity="10" lower="-1" upper="1"/>
</joint>
<joint name="j23" type="revolute">
<parent link="link2"/>
<child link="link3"/>
<mimic joint="j12"/>
<limit effort="10" velocity="10" lower="-1" upper="1"/>
</joint>
<joint name="j34" type="revolute">
<parent link="link3"/>
<child link="link4"/>
<mimic joint="j23"/>
<limit effort="10" velocity="10" lower="-1" upper="1"/>
</joint>
</robot>
</urdf>

15
joint_state_publisher/test/test_mimic_chain.launch Normal file
View File

@ -0,0 +1,15 @@
<?xml version="1.0"?>
<launch>
<param name="robot_description" textfile="$(find joint_state_publisher)/test/mimic_chain.urdf"/>
<node pkg="joint_state_publisher" type="joint_state_publisher" name="dut_joint_state_publisher">
<param name="rate" value="10"/>
</node>
<test pkg="rostest" type="hztest" name="hztest" test-name="hztest_mimic_chain">
<param name="topic" value="joint_states"/>
<param name="hz" value="10"/>
<param name="hzerror" value="0.1"/>
<param name="test_duration" value="10"/>
<param name="wait_time" value="10"/>
</test>
</launch>

15
joint_state_publisher/test/test_mimic_cycle.launch Normal file
View File

@ -0,0 +1,15 @@
<?xml version="1.0"?>
<launch>
<param name="robot_description" textfile="$(find joint_state_publisher)/test/mimic_cycle.urdf"/>
<node pkg="joint_state_publisher" type="joint_state_publisher" name="dut_joint_state_publisher">
<param name="rate" value="10"/>
</node>
<test pkg="rostest" type="hztest" name="hztest" test-name="hztest_mimic_cycle">
<param name="topic" value="joint_states"/>
<param name="hz" value="0"/>
<param name="hzerror" value="0.1"/>
<param name="test_duration" value="10"/>
<param name="wait_time" value="10"/>
</test>
</launch>