From d801614c1a0f8bf7984e11152463e39eebf692e1 Mon Sep 17 00:00:00 2001
From: Luca <luca.marchionni@pal-robotics.com>
Date: Thu, 10 Nov 2016 23:52:22 +0100
Subject: [PATCH] Removed xacro if

---
 talos_description/urdf/arm/arm.urdf.xacro    | 50 ++------------------
 talos_description/urdf/leg/leg_v2.urdf.xacro | 11 ++---
 2 files changed, 6 insertions(+), 55 deletions(-)

diff --git a/talos_description/urdf/arm/arm.urdf.xacro b/talos_description/urdf/arm/arm.urdf.xacro
index 80e1ff6..403fd48 100644
--- a/talos_description/urdf/arm/arm.urdf.xacro
+++ b/talos_description/urdf/arm/arm.urdf.xacro
@@ -66,13 +66,8 @@
       <origin xyz="0.00000 ${0.15750*reflect} 0.23200" 
               rpy="${0.0 * deg_to_rad} ${0.0 * deg_to_rad} ${0.0 * deg_to_rad}"/>
       <axis xyz="0 0 1" />
-      <xacro:if value="${reflect == 1}">
-        <limit lower="${-90.0 * deg_to_rad}" upper="${30.0 * deg_to_rad}" effort="${arm_1_max_effort}" velocity="${arm_1_max_vel}" />
-      </xacro:if>
-      <xacro:if value="${reflect == -1}">
-        <limit lower="${-30.0 * deg_to_rad}" upper="${90.0 * deg_to_rad}" effort="${arm_1_max_effort}" velocity="${arm_1_max_vel}" />
-      </xacro:if>
-       <dynamics friction="${arm_friction}" damping="${arm_damping}"/>
+      <limit lower="${(-60.0 - 30.0*reflect)* deg_to_rad}" upper="${(60.0 - 30.0*reflect)* deg_to_rad}" effort="${arm_1_max_effort}" velocity="${arm_1_max_vel}" />
+      <dynamics friction="${arm_friction}" damping="${arm_damping}"/>
 <!--        <safety_controller k_position="20"
                           k_velocity="20"
                           soft_lower_limit="${-90.00000 * deg_to_rad + arm_eps}"
@@ -110,12 +105,7 @@
       <origin xyz="0.00493 ${0.13650*reflect} 0.04673" 
               rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/>
       <axis xyz="1 0 0" />
-      <xacro:if value="${reflect == 1}">
-        <limit lower="${0.00000 * deg_to_rad}" upper="${165.00000 * deg_to_rad}" effort="${arm_2_max_effort}" velocity="${arm_2_max_vel}" />
-      </xacro:if>
-      <xacro:if value="${reflect == -1}">
-        <limit lower="${-165.00000 * deg_to_rad}" upper="${0.00000 * deg_to_rad}" effort="${arm_2_max_effort}" velocity="${arm_2_max_vel}" />
-      </xacro:if>
+      <limit lower="${(-82.50000 + 82.5*reflect)* deg_to_rad}" upper="${(82.50000 + 82.5000*reflect)* deg_to_rad}" effort="${arm_2_max_effort}" velocity="${arm_2_max_vel}" />      
       <dynamics friction="${arm_friction}" damping="${arm_damping}"/>
 <!--        <safety_controller k_position="20"
                           k_velocity="20"
@@ -246,40 +236,6 @@
     <!--************************-->
      <xacro:talos_wrist name="arm" parent="${name}_${side}_4_link" side="${side}" reflect="${reflect}" />
 
-    <!--***********************-->
-    <!--        TOOL           -->
-    <!--***********************-->
-<!--     <link name="${name}_${side}_tool_link">
-      <inertial>
-        <mass value="0.1" />
-        <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0" />
-      </inertial>
-      <visual>
-        <origin xyz="0.001 0 0" rpy="0 ${90.0 * deg_to_rad} 0" />
-        <geometry>
-          <cylinder radius="0.005" length="0.005"/>
-        </geometry>
-        <material name="LightGrey" />
-      </visual>
-      <collision>
-        <origin xyz="0.001 0 0" rpy="0 ${90.0 * deg_to_rad} 0" />
-        <geometry>
-          <cylinder radius="0.005" length="0.005"/>
-        </geometry>
-      </collision>
-    </link>
-
-    <joint name="${name}_${side}_tool_joint" type="fixed">
-      <parent link="${name}_${side}_7_link" />
-      <child link="${name}_${side}_tool_link" />
-      <xacro:if value="${reflect == 1}">
-        <origin xyz="0 0 -0.051" rpy="0 ${90.0 * deg_to_rad} 0" />
-      </xacro:if>
-      <xacro:if value="${reflect == -1}">
-        <origin xyz="0 0 -0.051" rpy="${180.0 * deg_to_rad} ${90.0 * deg_to_rad} ${0.0 * deg_to_rad}" />
-      </xacro:if>
-    </joint>
- -->
     <!-- extensions -->
     <xacro:talos_arm_simple_transmission name="${name}" side="${side}" number="1" reduction="1.0" />
     <xacro:talos_arm_simple_transmission name="${name}" side="${side}" number="2" reduction="1.0" />
diff --git a/talos_description/urdf/leg/leg_v2.urdf.xacro b/talos_description/urdf/leg/leg_v2.urdf.xacro
index a0ef763..5bedc07 100644
--- a/talos_description/urdf/leg/leg_v2.urdf.xacro
+++ b/talos_description/urdf/leg/leg_v2.urdf.xacro
@@ -46,12 +46,7 @@
       <origin xyz="-0.02 ${reflect*0.08500} -0.27105" 
               rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/>
       <axis xyz="0 0 1" />
-     <xacro:if value="${reflect == 1}">
-        <limit lower="-0.5236" upper="1.571" effort="100" velocity="3.87" />
-      </xacro:if>
-      <xacro:if value="${reflect == -1}">
-        <limit lower="-1.571" upper="0.5236" effort="100" velocity="3.87" />
-      </xacro:if>
+      <limit lower="${(-55.0 + 35.0*reflect)*deg_to_rad}" upper="${(55.0 + 35.0*reflect)*deg_to_rad}" effort="100" velocity="3.87" />
       <dynamics friction="${leg_friction}" damping="${leg_damping}"/>
     </joint>
 
@@ -89,7 +84,7 @@
       <origin xyz="0.00000 0.00000 0.00000" 
               rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/>
       <axis xyz="1 0 0" />
-      <limit lower="-0.524" upper="0.5236" effort="160" velocity="5.8" />
+      <limit lower="-0.5236" upper="0.5236" effort="160" velocity="5.8" />
       <dynamics friction="${leg_friction}" damping="${leg_damping}"/>
     </joint>
 
@@ -249,7 +244,7 @@
       <origin xyz="0.00000 0.00000 0.00000" 
               rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/>
       <axis xyz="1 0 0" />
-      <limit lower="-0.524" upper="0.524" effort="100" velocity="4.8" />
+      <limit lower="-0.5236" upper="0.5236" effort="100" velocity="4.8" />
       <dynamics friction="${leg_friction}" damping="${leg_damping}"/>
     </joint>
  
-- 
GitLab