From 1881101799f9f4f328503e80b06fb8981c3b24d5 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Thu, 1 Sep 2022 10:27:02 +0200
Subject: [PATCH] Adapt ACM configuration to Melodic
Melodic's srdfdom/MoveIt don't support tags <disable_default_collisions> and <enable_collisions>.
---
config/arm.xacro | 19 +++++--------------
config/hand.xacro | 16 ++++++++++------
2 files changed, 15 insertions(+), 20 deletions(-)
diff --git a/config/arm.xacro b/config/arm.xacro
index fef366a..8b0de75 100644
--- a/config/arm.xacro
+++ b/config/arm.xacro
@@ -41,24 +41,15 @@
<xacro:disable_collisions_for />
</xacro:if>
</xacro:macro>
- <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(8)]}" />
-
- <!-- Disable all collision checking for ${link}_sc -->
+ <xacro:disable_collisions_for link="$(arg arm_id)_link${link}" others="${[link_fmt.format(i) for i in python.range(8)]}" />
+ <!-- Disable collision checking between normal and "sc" links -->
<xacro:property name="link_fmt" value="$(arg arm_id)_link{}_sc" />
- <disable_default_collisions link="${link_fmt.format(link)}" />
- <!-- 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:disable_collisions_for link="$(arg arm_id)_link${link}" others="${[link_fmt.format(i) for i in python.range(8)]}" />
+ <!-- Disable collisions between not enabled sc links -->
+ <xacro:disable_collisions_for link="${link_fmt.format(link)}" others="${[link_fmt.format(i) for i in python.range(link, 8) if i not in enabled]}" />
</xacro:macro>
<!-- Configure ACM -->
diff --git a/config/hand.xacro b/config/hand.xacro
index 47a06b6..81ff24d 100644
--- a/config/hand.xacro
+++ b/config/hand.xacro
@@ -24,18 +24,22 @@
<!--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" />
- <!-- 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(8)]}" />
+ <xacro:disable_collisions_for link="$(arg arm_id)_hand_sc" others="${[link_fmt.format(i) for i in python.range(8)]}" />
+ <disable_collisions link1="$(arg arm_id)_hand_sc" link2="$(arg arm_id)_hand" reason="Never" />
<!-- Disable collision of fingers with all arm links -->
- <xacro:property name="others" value="${[link_fmt.format(i) for i in python.range(8)] + [xacro.arg('arm_id') + '_hand']}" />
+ <xacro:property name="others" value="${[link_fmt.format(i) for i in python.range(8)] + [xacro.arg('arm_id') + '_hand', xacro.arg('arm_id') + '_hand_sc']}" />
<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" />
+ <!-- Disable collision of hand+fingers with all arm sc links -->
+ <xacro:property name="link_fmt" value="$(arg arm_id)_link{}_sc" />
+ <xacro:disable_collisions_for link="$(arg arm_id)_hand" others="${[link_fmt.format(i) for i in python.range(8)]}" />
+ <xacro:disable_collisions_for link="$(arg arm_id)_leftfinger" others="${[link_fmt.format(i) for i in python.range(8)]}" />
+ <xacro:disable_collisions_for link="$(arg arm_id)_rightfinger" others="${[link_fmt.format(i) for i in python.range(8)]}" />
+ <!-- Disable collision of hand_sc with selected arm sc links -->
+ <xacro:disable_collisions_for link="$(arg arm_id)_hand_sc" others="${[link_fmt.format(i) for i in [4,5,6,7]]}" />
</xacro:macro>
</robot>
--
GitLab