Alessandro Tondo
Shane Loretz
parent
2755753e43
commit
7de89ed9bd
|
|
@ -6,3 +6,10 @@ find_package(catkin REQUIRED)
|
||||||
catkin_package()
|
catkin_package()
|
||||||
|
|
||||||
install(PROGRAMS joint_state_publisher/joint_state_publisher DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
|
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()
|
||||||
|
|
|
||||||
|
|
@ -214,9 +214,21 @@ class JointStatePublisher():
|
||||||
elif name in self.dependent_joints:
|
elif name in self.dependent_joints:
|
||||||
param = self.dependent_joints[name]
|
param = self.dependent_joints[name]
|
||||||
parent = param['parent']
|
parent = param['parent']
|
||||||
joint = self.free_joints[parent]
|
|
||||||
factor = param.get('factor', 1)
|
factor = param.get('factor', 1)
|
||||||
offset = param.get('offset', 0)
|
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:
|
if has_position and 'position' in joint:
|
||||||
msg.position[i] = joint['position'] * factor + offset
|
msg.position[i] = joint['position'] * factor + offset
|
||||||
|
|
|
||||||
|
|
@ -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>
|
||||||
|
|
@ -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>
|
||||||
|
|
@ -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>
|
||||||
|
|
@ -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>
|
||||||
Loading…
Reference in New Issue