From adff2fdb2894b7781a7f5c06bab3a155b0cb3ea4 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Thu, 1 Sep 2022 11:37:14 +0200
Subject: [PATCH] srdf: Use loop macros to reduce code redundancy
---
config/arm.xacro | 71 +++++++++++++++++++++++++----------------------
config/hand.xacro | 42 ++++++++--------------------
2 files changed, 49 insertions(+), 64 deletions(-)
diff --git a/config/arm.xacro b/config/arm.xacro
index 7cb6ebf..ea345aa 100644
--- a/config/arm.xacro
+++ b/config/arm.xacro
@@ -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>
diff --git a/config/hand.xacro b/config/hand.xacro
index 3d69125..8c78a14 100644
--- a/config/hand.xacro
+++ b/config/hand.xacro
@@ -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-->
@@ -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>
--
GitLab