Skip to content

Commit

Permalink
srdf: Use loop macros to reduce code redundancy
Browse files Browse the repository at this point in the history
  • Loading branch information
@rhaschke
rhaschke committed Sep 1, 2022
1 parent 7b9275f commit adff2fd
Show file tree
Hide file tree
Showing 2 changed files with 49 additions and 64 deletions.
71 changes: 38 additions & 33 deletions config/arm.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,39 +33,44 @@
<!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
<virtual_joint name="virtual_joint" type="fixed" parent_frame="world" child_link="$(arg arm_id)_link0" />

<xacro:macro name="collision" params="link">
<!-- Enable (environmental) collisions of ${link}_sc -->
<xacro:if value="${not link.endswith('link8')}">
<disable_default_collisions link="${link}_sc" />
<xacro:macro name="disable_collisions_for" params="link:=^ others:=^">
<xacro:if value="${others}">
<xacro:property name="other" value="${others.pop(0)}" />
<disable_collisions link1="${link}" link2="${other}" reason="Never" />
<!-- recursively call -->
<xacro:disable_collisions_for />
</xacro:if>
<!-- Disable collisions of link with any other arm link, as these are handled by the "sc" links -->
<disable_collisions link1="${link}" link2="$(arg arm_id)_link0" reason="Never" />
<disable_collisions link1="${link}" link2="$(arg arm_id)_link1" reason="Never" />
<disable_collisions link1="${link}" link2="$(arg arm_id)_link2" reason="Never" />
<disable_collisions link1="${link}" link2="$(arg arm_id)_link3" reason="Never" />
<disable_collisions link1="${link}" link2="$(arg arm_id)_link4" reason="Never" />
<disable_collisions link1="${link}" link2="$(arg arm_id)_link5" reason="Never" />
<disable_collisions link1="${link}" link2="$(arg arm_id)_link6" reason="Never" />
<disable_collisions link1="${link}" link2="$(arg arm_id)_link7" reason="Never" />
<disable_collisions link1="${link}" link2="$(arg arm_id)_link8" reason="Never" />
</xacro:macro>
<xacro:collision link="$(arg arm_id)_link0"/>
<enable_collisions link1="$(arg arm_id)_link0_sc" link2="$(arg arm_id)_link5_sc" />
<enable_collisions link1="$(arg arm_id)_link0_sc" link2="$(arg arm_id)_link6_sc" />
<enable_collisions link1="$(arg arm_id)_link0_sc" link2="$(arg arm_id)_link7_sc" />
<xacro:collision link="$(arg arm_id)_link1" />
<enable_collisions link1="$(arg arm_id)_link1_sc" link2="$(arg arm_id)_link5_sc" />
<enable_collisions link1="$(arg arm_id)_link1_sc" link2="$(arg arm_id)_link6_sc" />
<enable_collisions link1="$(arg arm_id)_link1_sc" link2="$(arg arm_id)_link7_sc" />
<xacro:collision link="$(arg arm_id)_link2" />
<enable_collisions link1="$(arg arm_id)_link2_sc" link2="$(arg arm_id)_link5_sc" />
<enable_collisions link1="$(arg arm_id)_link2_sc" link2="$(arg arm_id)_link6_sc" />
<enable_collisions link1="$(arg arm_id)_link2_sc" link2="$(arg arm_id)_link7_sc" />
<xacro:collision link="$(arg arm_id)_link3" />
<enable_collisions link1="$(arg arm_id)_link3_sc" link2="$(arg arm_id)_link7_sc" />
<xacro:collision link="$(arg arm_id)_link4" />
<xacro:collision link="$(arg arm_id)_link5" />
<xacro:collision link="$(arg arm_id)_link6" />
<xacro:collision link="$(arg arm_id)_link7" />
<xacro:collision link="$(arg arm_id)_link8" />
<xacro:macro name="enable_collisions_for" params="link:=^ others:=^">
<xacro:if value="${others}">
<xacro:property name="other" value="${others.pop(0)}" />
<enable_collisions link1="${link}" link2="${other}" />
<!-- recursively call -->
<xacro:enable_collisions_for />
</xacro:if>
</xacro:macro>
<xacro:macro name="configure_collisions" params="link enabled:=${[]}">
<!-- Disable collision checking between normal links, as these are handled by "sc" links -->
<xacro:property name="link_fmt" value="$(arg arm_id)_link{}" />
<xacro:disable_collisions_for link="${link_fmt.format(link)}" others="${[link_fmt.format(i) for i in python.range(9)]}" />

<!-- Disable all collision checking for ${link}_sc -->
<xacro:property name="link_fmt" value="$(arg arm_id)_link{}_sc" />
<xacro:if value="${link != 8}">
<disable_default_collisions link="${link_fmt.format(link)}" />
</xacro:if>
<!-- Re-enable collisions checking for selected links -->
<xacro:enable_collisions_for link="${link_fmt.format(link)}" others="${[link_fmt.format(i) for i in enabled]}" />
</xacro:macro>

<!-- Configure ACM -->
<xacro:configure_collisions link="0" enabled="${[5,6,7]}" />
<xacro:configure_collisions link="1" enabled="${[5,6,7]}" />
<xacro:configure_collisions link="2" enabled="${[5,6,7]}" />
<xacro:configure_collisions link="3" enabled="${[7]}" />
<xacro:configure_collisions link="4" />
<xacro:configure_collisions link="5" />
<xacro:configure_collisions link="6" />
<xacro:configure_collisions link="7" />
<xacro:configure_collisions link="8" />
</robot>
42 changes: 11 additions & 31 deletions config/hand.xacro
View file
Original file line number Diff line number Diff line change
@@ -1,19 +1,5 @@
<?xml version="1.0" encoding="UTF-8"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
<xacro:macro name="finger_collisions" params="finger">
<!-- Disable all self-collisions between finger and arm links,
as these are already covered by the coarser hand collision model -->
<disable_collisions link1="${finger}" link2="$(arg arm_id)_link0" reason="Never" />
<disable_collisions link1="${finger}" link2="$(arg arm_id)_link1" reason="Never" />
<disable_collisions link1="${finger}" link2="$(arg arm_id)_link2" reason="Never" />
<disable_collisions link1="${finger}" link2="$(arg arm_id)_link3" reason="Never" />
<disable_collisions link1="${finger}" link2="$(arg arm_id)_link4" reason="Never" />
<disable_collisions link1="${finger}" link2="$(arg arm_id)_link5" reason="Never" />
<disable_collisions link1="${finger}" link2="$(arg arm_id)_link6" reason="Never" />
<disable_collisions link1="${finger}" link2="$(arg arm_id)_link7" reason="Never" />
<disable_collisions link1="${finger}" link2="$(arg arm_id)_link8" reason="Never" />
<disable_collisions link1="${finger}" link2="$(arg arm_id)_hand" reason="Never" />
</xacro:macro>
<xacro:macro name="hand">
<!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
<!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
Expand All @@ -38,24 +24,18 @@
<!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
<!--PASSIVE JOINT: Purpose: this element is used to mark joints that are not actuated-->
<!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
<!-- Disable collisions for hand_sc by default (with environment) -->
<disable_default_collisions link="$(arg arm_id)_hand_sc" />
<enable_collisions link1="$(arg arm_id)_hand_sc" link2="$(arg arm_id)_link0_sc" />
<enable_collisions link1="$(arg arm_id)_hand_sc" link2="$(arg arm_id)_link1_sc" />
<enable_collisions link1="$(arg arm_id)_hand_sc" link2="$(arg arm_id)_link2_sc" />
<enable_collisions link1="$(arg arm_id)_hand_sc" link2="$(arg arm_id)_link3_sc" />
<!-- Disable collision of hand with all arm links. These are handled by the *_sc links -->
<disable_collisions link1="$(arg arm_id)_hand" link2="$(arg arm_id)_link0" reason="Never" />
<disable_collisions link1="$(arg arm_id)_hand" link2="$(arg arm_id)_link1" reason="Never" />
<disable_collisions link1="$(arg arm_id)_hand" link2="$(arg arm_id)_link2" reason="Never" />
<disable_collisions link1="$(arg arm_id)_hand" link2="$(arg arm_id)_link3" reason="Never" />
<disable_collisions link1="$(arg arm_id)_hand" link2="$(arg arm_id)_link4" reason="Never" />
<disable_collisions link1="$(arg arm_id)_hand" link2="$(arg arm_id)_link5" reason="Never" />
<disable_collisions link1="$(arg arm_id)_hand" link2="$(arg arm_id)_link6" reason="Never" />
<disable_collisions link1="$(arg arm_id)_hand" link2="$(arg arm_id)_link7" reason="Never" />
<disable_collisions link1="$(arg arm_id)_hand" link2="$(arg arm_id)_link8" reason="Never" />

<xacro:finger_collisions finger="$(arg arm_id)_leftfinger" />
<xacro:finger_collisions finger="$(arg arm_id)_rightfinger" />
<!-- Reenable collision of hand_sc for selected arm sc links -->
<xacro:property name="link_fmt" value="$(arg arm_id)_link{}_sc" />
<xacro:enable_collisions_for link="$(arg arm_id)_hand_sc" others="${[link_fmt.format(i) for i in [0,1,2,3]]}" />
<!-- Disable collision of hand link with all arm links. These are handled by the *_sc links -->
<xacro:property name="link_fmt" value="$(arg arm_id)_link{}" />
<xacro:disable_collisions_for link="$(arg arm_id)_hand" others="${[link_fmt.format(i) for i in python.range(9)]}" />
<!-- Disable collision of fingers with all arm links -->
<xacro:property name="others" value="${[link_fmt.format(i) for i in python.range(9)] + [xacro.arg('arm_id') + '_hand']}" />
<xacro:disable_collisions_for link="$(arg arm_id)_leftfinger" others="${list(others)}" />
<xacro:disable_collisions_for link="$(arg arm_id)_rightfinger" others="${list(others)}" />
<disable_collisions link1="$(arg arm_id)_leftfinger" link2="$(arg arm_id)_rightfinger" reason="Never" />
</xacro:macro>
</robot>

0 comments on commit adff2fd

Please sign in to comment.