From 23ec28016aa536633f32ae7b8a12372b3a5b475f Mon Sep 17 00:00:00 2001
From: Sam Pfeiffer <sam.pfeiffer@pal-robotics.com>
Date: Mon, 26 Sep 2016 18:16:17 +0200
Subject: [PATCH] Fix gripper movement in simulation. Add colors to model
---
talos_description/gazebo/gazebo.urdf.xacro | 1 +
talos_description/urdf/arm/arm.urdf.xacro | 4 +
talos_description/urdf/arm/wrist.urdf.xacro | 1 +
.../urdf/gripper/gripper.gazebo.xacro | 175 ++++++++++++++----
.../urdf/gripper/gripper.transmission.xacro | 13 ++
.../urdf/gripper/gripper_v2.urdf.xacro | 37 ++--
talos_description/urdf/head/head.urdf.xacro | 12 ++
talos_description/urdf/leg/leg_v2.urdf.xacro | 19 ++
.../urdf/sensors/xtion_pro_live.gazebo.xacro | 2 +
9 files changed, 213 insertions(+), 51 deletions(-)
diff --git a/talos_description/gazebo/gazebo.urdf.xacro b/talos_description/gazebo/gazebo.urdf.xacro
index 916a7dc..9ec18fd 100644
--- a/talos_description/gazebo/gazebo.urdf.xacro
+++ b/talos_description/gazebo/gazebo.urdf.xacro
@@ -14,6 +14,7 @@
<plugin filename="libgazebo_world_odometry.so" name="gazebo_ros_odometry">
<frameName>torso_2_link</frameName>
<topicName>floating_base_pose</topicName>
+ <updateRate>100.0</updateRate>
</plugin>
</gazebo>
diff --git a/talos_description/urdf/arm/arm.urdf.xacro b/talos_description/urdf/arm/arm.urdf.xacro
index 1c7f08a..f020b56 100644
--- a/talos_description/urdf/arm/arm.urdf.xacro
+++ b/talos_description/urdf/arm/arm.urdf.xacro
@@ -205,18 +205,22 @@
<gazebo reference="${name}_${side}_1_link">
<mu1>0.9</mu1>
<mu2>0.9</mu2>
+ <material>Gazebo/FlatBlack</material>
</gazebo>
<gazebo reference="${name}_${side}_2_link">
<mu1>0.9</mu1>
<mu2>0.9</mu2>
+ <material>Gazebo/FlatBlack</material>
</gazebo>
<gazebo reference="${name}_${side}_3_link">
<mu1>0.9</mu1>
<mu2>0.9</mu2>
+ <material>Gazebo/FlatBlack</material>
</gazebo>
<gazebo reference="${name}_${side}_4_link">
<mu1>0.9</mu1>
<mu2>0.9</mu2>
+ <material>Gazebo/FlatBlack</material>
</gazebo>
<gazebo reference="${name}_${side}_1_joint">
diff --git a/talos_description/urdf/arm/wrist.urdf.xacro b/talos_description/urdf/arm/wrist.urdf.xacro
index f2c3af7..efa3435 100644
--- a/talos_description/urdf/arm/wrist.urdf.xacro
+++ b/talos_description/urdf/arm/wrist.urdf.xacro
@@ -167,6 +167,7 @@
<gazebo reference="${name}_${side}_7_link">
<mu1>0.9</mu1>
<mu2>0.9</mu2>
+ <material>Gazebo/FlatBlack</material>
</gazebo>
<gazebo reference="${name}_${side}_5_joint">
diff --git a/talos_description/urdf/gripper/gripper.gazebo.xacro b/talos_description/urdf/gripper/gripper.gazebo.xacro
index 5a1f5e1..9a2a741 100644
--- a/talos_description/urdf/gripper/gripper.gazebo.xacro
+++ b/talos_description/urdf/gripper/gripper.gazebo.xacro
@@ -10,43 +10,144 @@
-->
<robot xmlns:xacro="http://ros.org/wiki/xacro">
- <!-- MACROS FOR MIMIC JOINTS -->
- <xacro:macro name="finger_mimics" params="name">
- <plugin name="mimic_${name}_inner_double_joint" filename="libroboticsgroup_gazebo_mimic_joint_plugin.so">
- <joint>${name}_joint</joint>
- <mimicJoint>${name}_inner_double_joint</mimicJoint>
- <multiplier>1.0</multiplier>
- <offset>0.0</offset>
- <hasPID/>
- </plugin>
- <plugin name="mimic_${name}_fingertip_1_joint" filename="libroboticsgroup_gazebo_mimic_joint_plugin.so">
- <joint>${name}_joint</joint>
- <mimicJoint>${name}_fingertip_1_joint</mimicJoint>
- <multiplier>-1.0</multiplier>
- <offset>0.0</offset>
- <hasPID/>
- </plugin>
- <plugin name="mimic_${name}_fingertip_2_joint" filename="libroboticsgroup_gazebo_mimic_joint_plugin.so">
- <joint>${name}_joint</joint>
- <mimicJoint>${name}_fingertip_2_joint</mimicJoint>
- <multiplier>-1.0</multiplier>
- <offset>0.0</offset>
- <hasPID/>
- </plugin>
- <plugin name="mimic_${name}_motor_single_joint" filename="libroboticsgroup_gazebo_mimic_joint_plugin.so">
- <joint>${name}_joint</joint>
- <mimicJoint>${name}_motor_single_joint</mimicJoint>
- <multiplier>1.0</multiplier>
- <offset>0.0</offset>
- <hasPID/>
- </plugin>
- <plugin name="mimic_${name}_fingertip_3_joint" filename="libroboticsgroup_gazebo_mimic_joint_plugin.so">
- <joint>${name}_joint</joint>
- <mimicJoint>${name}_fingertip_3_joint</mimicJoint>
- <multiplier>-1.0</multiplier>
- <offset>0.0</offset>
- <hasPID/>
- </plugin>
+ <!-- VIRTUAL (mimic) JOINTS -->
+ <xacro:macro name="virtual_joints" params="name">
+ <gazebo>
+ <plugin name="mimic_${name}_inner_double_joint" filename="libroboticsgroup_gazebo_mimic_joint_plugin.so">
+ <joint>${name}_joint</joint>
+ <mimicJoint>${name}_inner_double_joint</mimicJoint>
+ <multiplier>1.0</multiplier>
+ <offset>0.0</offset>
+ <!-- <hasPID/> -->
+ </plugin>
+ <plugin name="mimic_${name}_fingertip_1_joint" filename="libroboticsgroup_gazebo_mimic_joint_plugin.so">
+ <joint>${name}_joint</joint>
+ <mimicJoint>${name}_fingertip_1_joint</mimicJoint>
+ <multiplier>-1.0</multiplier>
+ <offset>0.0</offset>
+ <!-- <hasPID/> -->
+ </plugin>
+
+ <plugin name="mimic_${name}_fingertip_2_joint" filename="libroboticsgroup_gazebo_mimic_joint_plugin.so">
+ <joint>${name}_joint</joint>
+ <mimicJoint>${name}_fingertip_2_joint</mimicJoint>
+ <multiplier>-1.0</multiplier>
+ <offset>0.0</offset>
+ <!-- <hasPID/> -->
+ </plugin>
+
+ <plugin name="mimic_${name}_inner_single_joint" filename="libroboticsgroup_gazebo_mimic_joint_plugin.so">
+ <joint>${name}_joint</joint>
+ <mimicJoint>${name}_inner_single_joint</mimicJoint>
+ <multiplier>-1.0</multiplier>
+ <offset>0.0</offset>
+ <!-- <hasPID/> -->
+ </plugin>
+
+ <plugin name="mimic_${name}_motor_single_joint" filename="libroboticsgroup_gazebo_mimic_joint_plugin.so">
+ <joint>${name}_joint</joint>
+ <mimicJoint>${name}_motor_single_joint</mimicJoint>
+ <multiplier>-1.0</multiplier>
+ <offset>0.0</offset>
+ <!-- <hasPID/> -->
+ </plugin>
+ <plugin name="mimic_${name}_fingertip_3_joint" filename="libroboticsgroup_gazebo_mimic_joint_plugin.so">
+ <joint>${name}_joint</joint>
+ <mimicJoint>${name}_fingertip_3_joint</mimicJoint>
+ <multiplier>-1.0</multiplier>
+ <offset>0.0</offset>
+ <!-- <hasPID/> -->
+ </plugin>
+ </gazebo>
+
+<!-- <plugin filename="libgazebo_underactuated_finger.so" name="gazebo_${name}_underactuated">
+ <actuatedJoint>${name}_joint</actuatedJoint>
+ <virtualJoint>
+ <name>${name}_inner_double_joint</name>
+ <scale_factor>1.0</scale_factor>
+ </virtualJoint> -->
+<!-- <virtualJoint>
+ <name>${name}_fingertip_1_joint</name>
+ <scale_factor>-1.0</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>${name}_fingertip_2_joint</name>
+ <scale_factor>-1.0</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>${name}_motor_single_joint</name>
+ <scale_factor>1.0</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>${name}_fingertip_3_joint</name>
+ <scale_factor>-1.0</scale_factor>
+ </virtualJoint> -->
+<!-- </plugin> -->
+
+
+ <gazebo reference="${name}_joint">
+ <implicitSpringDamper>1</implicitSpringDamper>
+ <provideFeedback>1</provideFeedback>
+ </gazebo>
+
+ <gazebo reference="${name}_inner_double_joint">
+ <implicitSpringDamper>1</implicitSpringDamper>
+ <provideFeedback>1</provideFeedback>
+ </gazebo>
+
+ <gazebo reference="${name}_fingertip_1_joint">
+ <implicitSpringDamper>1</implicitSpringDamper>
+ <provideFeedback>1</provideFeedback>
+ </gazebo>
+
+ <gazebo reference="${name}_fingertip_2_joint">
+ <implicitSpringDamper>1</implicitSpringDamper>
+ <provideFeedback>1</provideFeedback>
+ </gazebo>
+
+ <gazebo reference="${name}_motor_single_joint">
+ <implicitSpringDamper>1</implicitSpringDamper>
+ <provideFeedback>1</provideFeedback>
+ </gazebo>
+
+ <gazebo reference="${name}_fingertip_3_joint">
+ <implicitSpringDamper>1</implicitSpringDamper>
+ <provideFeedback>1</provideFeedback>
+ </gazebo>
+
+ <gazebo reference="${name}_base_link">
+ <material>Gazebo/FlatBlack</material>
+ </gazebo>
+
+ <gazebo reference="${name}_motor_double_link">
+ <material>Gazebo/Orange</material>
+ </gazebo>
+
+ <gazebo reference="${name}_inner_double_link">
+ <material>Gazebo/Orange</material>
+ </gazebo>
+
+ <gazebo reference="${name}_fingertip_1_link">
+ <material>Gazebo/FlatBlack</material>
+ </gazebo>
+
+ <gazebo reference="${name}_motor_single_link">
+ <material>Gazebo/Orange</material>
+ </gazebo>
+
+
+ <gazebo reference="${name}_inner_single_link">
+ <material>Gazebo/Orange</material>
+ </gazebo>
+
+ <gazebo reference="${name}_fingertip_2_link">
+ <material>Gazebo/FlatBlack</material>
+ </gazebo>
+
+ <gazebo reference="${name}_fingertip_3_link">
+ <material>Gazebo/FlatBlack</material>
+ </gazebo>
+
</xacro:macro>
</robot>
\ No newline at end of file
diff --git a/talos_description/urdf/gripper/gripper.transmission.xacro b/talos_description/urdf/gripper/gripper.transmission.xacro
index 86e9439..ea9b889 100644
--- a/talos_description/urdf/gripper/gripper.transmission.xacro
+++ b/talos_description/urdf/gripper/gripper.transmission.xacro
@@ -22,4 +22,17 @@
</transmission>
</xacro:macro>
+ <xacro:macro name="gripper_virtual_transmission" params="name reduction" >
+ <transmission name="${name}_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <actuator name="${name}_motor" >
+ <mechanicalReduction>${reduction}</mechanicalReduction>
+ </actuator>
+ <joint name="${name}_joint" >
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ </transmission>
+ </xacro:macro>
+
+
</robot>
\ No newline at end of file
diff --git a/talos_description/urdf/gripper/gripper_v2.urdf.xacro b/talos_description/urdf/gripper/gripper_v2.urdf.xacro
index 7b0ccae..1ab5ac0 100644
--- a/talos_description/urdf/gripper/gripper_v2.urdf.xacro
+++ b/talos_description/urdf/gripper/gripper_v2.urdf.xacro
@@ -113,7 +113,7 @@
<origin xyz="0.00000 0.00525 -0.05598"
rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/>
<axis xyz="1 0 0" />
- <limit lower="${0.00000 * deg_to_rad}" upper="${0.00000 * deg_to_rad}" effort="1.0" velocity="1.0" />
+ <limit lower="${-60.0 * deg_to_rad}" upper="${0.0 * deg_to_rad}" effort="100.0" velocity="1.0" />
<mimic joint="${name}_joint" multiplier="${1.0}" offset="0.0" />
</joint>
@@ -152,9 +152,9 @@
<origin xyz="0.03200 0.04589 -0.06553"
rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/>
<axis xyz="1 0 0" />
- <limit lower="${0.00000 * deg_to_rad}" upper="${0.00000 * deg_to_rad}" effort="1.0" velocity="1.0" />
+ <limit lower="${0.0 * deg_to_rad}" upper="${60.0 * deg_to_rad}" effort="100.0" velocity="1.0" />
<mimic joint="${name}_joint" multiplier="${-1.0}" offset="0.0" />
- </joint>
+ </joint>
<link name="${name}_fingertip_2_link">
@@ -188,12 +188,12 @@
<origin xyz="-0.03200 0.04589 -0.06553"
rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/>
<axis xyz="1 0 0" />
- <limit lower="${0.00000 * deg_to_rad}" upper="${0.00000 * deg_to_rad}" effort="1.0" velocity="1.0" />
+ <limit lower="${0.0 * deg_to_rad}" upper="${60.0 * deg_to_rad}" effort="100.0" velocity="1.0" />
<mimic joint="${name}_joint" multiplier="${-1.0}" offset="0.0" />
- </joint>
+ </joint>
- <link name="${name}_motor_single_link">
+ <link name="${name}_motor_single_link">
<inertial>
<origin xyz="0.02589 -0.01284 -0.00640" rpy="0.00000 0.00000 0.00000"/>
<mass value="0.14765"/>
@@ -224,13 +224,13 @@
<origin xyz="0.00000 -0.02025 -0.03000"
rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/>
<axis xyz="1 0 0" />
- <limit lower="${0.00000 * deg_to_rad}" upper="${0.00000 * deg_to_rad}" effort="1.0" velocity="1.0" />
+ <limit lower="${0.0 * deg_to_rad}" upper="${60.0 * deg_to_rad}" effort="100.0" velocity="1.0" />
<mimic joint="${name}_joint" multiplier="${-1.0}" offset="0.0" />
- </joint>
+ </joint>
- <link name="${name}_inner_single_link">
+ <link name="${name}_inner_single_link">
<inertial>
<origin xyz="0.00000 -0.03253 -0.01883" rpy="0.00000 0.00000 0.00000"/>
<mass value="0.05356"/>
@@ -261,10 +261,10 @@
<origin xyz="0.00000 -0.00525 -0.05598"
rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/>
<axis xyz="1 0 0" />
- <limit lower="${0.00000 * deg_to_rad}" upper="${0.00000 * deg_to_rad}" effort="1.0" velocity="1.0" />
+ <limit lower="${0.0 * deg_to_rad}" upper="${60.0 * deg_to_rad}" effort="100.0" velocity="1.0" />
<mimic joint="${name}_joint" multiplier="${-1.0}" offset="0.0" />
- </joint>
+ </joint>
<link name="${name}_fingertip_3_link">
@@ -298,14 +298,23 @@
<origin xyz="0.00000 -0.04589 -0.06553"
rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${180.00000 * deg_to_rad}"/>
<axis xyz="1 0 0" />
- <limit lower="${0.00000 * deg_to_rad}" upper="${0.00000 * deg_to_rad}" effort="1.0" velocity="1.0" />
+ <limit lower="${0.0 * deg_to_rad}" upper="${60.0 * deg_to_rad}" effort="100.0" velocity="1.0" />
<mimic joint="${name}_joint" multiplier="${-1.0}" offset="0.0" />
- </joint>
+ </joint>
+
- <xacro:finger_mimics name="${name}" />
+ <xacro:virtual_joints name="${name}" />
<xacro:gripper_transmission name="${name}" reduction="1.0" />
+ <!-- virtual mimic joints -->
+ <xacro:gripper_virtual_transmission name="${name}_inner_double" reduction="1.0"/>
+ <xacro:gripper_virtual_transmission name="${name}_fingertip_1" reduction="1.0"/>
+ <xacro:gripper_virtual_transmission name="${name}_fingertip_2" reduction="1.0"/>
+ <xacro:gripper_virtual_transmission name="${name}_inner_single" reduction="1.0"/>
+ <xacro:gripper_virtual_transmission name="${name}_motor_single" reduction="1.0"/>
+ <xacro:gripper_virtual_transmission name="${name}_fingertip_3" reduction="1.0"/>
+
</xacro:macro>
</robot>
diff --git a/talos_description/urdf/head/head.urdf.xacro b/talos_description/urdf/head/head.urdf.xacro
index a24c9c9..c2cdb35 100644
--- a/talos_description/urdf/head/head.urdf.xacro
+++ b/talos_description/urdf/head/head.urdf.xacro
@@ -72,8 +72,15 @@
<gazebo reference="${name}_joint">
<implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
+
+ <gazebo reference="${name}_link">
+ <material>Gazebo/FlatBlack</material>
+ </gazebo>
+
</xacro:macro>
+
+
<!--************************-->
<!-- HEAD_2 (PAN) -->
<!--************************-->
@@ -127,6 +134,11 @@
</gazebo>
+ <gazebo reference="${name}_link">
+ <material>Gazebo/White</material>
+ </gazebo>
+
+
</xacro:macro>
<xacro:macro name="talos_head" params="name parent">
diff --git a/talos_description/urdf/leg/leg_v2.urdf.xacro b/talos_description/urdf/leg/leg_v2.urdf.xacro
index 9b29827..5f5a4e9 100644
--- a/talos_description/urdf/leg/leg_v2.urdf.xacro
+++ b/talos_description/urdf/leg/leg_v2.urdf.xacro
@@ -330,6 +330,25 @@
<provideFeedback>1</provideFeedback>
</gazebo>
+ <gazebo reference="leg_${prefix}_1_link">
+ <material>Gazebo/FlatBlack</material>
+ </gazebo>
+ <gazebo reference="leg_${prefix}_2_link">
+ <material>Gazebo/FlatBlack</material>
+ </gazebo>
+ <gazebo reference="leg_${prefix}_3_link">
+ <material>Gazebo/FlatBlack</material>
+ </gazebo>
+ <gazebo reference="leg_${prefix}_4_link">
+ <material>Gazebo/FlatBlack</material>
+ </gazebo>
+ <gazebo reference="leg_${prefix}_5_link">
+ <material>Gazebo/White</material>
+ </gazebo>
+ <gazebo reference="leg_${prefix}_6_link">
+ <material>Gazebo/White</material>
+ </gazebo>
+
</xacro:macro>
</robot>
diff --git a/talos_description/urdf/sensors/xtion_pro_live.gazebo.xacro b/talos_description/urdf/sensors/xtion_pro_live.gazebo.xacro
index 92066ec..6ac77d3 100644
--- a/talos_description/urdf/sensors/xtion_pro_live.gazebo.xacro
+++ b/talos_description/urdf/sensors/xtion_pro_live.gazebo.xacro
@@ -90,6 +90,8 @@
<distalostionT2>0.00000001</distalostionT2>
</plugin>
</sensor>
+
+ <material>Gazebo/FlatBlack</material>
</gazebo>
</xacro:macro>
</robot>
--
GitLab