Skip to content

Commit a902c0b

Browse files
jjaime2jjaime2
committed
Configure semantic robot description with end_effector_tool
1 parent 8852f74 commit a902c0b

File tree

3 files changed

+62
-43
lines changed

3 files changed

+62
-43
lines changed

‎ada_moveit/config/ada.srdf

Lines changed: 56 additions & 43 deletions
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,10 @@
33
This is a format for representing semantic information about the robot structure.
44
A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
55
-->
6-
<robot name="ada">
6+
<robot name="ada" xmlns:xacro="http://www.ros.org/wiki/xacro">
7+
<xacro:arg name="end_effector_tool" default="fork" />
8+
<xacro:property name="end_effector_tool" value="$(arg end_effector_tool)" />
9+
710
<!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
811
<!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
912
<!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
@@ -21,9 +24,11 @@
2124
<joint name="j2n6s200_joint_finger_1"/>
2225
<joint name="j2n6s200_joint_finger_2"/>
2326
</group>
24-
<group name="articulable_fork">
25-
<chain base_link="af_link_base" tip_link="forkTip"/>
26-
</group>
27+
<xacro:if value="${end_effector_tool == 'articulable_fork'}">
28+
<group name="articulable_fork">
29+
<chain base_link="af_link_base" tip_link="forkTip"/>
30+
</group>
31+
</xacro:if>
2732
<group name="ada">
2833
<chain base_link="j2n6s200_link_base" tip_link="forkTip"/>
2934
</group>
@@ -44,10 +49,6 @@
4449
<joint name="j2n6s200_joint_5" value="0.0"/>
4550
<joint name="j2n6s200_joint_6" value="0.0"/>
4651
</group_state>
47-
<group_state name="Home" group="articulable_fork">
48-
<joint name="af_joint_1" value="0.0"/>
49-
<joint name="af_joint_2" value="0.0"/>
50-
</group_state>
5152
<group_state name="Open" group="hand">
5253
<joint name="j2n6s200_joint_finger_1" value="0"/>
5354
<joint name="j2n6s200_joint_finger_2" value="0"/>
@@ -60,15 +61,23 @@
6061
<joint name="j2n6s200_joint_finger_1" value="1.46977"/>
6162
<joint name="j2n6s200_joint_finger_2" value="1.46977"/>
6263
</group_state>
64+
<xacro:if value="${end_effector_tool == 'articulable_fork'}">
65+
<group_state name="Home" group="articulable_fork">
66+
<joint name="af_joint_1" value="0.0"/>
67+
<joint name="af_joint_2" value="0.0"/>
68+
</group_state>
69+
</xacro:if>
6370
<group_state name="Home" group="ada">
6471
<joint name="j2n6s200_joint_1" value="-1.47568"/>
6572
<joint name="j2n6s200_joint_2" value="2.92779"/>
6673
<joint name="j2n6s200_joint_3" value="1.00845"/>
6774
<joint name="j2n6s200_joint_4" value="-2.0847"/>
6875
<joint name="j2n6s200_joint_5" value="1.43588"/>
6976
<joint name="j2n6s200_joint_6" value="1.32575"/>
70-
<joint name="af_joint_1" value="0.0"/>
71-
<joint name="af_joint_2" value="0.0"/>
77+
<xacro:if value="${end_effector_tool == 'articulable_fork'}">
78+
<joint name="af_joint_1" value="0.0"/>
79+
<joint name="af_joint_2" value="0.0"/>
80+
</xacro:if>
7281
</group_state>
7382
<group_state name="Candle" group="ada">
7483
<joint name="j2n6s200_joint_1" value="0.0"/>
@@ -77,8 +86,10 @@
7786
<joint name="j2n6s200_joint_4" value="0.0"/>
7887
<joint name="j2n6s200_joint_5" value="0.0"/>
7988
<joint name="j2n6s200_joint_6" value="0.0"/>
80-
<joint name="af_joint_1" value="0.0"/>
81-
<joint name="af_joint_2" value="0.0"/>
89+
<xacro:if value="${end_effector_tool == 'articulable_fork'}">
90+
<joint name="af_joint_1" value="0.0"/>
91+
<joint name="af_joint_2" value="0.0"/>
92+
</xacro:if>
8293
</group_state>
8394
<!--END EFFECTOR: Purpose: Delineate group(s) as end-effectors for other group(s).-->
8495
<end_effector name="hand" parent_link="j2n6s200_link_6" group="hand" parent_group="jaco_arm"/>
@@ -297,36 +308,38 @@
297308
<disable_collisions link1="uncalibrated_camera_link" link2="screwHeadRight" reason="Adjacent"/>
298309

299310
<!-- Disable collisions for Articulable Fork links -->
300-
<disable_collisions link1="af_link_base" link2="forkHandle" reason="Adjacent"/>
301-
<disable_collisions link1="af_link_base" link2="af_link_1" reason="Adjacent"/>
302-
<disable_collisions link1="af_link_base" link2="af_link_2" reason="Adjacent"/>
303-
<disable_collisions link1="af_link_base" link2="af_ft_adapter_link" reason="Adjacent"/>
304-
<disable_collisions link1="af_link_base" link2="af_motor_fixture_link" reason="Adjacent"/>
305-
<disable_collisions link1="af_link_base" link2="forkTine" reason="Never"/>
306-
<disable_collisions link1="af_link_base" link2="forque" reason="Never"/>
307-
<disable_collisions link1="af_link_1" link2="af_link_base" reason="Adjacent"/>
308-
<disable_collisions link1="af_link_1" link2="af_link_2" reason="Adjacent"/>
309-
<disable_collisions link1="af_link_1" link2="af_ft_adapter_link" reason="Adjacent"/>
310-
<disable_collisions link1="af_link_1" link2="af_motor_fixture_link" reason="Adjacent"/>
311-
<disable_collisions link1="af_link_1" link2="forkTine" reason="Adjacent"/>
312-
<disable_collisions link1="af_link_1" link2="forque" reason="Adjacent"/>
313-
<disable_collisions link1="af_link_2" link2="af_link_1" reason="Adjacent"/>
314-
<disable_collisions link1="af_link_2" link2="af_link_base" reason="Adjacent"/>
315-
<disable_collisions link1="af_link_2" link2="af_ft_adapter_link" reason="Adjacent"/>
316-
<disable_collisions link1="af_link_2" link2="af_motor_fixture_link" reason="Adjacent"/>
317-
<disable_collisions link1="af_link_2" link2="forkTine" reason="Adjacent"/>
318-
<disable_collisions link1="af_link_2" link2="forque" reason="Adjacent"/>
319-
<disable_collisions link1="af_ft_adapter_link" link2="af_link_1" reason="Adjacent"/>
320-
<disable_collisions link1="af_ft_adapter_link" link2="af_link_2" reason="Adjacent"/>
321-
<disable_collisions link1="af_ft_adapter_link" link2="af_link_base" reason="Adjacent"/>
322-
<disable_collisions link1="af_ft_adapter_link" link2="af_motor_fixture_link" reason="Adjacent"/>
323-
<disable_collisions link1="af_ft_adapter_link" link2="forkTine" reason="Adjacent"/>
324-
<disable_collisions link1="af_ft_adapter_link" link2="forque" reason="Adjacent"/>
325-
<disable_collisions link1="af_motor_fixture_link" link2="af_link_1" reason="Adjacent"/>
326-
<disable_collisions link1="af_motor_fixture_link" link2="af_link_2" reason="Adjacent"/>
327-
<disable_collisions link1="af_motor_fixture_link" link2="af_ft_adapter_link" reason="Adjacent"/>
328-
<disable_collisions link1="af_motor_fixture_link" link2="af_link_base" reason="Adjacent"/>
329-
<disable_collisions link1="af_motor_fixture_link" link2="forkTine" reason="Adjacent"/>
330-
<disable_collisions link1="af_motor_fixture_link" link2="forque" reason="Adjacent"/>
311+
<xacro:if value="${end_effector_tool == 'articulable_fork'}">
312+
<disable_collisions link1="af_link_base" link2="forkHandle" reason="Adjacent"/>
313+
<disable_collisions link1="af_link_base" link2="af_link_1" reason="Adjacent"/>
314+
<disable_collisions link1="af_link_base" link2="af_link_2" reason="Adjacent"/>
315+
<disable_collisions link1="af_link_base" link2="af_ft_adapter_link" reason="Adjacent"/>
316+
<disable_collisions link1="af_link_base" link2="af_motor_fixture_link" reason="Adjacent"/>
317+
<disable_collisions link1="af_link_base" link2="forkTine" reason="Never"/>
318+
<disable_collisions link1="af_link_base" link2="forque" reason="Never"/>
319+
<disable_collisions link1="af_link_1" link2="af_link_base" reason="Adjacent"/>
320+
<disable_collisions link1="af_link_1" link2="af_link_2" reason="Adjacent"/>
321+
<disable_collisions link1="af_link_1" link2="af_ft_adapter_link" reason="Adjacent"/>
322+
<disable_collisions link1="af_link_1" link2="af_motor_fixture_link" reason="Adjacent"/>
323+
<disable_collisions link1="af_link_1" link2="forkTine" reason="Adjacent"/>
324+
<disable_collisions link1="af_link_1" link2="forque" reason="Adjacent"/>
325+
<disable_collisions link1="af_link_2" link2="af_link_1" reason="Adjacent"/>
326+
<disable_collisions link1="af_link_2" link2="af_link_base" reason="Adjacent"/>
327+
<disable_collisions link1="af_link_2" link2="af_ft_adapter_link" reason="Adjacent"/>
328+
<disable_collisions link1="af_link_2" link2="af_motor_fixture_link" reason="Adjacent"/>
329+
<disable_collisions link1="af_link_2" link2="forkTine" reason="Adjacent"/>
330+
<disable_collisions link1="af_link_2" link2="forque" reason="Adjacent"/>
331+
<disable_collisions link1="af_ft_adapter_link" link2="af_link_1" reason="Adjacent"/>
332+
<disable_collisions link1="af_ft_adapter_link" link2="af_link_2" reason="Adjacent"/>
333+
<disable_collisions link1="af_ft_adapter_link" link2="af_link_base" reason="Adjacent"/>
334+
<disable_collisions link1="af_ft_adapter_link" link2="af_motor_fixture_link" reason="Adjacent"/>
335+
<disable_collisions link1="af_ft_adapter_link" link2="forkTine" reason="Adjacent"/>
336+
<disable_collisions link1="af_ft_adapter_link" link2="forque" reason="Adjacent"/>
337+
<disable_collisions link1="af_motor_fixture_link" link2="af_link_1" reason="Adjacent"/>
338+
<disable_collisions link1="af_motor_fixture_link" link2="af_link_2" reason="Adjacent"/>
339+
<disable_collisions link1="af_motor_fixture_link" link2="af_ft_adapter_link" reason="Adjacent"/>
340+
<disable_collisions link1="af_motor_fixture_link" link2="af_link_base" reason="Adjacent"/>
341+
<disable_collisions link1="af_motor_fixture_link" link2="forkTine" reason="Adjacent"/>
342+
<disable_collisions link1="af_motor_fixture_link" link2="forque" reason="Adjacent"/>
343+
</xacro:if>
331344

332345
</robot>

‎ada_moveit/launch/demo.launch.py

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -106,6 +106,9 @@ def generate_launch_description():
106106
builder = builder.robot_description(
107107
mappings={"sim": sim, "end_effector_tool": end_effector_tool}
108108
)
109+
builder = builder.robot_description_semantic(
110+
mappings={"end_effector_tool": end_effector_tool}
111+
)
109112
moveit_config = builder.to_moveit_configs()
110113

111114
# Launch argument for whether to use moveit servo or not

‎ada_moveit/launch/move_group.launch.py

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,9 @@ def get_move_group_launch(context):
2828
builder = builder.robot_description(
2929
mappings={"sim": sim, "end_effector_tool": end_effector_tool}
3030
)
31+
builder = builder.robot_description_semantic(
32+
mappings={"end_effector_tool": end_effector_tool}
33+
)
3134
moveit_config = builder.to_moveit_configs()
3235

3336
# If sim is mock, set moveit_config.sensors_3d to an empty dictionary

0 commit comments

Comments
 (0)