Skip to content

Commit

Permalink
Add temporary fix for mimic-joint issue ros-controls/gazebo_ros2_cont…
Browse files Browse the repository at this point in the history 
…rol#173
  • Loading branch information
@doterkuile
doterkuile committed Mar 5, 2024
1 parent 25b8937 commit 36f4f49
Show file tree
Hide file tree
Showing 2 changed files with 83 additions and 107 deletions.
160 changes: 53 additions & 107 deletions pal_robotiq_description/urdf/robotiq.ros2_control.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,23 @@ See the License for the specific language governing permissions and
limitations under the License.
-->
<robot xmlns:xacro="http://ros.org/wiki/xacro">

<xacro:macro name="ros2_control_mimic_joint" params="mimic_joint_name actuated_joint_name multiplier" >

<joint name="${mimic_joint_name}">
<param name="mimic">${actuated_joint_name}</param>
<param name="multiplier">${multiplier}</param>
<command_interface name="position" />

<state_interface name="position" />
<state_interface name="velocity" />
<state_interface name="effort" />
</joint>

</xacro:macro>

<xacro:macro name="ros2_control_robotiq_85_gripper" params="name reduction" >

<joint name="${name}_finger_joint">
<command_interface name="position" />

Expand All @@ -24,64 +40,30 @@ limitations under the License.
<state_interface name="effort" />
</joint>

<joint name="${name}_right_outer_knuckle_joint">
<param name="mimic">${name}_finger_joint</param>
<param name="multiplier">1</param>
<command_interface name="position"/>
<xacro:ros2_control_mimic_joint mimic_joint_name="${name}_right_outer_knuckle_joint"
actuated_joint_name="${name}_finger_joint"
multiplier="1"/>

<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>

<joint name="${name}_right_inner_knuckle_joint">
<param name="mimic">${name}_finger_joint</param>
<param name="multiplier">1</param>
<command_interface name="position"/>

<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>

<joint name="${name}_right_inner_finger_joint">
<param name="mimic">${name}_finger_joint</param>
<param name="multiplier">-1</param>
<command_interface name="position"/>
<xacro:ros2_control_mimic_joint mimic_joint_name="${name}_right_inner_knuckle_joint"
actuated_joint_name="${name}_finger_joint"
multiplier="1"/>

<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<xacro:ros2_control_mimic_joint mimic_joint_name="${name}_right_inner_finger_joint"
actuated_joint_name="${name}_finger_joint"
multiplier="-1"/>

<joint name="${name}_left_inner_finger_joint">
<param name="mimic">${name}_finger_joint</param>
<param name="multiplier">-1</param>
<command_interface name="position"/>
<xacro:ros2_control_mimic_joint mimic_joint_name="${name}_left_inner_finger_joint"
actuated_joint_name="${name}_finger_joint"
multiplier="-1"/>

<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="${name}_left_inner_knuckle_joint">
<param name="mimic">${name}_finger_joint</param>
<param name="multiplier">1</param>
<command_interface name="position"/>

<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="${name}_left_outer_knuckle_joint">
<param name="mimic">${name}_finger_joint</param>
<param name="multiplier">1</param>
<command_interface name="position"/>

<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<xacro:ros2_control_mimic_joint mimic_joint_name="${name}_left_inner_knuckle_joint"
actuated_joint_name="${name}_finger_joint"
multiplier="1"/>

<xacro:ros2_control_mimic_joint mimic_joint_name="${name}_left_outer_knuckle_joint"
actuated_joint_name="${name}_finger_joint"
multiplier="1"/>

<xacro:include filename="$(find pal_robotiq_description)/urdf/robotiq.transmission.xacro" />
<xacro:robotiq_transmission prefix="${name}_finger" reduction="${reduction}" />

Expand All @@ -96,70 +78,34 @@ limitations under the License.
<state_interface name="effort" />
</joint>

<joint name="${name}_right_outer_knuckle_joint">
<param name="mimic">${name}_finger_joint</param>
<param name="multiplier">-1</param>
<command_interface name="position"/>

<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>

<joint name="${name}_right_inner_knuckle_joint">
<param name="mimic">${name}_finger_joint</param>
<param name="multiplier">-1</param>
<command_interface name="position"/>

<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>

<joint name="${name}_right_inner_finger_joint">
<param name="mimic">${name}_finger_joint</param>
<param name="multiplier">1</param>
<command_interface name="position"/>
<xacro:ros2_control_mimic_joint mimic_joint_name="${name}_right_outer_knuckle_joint"
actuated_joint_name="${name}_finger_joint"
multiplier="1"/>

<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<xacro:ros2_control_mimic_joint mimic_joint_name="${name}_right_inner_knuckle_joint"
actuated_joint_name="${name}_finger_joint"
multiplier="1"/>

<joint name="${name}_left_inner_finger_joint">
<param name="mimic">${name}_finger_joint</param>
<param name="multiplier">1</param>
<command_interface name="position"/>
<xacro:ros2_control_mimic_joint mimic_joint_name="${name}_right_inner_finger_joint"
actuated_joint_name="${name}_finger_joint"
multiplier="-1"/>

<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<xacro:ros2_control_mimic_joint mimic_joint_name="${name}_left_inner_finger_joint"
actuated_joint_name="${name}_finger_joint"
multiplier="-1"/>

<joint name="${name}_left_inner_knuckle_joint">
<param name="mimic">${name}_finger_joint</param>
<param name="multiplier">-1</param>
<command_interface name="position"/>
<xacro:ros2_control_mimic_joint mimic_joint_name="${name}_left_inner_knuckle_joint"
actuated_joint_name="${name}_finger_joint"
multiplier="1"/>

<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="${name}_left_outer_knuckle_joint">
<param name="mimic">${name}_finger_joint</param>
<param name="multiplier">1</param>
<command_interface name="position"/>

<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<xacro:ros2_control_mimic_joint mimic_joint_name="${name}_left_outer_knuckle_joint"
actuated_joint_name="${name}_finger_joint"
multiplier="1"/>

<xacro:include filename="$(find pal_robotiq_description)/urdf/robotiq.transmission.xacro" />
<xacro:robotiq_transmission prefix="${name}_finger" reduction="${reduction}" />

</xacro:macro>



</robot>
30 changes: 30 additions & 0 deletions pal_robotiq_description/urdf/robotiq_85_gripper.urdf.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -333,6 +333,29 @@
<xacro:inner_knuckle prefix="${prefix}" fingerprefix="${fingerprefix}" reflect="${reflect}"/>
</xacro:macro>

<!--Dummy joint/link to prevent error with mimic joint
See https://github.com/ros-controls/gazebo_ros2_control/issues/173
-->

<xacro:macro name="dummy_mimic_joint" params="name parent">

<link name="${name}_dummy_mimic_link"/>

<joint name="${name}_joint_mimic" type="fixed">
<parent link="${parent}" />
<child link="${name}_dummy_mimic_link" />
</joint>

</xacro:macro>

<xacro:macro name="finger_mimic_dummy" params="prefix fingerprefix">

<xacro:dummy_mimic_joint name="${prefix}${fingerprefix}_outer_finger" parent="${prefix}base_link"/>
<xacro:dummy_mimic_joint name="${prefix}${fingerprefix}_inner_finger" parent="${prefix}base_link"/>
<xacro:dummy_mimic_joint name="${prefix}${fingerprefix}_inner_finger_pad" parent="${prefix}base_link"/>
<xacro:dummy_mimic_joint name="${prefix}${fingerprefix}_inner_knuckle" parent="${prefix}base_link"/>
</xacro:macro>

<xacro:macro name="finger_joint" params="prefix">
<joint name="${prefix}finger_joint" type="revolute">
<origin xyz="0 -0.0306011 0.054904" rpy="0 0 ${pi}"/>
Expand Down Expand Up @@ -371,12 +394,19 @@
<xacro:pal_robotiq_coupling_link prefix="${prefix}"/>
<xacro:finger_links prefix="${prefix}" fingerprefix="left" reflect="1"/>
<xacro:finger_links prefix="${prefix}" fingerprefix="right" reflect="-1"/>

<xacro:finger_mimic_dummy prefix="${prefix}" fingerprefix="left"/>
<xacro:finger_mimic_dummy prefix="${prefix}" fingerprefix="right"/>

<xacro:robotiq_base_joint prefix="${prefix}" />
<xacro:pal_robotiq_coupling_joint prefix="${prefix}" parent="${parent}">
<xacro:insert_block name="origin" />
</xacro:pal_robotiq_coupling_joint>
<xacro:finger_joint prefix="${prefix}"/>
<xacro:right_outer_knuckle_joint prefix="${prefix}"/>

<xacro:dummy_mimic_joint name="${prefix}right_outer_knuckle" parent="${prefix}base_link"/>

<!--This xacro is used to be able to grasp object in simulmation more robustly, it could cause segfault at grasping time -->
<xacro:gazebo_gripper_grasp name="${prefix}grasp" name_prefix_left="${prefix}left_inner_finger" name_prefix_right="${prefix}right_inner_finger" parent_joint="${parent}"/>
</xacro:macro>
Expand Down

0 comments on commit 36f4f49

Please sign in to comment.