diff --git a/python/example_robot_data/robots_loader.py b/python/example_robot_data/robots_loader.py
index 89e14275499da996055dc642bffc66f340f6ee89..128042b0e9a6ede38b9f05123f228ce63ea326d2 100644
--- a/python/example_robot_data/robots_loader.py
+++ b/python/example_robot_data/robots_loader.py
@@ -283,6 +283,10 @@ class TiagoLoader(RobotLoader):
urdf_filename = "tiago.urdf"
+class TiagoDualLoader(TiagoLoader):
+ urdf_filename = "tiago_dual.urdf"
+
+
class TiagoNoHandLoader(TiagoLoader):
urdf_filename = "tiago_no_hand.urdf"
@@ -485,6 +489,7 @@ ROBOTS = {
'talos_full': TalosFullLoader,
'talos_full_box': TalosFullBoxLoader,
'tiago': TiagoLoader,
+ 'tiago_dual': TiagoDualLoader,
'tiago_no_hand': TiagoNoHandLoader,
'ur3': UR5Loader,
'ur3_gripper': UR3GripperLoader,
diff --git a/robots/tiago_description/meshes/arm/arm_5-wrist-2010.stl b/robots/tiago_description/meshes/arm/arm_5-wrist-2010.stl
new file mode 100644
index 0000000000000000000000000000000000000000..b9db2978c31865ffcf1a6bc3c00eeaca0781cac8
Binary files /dev/null and b/robots/tiago_description/meshes/arm/arm_5-wrist-2010.stl differ
diff --git a/robots/tiago_description/meshes/arm/arm_5_collision-wrist-2010.dae b/robots/tiago_description/meshes/arm/arm_5_collision-wrist-2010.dae
new file mode 100644
index 0000000000000000000000000000000000000000..24c4d5e23c18c333acef014447f72671a448887b
--- /dev/null
+++ b/robots/tiago_description/meshes/arm/arm_5_collision-wrist-2010.dae
@@ -0,0 +1,63 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
+ <asset>
+ <contributor>
+ <author>VCGLab</author>
+ <authoring_tool>VCGLib | MeshLab</authoring_tool>
+ </contributor>
+ <up_axis>Y_UP</up_axis>
+ <created>Fri Apr 8 11:36:03 2016</created>
+ <modified>Fri Apr 8 11:36:03 2016</modified>
+ </asset>
+ <library_images/>
+ <library_materials/>
+ <library_effects/>
+ <library_geometries>
+ <geometry id="shape0-lib" name="shape0">
+ <mesh>
+ <source id="shape0-lib-positions" name="position">
+ <float_array id="shape0-lib-positions-array" count="156">-0.0560095 -0.00347675 0.00947676 0.00161033 0.0660705 0.0224633 -0.0385687 -0.0249916 0.158304 0.00895468 -0.0488647 0.177351 0.0116696 0.0552533 0.135901 0.0351697 0.0527178 0.0214259 0.0182917 0.0631052 0.0229375 0.0441234 0.0399774 0.0180597 0.0530331 0.0156898 0.0107575 0.0282007 0.0535731 0.0973136 -0.0291268 -0.0257986 0.176724 -0.0321194 0.0240883 0.173576 0.00534193 -0.0661357 0.0218512 0.000542164 -0.0588545 0.108242 -0.0301042 -0.0419261 0.160774 -0.00907717 -0.0239411 0.189561 -0.0136406 -0.0504551 0.170105 0.054466 -0.00921286 0.00983091 0.0268477 -0.059261 0.0217946 0.0436465 -0.0196301 0.132571 0.0461325 -0.0376623 0.016739 -0.0394506 0.023971 0.154001 -0.0345401 0.0537354 0.0214152 -0.0275655 0.0438715 0.163363 -0.041052 0.0374688 0.096997 -0.015313 0.0505675 0.166589 0.0406946 -0.0393468 0.0777242 0.0313938 -0.0522382 0.0727926 0.0295071 -0.0425489 0.162188 -0.0316513 -0.0481608 0.117059 -0.0163449 -0.0564587 0.113215 0.0460269 0.0145006 0.127259 0.0498183 -0.0135679 0.0756917 -0.0480934 -0.00890549 0.117781 -0.048659 -0.032158 0.0152126 -0.0524975 0.0229353 0.0126262 -0.0102538 0.0590898 0.10079 -0.0163435 0.0635674 0.0217205 0.049387 0.0183456 0.0675194 -0.0110815 0.030729 0.187324 0.0211535 -0.0543077 0.12122 0.0146362 -0.0225282 0.186745 -0.0349699 -0.0532071 0.0207903 -0.0175223 -0.0639367 0.0221045 0.0363887 0.0230592 0.166637 0.0396144 0.039185 0.0995444 0.030424 0.0412353 0.161048 -0.0502362 0.00946436 0.0889365 -0.0303766 0.0523895 0.0792112 0.0168719 0.0259823 0.187064 0.0127023 0.0502209 0.173778 0.0339124 -0.0250991 0.171655</float_array>
+ <technique_common>
+ <accessor count="52" source="#shape0-lib-positions-array" stride="3">
+ <param name="X" type="float"/>
+ <param name="Y" type="float"/>
+ <param name="Z" type="float"/>
+ </accessor>
+ </technique_common>
+ </source>
+ <source id="shape0-lib-normals" name="normal">
+ <float_array id="shape0-lib-normals-array" count="300">0.00514941 0.0374799 -0.999284 -0.00936589 0.119625 -0.992775 0.00764127 0.233704 -0.972278 -0.00215116 0.220239 -0.975444 -0.540122 0.0146265 0.84146 -0.973232 0.00266768 0.229808 -0.837143 -0.358135 0.413438 0.0808872 0.303171 -0.949497 0.900621 -0.422153 0.103286 0.924806 -0.333141 0.183713 0.613306 -0.0334601 0.789136 0.131105 -0.358033 0.924459 -0.00630557 -0.201396 -0.97949 -0.00713082 -0.197855 -0.980205 -0.182964 0.366941 -0.912074 -0.140487 0.987829 0.0667596 0.291347 0.952982 0.0833191 0.823392 0.565171 0.0510594 0.112407 0.323689 -0.939463 0.120432 0.331841 -0.935616 0.776936 0.626288 0.0642902 0.772736 0.627647 0.0945457 -0.551252 0.0195581 0.834109 -0.89006 -0.0246743 0.455174 -0.936413 0.0139489 0.350623 0.748554 -0.662621 0.0245196 -0.0341721 -0.243685 -0.969252 -0.107964 -0.988116 0.109408 -0.0945796 -0.992426 0.078389 -0.116055 -0.989791 0.0827372 -0.609025 -0.538753 0.582094 -0.440915 -0.480422 0.758148 -0.219009 -0.552451 0.80426 0.0536713 -0.250443 -0.966642 0.0956532 -0.307037 -0.946878 0.942221 -0.328171 0.0672613 0.941892 -0.327449 0.0749419 0.961686 -0.269549 0.0500401 0.0163198 0.0414733 0.999006 0.118611 -0.0120025 0.992868 -0.984614 0.104755 0.139861 -0.95369 0.28559 0.0944159 -0.86741 0.496555 0.0321346 0.172324 0.981919 0.0783531 0.94217 0.332102 0.044982 0.933902 0.353256 0.0551137 0.526252 0.849445 0.0387539 -0.871496 0.367964 0.324188 -0.877266 0.460979 0.133799 -0.488236 0.578659 0.653284 -0.581545 0.474858 0.660542 0.799687 -0.597912 0.0547914 0.793049 -0.603592 0.082159 -0.136131 -0.335535 -0.93214 -0.450996 -0.885107 0.114843 -0.516425 -0.84501 0.138792 0.993341 -0.0533888 0.102094 -0.997345 -0.002035 0.0727955 -0.96978 -0.236736 0.0590163 -0.843792 -0.533628 0.0570603 -0.953748 0.28992 0.0794417 -0.76976 0.634458 0.0702271 -0.755443 0.648876 0.0909156 -0.426518 0.900584 0.0838482 0.0258412 0.995366 0.092625 -0.0286354 0.991588 0.126228 0.996591 0.0550581 0.0614401 0.996857 0.0320307 0.0724639 0.997917 0.0283288 0.0579515 0.0913691 0.490689 0.866531 -0.170823 0.694284 0.699135 -0.0216781 0.990979 0.132252 0.0247572 -0.989832 0.140068 0.513327 -0.855167 0.0720033 0.61881 -0.779461 0.0975438 0.159116 -0.982993 0.0916889 0.404915 -0.897447 0.175023 0.137883 -0.982499 0.125238 0.536933 -0.383804 0.751264 0.609798 -0.491333 0.621883 -0.526223 -0.84804 0.0625938 -0.459805 -0.88454 0.0785406 0.725063 0.0343839 0.687824 0.924725 0.349691 0.150334 0.941826 0.327835 0.0740734 0.917368 0.378075 0.124478 -0.93521 -0.344185 0.0831846 -0.89595 -0.434174 0.0936297 -0.90329 -0.420575 0.0847596 -0.990743 0.125593 0.0515298 -0.497873 0.861017 0.103785 -0.383292 0.919684 0.0852547 -0.475364 0.878088 0.0546927 0.688082 0.409927 0.598752 0.527696 0.837197 0.143659 0.660255 0.445145 0.604904 0.358694 0.926551 0.113322 0.304002 -0.950467 0.0647629 0.969788 -0.024578 0.242709 0.970899 -0.0307507 0.237507</float_array>
+ <technique_common>
+ <accessor count="100" source="#shape0-lib-normals-array" stride="3">
+ <param name="X" type="float"/>
+ <param name="Y" type="float"/>
+ <param name="Z" type="float"/>
+ </accessor>
+ </technique_common>
+ </source>
+ <vertices id="shape0-lib-vertices">
+ <input semantic="POSITION" source="#shape0-lib-positions"/>
+ </vertices>
+ <triangles count="100">
+ <input offset="0" semantic="VERTEX" source="#shape0-lib-vertices"/>
+ <input offset="1" semantic="NORMAL" source="#shape0-lib-normals"/>
+ <p>0 0 8 0 17 0 0 1 35 1 8 1 8 2 37 2 1 2 35 3 37 3 8 3 10 4 15 4 39 4 33 5 2 5 21 5 10 6 2 6 14 6 6 7 8 7 1 7 19 8 28 8 26 8 19 9 51 9 28 9 51 10 49 10 41 10 3 11 41 11 15 11 12 12 34 12 17 12 17 13 34 13 0 13 35 14 22 14 37 14 37 15 36 15 1 15 6 16 4 16 9 16 45 17 7 17 5 17 6 18 7 18 8 18 5 19 7 19 6 19 5 20 9 20 45 20 45 21 9 21 46 21 39 22 11 22 10 22 10 23 11 23 2 23 2 24 11 24 21 24 27 25 18 25 20 25 43 26 34 26 12 26 30 27 13 27 16 27 13 28 43 28 12 28 43 29 13 29 30 29 10 30 14 30 16 30 10 31 16 31 15 31 15 32 16 32 3 32 17 33 20 33 12 33 12 34 20 34 18 34 26 35 32 35 19 35 20 36 32 36 26 36 20 37 17 37 32 37 39 38 15 38 49 38 41 39 49 39 15 39 33 40 21 40 47 40 21 41 24 41 47 41 35 42 24 42 22 42 4 43 6 43 1 43 7 44 38 44 8 44 38 45 7 45 45 45 6 46 9 46 5 46 11 47 23 47 21 47 24 48 21 48 23 48 23 49 39 49 25 49 39 50 23 50 11 50 27 51 20 51 26 51 27 52 26 52 28 52 43 53 42 53 34 53 16 54 29 54 30 54 16 55 14 55 29 55 31 56 19 56 32 56 0 57 33 57 47 57 0 58 34 58 33 58 29 59 34 59 42 59 24 60 35 60 47 60 24 61 48 61 22 61 23 62 48 62 24 62 25 63 36 63 37 63 36 64 4 64 1 64 4 65 36 65 25 65 8 66 38 66 17 66 32 67 17 67 38 67 31 68 32 68 38 68 39 69 49 69 50 69 39 70 50 70 25 70 4 71 25 71 50 71 3 72 16 72 13 72 27 73 40 73 18 73 28 74 40 74 27 74 40 75 13 75 12 75 3 76 40 76 28 76 13 77 40 77 3 77 3 78 51 78 41 78 51 79 3 79 28 79 42 80 43 80 29 80 29 81 43 81 30 81 49 82 51 82 44 82 46 83 44 83 31 83 45 84 31 84 38 84 31 85 45 85 46 85 34 86 2 86 33 86 29 87 14 87 2 87 2 88 34 88 29 88 35 89 0 89 47 89 23 90 25 90 48 90 37 91 48 91 25 91 48 92 37 92 22 92 49 93 44 93 46 93 9 94 50 94 46 94 50 95 49 95 46 95 4 96 50 96 9 96 18 97 40 97 12 97 44 98 51 98 31 98 19 99 31 99 51 99</p>
+ </triangles>
+ </mesh>
+ </geometry>
+ </library_geometries>
+ <library_visual_scenes>
+ <visual_scene id="VisualSceneNode" name="VisualScene">
+ <node id="node" name="node">
+ <instance_geometry url="#shape0-lib">
+ <bind_material>
+ <technique_common/>
+ </bind_material>
+ </instance_geometry>
+ </node>
+ </visual_scene>
+ </library_visual_scenes>
+ <scene>
+ <instance_visual_scene url="#VisualSceneNode"/>
+ </scene>
+</COLLADA>
diff --git a/robots/tiago_description/meshes/arm/arm_6-wrist-2010.stl b/robots/tiago_description/meshes/arm/arm_6-wrist-2010.stl
new file mode 100644
index 0000000000000000000000000000000000000000..5a7391a1bb43140ddd4977bbb0084c9a524b634f
Binary files /dev/null and b/robots/tiago_description/meshes/arm/arm_6-wrist-2010.stl differ
diff --git a/robots/tiago_description/meshes/arm/arm_6_collision-wrist-2010.dae b/robots/tiago_description/meshes/arm/arm_6_collision-wrist-2010.dae
new file mode 100644
index 0000000000000000000000000000000000000000..5c60e1344787cd202b0ad59702137df69f8bda16
--- /dev/null
+++ b/robots/tiago_description/meshes/arm/arm_6_collision-wrist-2010.dae
@@ -0,0 +1,63 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
+ <asset>
+ <contributor>
+ <author>VCGLab</author>
+ <authoring_tool>VCGLib | MeshLab</authoring_tool>
+ </contributor>
+ <up_axis>Y_UP</up_axis>
+ <created>Fri Apr 8 14:02:14 2016</created>
+ <modified>Fri Apr 8 14:02:14 2016</modified>
+ </asset>
+ <library_images/>
+ <library_materials/>
+ <library_effects/>
+ <library_geometries>
+ <geometry id="shape0-lib" name="shape0">
+ <mesh>
+ <source id="shape0-lib-positions" name="position">
+ <float_array id="shape0-lib-positions-array" count="138">-0.036859 -0.021497 0 -0.038774 -1.69487e-09 0.013937 -0.019152 -0.034535 0.013937 1.73045e-09 -0.039588 0.013937 -0.020373 -0.038062 0 -0.032944 -0.020636 0.013937 0.03341 -0.019242 0.014227 0.019152 -0.034535 0.013937 0.020373 -0.038062 0 -0.032944 0.020636 0.013937 -0.036859 0.021497 0 -1.8215e-09 0.041671 0 -0.020373 0.038062 0 -1.73045e-09 0.039588 0.013937 -0.019152 0.034535 0.013937 0.04733 0.018629 0.015678 0.04733 0.022725 0 0.019152 0.034535 0.013937 0.036859 0.022725 0 0.020373 0.038062 0 0.038774 1.69487e-09 0.020319 0.03341 0.019242 0.014227 -0.041671 -1.8215e-09 0 -0.038774 -1.69487e-09 -0.013937 -0.019152 -0.034535 -0.013937 1.73045e-09 -0.039588 -0.013937 -0.032944 -0.020636 -0.013937 0.036859 -0.022725 0 1.8215e-09 -0.041671 0 0.04733 2.06886e-09 -0.022291 0.04733 -0.018629 -0.015678 0.03341 -0.019242 -0.014227 -0.032944 0.020636 -0.013937 -1.73045e-09 0.039588 -0.013937 -0.019152 0.034535 -0.013937 0.04733 0.018629 -0.015678 0.03341 0.019242 -0.014227 0.019152 0.034535 -0.013937 0.04733 -0.018629 0.015678 0.041671 1.8215e-09 0 0.04733 2.06886e-09 0.022291 0.04733 -0.022725 0 0.038774 1.69487e-09 -0.018953 0 0 0.019322 0.019152 -0.034535 -0.013937 0 0 -0.019322</float_array>
+ <technique_common>
+ <accessor count="46" source="#shape0-lib-positions-array" stride="3">
+ <param name="X" type="float"/>
+ <param name="Y" type="float"/>
+ <param name="Z" type="float"/>
+ </accessor>
+ </technique_common>
+ </source>
+ <source id="shape0-lib-normals" name="normal">
+ <float_array id="shape0-lib-normals-array" count="270">-0.956373 -0.21408 0.198796 -0.924961 -0.261316 0.275972 -0.137458 -0.038834 0.989746 -0.168633 -0.951941 0.255679 -0.252484 -0.956972 0.143027 -0.0355387 -0.1347 0.990249 -0.688801 -0.685516 0.235839 -0.690164 -0.684851 0.233778 0.0175231 -0.970177 0.241763 0 -0.967525 0.252774 0.0912315 -0.103838 0.990401 0.168633 -0.951941 0.255679 0.252484 -0.956972 0.143027 0.0355387 -0.1347 0.990249 0.661573 -0.711135 0.237925 0.688489 -0.648067 0.325565 -0.0874386 -0.278441 0.956465 -0.212253 -0.326909 0.920912 -0.956373 0.21408 0.198796 -0.924961 0.261316 0.275971 -0.0998015 0.0990332 0.990067 -0.168633 0.951941 0.255679 -0.252484 0.956972 0.143027 -0.0355387 0.1347 0.990249 -0.688801 0.685516 0.235839 -0.690164 0.684851 0.233778 0.0175231 0.970177 0.241763 0 0.967525 0.252774 -0.0245566 0.295514 0.955023 0.168633 0.951941 0.255679 0.252484 0.956972 0.143027 0.0355387 0.1347 0.990249 0.688489 0.648067 0.325565 0.661573 0.711136 0.237925 -0.212253 0.326909 0.920912 -0.0874386 0.278441 0.956465 -0.956373 -0.21408 -0.198796 -0.924961 -0.261316 -0.275971 -0.0998015 -0.0990332 -0.990067 -0.168633 -0.951941 -0.255679 -0.252484 -0.956972 -0.143027 -0.0355387 -0.1347 -0.990249 -0.688801 -0.685516 -0.235839 -0.690164 -0.684851 -0.233778 0.0175231 -0.970177 -0.241763 0 -0.967525 -0.252774 0.00923595 -0.240937 -0.970497 0.168633 -0.951941 -0.255679 0.252484 -0.956972 -0.143027 0.0355387 -0.1347 -0.990249 0.688489 -0.648067 -0.325565 0.661573 -0.711135 -0.237925 -0.345074 -0.313983 -0.884499 -0.0919974 -0.213247 -0.972657 -0.956373 0.21408 -0.198796 -0.924961 0.261316 -0.275972 -0.137458 0.0388339 -0.989746 -0.168633 0.951941 -0.255679 -0.252484 0.956972 -0.143027 -0.0355387 0.1347 -0.990249 -0.688801 0.685516 -0.235839 -0.690164 0.684851 -0.233778 0.0175231 0.970177 -0.241763 0 0.967525 -0.252774 0.0912314 0.103838 -0.990401 0.168633 0.951941 -0.255679 0.252484 0.956972 -0.143027 0.0355387 0.1347 -0.990249 0.661573 0.711136 -0.237925 0.688489 0.648067 -0.325565 1 0 0 -0.0919974 0.213247 -0.972657 -0.345074 0.313983 -0.884499 1 0 0 1 0 0 0.942197 0.234627 -0.239195 -4.37114e-08 1 -9.05704e-16 1 0 0 1 0 0 0.959865 0.239026 -0.146717 0.268302 0.674582 0.687716 1 0 0 -0.0245566 -0.295514 0.955023 -0.137458 0.038834 0.989746 -0.0998015 -0.0990332 0.990067 0.0912314 0.103838 0.990401 0.0912315 -0.103838 -0.990401 0.00923593 0.240937 -0.970497 -0.0998015 0.0990332 -0.990067 -0.137458 -0.038834 -0.989746</float_array>
+ <technique_common>
+ <accessor count="90" source="#shape0-lib-normals-array" stride="3">
+ <param name="X" type="float"/>
+ <param name="Y" type="float"/>
+ <param name="Z" type="float"/>
+ </accessor>
+ </technique_common>
+ </source>
+ <vertices id="shape0-lib-vertices">
+ <input semantic="POSITION" source="#shape0-lib-positions"/>
+ </vertices>
+ <triangles count="90">
+ <input offset="0" semantic="VERTEX" source="#shape0-lib-vertices"/>
+ <input offset="1" semantic="NORMAL" source="#shape0-lib-normals"/>
+ <p>1 0 22 0 0 0 0 1 5 1 1 1 1 2 5 2 43 2 2 3 4 3 28 3 28 4 3 4 2 4 43 5 2 5 3 5 5 6 0 6 4 6 4 7 2 7 5 7 38 8 6 8 27 8 27 9 41 9 38 9 43 10 7 10 6 10 28 11 8 11 7 11 7 12 3 12 28 12 43 13 3 13 7 13 7 14 8 14 27 14 27 15 6 15 7 15 20 16 6 16 38 16 38 17 40 17 20 17 10 18 22 18 1 18 1 19 9 19 10 19 43 20 14 20 9 20 11 21 12 21 14 21 14 22 13 22 11 22 43 23 13 23 14 23 12 24 10 24 9 24 9 25 14 25 12 25 18 26 21 26 15 26 15 27 16 27 18 27 43 28 20 28 21 28 17 29 19 29 11 29 11 30 13 30 17 30 43 31 17 31 13 31 17 32 21 32 18 32 18 33 19 33 17 33 20 34 40 34 15 34 15 35 21 35 20 35 0 36 22 36 23 36 23 37 26 37 0 37 45 38 24 38 26 38 28 39 4 39 24 39 24 40 25 40 28 40 45 41 25 41 24 41 4 42 0 42 26 42 26 43 24 43 4 43 27 44 31 44 30 44 30 45 41 45 27 45 45 46 42 46 31 46 44 47 8 47 28 47 28 48 25 48 44 48 45 49 44 49 25 49 44 50 31 50 27 50 27 51 8 51 44 51 42 52 29 52 30 52 30 53 31 53 42 53 23 54 22 54 10 54 10 55 32 55 23 55 45 56 23 56 32 56 34 57 12 57 11 57 11 58 33 58 34 58 45 59 34 59 33 59 32 60 10 60 12 60 12 61 34 61 32 61 35 62 36 62 18 62 18 63 16 63 35 63 45 64 37 64 36 64 11 65 19 65 37 65 37 66 33 66 11 66 45 67 33 67 37 67 37 68 19 68 18 68 18 69 36 69 37 69 16 70 15 70 41 70 42 71 36 71 35 71 35 72 29 72 42 72 29 73 35 73 41 73 38 74 41 74 40 74 40 75 41 75 39 75 39 76 20 76 40 76 15 77 40 77 41 77 30 78 29 78 41 78 42 79 39 79 41 79 41 80 29 80 42 80 35 81 16 81 41 81 20 82 43 82 6 82 1 83 43 83 9 83 43 84 5 84 2 84 17 85 43 85 21 85 44 86 45 86 31 86 42 87 45 87 36 87 34 88 45 88 32 88 23 89 45 89 26 89</p>
+ </triangles>
+ </mesh>
+ </geometry>
+ </library_geometries>
+ <library_visual_scenes>
+ <visual_scene id="VisualSceneNode" name="VisualScene">
+ <node id="node" name="node">
+ <instance_geometry url="#shape0-lib">
+ <bind_material>
+ <technique_common/>
+ </bind_material>
+ </instance_geometry>
+ </node>
+ </visual_scene>
+ </library_visual_scenes>
+ <scene>
+ <instance_visual_scene url="#VisualSceneNode"/>
+ </scene>
+</COLLADA>
diff --git a/robots/tiago_description/robots/tiago_dual.urdf b/robots/tiago_description/robots/tiago_dual.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..8f3d204d072d31c4d922b38913ffa85f03ee88d6
--- /dev/null
+++ b/robots/tiago_description/robots/tiago_dual.urdf
@@ -0,0 +1,5479 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- | This document was autogenerated by xacro from tiago_dual.urdf.xacro | -->
+<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
+<!-- =================================================================================== -->
+<!--
+ Copyright (c) 2019, PAL Robotics, S.L.
+ All rights reserved.
+
+ This work is licensed under the Creative Commons Attribution-NonCommercial-NoDerivs 3.0 Unported License.
+ To view a copy of this license, visit http://creativecommons.org/licenses/by-nc-nd/3.0/ or send a letter to
+ Creative Commons, 444 Castro Street, Suite 900, Mountain View, California, 94041, USA.
+-->
+<robot name="tiago_dual">
+ <material name="Hey5DarkGrey">
+ <color rgba="0.1 0.1 0.1 1.0"/>
+ </material>
+ <!--TODO: Check-->
+ <gazebo>
+ <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
+ <ns/>
+ <robotSimType>pal_hardware_gazebo/PalHardwareGazebo</robotSimType>
+ <robotNamespace/>
+ <controlPeriod>0.001</controlPeriod>
+ </plugin>
+ <plugin filename="libgazebo_ros_p3d.so" name="gazebo_ros_p3d">
+ <bodyName>base_footprint</bodyName>
+ <topicName>ground_truth_odom</topicName>
+ <alwaysOn>true</alwaysOn>
+ <updateRate>100.0</updateRate>
+ </plugin>
+ </gazebo>
+ <gazebo>
+ <plugin filename="libgazebo_world_odometry.so" name="gazebo_ros_odometry">
+ <frameName>base_footprint</frameName>
+ <topicName>floating_base_pose_simulated</topicName>
+ </plugin>
+ </gazebo>
+ <gazebo reference="base_imu_link">
+ <!-- this is expected to be reparented to pelvis with appropriate offset
+ when imu_link is reduced by fixed joint reduction -->
+ <!-- todo: this is working with gazebo 1.4, need to write a unit test -->
+ <sensor name="imu_sensor" type="imu">
+ <always_on>1</always_on>
+ <update_rate>1000.0</update_rate>
+ <imu>
+ <noise>
+ <type>gaussian</type>
+ <!-- Noise parameters from Boston Dynamics
+ (http://gazebosim.org/wiki/Sensor_noise):
+ rates (rad/s): mean=0, stddev=2e-4
+ accels (m/s/s): mean=0, stddev=1.7e-2
+ rate bias (rad/s): 5e-6 - 1e-5
+ accel bias (m/s/s): 1e-1
+ Experimentally, simulation provide rates with noise of
+ about 1e-3 rad/s and accels with noise of about 1e-1 m/s/s.
+ So we don't expect to see the noise unless number of inner iterations
+ are increased.
+
+ We will add bias. In this model, bias is sampled once for rates
+ and once for accels at startup; the sign (negative or positive)
+ of each bias is then switched with equal probability. Thereafter,
+ the biases are fixed additive offsets. We choose
+ bias means and stddevs to produce biases close to the provided
+ data. -->
+ <rate>
+ <mean>0.0</mean>
+ <stddev>2e-4</stddev>
+ <bias_mean>0.0000075</bias_mean>
+ <bias_stddev>0.0000008</bias_stddev>
+ </rate>
+ <accel>
+ <mean>0.0</mean>
+ <stddev>1.7e-2</stddev>
+ <bias_mean>0.1</bias_mean>
+ <bias_stddev>0.001</bias_stddev>
+ </accel>
+ </noise>
+ </imu>
+ </sensor>
+ </gazebo>
+ <material name="Blue">
+ <color rgba="0.0 0.0 0.8 1.0"/>
+ </material>
+ <material name="Green">
+ <color rgba="0.0 0.8 0.0 1.0"/>
+ </material>
+ <material name="Grey">
+ <color rgba="0.7 0.7 0.7 1.0"/>
+ </material>
+ <material name="LightGrey">
+ <color rgba="0.9 0.9 0.9 1.0"/>
+ </material>
+ <material name="DarkGrey">
+ <color rgba="0.1 0.1 0.1 1.0"/>
+ </material>
+ <material name="Red">
+ <color rgba="0.8 0.0 0.0 1.0"/>
+ </material>
+ <material name="White">
+ <color rgba="1.0 1.0 1.0 1.0"/>
+ </material>
+ <material name="Yellow">
+ <color rgba="0.96 0.88 0.14 1.0"/>
+ </material>
+ <!-- Base -->
+ <link name="base_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="-0.00000106 0.00000109 0.0000387"/>
+ <mass value="28.26649"/>
+ <inertia ixx="0.465408937" ixy="0.002160024" ixz="-0.001760255" iyy="0.483193291" iyz="-0.000655952" izz="0.550939703"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/pmb2/base/base.stl"/>
+ </geometry>
+ <material name="White"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/pmb2/base/base_collision.stl"/>
+ </geometry>
+ <material name="White"/>
+ </collision>
+ </link>
+ <gazebo reference="base_link">
+ <material>Gazebo/White</material>
+ </gazebo>
+ <!-- Base footprint -->
+ <link name="base_footprint">
+ <inertial>
+ <mass value="0.0"/>
+ <origin xyz="0.0 0.0 0.0"/>
+ <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
+ </inertial>
+ </link>
+ <joint name="base_footprint_joint" type="fixed">
+ <origin rpy="0 0 0" xyz="0 0 0.0985"/>
+ <child link="base_link"/>
+ <parent link="base_footprint"/>
+ </joint>
+ <link name="base_cover_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.22414000000 0.00002000000 0.04681000000"/>
+ <mass value="0.25821000000"/>
+ <inertia ixx="0.00023719188" ixy="0.00000006119" ixz="-0.00000011799" iyy="0.00028335052" iyz="0.00000000774" izz="0.00038497063"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/pmb2/base/base_ring.stl"/>
+ </geometry>
+ <material name="Orange"/>
+ </visual>
+ </link>
+ <joint name="cover_joint" type="fixed">
+ <parent link="base_footprint"/>
+ <child link="base_cover_link"/>
+ <origin rpy="0 0 0" xyz="0 0 0.13"/>
+ </joint>
+ <gazebo reference="base_cover_link">
+ <material>Gazebo/Orange</material>
+ </gazebo>
+ <link name="base_antenna_left_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.000001"/>
+ <inertia ixx="0.00000002371" ixy="0.00000006119" ixz="0.00000001179" iyy="0.00000002833" iyz="0.00000000774" izz="0.00000003849"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/pmb2/objects/antenna.stl"/>
+ </geometry>
+ <material name="Black"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0.085"/>
+ <geometry>
+ <cylinder length="0.17" radius="0.005"/>
+ </geometry>
+ <material name="Black"/>
+ </collision>
+ </link>
+ <gazebo reference="base_antenna_left_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="base_antenna_left_joint" type="fixed">
+ <parent link="base_link"/>
+ <child link="base_antenna_left_link"/>
+ <origin rpy="0 0 0" xyz="-0.201 0.1062 0.195"/>
+ </joint>
+ <link name="base_antenna_right_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.000001"/>
+ <inertia ixx="0.00000002371" ixy="0.00000006119" ixz="0.00000001179" iyy="0.00000002833" iyz="0.00000000774" izz="0.00000003849"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/pmb2/objects/antenna.stl"/>
+ </geometry>
+ <material name="Black"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0.085"/>
+ <geometry>
+ <cylinder length="0.17" radius="0.005"/>
+ </geometry>
+ <material name="Black"/>
+ </collision>
+ </link>
+ <gazebo reference="base_antenna_right_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="base_antenna_right_joint" type="fixed">
+ <parent link="base_link"/>
+ <child link="base_antenna_right_link"/>
+ <origin rpy="0 0 0" xyz="-0.201 -0.1062 0.195"/>
+ </joint>
+ <link name="base_imu_link">
+ <inertial>
+ <mass value="0.01"/>
+ <origin xyz="0 0 0"/>
+ <inertia ixx="0.000001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.000001"/>
+ </inertial>
+ <!--visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.01 0.01 0.01"/>
+ </geometry>
+ </visual-->
+ </link>
+ <joint name="base_imu_joint" type="fixed">
+ <parent link="base_link"/>
+ <child link="base_imu_link"/>
+ <origin rpy="0 0 -1.5707963267948966" xyz="0.10171 0 0.11282"/>
+ </joint>
+ <gazebo reference="base_imu_link">
+ <sensor name="base_imu_sensor" type="imu">
+ <always_on>1</always_on>
+ <update_rate>100.0</update_rate>
+ <imu>
+ <noise>
+ <type>gaussian</type>
+ <rate>
+ <mean>0.0</mean>
+ <stddev>2e-4</stddev>
+ <bias_mean>0.0000075</bias_mean>
+ <bias_stddev>0.0000008</bias_stddev>
+ </rate>
+ <accel>
+ <mean>0.0</mean>
+ <stddev>1.7e-2</stddev>
+ <bias_mean>0.1</bias_mean>
+ <bias_stddev>0.001</bias_stddev>
+ </accel>
+ </noise>
+ </imu>
+ </sensor>
+ </gazebo>
+ <link name="wheel_right_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 -0.00207"/>
+ <mass value="1.82362"/>
+ <inertia ixx="0.00499743171" ixy="4.464e-08" ixz="-0.00000002245" iyy="0.00499741733" iyz="-1.64e-09" izz="0.00839239692"/>
+ </inertial>
+ <visual>
+ <origin rpy="-0.0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/pmb2/wheels/wheel.stl"/>
+ </geometry>
+ <material name="DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.0985"/>
+ <!-- length="${width}"/> -->
+ </geometry>
+ </collision>
+ </link>
+ <link name="suspension_right_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 -0.02"/>
+ <mass value="10"/>
+ <inertia ixx="1.0" ixy="0" ixz="0" iyy="1.0" iyz="0" izz="1.0"/>
+ </inertial>
+ <kinematic>0</kinematic>
+ <gravity>1</gravity>
+ </link>
+ <joint name="suspension_right_joint" type="prismatic">
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="100.0" lower="-0.005" upper="0.005" velocity="1.0"/>
+ <child link="suspension_right_link"/>
+ <parent link="base_link"/>
+ <calibration rising="0.0"/>
+ <dynamics damping="100"/>
+ <safety_controller k_position="0" k_velocity="10" soft_lower_limit="-0.005" soft_upper_limit="0.005"/>
+ </joint>
+ <joint name="wheel_right_joint" type="continuous">
+ <parent link="suspension_right_link"/>
+ <child link="wheel_right_link"/>
+ <origin rpy="-1.5707963267948966 0 0" xyz="0 -0.2022 0.0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="6.0" velocity="10.152284263959391"/>
+ </joint>
+ <transmission name="wheel_right_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <actuator name="wheel_right_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ <joint name="wheel_right_joint">
+ <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ </transmission>
+ <!-- Contact model for the wheel surface -->
+ <gazebo reference="wheel_right_link">
+ <kp>1000000.0</kp>
+ <kd>100.0</kd>
+ <!--
+ <mu1>10.0</mu1>
+ <mu2>10.0</mu2>
+ -->
+ <mu1>100000000000000.0</mu1>
+ <mu2>100000000000000.0</mu2>
+ <!--<fdir1>1 0 0</fdir1>-->
+ <maxVel>1.0</maxVel>
+ <minDepth>0.002</minDepth>
+ <material>Gazebo/DarkGrey</material>
+ </gazebo>
+ <gazebo reference="wheel_{side}_joint">
+ <implicitSpringDamper>1</implicitSpringDamper>
+ </gazebo>
+ <link name="wheel_left_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0.00207"/>
+ <mass value="1.82362"/>
+ <inertia ixx="0.00499743171" ixy="-4.464e-08" ixz="-0.00000002245" iyy="0.00499741733" iyz="1.64e-09" izz="0.00839239692"/>
+ </inertial>
+ <visual>
+ <origin rpy="-3.141592653589793 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/pmb2/wheels/wheel.stl"/>
+ </geometry>
+ <material name="DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.0985"/>
+ <!-- length="${width}"/> -->
+ </geometry>
+ </collision>
+ </link>
+ <link name="suspension_left_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 -0.02"/>
+ <mass value="10"/>
+ <inertia ixx="1.0" ixy="0" ixz="0" iyy="1.0" iyz="0" izz="1.0"/>
+ </inertial>
+ <kinematic>0</kinematic>
+ <gravity>1</gravity>
+ </link>
+ <joint name="suspension_left_joint" type="prismatic">
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="100.0" lower="-0.005" upper="0.005" velocity="1.0"/>
+ <child link="suspension_left_link"/>
+ <parent link="base_link"/>
+ <calibration rising="0.0"/>
+ <dynamics damping="100"/>
+ <safety_controller k_position="0" k_velocity="10" soft_lower_limit="-0.005" soft_upper_limit="0.005"/>
+ </joint>
+ <joint name="wheel_left_joint" type="continuous">
+ <parent link="suspension_left_link"/>
+ <child link="wheel_left_link"/>
+ <origin rpy="-1.5707963267948966 0 0" xyz="0 0.2022 0.0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="6.0" velocity="10.152284263959391"/>
+ </joint>
+ <transmission name="wheel_left_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <actuator name="wheel_left_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ <joint name="wheel_left_joint">
+ <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ </transmission>
+ <!-- Contact model for the wheel surface -->
+ <gazebo reference="wheel_left_link">
+ <kp>1000000.0</kp>
+ <kd>100.0</kd>
+ <!--
+ <mu1>10.0</mu1>
+ <mu2>10.0</mu2>
+ -->
+ <mu1>100000000000000.0</mu1>
+ <mu2>100000000000000.0</mu2>
+ <!--<fdir1>1 0 0</fdir1>-->
+ <maxVel>1.0</maxVel>
+ <minDepth>0.002</minDepth>
+ <material>Gazebo/DarkGrey</material>
+ </gazebo>
+ <gazebo reference="wheel_{side}_joint">
+ <implicitSpringDamper>1</implicitSpringDamper>
+ </gazebo>
+ <link name="caster_front_right_1_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="-0.00634599 0.00005347 -0.02037519"/>
+ <mass value="0.05144761"/>
+ <inertia ixx="0.00001513" ixy="-0.00000003" ixz="0.00000267" iyy="0.00001309" iyz="-0.00000005" izz="0.00001198"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/pmb2/wheels/caster_1.stl"/>
+ </geometry>
+ <material name="Grey"/>
+ </visual>
+ </link>
+ <link name="caster_front_right_2_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.08855784"/>
+ <inertia ixx="0.00001075" ixy="0" ixz="0" iyy="0.00001541" iyz="0" izz="0.00001075"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/pmb2/wheels/caster_2.stl"/>
+ </geometry>
+ <material name="DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.025"/>
+ </geometry>
+ </collision>
+ </link>
+ <joint name="caster_front_right_1_joint" type="continuous">
+ <parent link="base_link"/>
+ <child link="caster_front_right_1_link"/>
+ <origin rpy="0 0 0" xyz="0.1695 -0.102 -0.0335"/>
+ <axis xyz="0 0 1"/>
+ <dynamics damping="0.005" friction="0.0"/>
+ </joint>
+ <joint name="caster_front_right_2_joint" type="continuous">
+ <parent link="caster_front_right_1_link"/>
+ <child link="caster_front_right_2_link"/>
+ <origin rpy="-1.5707963267948966 0 0" xyz="-0.016 0.0000 -0.040"/>
+ <axis xyz="0 0 1"/>
+ </joint>
+ <!-- Contact model for the caster wheel surface -->
+ <gazebo reference="caster_front_right_1_link">
+ <kp>100000000.0</kp>
+ <kd>10.0</kd>
+ <mu1>0.1</mu1>
+ <mu2>0.1</mu2>
+ <!--<fdir1>1 0 0</fdir1>-->
+ <maxVel>10.0</maxVel>
+ <minDepth>0.0005</minDepth>
+ <material>Gazebo/DarkGrey</material>
+ </gazebo>
+ <gazebo reference="caster_front_right_2_link">
+ <kp>10000000.0</kp>
+ <kd>10.0</kd>
+ <mu1>0.1</mu1>
+ <mu2>0.1</mu2>
+ <!--<fdir1>1 0 0</fdir1>-->
+ <maxVel>10.0</maxVel>
+ <minDepth>0.0005</minDepth>
+ <material>Gazebo/DarkGrey</material>
+ </gazebo>
+ <gazebo reference="caster_{side}_1_joint">
+ <implicitSpringDamper>0</implicitSpringDamper>
+ </gazebo>
+ <gazebo reference="caster_{side}_2_joint">
+ <implicitSpringDamper>0</implicitSpringDamper>
+ </gazebo>
+ <link name="caster_front_left_1_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="-0.00634599 0.00005347 -0.02037519"/>
+ <mass value="0.05144761"/>
+ <inertia ixx="0.00001513" ixy="-0.00000003" ixz="0.00000267" iyy="0.00001309" iyz="-0.00000005" izz="0.00001198"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/pmb2/wheels/caster_1.stl"/>
+ </geometry>
+ <material name="Grey"/>
+ </visual>
+ <!--collision>
+ <origin xyz="0 0 0" rpy="0 0 0"/>
+ <geometry>
+ <cylinder radius="${radius}" length="${width}"/>
+ </geometry>
+ </collision-->
+ </link>
+ <link name="caster_front_left_2_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.08855784"/>
+ <inertia ixx="0.00001075" ixy="0" ixz="0" iyy="0.00001541" iyz="0" izz="0.00001075"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/pmb2/wheels/caster_2.stl"/>
+ </geometry>
+ <material name="DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.025"/>
+ </geometry>
+ </collision>
+ </link>
+ <joint name="caster_front_left_1_joint" type="continuous">
+ <parent link="base_link"/>
+ <child link="caster_front_left_1_link"/>
+ <origin rpy="0 0 0" xyz="0.1695 0.102 -0.0335"/>
+ <axis xyz="0 0 1"/>
+ <dynamics damping="0.005" friction="0.0"/>
+ </joint>
+ <joint name="caster_front_left_2_joint" type="continuous">
+ <parent link="caster_front_left_1_link"/>
+ <child link="caster_front_left_2_link"/>
+ <origin rpy="-1.5707963267948966 0 0" xyz="-0.016 0.0000 -0.040"/>
+ <axis xyz="0 0 1"/>
+ </joint>
+ <!-- Contact model for the caster wheel surface -->
+ <gazebo reference="caster_front_left_1_link">
+ <kp>100000000.0</kp>
+ <kd>10.0</kd>
+ <mu1>0.1</mu1>
+ <mu2>0.1</mu2>
+ <!--<fdir1>1 0 0</fdir1>-->
+ <maxVel>10.0</maxVel>
+ <minDepth>0.0005</minDepth>
+ <material>Gazebo/DarkGrey</material>
+ </gazebo>
+ <gazebo reference="caster_front_left_2_link">
+ <kp>10000000.0</kp>
+ <kd>10.0</kd>
+ <mu1>0.1</mu1>
+ <mu2>0.1</mu2>
+ <!--<fdir1>1 0 0</fdir1>-->
+ <maxVel>10.0</maxVel>
+ <minDepth>0.0005</minDepth>
+ <material>Gazebo/DarkGrey</material>
+ </gazebo>
+ <gazebo reference="caster_{side}_1_joint">
+ <implicitSpringDamper>0</implicitSpringDamper>
+ </gazebo>
+ <gazebo reference="caster_{side}_2_joint">
+ <implicitSpringDamper>0</implicitSpringDamper>
+ </gazebo>
+ <link name="caster_back_right_1_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="-0.00634599 0.00005347 -0.02037519"/>
+ <mass value="0.05144761"/>
+ <inertia ixx="0.00001513" ixy="-0.00000003" ixz="0.00000267" iyy="0.00001309" iyz="-0.00000005" izz="0.00001198"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/pmb2/wheels/caster_1.stl"/>
+ </geometry>
+ <material name="Grey"/>
+ </visual>
+ </link>
+ <link name="caster_back_right_2_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.08855784"/>
+ <inertia ixx="0.00001075" ixy="0" ixz="0" iyy="0.00001541" iyz="0" izz="0.00001075"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/pmb2/wheels/caster_2.stl"/>
+ </geometry>
+ <material name="DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.025"/>
+ </geometry>
+ </collision>
+ </link>
+ <joint name="caster_back_right_1_joint" type="continuous">
+ <parent link="base_link"/>
+ <child link="caster_back_right_1_link"/>
+ <origin rpy="0 0 0" xyz="-0.17350000000000002 -0.102 -0.0335"/>
+ <axis xyz="0 0 1"/>
+ <dynamics damping="0.005" friction="0.0"/>
+ </joint>
+ <joint name="caster_back_right_2_joint" type="continuous">
+ <parent link="caster_back_right_1_link"/>
+ <child link="caster_back_right_2_link"/>
+ <origin rpy="-1.5707963267948966 0 0" xyz="-0.016 0.0000 -0.040"/>
+ <axis xyz="0 0 1"/>
+ </joint>
+ <!-- Contact model for the caster wheel surface -->
+ <gazebo reference="caster_back_right_1_link">
+ <kp>100000000.0</kp>
+ <kd>10.0</kd>
+ <mu1>0.1</mu1>
+ <mu2>0.1</mu2>
+ <!--<fdir1>1 0 0</fdir1>-->
+ <maxVel>10.0</maxVel>
+ <minDepth>0.0005</minDepth>
+ <material>Gazebo/DarkGrey</material>
+ </gazebo>
+ <gazebo reference="caster_back_right_2_link">
+ <kp>10000000.0</kp>
+ <kd>10.0</kd>
+ <mu1>0.1</mu1>
+ <mu2>0.1</mu2>
+ <!--<fdir1>1 0 0</fdir1>-->
+ <maxVel>10.0</maxVel>
+ <minDepth>0.0005</minDepth>
+ <material>Gazebo/DarkGrey</material>
+ </gazebo>
+ <gazebo reference="caster_{side}_1_joint">
+ <implicitSpringDamper>0</implicitSpringDamper>
+ </gazebo>
+ <gazebo reference="caster_{side}_2_joint">
+ <implicitSpringDamper>0</implicitSpringDamper>
+ </gazebo>
+ <link name="caster_back_left_1_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="-0.00634599 0.00005347 -0.02037519"/>
+ <mass value="0.05144761"/>
+ <inertia ixx="0.00001513" ixy="-0.00000003" ixz="0.00000267" iyy="0.00001309" iyz="-0.00000005" izz="0.00001198"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/pmb2/wheels/caster_1.stl"/>
+ </geometry>
+ <material name="Grey"/>
+ </visual>
+ </link>
+ <link name="caster_back_left_2_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.08855784"/>
+ <inertia ixx="0.00001075" ixy="0" ixz="0" iyy="0.00001541" iyz="0" izz="0.00001075"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/pmb2/wheels/caster_2.stl"/>
+ </geometry>
+ <material name="DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.025"/>
+ </geometry>
+ </collision>
+ </link>
+ <joint name="caster_back_left_1_joint" type="continuous">
+ <parent link="base_link"/>
+ <child link="caster_back_left_1_link"/>
+ <origin rpy="0 0 0" xyz="-0.17350000000000002 0.102 -0.0335"/>
+ <axis xyz="0 0 1"/>
+ <dynamics damping="0.005" friction="0.0"/>
+ </joint>
+ <joint name="caster_back_left_2_joint" type="continuous">
+ <parent link="caster_back_left_1_link"/>
+ <child link="caster_back_left_2_link"/>
+ <origin rpy="-1.5707963267948966 0 0" xyz="-0.016 0.0000 -0.040"/>
+ <axis xyz="0 0 1"/>
+ </joint>
+ <!-- Contact model for the caster wheel surface -->
+ <gazebo reference="caster_back_left_1_link">
+ <kp>100000000.0</kp>
+ <kd>10.0</kd>
+ <mu1>0.1</mu1>
+ <mu2>0.1</mu2>
+ <!--<fdir1>1 0 0</fdir1>-->
+ <maxVel>10.0</maxVel>
+ <minDepth>0.0005</minDepth>
+ <material>Gazebo/DarkGrey</material>
+ </gazebo>
+ <gazebo reference="caster_back_left_2_link">
+ <kp>10000000.0</kp>
+ <kd>10.0</kd>
+ <mu1>0.1</mu1>
+ <mu2>0.1</mu2>
+ <!--<fdir1>1 0 0</fdir1>-->
+ <maxVel>10.0</maxVel>
+ <minDepth>0.0005</minDepth>
+ <material>Gazebo/DarkGrey</material>
+ </gazebo>
+ <gazebo reference="caster_{side}_1_joint">
+ <implicitSpringDamper>0</implicitSpringDamper>
+ </gazebo>
+ <gazebo reference="caster_{side}_2_joint">
+ <implicitSpringDamper>0</implicitSpringDamper>
+ </gazebo>
+ <!-- Bottom fixed part of column -->
+ <link name="torso_fixed_link">
+ <inertial>
+ <origin rpy="0.00000 0.00000 0.00000" xyz="0.02435 -0.00464 0.15900"/>
+ <mass value="14.01360"/>
+ <inertia ixx="0.38341100000" ixy="0.00036400000" ixz="-0.02438900000" iyy="0.40577300000" iyz="-0.00078200000" izz="0.07699800000"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/torso/torso_fix.stl" scale="1 1 1"/>
+ </geometry>
+ <material name="LightGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0.085 0.0 0.02"/>
+ <geometry>
+ <box size="0.4 0.25 0.04"/>
+ </geometry>
+ </collision>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0.05"/>
+ <geometry>
+ <box size="0.25 0.25 0.1"/>
+ </geometry>
+ </collision>
+ </link>
+ <joint name="torso_fixed_joint" type="fixed">
+ <origin rpy="0 0 0" xyz="-0.062 0.0 0.193"/>
+ <parent link="base_link"/>
+ <child link="torso_fixed_link"/>
+ <axis xyz="0 0 0"/>
+ </joint>
+ <!-- Middle extending -->
+ <link name="torso_fixed_column_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.01"/>
+ <inertia ixx="0.000001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.000001"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0.275"/>
+ <geometry>
+ <box size="0.18 0.2 0.55"/>
+ </geometry>
+ <material name="DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0.275"/>
+ <geometry>
+ <box size="0.23 0.23 0.55"/>
+ </geometry>
+ <material name="DarkGrey"/>
+ </collision>
+ </link>
+ <gazebo reference="torso_fixed_column_link">
+ <material>Gazebo/DarkGrey</material>
+ </gazebo>
+ <joint name="torso_fixed_column_joint" type="fixed">
+ <origin rpy="0 0 0" xyz="-0.062 0 0.195"/>
+ <parent link="base_link"/>
+ <child link="torso_fixed_column_link"/>
+ <axis xyz="0 0 0"/>
+ </joint>
+ <link name="torso_lift_link">
+ <inertial>
+ <origin rpy="0.00000 0.00000 0.00000" xyz="0.04361 -0.00116 -0.17334"/>
+ <mass value="9.594213"/>
+ <inertia ixx="0.326395" ixy="-0.000844" ixz="-0.018905" iyy="0.235332" iyz="0.009042" izz="0.176168"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/torso/torso_lift_with_arm.stl" scale="1 1 1"/>
+ </geometry>
+ <material name="White"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/torso/torso_lift_collision_core.dae" scale="1 1 1"/>
+ </geometry>
+ <material name="LightGrey"/>
+ </collision>
+ <!-- left shoulder collision-->
+ <collision>
+ <origin rpy="0 0 0" xyz="-0.131 0.17 -0.02556"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/torso/torso_lift_collision_shoulder.dae" scale="1 1 1"/>
+ </geometry>
+ <material name="LightGrey"/>
+ </collision>
+ <!-- right shoulder collision-->
+ <collision>
+ <origin rpy="0 0 0" xyz="-0.131 -0.19 -0.02556"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/torso/torso_lift_collision_shoulder.dae" scale="1 1 1"/>
+ </geometry>
+ <material name="LightGrey"/>
+ </collision>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/torso/torso_lift_collision_neck.dae" scale="1 1 1"/>
+ </geometry>
+ <material name="LightGrey"/>
+ </collision>
+ </link>
+ <joint name="torso_lift_joint" type="prismatic">
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.597"/>
+ <parent link="torso_fixed_link"/>
+ <child link="torso_lift_link"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2000" lower="0" upper="0.35" velocity="0.07"/>
+ <calibration rising="0.0"/>
+ <dynamics damping="1000"/>
+ <safety_controller k_position="20" k_velocity="20" soft_lower_limit="0.0" soft_upper_limit="0.35"/>
+ </joint>
+ <transmission name="torso_lift_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <actuator name="torso_lift_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ <joint name="torso_lift_joint">
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ </transmission>
+ <!-- includes -->
+ <link name="head_1_link">
+ <inertial>
+ <origin rpy="0.00000 0.00000 0.00000" xyz="-0.00508 0.00237 0.07229"/>
+ <mass value="0.62220"/>
+ <inertia ixx="0.00119200000" ixy="-0.00000700000" ixz="0.00009600000" iyy="0.00140200000" iyz="0.00000400000" izz="0.00088900000"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/head/head_1.stl" scale="1 1 1"/>
+ </geometry>
+ <material name="White"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0.0" xyz="0.0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/head/head_1_collision.dae" scale="1 1 1"/>
+ </geometry>
+ <material name="White"/>
+ </collision>
+ </link>
+ <joint name="head_1_joint" type="revolute">
+ <parent link="torso_lift_link"/>
+ <child link="head_1_link"/>
+ <origin rpy="0 0 0" xyz="0.182 0.0 0.0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="5.197" lower="-1.3089969389957472" upper="1.3089969389957472" velocity="3.0"/>
+ <dynamics damping="0.5" friction="1.0"/>
+ <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-1.2389969389957471" soft_upper_limit="1.2389969389957471"/>
+ </joint>
+ <gazebo reference="head_1_link">
+ <mu1>0.9</mu1>
+ <mu2>0.9</mu2>
+ </gazebo>
+ <gazebo reference="head_1_joint">
+ <implicitSpringDamper>1</implicitSpringDamper>
+ </gazebo>
+ <transmission name="head_1_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <actuator name="head_1_motor">
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ <joint name="head_1_joint">
+ <offset>0.0</offset>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ </transmission>
+ <link name="head_2_link">
+ <inertial>
+ <origin rpy="0.00000 0.00000 0.00000" xyz="0.04700 0.05542 0.00022"/>
+ <mass value="0.66123"/>
+ <inertia ixx="0.00462000000" ixy="0.00068900000" ixz="0.00000600000" iyy="0.00486100000" iyz="0.00000600000" izz="0.00313200000"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/head/head_2.stl" scale="1 1 1"/>
+ </geometry>
+ <material name="White"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/head/head_2_collision.dae" scale="1 1 1"/>
+ </geometry>
+ </collision>
+ </link>
+ <joint name="head_2_joint" type="revolute">
+ <parent link="head_1_link"/>
+ <child link="head_2_link"/>
+ <origin rpy="1.5707963267948966 0 0" xyz="0.005 0.0 0.098"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.77" lower="-1.0471975511965976" upper="0.7853981633974483" velocity="3.0"/>
+ <dynamics damping="0.5" friction="1.0"/>
+ <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-0.9771975511965976" soft_upper_limit="0.7153981633974482"/>
+ </joint>
+ <transmission name="head_2_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <actuator name="head_2_motor">
+ <mechanicalReduction>1</mechanicalReduction>
+ </actuator>
+ <joint name="head_2_joint">
+ <offset>0.0</offset>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ </transmission>
+ <!-- frames in the center of the camera -->
+ <joint name="xtion_joint" type="fixed">
+ <origin rpy="-1.57079632679 0.0 0.0" xyz="0.0908 0.08 0.0"/>
+ <parent link="head_2_link"/>
+ <child link="xtion_link"/>
+ </joint>
+ <link name="xtion_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="-0.00936000000 -0.00003000000 -0.00273000000"/>
+ <mass value="0.21970000000"/>
+ <inertia ixx="0.00000429247" ixy="0.00000000000" ixz="0.00000002565" iyy="0.00000008027" iyz="0.00000000000" izz="0.00000427339"/>
+ </inertial>
+ <!--
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/sensors/xtion_pro_live/xtion_pro_live.dae" />
+ </geometry>
+ <material name="Grey">
+ <color rgba="0.5 0.5 0.5 1"/>
+ </material>
+ </visual>
+ <collision>
+ <origin xyz="-0.01 0.0025 0" rpy="0 0 0"/>
+ <geometry>
+ <box size="0.04 0.185 0.03"/>
+ </geometry>
+ </collision>
+ -->
+ </link>
+ <joint name="xtion_optical_joint" type="fixed">
+ <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
+ <parent link="xtion_link"/>
+ <child link="xtion_optical_frame"/>
+ </joint>
+ <link name="xtion_optical_frame">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.001"/>
+ <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
+ </inertial>
+ </link>
+ <!-- frames of the depth sensor -->
+ <joint name="xtion_depth_joint" type="fixed">
+ <origin rpy="0 0 0" xyz="0.0 0.047 0.0"/>
+ <parent link="xtion_link"/>
+ <child link="xtion_depth_frame"/>
+ </joint>
+ <link name="xtion_depth_frame">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.001"/>
+ <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
+ </inertial>
+ </link>
+ <joint name="xtion_depth_optical_joint" type="fixed">
+ <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
+ <parent link="xtion_depth_frame"/>
+ <child link="xtion_depth_optical_frame"/>
+ </joint>
+ <link name="xtion_depth_optical_frame">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.001"/>
+ <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
+ </inertial>
+ </link>
+ <!-- frames of the rgb sensor -->
+ <joint name="xtion_rgb_joint" type="fixed">
+ <origin rpy="0 0 0" xyz="0.0 0.022 0.0"/>
+ <parent link="xtion_link"/>
+ <child link="xtion_rgb_frame"/>
+ </joint>
+ <link name="xtion_rgb_frame">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.001"/>
+ <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
+ </inertial>
+ </link>
+ <joint name="xtion_rgb_optical_joint" type="fixed">
+ <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
+ <parent link="xtion_rgb_frame"/>
+ <child link="xtion_rgb_optical_frame"/>
+ </joint>
+ <link name="xtion_rgb_optical_frame">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.001"/>
+ <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
+ </inertial>
+ </link>
+ <gazebo reference="xtion_rgb_frame">
+ <sensor name="xtion_frame_sensor" type="depth">
+ <always_on>true</always_on>
+ <update_rate>30.0</update_rate>
+ <camera>
+ <horizontal_fov>1.09955742876</horizontal_fov>
+ <image>
+ <format>B8G8R8</format>
+ <width>640</width>
+ <height>480</height>
+ </image>
+ <clip>
+ <near>0.3</near>
+ <far>8.0</far>
+ </clip>
+ </camera>
+ <plugin filename="libgazebo_ros_openni_kinect.so" name="xtion_frame_controller">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>30.0</updateRate>
+ <cameraName>xtion</cameraName>
+ <imageTopicName>rgb/image_raw</imageTopicName>
+ <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
+ <depthImageTopicName>depth_registered/image_raw</depthImageTopicName>
+ <depthImageCameraInfoTopicName>depth_registered/camera_info</depthImageCameraInfoTopicName>
+ <pointCloudTopicName>depth_registered/points</pointCloudTopicName>
+ <frameName>xtion_rgb_optical_frame</frameName>
+ <pointCloudCutoff>0.5</pointCloudCutoff>
+ <pointCloudCutoffMax>8.0</pointCloudCutoffMax>
+ <distortionK1>0.00000001</distortionK1>
+ <distortionK2>0.00000001</distortionK2>
+ <distortionK3>0.00000001</distortionK3>
+ <distortionT1>0.00000001</distortionT1>
+ <distortionT2>0.00000001</distortionT2>
+ </plugin>
+ </sensor>
+ </gazebo>
+ <!--************************-->
+ <!-- SHOULDER -->
+ <!--************************-->
+ <link name="arm_left_1_link">
+ <inertial>
+ <!--<origin xyz="0.061191 ${reflect * -0.022397} -0.012835" rpy="0.00000 0.00000 0.00000"/>-->
+ <origin rpy="0.00000 0.00000 0.00000" xyz="0.061191 -0.022397 0.012835"/>
+ <mass value="1.563428"/>
+ <inertia ixx="0.002471" ixy="0.001809" ixz="-0.001202" iyy="0.006132" iyz="-0.000494" izz="0.006704"/>
+ </inertial>
+ <!-- M90 module -->
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/arm/arm_1.stl" scale="1 -1 -1"/>
+ </geometry>
+ <material name="White"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/arm/arm_1_collision.dae" scale="1 -1 -1"/>
+ </geometry>
+ </collision>
+ </link>
+ <joint name="arm_left_1_joint" type="revolute">
+ <parent link="torso_lift_link"/>
+ <child link="arm_left_1_link"/>
+ <origin rpy="3.141592653589793 0 1.5707963267948966" xyz="0.02556 0.19 -0.171"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="43.0" lower="-1.1780972450961724" upper="1.5707963267948966" velocity="1.95"/>
+ <dynamics damping="1.0" friction="1.0"/>
+ <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-1.1080972450961724" soft_upper_limit="1.5007963267948965"/>
+ </joint>
+ <link name="arm_left_2_link">
+ <inertial>
+ <origin rpy="0.00000 0.00000 0.00000" xyz="0.030432 0.000229 0.005942"/>
+ <mass value="1.800397"/>
+ <inertia ixx="0.001809" ixy="-1.5e-05" ixz="-0.000254" iyy="0.004333" iyz="-1.4e-05" izz="0.004341"/>
+ </inertial>
+ <visual>
+ <!-- <origin xyz="0 0 0" rpy="${180 * deg_to_rad} ${0 * deg_to_rad} ${-90 * deg_to_rad}" /> -->
+ <origin rpy="0.0 0.0 0.0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/arm/arm_2.stl" scale="1 1 -1"/>
+ </geometry>
+ <material name="DarkGrey"/>
+ </visual>
+ <collision>
+ <!-- <origin xyz="0 0 0" rpy="${180 * deg_to_rad} ${0 * deg_to_rad} ${-90 * deg_to_rad}" /> -->
+ <origin rpy="0.0 0.0 0.0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/arm/arm_2_collision.dae" scale="1 1 -1"/>
+ </geometry>
+ </collision>
+ </link>
+ <joint name="arm_left_2_joint" type="revolute">
+ <parent link="arm_left_1_link"/>
+ <child link="arm_left_2_link"/>
+ <origin rpy="1.5707963267948966 0.0 0.0" xyz="0.125 -0.0195 0.031"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="43.0" lower="-1.1780972450961724" upper="1.5707963267948966" velocity="1.95"/>
+ <dynamics damping="1.0" friction="1.0"/>
+ <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-1.1080972450961724" soft_upper_limit="1.5007963267948965"/>
+ </joint>
+ <link name="arm_left_3_link">
+ <inertial>
+ <origin rpy="0.00000 0.00000 0.00000" xyz="0.007418 -0.004361 0.134194"/>
+ <mass value="1.8"/>
+ <inertia ixx="0.019541" ixy="-0.000105" ixz="-0.001717" iyy="0.019936" iyz="-0.000852" izz="0.002864"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/arm/arm_3.stl" scale="1 1 -1"/>
+ </geometry>
+ <material name="DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/arm/arm_3_collision.dae" scale="1 1 -1"/>
+ </geometry>
+ </collision>
+ </link>
+ <joint name="arm_left_3_joint" type="revolute">
+ <parent link="arm_left_2_link"/>
+ <child link="arm_left_3_link"/>
+ <origin rpy="-1.5707963267948966 3.141592653589793 -1.5707963267948966" xyz="0.0895 0.0 -0.0015"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="26" lower="-0.7853981633974483" upper="3.9269908169872414" velocity="2.35"/>
+ <dynamics damping="1.0" friction="1.0"/>
+ <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-0.7153981633974482" soft_upper_limit="3.8569908169872416"/>
+ </joint>
+ <!--************************-->
+ <!-- ELBOW -->
+ <!--************************-->
+ <link name="arm_left_4_link">
+ <inertial>
+ <origin rpy="0.00000 0.00000 0.00000" xyz="0.095658 -0.014666 -0.018133"/>
+ <mass value="1.432697"/>
+ <inertia ixx="0.002040" ixy="-0.000725" ixz="0.00115" iyy="0.008424" iyz="-0.000204" izz="0.008686"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/arm/arm_4.stl" scale="-1 -1 -1"/>
+ </geometry>
+ <material name="DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/arm/arm_4_collision.dae" scale="-1 -1 -1"/>
+ </geometry>
+ </collision>
+ </link>
+ <joint name="arm_left_4_joint" type="revolute">
+ <parent link="arm_left_3_link"/>
+ <child link="arm_left_4_link"/>
+ <origin rpy="1.5707963267948966 -1.5707963267948966 0.0" xyz="-0.02 -0.027 0.222"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="26" lower="-0.39269908169872414" upper="2.356194490192345" velocity="2.35"/>
+ <dynamics damping="1.0" friction="1.0"/>
+ <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-0.32269908169872413" soft_upper_limit="2.286194490192345"/>
+ </joint>
+ <gazebo reference="arm_left_1_link">
+ <mu1>0.9</mu1>
+ <mu2>0.9</mu2>
+ <material>Gazebo/DarkGrey</material>
+ </gazebo>
+ <gazebo reference="arm_left_2_link">
+ <mu1>0.9</mu1>
+ <mu2>0.9</mu2>
+ <material>Gazebo/DarkGrey</material>
+ </gazebo>
+ <gazebo reference="arm_left_3_link">
+ <mu1>0.9</mu1>
+ <mu2>0.9</mu2>
+ <material>Gazebo/DarkGrey</material>
+ </gazebo>
+ <gazebo reference="arm_left_4_link">
+ <mu1>0.9</mu1>
+ <mu2>0.9</mu2>
+ <material>Gazebo/DarkGrey</material>
+ </gazebo>
+ <gazebo reference="arm_left_1_joint">
+ <implicitSpringDamper>1</implicitSpringDamper>
+ </gazebo>
+ <gazebo reference="arm_left_2_joint">
+ <implicitSpringDamper>1</implicitSpringDamper>
+ </gazebo>
+ <gazebo reference="arm_left_3_joint">
+ <implicitSpringDamper>1</implicitSpringDamper>
+ </gazebo>
+ <gazebo reference="arm_left_4_joint">
+ <implicitSpringDamper>1</implicitSpringDamper>
+ </gazebo>
+ <!--************************-->
+ <!-- WRIST -->
+ <!--************************-->
+ <link name="arm_left_5_link">
+ <inertial>
+ <origin rpy="0.00000 0.00000 0.00000" xyz="0.001078 -0.000151 0.077173"/>
+ <mass value="1.655819"/>
+ <inertia ixx="0.00613800000" ixy="-0.00000400000" ixz="0.00015600000" iyy="0.00564000000" iyz="0.00002200000" izz="0.00179700000"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/arm/arm_5-wrist-2010.stl" scale="-1 -1 -1"/>
+ </geometry>
+ <material name="LightGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/arm/arm_5_collision-wrist-2010.dae" scale="-1 -1 -1"/>
+ </geometry>
+ </collision>
+ </link>
+ <joint name="arm_left_5_joint" type="revolute">
+ <parent link="arm_left_4_link"/>
+ <child link="arm_left_5_link"/>
+ <origin rpy="1.5707963267948966 -1.5707963267948966 -1.5707963267948966" xyz="0.162 -0.02 -0.027"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="3" lower="-2.0943951023931953" upper="2.0943951023931953" velocity="1.95"/>
+ <dynamics damping="1.0" friction="1.0"/>
+ <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-2.0743951023931952" soft_upper_limit="2.0743951023931952"/>
+ </joint>
+ <link name="arm_left_6_link">
+ <inertial>
+ <origin rpy="0.00000 0.00000 0.00000" xyz="-0.000153 -0.003122 0.000183"/>
+ <mass value="0.370063"/>
+ <inertia ixx="0.00021500000" ixy="0.00000000000" ixz="0.00000000000" iyy="0.00017000000" iyz="0.00000200000" izz="0.00017400000"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/arm/arm_6-wrist-2010.stl" scale="-1 1 1"/>
+ </geometry>
+ <material name="LightGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/arm/arm_6_collision-wrist-2010.dae" scale="-1 1 1"/>
+ </geometry>
+ </collision>
+ </link>
+ <joint name="arm_left_6_joint" type="revolute">
+ <parent link="arm_left_5_link"/>
+ <child link="arm_left_6_link"/>
+ <origin rpy="0.0 -1.5707963267948966 -1.5707963267948966" xyz="0 0 -0.15"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="6.6" lower="-1.413716694115407" upper="1.413716694115407" velocity="1.76"/>
+ <dynamics damping="1.0" friction="1.0"/>
+ <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-1.3937166941154069" soft_upper_limit="1.3937166941154069"/>
+ </joint>
+ <link name="arm_left_7_link">
+ <inertial>
+ <origin rpy="0.00000 0.00000 0.00000" xyz="0.000010 -0.000003 -0.007774"/>
+ <mass value="0.234252"/>
+ <inertia ixx="0.00014200000" ixy="0.00000000000" ixz="0.00000000000" iyy="0.00011300000" iyz="0.00000000000" izz="0.00005200000"/>
+ </inertial>
+ </link>
+ <joint name="arm_left_7_joint" type="revolute">
+ <parent link="arm_left_6_link"/>
+ <child link="arm_left_7_link"/>
+ <origin rpy="1.5707963267948966 0.0 1.5707963267948966" xyz="0 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="6.6" lower="-2.0943951023931953" upper="2.0943951023931953" velocity="1.76"/>
+ <dynamics damping="1.0" friction="1.0"/>
+ <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-2.0743951023931952" soft_upper_limit="2.0743951023931952"/>
+ </joint>
+ <gazebo reference="arm_left_5_link">
+ <mu1>0.9</mu1>
+ <mu2>0.9</mu2>
+ </gazebo>
+ <gazebo reference="arm_left_6_link">
+ <mu1>0.9</mu1>
+ <mu2>0.9</mu2>
+ </gazebo>
+ <gazebo reference="arm_left_7_link">
+ <mu1>0.9</mu1>
+ <mu2>0.9</mu2>
+ </gazebo>
+ <gazebo reference="arm_left_5_joint">
+ <implicitSpringDamper>1</implicitSpringDamper>
+ </gazebo>
+ <gazebo reference="arm_left_6_joint">
+ <implicitSpringDamper>1</implicitSpringDamper>
+ </gazebo>
+ <gazebo reference="arm_left_7_joint">
+ <implicitSpringDamper>1</implicitSpringDamper>
+ <provideFeedback>1</provideFeedback>
+ </gazebo>
+ <transmission name="arm_left_5_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <actuator name="arm_left_5_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ <joint name="arm_left_5_joint">
+ <offset>0</offset>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ </transmission>
+ <transmission name="arm_left_wrist_trans">
+ <type>transmission_interface/DifferentialTransmission</type>
+ <actuator name="arm_left_6_motor">
+ <role>actuator1</role>
+ <mechanicalReduction>-1.0</mechanicalReduction>
+ </actuator>
+ <actuator name="arm_left_7_motor">
+ <role>actuator2</role>
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ <joint name="arm_left_6_joint">
+ <role>joint1</role>
+ <offset>0</offset>
+ <mechanicalReduction>1.0</mechanicalReduction>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <ignoreTransmissionAbsoluteEncoder>1</ignoreTransmissionAbsoluteEncoder>
+ </joint>
+ <joint name="arm_left_7_joint">
+ <role>joint2</role>
+ <offset>0</offset>
+ <mechanicalReduction>1</mechanicalReduction>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <ignoreTransmissionAbsoluteEncoder>1</ignoreTransmissionAbsoluteEncoder>
+ </joint>
+ </transmission>
+ <!--***********************-->
+ <!-- TOOL -->
+ <!--***********************-->
+ <link name="arm_left_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 rpy="0 1.5707963267948966 0" xyz="0.001 0 0"/>
+ <geometry>
+ <cylinder length="0.005" radius="0.005"/>
+ </geometry>
+ <material name="LightGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 1.5707963267948966 0" xyz="0.001 0 0"/>
+ <geometry>
+ <cylinder length="0.005" radius="0.005"/>
+ </geometry>
+ </collision>
+ </link>
+ <joint name="arm_left_tool_joint" type="fixed">
+ <parent link="arm_left_7_link"/>
+ <child link="arm_left_tool_link"/>
+ <origin rpy="1.5707963267948966 1.5707963267948966 3.141592653589793" xyz="0 0 -0.046"/>
+ </joint>
+ <transmission name="arm_left_1_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <actuator name="arm_left_1_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ <joint name="arm_left_1_joint">
+ <offset>0</offset>
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ </transmission>
+ <transmission name="arm_left_2_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <actuator name="arm_left_2_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ <joint name="arm_left_2_joint">
+ <offset>0</offset>
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ </transmission>
+ <transmission name="arm_left_3_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <actuator name="arm_left_3_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ <joint name="arm_left_3_joint">
+ <offset>0</offset>
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ </transmission>
+ <transmission name="arm_left_4_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <actuator name="arm_left_4_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ <joint name="arm_left_4_joint">
+ <offset>0</offset>
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ </transmission>
+ <!--************************-->
+ <!-- ft sensor -->
+ <!--************************-->
+ <link name="wrist_left_ft_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 rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <cylinder length="0.0157" radius="0.0225"/>
+ </geometry>
+ <material name="LightGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <cylinder length="0.0157" radius="0.0225"/>
+ </geometry>
+ </collision>
+ </link>
+ <joint name="wrist_left_ft_joint" type="fixed">
+ <parent link="arm_left_tool_link"/>
+ <child link="wrist_left_ft_link"/>
+ <origin rpy="1.5707963267948966 0 1.5707963267948966" xyz="0.00785 0 0"/>
+ </joint>
+ <!--***********************-->
+ <!-- FT TOOL -->
+ <!--***********************-->
+ <link name="wrist_left_ft_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 rpy="0 1.5707963267948966 0" xyz="0.0 0 0"/>
+ <geometry>
+ <cylinder length="0.00975" radius="0.025"/>
+ </geometry>
+ <material name="FlatBlack"/>
+ </visual>
+ <collision>
+ <origin rpy="0 1.5707963267948966 0" xyz="0.0 0 0"/>
+ <geometry>
+ <cylinder length="0.00975" radius="0.025"/>
+ </geometry>
+ </collision>
+ </link>
+ <joint name="wrist_left_tool_joint" type="fixed">
+ <parent link="wrist_left_ft_link"/>
+ <child link="wrist_left_ft_tool_link"/>
+ <origin rpy="-1.5707963267948966 -1.5707963267948966 0" xyz="0 0 0.012725"/>
+ </joint>
+ <!-- Tool Link -->
+ <link name="hand_left_tool_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.0001"/>
+ <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001"/>
+ </inertial>
+ </link>
+ <joint name="hand_left_tool_joint" type="fixed">
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <parent link="wrist_left_ft_tool_link"/>
+ <child link="hand_left_tool_link"/>
+ <axis xyz="0 0 0"/>
+ </joint>
+ <joint name="hand_left_palm_joint" type="fixed">
+ <parent link="wrist_left_ft_tool_link"/>
+ <child link="hand_left_palm_link"/>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ </joint>
+ <link name="hand_left_palm_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0641 -0.0022 0.0021"/>
+ <!-- NOTE: Less than total palm mass of 0.598kg because actuator masses are specified separately -->
+ <mass value="0.4"/>
+ <inertia ixx="0.000305100" ixy="0.000005037" ixz="0.000015302" iyy="0.000811920" iyz="0.000007622" izz="0.000655851"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/palm.stl" scale="1 1 -1"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/palm_collision.stl" scale="1 1 -1"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_left_palm_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_left_thumb_joint" type="revolute">
+ <parent link="hand_left_palm_link"/>
+ <child link="hand_left_thumb_link"/>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="-2.6179938779914944" upper="6.283185307179586" velocity="3.35"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ <!-- TODO: Tune -->
+ <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-2.6179938779914944" soft_upper_limit="6.283185307179586"/>
+ </joint>
+ <link name="hand_left_thumb_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.05"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!--To stabilize simulation-->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.001"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.001"/>
+ </geometry>
+ </collision>
+ </link>
+ <transmission name="hand_left_thumb_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <actuator name="hand_left_thumb_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ <joint name="hand_left_thumb_joint">
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ </transmission>
+ <joint name="hand_left_index_joint" type="revolute">
+ <parent link="hand_left_palm_link"/>
+ <child link="hand_left_index_link"/>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="-3.0543261909900767" upper="6.806784082777885" velocity="3.35"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ <!-- TODO: Tune -->
+ <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-3.0543261909900767" soft_upper_limit="6.806784082777885"/>
+ </joint>
+ <link name="hand_left_index_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.05"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!--To stabilize simulation-->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.001"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.001"/>
+ </geometry>
+ </collision>
+ </link>
+ <transmission name="hand_left_index_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <actuator name="hand_left_index_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ <joint name="hand_left_index_joint">
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ </transmission>
+ <joint name="hand_left_mrl_joint" type="revolute">
+ <parent link="hand_left_palm_link"/>
+ <child link="hand_left_mrl_link"/>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="-2.530727415391778" upper="9.250245035569947" velocity="3.56"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ <!-- TODO: Tune -->
+ <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-2.530727415391778" soft_upper_limit="9.250245035569947"/>
+ </joint>
+ <link name="hand_left_mrl_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.05"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!--To stabilize simulation-->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.001"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.001"/>
+ </geometry>
+ </collision>
+ </link>
+ <transmission name="hand_left_mrl_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <actuator name="hand_left_mrl_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ <joint name="hand_left_mrl_joint">
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ </transmission>
+ <joint name="hand_left_thumb_abd_joint" type="revolute">
+ <parent link="hand_left_palm_link"/>
+ <child link="hand_left_thumb_abd_link"/>
+ <origin rpy="0 1.1344640137963142 0" xyz="0.0901 0.0134 -0.038"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="1.5707963267948966" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_left_thumb_abd_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="-0.0132 0.0037 -0.0017"/>
+ <mass value="0.014"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/thumb_abd_collision.stl" scale="1 1 -1"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/thumb_abd_collision.stl" scale="1 -1 1"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_left_thumb_abd_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_left_thumb_virtual_1_joint" type="revolute">
+ <parent link="hand_left_thumb_abd_link"/>
+ <child link="hand_left_thumb_virtual_1_link"/>
+ <origin rpy="1.1442378576074825 0 0" xyz="0.0225 0.003 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_left_thumb_virtual_1_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <mass value="0.01"/>
+ <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_left_thumb_virtual_1_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_left_thumb_flex_1_joint" type="revolute">
+ <parent link="hand_left_thumb_virtual_1_link"/>
+ <child link="hand_left_thumb_flex_1_link"/>
+ <origin rpy="0 0 0" xyz="0.013 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_left_thumb_flex_1_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
+ <mass value="0.009"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_collision.stl"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_collision.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_left_thumb_flex_1_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_left_thumb_virtual_2_joint" type="revolute">
+ <parent link="hand_left_thumb_flex_1_link"/>
+ <child link="hand_left_thumb_virtual_2_link"/>
+ <origin rpy="0 0 0" xyz="0.009 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_left_thumb_virtual_2_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <mass value="0.01"/>
+ <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_left_thumb_virtual_2_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_left_thumb_flex_2_joint" type="revolute">
+ <parent link="hand_left_thumb_virtual_2_link"/>
+ <child link="hand_left_thumb_flex_2_link"/>
+ <origin rpy="0 0 0" xyz="0.013 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_left_thumb_flex_2_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0096 0.003 0.0"/>
+ <mass value="0.015"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_tip_collision.stl"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_tip_collision.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_left_thumb_flex_2_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_left_index_abd_joint" type="revolute">
+ <parent link="hand_left_palm_link"/>
+ <child link="hand_left_index_abd_link"/>
+ <origin rpy="1.5707963267948966 0.5235987755982988 0" xyz="0.1265 0.0042 -0.0265"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="-0.5235987755982988" upper="0.5235987755982988" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_left_index_abd_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="-0.0018 0.0 0.0028"/>
+ <mass value="0.018"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_abd_collision.stl" scale="1 1 -1"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_abd_collision.stl" scale="1 1 -1"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_left_index_abd_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_left_index_virtual_1_joint" type="revolute">
+ <parent link="hand_left_index_abd_link"/>
+ <child link="hand_left_index_virtual_1_link"/>
+ <origin rpy="-1.5707963267948966 0 0" xyz="0.009 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_left_index_virtual_1_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <mass value="0.01"/>
+ <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_left_index_virtual_1_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_left_index_flex_1_joint" type="revolute">
+ <parent link="hand_left_index_virtual_1_link"/>
+ <child link="hand_left_index_flex_1_link"/>
+ <origin rpy="0 0 0" xyz="0.013 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_left_index_flex_1_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
+ <mass value="0.009"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_collision.stl"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_collision.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_left_index_flex_1_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_left_index_virtual_2_joint" type="revolute">
+ <parent link="hand_left_index_flex_1_link"/>
+ <child link="hand_left_index_virtual_2_link"/>
+ <origin rpy="0 0 0" xyz="0.009 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_left_index_virtual_2_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <mass value="0.01"/>
+ <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_left_index_virtual_2_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_left_index_flex_2_joint" type="revolute">
+ <parent link="hand_left_index_virtual_2_link"/>
+ <child link="hand_left_index_flex_2_link"/>
+ <origin rpy="0 0 0" xyz="0.013 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_left_index_flex_2_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
+ <mass value="0.009"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_collision.stl"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_collision.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_left_index_flex_2_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_left_index_virtual_3_joint" type="revolute">
+ <parent link="hand_left_index_flex_2_link"/>
+ <child link="hand_left_index_virtual_3_link"/>
+ <origin rpy="0 0 0" xyz="0.009 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_left_index_virtual_3_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <mass value="0.01"/>
+ <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_left_index_virtual_3_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_left_index_flex_3_joint" type="revolute">
+ <parent link="hand_left_index_virtual_3_link"/>
+ <child link="hand_left_index_flex_3_link"/>
+ <origin rpy="0 0 0" xyz="0.013 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_left_index_flex_3_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0096 0.003 0.0"/>
+ <mass value="0.015"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_tip_collision.stl"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_tip_collision.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_left_index_flex_3_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_left_middle_abd_joint" type="revolute">
+ <parent link="hand_left_palm_link"/>
+ <child link="hand_left_middle_abd_link"/>
+ <origin rpy="1.5707963267948966 0 0" xyz="0.1335 0.0042 -0.007"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="-0.5235987755982988" upper="0.5235987755982988" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_left_middle_abd_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="-0.0018 0.0 0.0028"/>
+ <mass value="0.018"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_abd_collision.stl" scale="1 1 -1"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_abd_collision.stl" scale="1 1 -1"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_left_middle_abd_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_left_middle_virtual_1_joint" type="revolute">
+ <parent link="hand_left_middle_abd_link"/>
+ <child link="hand_left_middle_virtual_1_link"/>
+ <origin rpy="-1.5707963267948966 0 0" xyz="0.009 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_left_middle_virtual_1_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <mass value="0.01"/>
+ <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_left_middle_virtual_1_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_left_middle_flex_1_joint" type="revolute">
+ <parent link="hand_left_middle_virtual_1_link"/>
+ <child link="hand_left_middle_flex_1_link"/>
+ <origin rpy="0 0 0" xyz="0.013 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_left_middle_flex_1_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
+ <mass value="0.009"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_collision.stl"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_collision.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_left_middle_flex_1_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_left_middle_virtual_2_joint" type="revolute">
+ <parent link="hand_left_middle_flex_1_link"/>
+ <child link="hand_left_middle_virtual_2_link"/>
+ <origin rpy="0 0 0" xyz="0.009 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_left_middle_virtual_2_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <mass value="0.01"/>
+ <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_left_middle_virtual_2_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_left_middle_flex_2_joint" type="revolute">
+ <parent link="hand_left_middle_virtual_2_link"/>
+ <child link="hand_left_middle_flex_2_link"/>
+ <origin rpy="0 0 0" xyz="0.013 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_left_middle_flex_2_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
+ <mass value="0.009"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_collision.stl"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_collision.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_left_middle_flex_2_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_left_middle_virtual_3_joint" type="revolute">
+ <parent link="hand_left_middle_flex_2_link"/>
+ <child link="hand_left_middle_virtual_3_link"/>
+ <origin rpy="0 0 0" xyz="0.009 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_left_middle_virtual_3_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <mass value="0.01"/>
+ <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_left_middle_virtual_3_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_left_middle_flex_3_joint" type="revolute">
+ <parent link="hand_left_middle_virtual_3_link"/>
+ <child link="hand_left_middle_flex_3_link"/>
+ <origin rpy="0 0 0" xyz="0.013 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_left_middle_flex_3_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0096 0.003 0.0"/>
+ <mass value="0.015"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_tip_collision.stl"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_tip_collision.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_left_middle_flex_3_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_left_ring_abd_joint" type="revolute">
+ <parent link="hand_left_palm_link"/>
+ <child link="hand_left_ring_abd_link"/>
+ <origin rpy="1.5707963267948966 -0.2617993877991494 0" xyz="0.1265 0.0042 0.0125"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="-0.5235987755982988" upper="0.5235987755982988" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_left_ring_abd_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="-0.0018 0.0 0.0028"/>
+ <mass value="0.018"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_abd_collision.stl" scale="1 1 -1"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_abd_collision.stl" scale="1 1 -1"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_left_ring_abd_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_left_ring_virtual_1_joint" type="revolute">
+ <parent link="hand_left_ring_abd_link"/>
+ <child link="hand_left_ring_virtual_1_link"/>
+ <origin rpy="-1.5707963267948966 0 0" xyz="0.009 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_left_ring_virtual_1_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <mass value="0.01"/>
+ <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_left_ring_virtual_1_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_left_ring_flex_1_joint" type="revolute">
+ <parent link="hand_left_ring_virtual_1_link"/>
+ <child link="hand_left_ring_flex_1_link"/>
+ <origin rpy="0 0 0" xyz="0.013 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_left_ring_flex_1_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
+ <mass value="0.009"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_collision.stl"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_collision.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_left_ring_flex_1_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_left_ring_virtual_2_joint" type="revolute">
+ <parent link="hand_left_ring_flex_1_link"/>
+ <child link="hand_left_ring_virtual_2_link"/>
+ <origin rpy="0 0 0" xyz="0.009 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_left_ring_virtual_2_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <mass value="0.01"/>
+ <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_left_ring_virtual_2_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_left_ring_flex_2_joint" type="revolute">
+ <parent link="hand_left_ring_virtual_2_link"/>
+ <child link="hand_left_ring_flex_2_link"/>
+ <origin rpy="0 0 0" xyz="0.013 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_left_ring_flex_2_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
+ <mass value="0.009"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_collision.stl"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_collision.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_left_ring_flex_2_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_left_ring_virtual_3_joint" type="revolute">
+ <parent link="hand_left_ring_flex_2_link"/>
+ <child link="hand_left_ring_virtual_3_link"/>
+ <origin rpy="0 0 0" xyz="0.009 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_left_ring_virtual_3_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <mass value="0.01"/>
+ <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_left_ring_virtual_3_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_left_ring_flex_3_joint" type="revolute">
+ <parent link="hand_left_ring_virtual_3_link"/>
+ <child link="hand_left_ring_flex_3_link"/>
+ <origin rpy="0 0 0" xyz="0.013 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_left_ring_flex_3_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0096 0.003 0.0"/>
+ <mass value="0.015"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_tip_collision.stl"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_tip_collision.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_left_ring_flex_3_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_left_little_abd_joint" type="revolute">
+ <parent link="hand_left_palm_link"/>
+ <child link="hand_left_little_abd_link"/>
+ <origin rpy="1.5707963267948966 -0.5235987755982988 0" xyz="0.1165 0.0042 0.032"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="-0.5235987755982988" upper="0.5235987755982988" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_left_little_abd_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="-0.0018 0.0 0.0028"/>
+ <mass value="0.018"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_abd_collision.stl" scale="1 1 -1"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_abd_collision.stl" scale="1 1 -1"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_left_little_abd_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_left_little_virtual_1_joint" type="revolute">
+ <parent link="hand_left_little_abd_link"/>
+ <child link="hand_left_little_virtual_1_link"/>
+ <origin rpy="-1.5707963267948966 0 0" xyz="0.009 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_left_little_virtual_1_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <mass value="0.01"/>
+ <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_left_little_virtual_1_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_left_little_flex_1_joint" type="revolute">
+ <parent link="hand_left_little_virtual_1_link"/>
+ <child link="hand_left_little_flex_1_link"/>
+ <origin rpy="0 0 0" xyz="0.013 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_left_little_flex_1_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
+ <mass value="0.009"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_collision.stl"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_collision.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_left_little_flex_1_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_left_little_virtual_2_joint" type="revolute">
+ <parent link="hand_left_little_flex_1_link"/>
+ <child link="hand_left_little_virtual_2_link"/>
+ <origin rpy="0 0 0" xyz="0.009 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_left_little_virtual_2_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <mass value="0.01"/>
+ <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_left_little_virtual_2_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_left_little_flex_2_joint" type="revolute">
+ <parent link="hand_left_little_virtual_2_link"/>
+ <child link="hand_left_little_flex_2_link"/>
+ <origin rpy="0 0 0" xyz="0.013 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_left_little_flex_2_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
+ <mass value="0.009"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_collision.stl"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_collision.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_left_little_flex_2_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_left_little_virtual_3_joint" type="revolute">
+ <parent link="hand_left_little_flex_2_link"/>
+ <child link="hand_left_little_virtual_3_link"/>
+ <origin rpy="0 0 0" xyz="0.009 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_left_little_virtual_3_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <mass value="0.01"/>
+ <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_left_little_virtual_3_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_left_little_flex_3_joint" type="revolute">
+ <parent link="hand_left_little_virtual_3_link"/>
+ <child link="hand_left_little_flex_3_link"/>
+ <origin rpy="0 0 0" xyz="0.013 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_left_little_flex_3_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0096 0.003 0.0"/>
+ <mass value="0.015"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_tip_collision.stl"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_tip_collision.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_left_little_flex_3_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <!--Abductor-->
+ <transmission name="hand_left_thumb_abd_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_left_thumb_abd_joint">
+ <saturationLower>0.0</saturationLower>
+ <saturationUpper>1.5707963267948966</saturationUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_left_thumb_motor">
+ <mechanicalReduction>0.6544984694978736</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <!--First flexor-->
+ <transmission name="hand_left_thumb_virtual_1_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_left_thumb_virtual_1_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>2.0</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_left_thumb_motor">
+ <mechanicalReduction>0.16299357854713997</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="hand_left_thumb_flex_1_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_left_thumb_flex_1_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>2.0</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_left_thumb_motor">
+ <mechanicalReduction>0.16299357854713997</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <!--Second flexor-->
+ <transmission name="hand_left_thumb_virtual_2_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_left_thumb_virtual_2_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>2.0</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_left_thumb_motor">
+ <mechanicalReduction>0.16299357854713997</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="hand_left_thumb_flex_2_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_left_thumb_flex_2_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>2.0</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_left_thumb_motor">
+ <mechanicalReduction>0.16299357854713997</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <!--Abductor-->
+ <transmission name="hand_left_index_abd_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_left_index_abd_joint">
+ <saturationLower>0.0</saturationLower>
+ <saturationUpper>0.5235987755982988</saturationUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_left_index_motor">
+ <mechanicalReduction>0.49866550056980846</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <!--First flexor-->
+ <transmission name="hand_left_index_virtual_1_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_left_index_virtual_1_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.7</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_left_index_motor">
+ <mechanicalReduction>0.10288861079760754</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="hand_left_index_flex_1_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_left_index_flex_1_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.7</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_left_index_motor">
+ <mechanicalReduction>0.10288861079760754</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <!--Second flexor-->
+ <transmission name="hand_left_index_virtual_2_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_left_index_virtual_2_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.7</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_left_index_motor">
+ <mechanicalReduction>0.10288861079760754</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="hand_left_index_flex_2_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_left_index_flex_2_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.7</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_left_index_motor">
+ <mechanicalReduction>0.10288861079760754</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <!--Third flexor-->
+ <transmission name="hand_left_index_virtual_3_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_left_index_virtual_3_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.7</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_left_index_motor">
+ <mechanicalReduction>0.10288861079760754</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="hand_left_index_flex_3_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_left_index_flex_3_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.7</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_left_index_motor">
+ <mechanicalReduction>0.10288861079760754</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <!--Abductor-->
+ <transmission name="hand_left_middle_abd_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_left_middle_abd_joint">
+ <saturationLower>0.0</saturationLower>
+ <saturationUpper>0.0</saturationUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_left_mrl_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <!--First flexor-->
+ <transmission name="hand_left_middle_virtual_1_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_left_middle_virtual_1_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.0</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_left_mrl_motor">
+ <mechanicalReduction>0.06792452830188679</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="hand_left_middle_flex_1_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_left_middle_flex_1_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.0</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_left_mrl_motor">
+ <mechanicalReduction>0.06792452830188679</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <!--Second flexor-->
+ <transmission name="hand_left_middle_virtual_2_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_left_middle_virtual_2_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.0</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_left_mrl_motor">
+ <mechanicalReduction>0.06792452830188679</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="hand_left_middle_flex_2_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_left_middle_flex_2_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.0</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_left_mrl_motor">
+ <mechanicalReduction>0.06792452830188679</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <!--Third flexor-->
+ <transmission name="hand_left_middle_virtual_3_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_left_middle_virtual_3_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.0</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_left_mrl_motor">
+ <mechanicalReduction>0.06792452830188679</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="hand_left_middle_flex_3_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_left_middle_flex_3_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.0</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_left_mrl_motor">
+ <mechanicalReduction>0.06792452830188679</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <!--Abductor-->
+ <transmission name="hand_left_ring_abd_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_left_ring_abd_joint">
+ <saturationLower>-0.2617993877991494</saturationLower>
+ <saturationUpper>0.0</saturationUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_left_mrl_motor">
+ <mechanicalReduction>-0.23271056693257727</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <!--First flexor-->
+ <transmission name="hand_left_ring_virtual_1_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_left_ring_virtual_1_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.75</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_left_mrl_motor">
+ <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="hand_left_ring_flex_1_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_left_ring_flex_1_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.75</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_left_mrl_motor">
+ <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <!--Second flexor-->
+ <transmission name="hand_left_ring_virtual_2_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_left_ring_virtual_2_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.75</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_left_mrl_motor">
+ <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="hand_left_ring_flex_2_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_left_ring_flex_2_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.75</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_left_mrl_motor">
+ <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <!--Third flexor-->
+ <transmission name="hand_left_ring_virtual_3_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_left_ring_virtual_3_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.75</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_left_mrl_motor">
+ <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="hand_left_ring_flex_3_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_left_ring_flex_3_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.75</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_left_mrl_motor">
+ <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <!--Abductor-->
+ <transmission name="hand_left_little_abd_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_left_little_abd_joint">
+ <saturationLower>-0.5235987755982988</saturationLower>
+ <saturationUpper>0.0</saturationUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_left_mrl_motor">
+ <mechanicalReduction>-0.46542113386515455</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <!--First flexor-->
+ <transmission name="hand_left_little_virtual_1_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_left_little_virtual_1_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.75</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_left_mrl_motor">
+ <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="hand_left_little_flex_1_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_left_little_flex_1_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.75</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_left_mrl_motor">
+ <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <!--Second flexor-->
+ <transmission name="hand_left_little_virtual_2_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_left_little_virtual_2_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.75</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_left_mrl_motor">
+ <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="hand_left_little_flex_2_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_left_little_flex_2_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.75</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_left_mrl_motor">
+ <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <!--Third flexor-->
+ <transmission name="hand_left_little_virtual_3_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_left_little_virtual_3_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.75</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_left_mrl_motor">
+ <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="hand_left_little_flex_3_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_left_little_flex_3_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.75</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_left_mrl_motor">
+ <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <link name="hand_left_grasping_frame">
+ <inertial>
+ <mass value="0.0"/>
+ <origin xyz="0 0 0"/>
+ <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0" izz="0.0"/>
+ </inertial>
+ </link>
+ <joint name="hand_left_grasping_fixed_joint" type="fixed">
+ <parent link="hand_left_palm_link"/>
+ <child link="hand_left_grasping_frame"/>
+ <axis xyz="0 0 1"/>
+ <origin rpy="0 0 0" xyz="0.13 0.02 0"/>
+ </joint>
+ <gazebo>
+ <plugin filename="libgazebo_underactuated_finger.so" name="gazebo_hand_left_thumb">
+ <actuatedJoint>hand_left_thumb_joint</actuatedJoint>
+ <virtualJoint>
+ <name>hand_left_thumb_abd_joint</name>
+ <scale_factor>2.0</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_left_thumb_flex_1_joint</name>
+ <scale_factor>2.5</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_left_thumb_virtual_1_joint</name>
+ <scale_factor>3.0</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_left_thumb_flex_2_joint</name>
+ <scale_factor>3.5</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_left_thumb_virtual_2_joint</name>
+ <scale_factor>3.0</scale_factor>
+ </virtualJoint>
+ </plugin>
+ <plugin filename="libgazebo_underactuated_finger.so" name="gazebo_hand_left_index">
+ <actuatedJoint>hand_left_index_joint</actuatedJoint>
+ <virtualJoint>
+ <name>hand_left_index_abd_joint</name>
+ <scale_factor>5.0</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_left_index_flex_1_joint</name>
+ <scale_factor>2.5</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_left_index_virtual_1_joint</name>
+ <scale_factor>3.0</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_left_index_flex_2_joint</name>
+ <scale_factor>3.5</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_left_index_virtual_2_joint</name>
+ <scale_factor>3.5</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_left_index_flex_3_joint</name>
+ <scale_factor>4.0</scale_factor>
+ </virtualJoint>
+ </plugin>
+ <plugin filename="libgazebo_underactuated_finger.so" name="gazebo_hand_left_middle">
+ <actuatedJoint>hand_left_mrl_joint</actuatedJoint>
+ <virtualJoint>
+ <name>hand_left_middle_flex_1_joint</name>
+ <scale_factor>2.5</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_left_middle_virtual_1_joint</name>
+ <scale_factor>3.0</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_left_middle_flex_2_joint</name>
+ <scale_factor>3.5</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_left_middle_virtual_2_joint</name>
+ <scale_factor>3.5</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_left_middle_flex_3_joint</name>
+ <scale_factor>4.0</scale_factor>
+ </virtualJoint>
+ </plugin>
+ <plugin filename="libgazebo_underactuated_finger.so" name="gazebo_hand_left_ring">
+ <actuatedJoint>hand_left_mrl_joint</actuatedJoint>
+ <virtualJoint>
+ <name>hand_left_ring_flex_1_joint</name>
+ <scale_factor>2.5</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_left_ring_virtual_1_joint</name>
+ <scale_factor>3.0</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_left_ring_flex_2_joint</name>
+ <scale_factor>3.5</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_left_ring_virtual_2_joint</name>
+ <scale_factor>3.5</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_left_ring_flex_3_joint</name>
+ <scale_factor>4.0</scale_factor>
+ </virtualJoint>
+ </plugin>
+ <plugin filename="libgazebo_underactuated_finger.so" name="gazebo_hand_left_little">
+ <actuatedJoint>hand_left_mrl_joint</actuatedJoint>
+ <virtualJoint>
+ <name>hand_left_little_flex_1_joint</name>
+ <scale_factor>2.5</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_left_little_virtual_1_joint</name>
+ <scale_factor>3.0</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_left_little_flex_2_joint</name>
+ <scale_factor>3.5</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_left_little_virtual_2_joint</name>
+ <scale_factor>3.5</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_left_little_flex_3_joint</name>
+ <scale_factor>4.0</scale_factor>
+ </virtualJoint>
+ </plugin>
+ </gazebo>
+ <joint name="hand_left_safety_box_joint" type="fixed">
+ <parent link="wrist_left_ft_tool_link"/>
+ <child link="hand_left_safety_box"/>
+ <origin rpy="0 0 0" xyz="0.1 0.02 -0.02"/>
+ </joint>
+ <link name="hand_left_safety_box">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.0001"/>
+ <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
+ </inertial>
+ <!--
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <box size="0.2 0.115 0.18" />
+ </geometry>
+ </visual>
+ -->
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.2 0.115 0.18"/>
+ </geometry>
+ </collision>
+ </link>
+ <!--************************-->
+ <!-- SHOULDER -->
+ <!--************************-->
+ <link name="arm_right_1_link">
+ <inertial>
+ <!--<origin xyz="0.061191 ${reflect * -0.022397} -0.012835" rpy="0.00000 0.00000 0.00000"/>-->
+ <origin rpy="0.00000 0.00000 0.00000" xyz="0.061191 -0.022397 -0.012835"/>
+ <mass value="1.563428"/>
+ <inertia ixx="0.002471" ixy="-0.001809" ixz="-0.001202" iyy="0.006132" iyz="0.000494" izz="0.006704"/>
+ </inertial>
+ <!-- M90 module -->
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/arm/arm_1.stl" scale="1 -1 1"/>
+ </geometry>
+ <material name="White"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/arm/arm_1_collision.dae" scale="1 -1 1"/>
+ </geometry>
+ </collision>
+ </link>
+ <joint name="arm_right_1_joint" type="revolute">
+ <parent link="torso_lift_link"/>
+ <child link="arm_right_1_link"/>
+ <origin rpy="0 0 -1.5707963267948966" xyz="0.02556 -0.19 -0.171"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="43.0" lower="-1.1780972450961724" upper="1.5707963267948966" velocity="1.95"/>
+ <dynamics damping="1.0" friction="1.0"/>
+ <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-1.1080972450961724" soft_upper_limit="1.5007963267948965"/>
+ </joint>
+ <link name="arm_right_2_link">
+ <inertial>
+ <origin rpy="0.00000 0.00000 0.00000" xyz="0.030432 0.000229 -0.005942"/>
+ <mass value="1.800397"/>
+ <inertia ixx="0.001809" ixy="1.5e-05" ixz="0.000254" iyy="0.004333" iyz="1.4e-05" izz="0.004341"/>
+ </inertial>
+ <visual>
+ <!-- <origin xyz="0 0 0" rpy="${180 * deg_to_rad} ${0 * deg_to_rad} ${-90 * deg_to_rad}" /> -->
+ <origin rpy="0.0 0.0 0.0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/arm/arm_2.stl" scale="1 1 1"/>
+ </geometry>
+ <material name="DarkGrey"/>
+ </visual>
+ <collision>
+ <!-- <origin xyz="0 0 0" rpy="${180 * deg_to_rad} ${0 * deg_to_rad} ${-90 * deg_to_rad}" /> -->
+ <origin rpy="0.0 0.0 0.0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/arm/arm_2_collision.dae" scale="1 1 1"/>
+ </geometry>
+ </collision>
+ </link>
+ <joint name="arm_right_2_joint" type="revolute">
+ <parent link="arm_right_1_link"/>
+ <child link="arm_right_2_link"/>
+ <origin rpy="-1.5707963267948966 0.0 0.0" xyz="0.125 -0.0195 -0.031"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="43.0" lower="-1.1780972450961724" upper="1.5707963267948966" velocity="1.95"/>
+ <dynamics damping="1.0" friction="1.0"/>
+ <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-1.1080972450961724" soft_upper_limit="1.5007963267948965"/>
+ </joint>
+ <link name="arm_right_3_link">
+ <inertial>
+ <origin rpy="0.00000 0.00000 0.00000" xyz="0.007418 -0.004361 -0.134194"/>
+ <mass value="1.8"/>
+ <inertia ixx="0.019541" ixy="-0.000105" ixz="-0.001717" iyy="0.019936" iyz="0.000852" izz="0.002864"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/arm/arm_3.stl" scale="1 1 1"/>
+ </geometry>
+ <material name="DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/arm/arm_3_collision.dae" scale="1 1 1"/>
+ </geometry>
+ </collision>
+ </link>
+ <joint name="arm_right_3_joint" type="revolute">
+ <parent link="arm_right_2_link"/>
+ <child link="arm_right_3_link"/>
+ <origin rpy="-1.5707963267948966 0.0 1.5707963267948966" xyz="0.0895 0.0 -0.0015"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="26" lower="-0.7853981633974483" upper="3.9269908169872414" velocity="2.35"/>
+ <dynamics damping="1.0" friction="1.0"/>
+ <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-0.7153981633974482" soft_upper_limit="3.8569908169872416"/>
+ </joint>
+ <!--************************-->
+ <!-- ELBOW -->
+ <!--************************-->
+ <link name="arm_right_4_link">
+ <inertial>
+ <origin rpy="0.00000 0.00000 0.00000" xyz="-0.095658 0.014666 0.018133"/>
+ <mass value="1.432697"/>
+ <inertia ixx="0.002040" ixy="0.000725" ixz="-0.00115" iyy="0.008424" iyz="-0.000204" izz="0.008686"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/arm/arm_4.stl" scale="1 1 1"/>
+ </geometry>
+ <material name="DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/arm/arm_4_collision.dae" scale="1 1 1"/>
+ </geometry>
+ </collision>
+ </link>
+ <joint name="arm_right_4_joint" type="revolute">
+ <parent link="arm_right_3_link"/>
+ <child link="arm_right_4_link"/>
+ <origin rpy="-1.5707963267948966 -1.5707963267948966 0.0" xyz="-0.02 -0.027 -0.222"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="26" lower="-0.39269908169872414" upper="2.356194490192345" velocity="2.35"/>
+ <dynamics damping="1.0" friction="1.0"/>
+ <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-0.32269908169872413" soft_upper_limit="2.286194490192345"/>
+ </joint>
+ <gazebo reference="arm_right_1_link">
+ <mu1>0.9</mu1>
+ <mu2>0.9</mu2>
+ <material>Gazebo/DarkGrey</material>
+ </gazebo>
+ <gazebo reference="arm_right_2_link">
+ <mu1>0.9</mu1>
+ <mu2>0.9</mu2>
+ <material>Gazebo/DarkGrey</material>
+ </gazebo>
+ <gazebo reference="arm_right_3_link">
+ <mu1>0.9</mu1>
+ <mu2>0.9</mu2>
+ <material>Gazebo/DarkGrey</material>
+ </gazebo>
+ <gazebo reference="arm_right_4_link">
+ <mu1>0.9</mu1>
+ <mu2>0.9</mu2>
+ <material>Gazebo/DarkGrey</material>
+ </gazebo>
+ <gazebo reference="arm_right_1_joint">
+ <implicitSpringDamper>1</implicitSpringDamper>
+ </gazebo>
+ <gazebo reference="arm_right_2_joint">
+ <implicitSpringDamper>1</implicitSpringDamper>
+ </gazebo>
+ <gazebo reference="arm_right_3_joint">
+ <implicitSpringDamper>1</implicitSpringDamper>
+ </gazebo>
+ <gazebo reference="arm_right_4_joint">
+ <implicitSpringDamper>1</implicitSpringDamper>
+ </gazebo>
+ <!--************************-->
+ <!-- WRIST -->
+ <!--************************-->
+ <link name="arm_right_5_link">
+ <inertial>
+ <origin rpy="0.00000 0.00000 0.00000" xyz="0.001078 -0.000151 -0.077173"/>
+ <mass value="1.655819"/>
+ <inertia ixx="0.00613800000" ixy="-0.00000400000" ixz="0.00015600000" iyy="0.00564000000" iyz="0.00002200000" izz="0.00179700000"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/arm/arm_5-wrist-2010.stl" scale="1 1 1"/>
+ </geometry>
+ <material name="LightGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/arm/arm_5_collision.dae" scale="1 1 1"/>
+ </geometry>
+ </collision>
+ </link>
+ <joint name="arm_right_5_joint" type="revolute">
+ <parent link="arm_right_4_link"/>
+ <child link="arm_right_5_link"/>
+ <origin rpy="1.5707963267948966 -1.5707963267948966 -1.5707963267948966" xyz="-0.162 0.02 0.027"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="3" lower="-2.0943951023931953" upper="2.0943951023931953" velocity="1.95"/>
+ <dynamics damping="1.0" friction="1.0"/>
+ <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-2.0743951023931952" soft_upper_limit="2.0743951023931952"/>
+ </joint>
+ <link name="arm_right_6_link">
+ <inertial>
+ <origin rpy="0.00000 0.00000 0.00000" xyz="-0.000153 -0.003122 0.000183"/>
+ <mass value="0.370063"/>
+ <inertia ixx="0.00021500000" ixy="0.00000000000" ixz="0.00000000000" iyy="0.00017000000" iyz="0.00000200000" izz="0.00017400000"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/arm/arm_6-wrist-2010.stl" scale="1 1 1"/>
+ </geometry>
+ <material name="LightGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/arm/arm_6_collision-wrist-2010.dae" scale="1 1 1"/>
+ </geometry>
+ </collision>
+ </link>
+ <joint name="arm_right_6_joint" type="revolute">
+ <parent link="arm_right_5_link"/>
+ <child link="arm_right_6_link"/>
+ <origin rpy="0.0 -1.5707963267948966 -1.5707963267948966" xyz="0 0 0.15"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="6.6" lower="-1.413716694115407" upper="1.413716694115407" velocity="1.76"/>
+ <dynamics damping="1.0" friction="1.0"/>
+ <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-1.3937166941154069" soft_upper_limit="1.3937166941154069"/>
+ </joint>
+ <link name="arm_right_7_link">
+ <inertial>
+ <origin rpy="0.00000 0.00000 0.00000" xyz="0.000010 -0.000003 -0.007774"/>
+ <mass value="0.234252"/>
+ <inertia ixx="0.00014200000" ixy="0.00000000000" ixz="0.00000000000" iyy="0.00011300000" iyz="0.00000000000" izz="0.00005200000"/>
+ </inertial>
+ </link>
+ <joint name="arm_right_7_joint" type="revolute">
+ <parent link="arm_right_6_link"/>
+ <child link="arm_right_7_link"/>
+ <origin rpy="1.5707963267948966 0.0 1.5707963267948966" xyz="0 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="6.6" lower="-2.0943951023931953" upper="2.0943951023931953" velocity="1.76"/>
+ <dynamics damping="1.0" friction="1.0"/>
+ <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-2.0743951023931952" soft_upper_limit="2.0743951023931952"/>
+ </joint>
+ <gazebo reference="arm_right_5_link">
+ <mu1>0.9</mu1>
+ <mu2>0.9</mu2>
+ </gazebo>
+ <gazebo reference="arm_right_6_link">
+ <mu1>0.9</mu1>
+ <mu2>0.9</mu2>
+ </gazebo>
+ <gazebo reference="arm_right_7_link">
+ <mu1>0.9</mu1>
+ <mu2>0.9</mu2>
+ </gazebo>
+ <gazebo reference="arm_right_5_joint">
+ <implicitSpringDamper>1</implicitSpringDamper>
+ </gazebo>
+ <gazebo reference="arm_right_6_joint">
+ <implicitSpringDamper>1</implicitSpringDamper>
+ </gazebo>
+ <gazebo reference="arm_right_7_joint">
+ <implicitSpringDamper>1</implicitSpringDamper>
+ <provideFeedback>1</provideFeedback>
+ </gazebo>
+ <transmission name="arm_right_5_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <actuator name="arm_right_5_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ <joint name="arm_right_5_joint">
+ <offset>0</offset>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ </joint>
+ </transmission>
+ <transmission name="arm_right_wrist_trans">
+ <type>transmission_interface/DifferentialTransmission</type>
+ <actuator name="arm_right_6_motor">
+ <role>actuator1</role>
+ <mechanicalReduction>-1.0</mechanicalReduction>
+ </actuator>
+ <actuator name="arm_right_7_motor">
+ <role>actuator2</role>
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ <joint name="arm_right_6_joint">
+ <role>joint1</role>
+ <offset>0</offset>
+ <mechanicalReduction>1.0</mechanicalReduction>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <ignoreTransmissionAbsoluteEncoder>1</ignoreTransmissionAbsoluteEncoder>
+ </joint>
+ <joint name="arm_right_7_joint">
+ <role>joint2</role>
+ <offset>0</offset>
+ <mechanicalReduction>-1</mechanicalReduction>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <ignoreTransmissionAbsoluteEncoder>1</ignoreTransmissionAbsoluteEncoder>
+ </joint>
+ </transmission>
+ <!--***********************-->
+ <!-- TOOL -->
+ <!--***********************-->
+ <link name="arm_right_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 rpy="0 1.5707963267948966 0" xyz="0.001 0 0"/>
+ <geometry>
+ <cylinder length="0.005" radius="0.005"/>
+ </geometry>
+ <material name="LightGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 1.5707963267948966 0" xyz="0.001 0 0"/>
+ <geometry>
+ <cylinder length="0.005" radius="0.005"/>
+ </geometry>
+ </collision>
+ </link>
+ <joint name="arm_right_tool_joint" type="fixed">
+ <parent link="arm_right_7_link"/>
+ <child link="arm_right_tool_link"/>
+ <origin rpy="1.5707963267948966 -1.5707963267948966 3.141592653589793" xyz="0 0 0.046"/>
+ </joint>
+ <transmission name="arm_right_1_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <actuator name="arm_right_1_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ <joint name="arm_right_1_joint">
+ <offset>0</offset>
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ </transmission>
+ <transmission name="arm_right_2_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <actuator name="arm_right_2_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ <joint name="arm_right_2_joint">
+ <offset>0</offset>
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ </transmission>
+ <transmission name="arm_right_3_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <actuator name="arm_right_3_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ <joint name="arm_right_3_joint">
+ <offset>0</offset>
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ </transmission>
+ <transmission name="arm_right_4_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <actuator name="arm_right_4_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ <joint name="arm_right_4_joint">
+ <offset>0</offset>
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ </transmission>
+ <!--************************-->
+ <!-- ft sensor -->
+ <!--************************-->
+ <link name="wrist_right_ft_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 rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <cylinder length="0.0157" radius="0.0225"/>
+ </geometry>
+ <material name="LightGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <cylinder length="0.0157" radius="0.0225"/>
+ </geometry>
+ </collision>
+ </link>
+ <joint name="wrist_right_ft_joint" type="fixed">
+ <parent link="arm_right_tool_link"/>
+ <child link="wrist_right_ft_link"/>
+ <origin rpy="1.5707963267948966 0 1.5707963267948966" xyz="0.00785 0 0"/>
+ </joint>
+ <!--***********************-->
+ <!-- FT TOOL -->
+ <!--***********************-->
+ <link name="wrist_right_ft_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 rpy="0 1.5707963267948966 0" xyz="0.0 0 0"/>
+ <geometry>
+ <cylinder length="0.00975" radius="0.025"/>
+ </geometry>
+ <material name="FlatBlack"/>
+ </visual>
+ <collision>
+ <origin rpy="0 1.5707963267948966 0" xyz="0.0 0 0"/>
+ <geometry>
+ <cylinder length="0.00975" radius="0.025"/>
+ </geometry>
+ </collision>
+ </link>
+ <joint name="wrist_right_tool_joint" type="fixed">
+ <parent link="wrist_right_ft_link"/>
+ <child link="wrist_right_ft_tool_link"/>
+ <origin rpy="-1.5707963267948966 -1.5707963267948966 0" xyz="0 0 0.012725"/>
+ </joint>
+ <!-- Tool Link -->
+ <link name="hand_right_tool_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.0001"/>
+ <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001"/>
+ </inertial>
+ </link>
+ <joint name="hand_right_tool_joint" type="fixed">
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <parent link="wrist_right_ft_tool_link"/>
+ <child link="hand_right_tool_link"/>
+ <axis xyz="0 0 0"/>
+ </joint>
+ <joint name="hand_right_palm_joint" type="fixed">
+ <parent link="wrist_right_ft_tool_link"/>
+ <child link="hand_right_palm_link"/>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ </joint>
+ <link name="hand_right_palm_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0641 -0.0022 0.0021"/>
+ <!-- NOTE: Less than total palm mass of 0.598kg because actuator masses are specified separately -->
+ <mass value="0.4"/>
+ <inertia ixx="0.000305100" ixy="0.000005037" ixz="0.000015302" iyy="0.000811920" iyz="0.000007622" izz="0.000655851"/>
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/palm.stl" scale="1 1 1"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/palm_collision.stl" scale="1 1 1"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_right_palm_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_right_thumb_joint" type="revolute">
+ <parent link="hand_right_palm_link"/>
+ <child link="hand_right_thumb_link"/>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="-2.6179938779914944" upper="6.283185307179586" velocity="3.35"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ <!-- TODO: Tune -->
+ <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-2.6179938779914944" soft_upper_limit="6.283185307179586"/>
+ </joint>
+ <link name="hand_right_thumb_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.05"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!--To stabilize simulation-->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.001"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.001"/>
+ </geometry>
+ </collision>
+ </link>
+ <transmission name="hand_right_thumb_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <actuator name="hand_right_thumb_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ <joint name="hand_right_thumb_joint">
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ </transmission>
+ <joint name="hand_right_index_joint" type="revolute">
+ <parent link="hand_right_palm_link"/>
+ <child link="hand_right_index_link"/>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="-3.0543261909900767" upper="6.806784082777885" velocity="3.35"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ <!-- TODO: Tune -->
+ <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-3.0543261909900767" soft_upper_limit="6.806784082777885"/>
+ </joint>
+ <link name="hand_right_index_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.05"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!--To stabilize simulation-->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.001"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.001"/>
+ </geometry>
+ </collision>
+ </link>
+ <transmission name="hand_right_index_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <actuator name="hand_right_index_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ <joint name="hand_right_index_joint">
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ </transmission>
+ <joint name="hand_right_mrl_joint" type="revolute">
+ <parent link="hand_right_palm_link"/>
+ <child link="hand_right_mrl_link"/>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="-2.530727415391778" upper="9.250245035569947" velocity="3.56"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ <!-- TODO: Tune -->
+ <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-2.530727415391778" soft_upper_limit="9.250245035569947"/>
+ </joint>
+ <link name="hand_right_mrl_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.05"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!--To stabilize simulation-->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.001"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <sphere radius="0.001"/>
+ </geometry>
+ </collision>
+ </link>
+ <transmission name="hand_right_mrl_trans">
+ <type>transmission_interface/SimpleTransmission</type>
+ <actuator name="hand_right_mrl_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ <joint name="hand_right_mrl_joint">
+ <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+ </joint>
+ </transmission>
+ <joint name="hand_right_thumb_abd_joint" type="revolute">
+ <parent link="hand_right_palm_link"/>
+ <child link="hand_right_thumb_abd_link"/>
+ <origin rpy="0 -1.1344640137963142 0" xyz="0.0901 0.0134 0.038"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="1.5707963267948966" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_right_thumb_abd_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="-0.0132 0.0037 -0.0017"/>
+ <mass value="0.014"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/thumb_abd_collision.stl" scale="1 1 1"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/thumb_abd_collision.stl" scale="1 1 1"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_right_thumb_abd_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_right_thumb_virtual_1_joint" type="revolute">
+ <parent link="hand_right_thumb_abd_link"/>
+ <child link="hand_right_thumb_virtual_1_link"/>
+ <origin rpy="-1.1442378576074825 0 0" xyz="0.0225 0.003 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_right_thumb_virtual_1_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <mass value="0.01"/>
+ <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_right_thumb_virtual_1_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_right_thumb_flex_1_joint" type="revolute">
+ <parent link="hand_right_thumb_virtual_1_link"/>
+ <child link="hand_right_thumb_flex_1_link"/>
+ <origin rpy="0 0 0" xyz="0.013 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_right_thumb_flex_1_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
+ <mass value="0.009"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_collision.stl"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_collision.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_right_thumb_flex_1_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_right_thumb_virtual_2_joint" type="revolute">
+ <parent link="hand_right_thumb_flex_1_link"/>
+ <child link="hand_right_thumb_virtual_2_link"/>
+ <origin rpy="0 0 0" xyz="0.009 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_right_thumb_virtual_2_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <mass value="0.01"/>
+ <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_right_thumb_virtual_2_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_right_thumb_flex_2_joint" type="revolute">
+ <parent link="hand_right_thumb_virtual_2_link"/>
+ <child link="hand_right_thumb_flex_2_link"/>
+ <origin rpy="0 0 0" xyz="0.013 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_right_thumb_flex_2_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0096 0.003 0.0"/>
+ <mass value="0.015"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_tip_collision.stl"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_tip_collision.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_right_thumb_flex_2_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_right_index_abd_joint" type="revolute">
+ <parent link="hand_right_palm_link"/>
+ <child link="hand_right_index_abd_link"/>
+ <origin rpy="-1.5707963267948966 -0.5235987755982988 0" xyz="0.1265 0.0042 0.0265"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="-0.5235987755982988" upper="0.5235987755982988" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_right_index_abd_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="-0.0018 0.0 0.0028"/>
+ <mass value="0.018"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_abd_collision.stl" scale="1 1 1"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_abd_collision.stl" scale="1 1 1"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_right_index_abd_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_right_index_virtual_1_joint" type="revolute">
+ <parent link="hand_right_index_abd_link"/>
+ <child link="hand_right_index_virtual_1_link"/>
+ <origin rpy="1.5707963267948966 0 0" xyz="0.009 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_right_index_virtual_1_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <mass value="0.01"/>
+ <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_right_index_virtual_1_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_right_index_flex_1_joint" type="revolute">
+ <parent link="hand_right_index_virtual_1_link"/>
+ <child link="hand_right_index_flex_1_link"/>
+ <origin rpy="0 0 0" xyz="0.013 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_right_index_flex_1_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
+ <mass value="0.009"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_collision.stl"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_collision.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_right_index_flex_1_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_right_index_virtual_2_joint" type="revolute">
+ <parent link="hand_right_index_flex_1_link"/>
+ <child link="hand_right_index_virtual_2_link"/>
+ <origin rpy="0 0 0" xyz="0.009 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_right_index_virtual_2_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <mass value="0.01"/>
+ <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_right_index_virtual_2_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_right_index_flex_2_joint" type="revolute">
+ <parent link="hand_right_index_virtual_2_link"/>
+ <child link="hand_right_index_flex_2_link"/>
+ <origin rpy="0 0 0" xyz="0.013 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_right_index_flex_2_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
+ <mass value="0.009"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_collision.stl"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_collision.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_right_index_flex_2_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_right_index_virtual_3_joint" type="revolute">
+ <parent link="hand_right_index_flex_2_link"/>
+ <child link="hand_right_index_virtual_3_link"/>
+ <origin rpy="0 0 0" xyz="0.009 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_right_index_virtual_3_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <mass value="0.01"/>
+ <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_right_index_virtual_3_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_right_index_flex_3_joint" type="revolute">
+ <parent link="hand_right_index_virtual_3_link"/>
+ <child link="hand_right_index_flex_3_link"/>
+ <origin rpy="0 0 0" xyz="0.013 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_right_index_flex_3_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0096 0.003 0.0"/>
+ <mass value="0.015"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_tip_collision.stl"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_tip_collision.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_right_index_flex_3_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_right_middle_abd_joint" type="revolute">
+ <parent link="hand_right_palm_link"/>
+ <child link="hand_right_middle_abd_link"/>
+ <origin rpy="-1.5707963267948966 0 0" xyz="0.1335 0.0042 0.007"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="-0.5235987755982988" upper="0.5235987755982988" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_right_middle_abd_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="-0.0018 0.0 0.0028"/>
+ <mass value="0.018"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_abd_collision.stl" scale="1 1 1"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_abd_collision.stl" scale="1 1 1"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_right_middle_abd_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_right_middle_virtual_1_joint" type="revolute">
+ <parent link="hand_right_middle_abd_link"/>
+ <child link="hand_right_middle_virtual_1_link"/>
+ <origin rpy="1.5707963267948966 0 0" xyz="0.009 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_right_middle_virtual_1_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <mass value="0.01"/>
+ <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_right_middle_virtual_1_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_right_middle_flex_1_joint" type="revolute">
+ <parent link="hand_right_middle_virtual_1_link"/>
+ <child link="hand_right_middle_flex_1_link"/>
+ <origin rpy="0 0 0" xyz="0.013 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_right_middle_flex_1_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
+ <mass value="0.009"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_collision.stl"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_collision.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_right_middle_flex_1_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_right_middle_virtual_2_joint" type="revolute">
+ <parent link="hand_right_middle_flex_1_link"/>
+ <child link="hand_right_middle_virtual_2_link"/>
+ <origin rpy="0 0 0" xyz="0.009 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_right_middle_virtual_2_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <mass value="0.01"/>
+ <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_right_middle_virtual_2_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_right_middle_flex_2_joint" type="revolute">
+ <parent link="hand_right_middle_virtual_2_link"/>
+ <child link="hand_right_middle_flex_2_link"/>
+ <origin rpy="0 0 0" xyz="0.013 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_right_middle_flex_2_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
+ <mass value="0.009"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_collision.stl"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_collision.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_right_middle_flex_2_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_right_middle_virtual_3_joint" type="revolute">
+ <parent link="hand_right_middle_flex_2_link"/>
+ <child link="hand_right_middle_virtual_3_link"/>
+ <origin rpy="0 0 0" xyz="0.009 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_right_middle_virtual_3_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <mass value="0.01"/>
+ <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_right_middle_virtual_3_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_right_middle_flex_3_joint" type="revolute">
+ <parent link="hand_right_middle_virtual_3_link"/>
+ <child link="hand_right_middle_flex_3_link"/>
+ <origin rpy="0 0 0" xyz="0.013 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_right_middle_flex_3_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0096 0.003 0.0"/>
+ <mass value="0.015"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_tip_collision.stl"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_tip_collision.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_right_middle_flex_3_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_right_ring_abd_joint" type="revolute">
+ <parent link="hand_right_palm_link"/>
+ <child link="hand_right_ring_abd_link"/>
+ <origin rpy="-1.5707963267948966 0.2617993877991494 0" xyz="0.1265 0.0042 -0.0125"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="-0.5235987755982988" upper="0.5235987755982988" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_right_ring_abd_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="-0.0018 0.0 0.0028"/>
+ <mass value="0.018"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_abd_collision.stl" scale="1 1 1"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_abd_collision.stl" scale="1 1 1"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_right_ring_abd_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_right_ring_virtual_1_joint" type="revolute">
+ <parent link="hand_right_ring_abd_link"/>
+ <child link="hand_right_ring_virtual_1_link"/>
+ <origin rpy="1.5707963267948966 0 0" xyz="0.009 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_right_ring_virtual_1_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <mass value="0.01"/>
+ <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_right_ring_virtual_1_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_right_ring_flex_1_joint" type="revolute">
+ <parent link="hand_right_ring_virtual_1_link"/>
+ <child link="hand_right_ring_flex_1_link"/>
+ <origin rpy="0 0 0" xyz="0.013 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_right_ring_flex_1_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
+ <mass value="0.009"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_collision.stl"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_collision.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_right_ring_flex_1_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_right_ring_virtual_2_joint" type="revolute">
+ <parent link="hand_right_ring_flex_1_link"/>
+ <child link="hand_right_ring_virtual_2_link"/>
+ <origin rpy="0 0 0" xyz="0.009 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_right_ring_virtual_2_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <mass value="0.01"/>
+ <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_right_ring_virtual_2_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_right_ring_flex_2_joint" type="revolute">
+ <parent link="hand_right_ring_virtual_2_link"/>
+ <child link="hand_right_ring_flex_2_link"/>
+ <origin rpy="0 0 0" xyz="0.013 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_right_ring_flex_2_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
+ <mass value="0.009"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_collision.stl"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_collision.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_right_ring_flex_2_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_right_ring_virtual_3_joint" type="revolute">
+ <parent link="hand_right_ring_flex_2_link"/>
+ <child link="hand_right_ring_virtual_3_link"/>
+ <origin rpy="0 0 0" xyz="0.009 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_right_ring_virtual_3_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <mass value="0.01"/>
+ <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_right_ring_virtual_3_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_right_ring_flex_3_joint" type="revolute">
+ <parent link="hand_right_ring_virtual_3_link"/>
+ <child link="hand_right_ring_flex_3_link"/>
+ <origin rpy="0 0 0" xyz="0.013 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_right_ring_flex_3_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0096 0.003 0.0"/>
+ <mass value="0.015"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_tip_collision.stl"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_tip_collision.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_right_ring_flex_3_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_right_little_abd_joint" type="revolute">
+ <parent link="hand_right_palm_link"/>
+ <child link="hand_right_little_abd_link"/>
+ <origin rpy="-1.5707963267948966 0.5235987755982988 0" xyz="0.1165 0.0042 -0.032"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="-0.5235987755982988" upper="0.5235987755982988" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_right_little_abd_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="-0.0018 0.0 0.0028"/>
+ <mass value="0.018"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_abd_collision.stl" scale="1 1 1"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_abd_collision.stl" scale="1 1 1"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_right_little_abd_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_right_little_virtual_1_joint" type="revolute">
+ <parent link="hand_right_little_abd_link"/>
+ <child link="hand_right_little_virtual_1_link"/>
+ <origin rpy="1.5707963267948966 0 0" xyz="0.009 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_right_little_virtual_1_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <mass value="0.01"/>
+ <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_right_little_virtual_1_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_right_little_flex_1_joint" type="revolute">
+ <parent link="hand_right_little_virtual_1_link"/>
+ <child link="hand_right_little_flex_1_link"/>
+ <origin rpy="0 0 0" xyz="0.013 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_right_little_flex_1_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
+ <mass value="0.009"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_collision.stl"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_collision.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_right_little_flex_1_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_right_little_virtual_2_joint" type="revolute">
+ <parent link="hand_right_little_flex_1_link"/>
+ <child link="hand_right_little_virtual_2_link"/>
+ <origin rpy="0 0 0" xyz="0.009 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_right_little_virtual_2_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <mass value="0.01"/>
+ <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_right_little_virtual_2_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_right_little_flex_2_joint" type="revolute">
+ <parent link="hand_right_little_virtual_2_link"/>
+ <child link="hand_right_little_flex_2_link"/>
+ <origin rpy="0 0 0" xyz="0.013 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_right_little_flex_2_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
+ <mass value="0.009"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_collision.stl"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_collision.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_right_little_flex_2_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_right_little_virtual_3_joint" type="revolute">
+ <parent link="hand_right_little_flex_2_link"/>
+ <child link="hand_right_little_virtual_3_link"/>
+ <origin rpy="0 0 0" xyz="0.009 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_right_little_virtual_3_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <mass value="0.01"/>
+ <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.013 0.002 0.012"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_right_little_virtual_3_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <joint name="hand_right_little_flex_3_joint" type="revolute">
+ <parent link="hand_right_little_virtual_3_link"/>
+ <child link="hand_right_little_flex_3_link"/>
+ <origin rpy="0 0 0" xyz="0.013 0 0"/>
+ <axis xyz="0 0 1"/>
+ <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+ <dynamics damping="0.5" friction="0.0"/>
+ </joint>
+ <link name="hand_right_little_flex_3_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0.0096 0.003 0.0"/>
+ <mass value="0.015"/>
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+ <!-- To stabilize simulation -->
+ </inertial>
+ <visual>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_tip_collision.stl"/>
+ </geometry>
+ <material name="Hey5DarkGrey"/>
+ </visual>
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://example-robot-data/robots/tiago_description/meshes/hey5/finger_flex_tip_collision.stl"/>
+ </geometry>
+ </collision>
+ </link>
+ <gazebo reference="hand_right_little_flex_3_link">
+ <material>Gazebo/Black</material>
+ </gazebo>
+ <!--Abductor-->
+ <transmission name="hand_right_thumb_abd_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_right_thumb_abd_joint">
+ <saturationLower>0.0</saturationLower>
+ <saturationUpper>1.5707963267948966</saturationUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_right_thumb_motor">
+ <mechanicalReduction>0.6544984694978736</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <!--First flexor-->
+ <transmission name="hand_right_thumb_virtual_1_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_right_thumb_virtual_1_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>2.0</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_right_thumb_motor">
+ <mechanicalReduction>0.16299357854713997</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="hand_right_thumb_flex_1_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_right_thumb_flex_1_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>2.0</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_right_thumb_motor">
+ <mechanicalReduction>0.16299357854713997</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <!--Second flexor-->
+ <transmission name="hand_right_thumb_virtual_2_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_right_thumb_virtual_2_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>2.0</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_right_thumb_motor">
+ <mechanicalReduction>0.16299357854713997</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="hand_right_thumb_flex_2_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_right_thumb_flex_2_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>2.0</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_right_thumb_motor">
+ <mechanicalReduction>0.16299357854713997</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <!--Abductor-->
+ <transmission name="hand_right_index_abd_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_right_index_abd_joint">
+ <saturationLower>0.0</saturationLower>
+ <saturationUpper>0.5235987755982988</saturationUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_right_index_motor">
+ <mechanicalReduction>0.49866550056980846</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <!--First flexor-->
+ <transmission name="hand_right_index_virtual_1_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_right_index_virtual_1_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.7</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_right_index_motor">
+ <mechanicalReduction>0.10288861079760754</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="hand_right_index_flex_1_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_right_index_flex_1_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.7</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_right_index_motor">
+ <mechanicalReduction>0.10288861079760754</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <!--Second flexor-->
+ <transmission name="hand_right_index_virtual_2_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_right_index_virtual_2_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.7</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_right_index_motor">
+ <mechanicalReduction>0.10288861079760754</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="hand_right_index_flex_2_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_right_index_flex_2_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.7</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_right_index_motor">
+ <mechanicalReduction>0.10288861079760754</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <!--Third flexor-->
+ <transmission name="hand_right_index_virtual_3_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_right_index_virtual_3_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.7</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_right_index_motor">
+ <mechanicalReduction>0.10288861079760754</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="hand_right_index_flex_3_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_right_index_flex_3_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.7</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_right_index_motor">
+ <mechanicalReduction>0.10288861079760754</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <!--Abductor-->
+ <transmission name="hand_right_middle_abd_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_right_middle_abd_joint">
+ <saturationLower>0.0</saturationLower>
+ <saturationUpper>0.0</saturationUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_right_mrl_motor">
+ <mechanicalReduction>1.0</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <!--First flexor-->
+ <transmission name="hand_right_middle_virtual_1_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_right_middle_virtual_1_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.0</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_right_mrl_motor">
+ <mechanicalReduction>0.06792452830188679</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="hand_right_middle_flex_1_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_right_middle_flex_1_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.0</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_right_mrl_motor">
+ <mechanicalReduction>0.06792452830188679</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <!--Second flexor-->
+ <transmission name="hand_right_middle_virtual_2_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_right_middle_virtual_2_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.0</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_right_mrl_motor">
+ <mechanicalReduction>0.06792452830188679</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="hand_right_middle_flex_2_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_right_middle_flex_2_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.0</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_right_mrl_motor">
+ <mechanicalReduction>0.06792452830188679</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <!--Third flexor-->
+ <transmission name="hand_right_middle_virtual_3_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_right_middle_virtual_3_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.0</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_right_mrl_motor">
+ <mechanicalReduction>0.06792452830188679</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="hand_right_middle_flex_3_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_right_middle_flex_3_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.0</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_right_mrl_motor">
+ <mechanicalReduction>0.06792452830188679</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <!--Abductor-->
+ <transmission name="hand_right_ring_abd_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_right_ring_abd_joint">
+ <saturationLower>-0.2617993877991494</saturationLower>
+ <saturationUpper>0.0</saturationUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_right_mrl_motor">
+ <mechanicalReduction>-0.23271056693257727</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <!--First flexor-->
+ <transmission name="hand_right_ring_virtual_1_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_right_ring_virtual_1_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.75</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_right_mrl_motor">
+ <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="hand_right_ring_flex_1_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_right_ring_flex_1_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.75</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_right_mrl_motor">
+ <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <!--Second flexor-->
+ <transmission name="hand_right_ring_virtual_2_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_right_ring_virtual_2_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.75</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_right_mrl_motor">
+ <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="hand_right_ring_flex_2_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_right_ring_flex_2_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.75</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_right_mrl_motor">
+ <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <!--Third flexor-->
+ <transmission name="hand_right_ring_virtual_3_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_right_ring_virtual_3_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.75</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_right_mrl_motor">
+ <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="hand_right_ring_flex_3_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_right_ring_flex_3_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.75</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_right_mrl_motor">
+ <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <!--Abductor-->
+ <transmission name="hand_right_little_abd_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_right_little_abd_joint">
+ <saturationLower>-0.5235987755982988</saturationLower>
+ <saturationUpper>0.0</saturationUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_right_mrl_motor">
+ <mechanicalReduction>-0.46542113386515455</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <!--First flexor-->
+ <transmission name="hand_right_little_virtual_1_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_right_little_virtual_1_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.75</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_right_mrl_motor">
+ <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="hand_right_little_flex_1_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_right_little_flex_1_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.75</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_right_mrl_motor">
+ <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <!--Second flexor-->
+ <transmission name="hand_right_little_virtual_2_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_right_little_virtual_2_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.75</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_right_mrl_motor">
+ <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="hand_right_little_flex_2_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_right_little_flex_2_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.75</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_right_mrl_motor">
+ <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <!--Third flexor-->
+ <transmission name="hand_right_little_virtual_3_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_right_little_virtual_3_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.75</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_right_mrl_motor">
+ <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <transmission name="hand_right_little_flex_3_trans">
+ <type>hey5_transmissions/Hey5Reducer</type>
+ <joint name="hand_right_little_flex_3_joint">
+ <saturationLower>0.0</saturationLower>
+ <deadBandUpper>0.75</deadBandUpper>
+ <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+ </joint>
+ <actuator name="hand_right_mrl_motor">
+ <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+ </actuator>
+ </transmission>
+ <link name="hand_right_grasping_frame">
+ <inertial>
+ <mass value="0.0"/>
+ <origin xyz="0 0 0"/>
+ <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0" izz="0.0"/>
+ </inertial>
+ </link>
+ <joint name="hand_right_grasping_fixed_joint" type="fixed">
+ <parent link="hand_right_palm_link"/>
+ <child link="hand_right_grasping_frame"/>
+ <axis xyz="0 0 1"/>
+ <origin rpy="0 0 0" xyz="0.13 0.02 0"/>
+ </joint>
+ <gazebo>
+ <plugin filename="libgazebo_underactuated_finger.so" name="gazebo_hand_right_thumb">
+ <actuatedJoint>hand_right_thumb_joint</actuatedJoint>
+ <virtualJoint>
+ <name>hand_right_thumb_abd_joint</name>
+ <scale_factor>2.0</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_right_thumb_flex_1_joint</name>
+ <scale_factor>2.5</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_right_thumb_virtual_1_joint</name>
+ <scale_factor>3.0</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_right_thumb_flex_2_joint</name>
+ <scale_factor>3.5</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_right_thumb_virtual_2_joint</name>
+ <scale_factor>3.0</scale_factor>
+ </virtualJoint>
+ </plugin>
+ <plugin filename="libgazebo_underactuated_finger.so" name="gazebo_hand_right_index">
+ <actuatedJoint>hand_right_index_joint</actuatedJoint>
+ <virtualJoint>
+ <name>hand_right_index_abd_joint</name>
+ <scale_factor>5.0</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_right_index_flex_1_joint</name>
+ <scale_factor>2.5</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_right_index_virtual_1_joint</name>
+ <scale_factor>3.0</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_right_index_flex_2_joint</name>
+ <scale_factor>3.5</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_right_index_virtual_2_joint</name>
+ <scale_factor>3.5</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_right_index_flex_3_joint</name>
+ <scale_factor>4.0</scale_factor>
+ </virtualJoint>
+ </plugin>
+ <plugin filename="libgazebo_underactuated_finger.so" name="gazebo_hand_right_middle">
+ <actuatedJoint>hand_right_mrl_joint</actuatedJoint>
+ <virtualJoint>
+ <name>hand_right_middle_flex_1_joint</name>
+ <scale_factor>2.5</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_right_middle_virtual_1_joint</name>
+ <scale_factor>3.0</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_right_middle_flex_2_joint</name>
+ <scale_factor>3.5</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_right_middle_virtual_2_joint</name>
+ <scale_factor>3.5</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_right_middle_flex_3_joint</name>
+ <scale_factor>4.0</scale_factor>
+ </virtualJoint>
+ </plugin>
+ <plugin filename="libgazebo_underactuated_finger.so" name="gazebo_hand_right_ring">
+ <actuatedJoint>hand_right_mrl_joint</actuatedJoint>
+ <virtualJoint>
+ <name>hand_right_ring_flex_1_joint</name>
+ <scale_factor>2.5</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_right_ring_virtual_1_joint</name>
+ <scale_factor>3.0</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_right_ring_flex_2_joint</name>
+ <scale_factor>3.5</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_right_ring_virtual_2_joint</name>
+ <scale_factor>3.5</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_right_ring_flex_3_joint</name>
+ <scale_factor>4.0</scale_factor>
+ </virtualJoint>
+ </plugin>
+ <plugin filename="libgazebo_underactuated_finger.so" name="gazebo_hand_right_little">
+ <actuatedJoint>hand_right_mrl_joint</actuatedJoint>
+ <virtualJoint>
+ <name>hand_right_little_flex_1_joint</name>
+ <scale_factor>2.5</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_right_little_virtual_1_joint</name>
+ <scale_factor>3.0</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_right_little_flex_2_joint</name>
+ <scale_factor>3.5</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_right_little_virtual_2_joint</name>
+ <scale_factor>3.5</scale_factor>
+ </virtualJoint>
+ <virtualJoint>
+ <name>hand_right_little_flex_3_joint</name>
+ <scale_factor>4.0</scale_factor>
+ </virtualJoint>
+ </plugin>
+ </gazebo>
+ <joint name="hand_right_safety_box_joint" type="fixed">
+ <parent link="wrist_right_ft_tool_link"/>
+ <child link="hand_right_safety_box"/>
+ <origin rpy="0 0 0" xyz="0.1 0.02 0.02"/>
+ </joint>
+ <link name="hand_right_safety_box">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.0001"/>
+ <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
+ </inertial>
+ <!--
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <box size="0.2 0.115 0.18" />
+ </geometry>
+ </visual>
+ -->
+ <collision>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <box size="0.2 0.115 0.18"/>
+ </geometry>
+ </collision>
+ </link>
+ <!-- RGBD Laser Link -->
+ <joint name="rgbd_laser_joint" type="fixed">
+ <parent link="base_footprint"/>
+ <child link="rgbd_laser_link"/>
+ <origin rpy="0 0 0" xyz="-0.9 0 0"/>
+ </joint>
+ <link name="rgbd_laser_link">
+ <inertial>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ <mass value="0.0001"/>
+ <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
+ </inertial>
+ </link>
+</robot>
diff --git a/unittest/test_load.py b/unittest/test_load.py
index 8a537b81605e12bc8b6a0e06ac72fb46a4b55267..37af25534a264a24d807bbae12f80b00d42f6005 100755
--- a/unittest/test_load.py
+++ b/unittest/test_load.py
@@ -100,6 +100,9 @@ class RobotTestCase(unittest.TestCase):
def test_tiago(self):
self.check('tiago', 50, 48)
+ def test_tiago_dual(self):
+ self.check('tiago_dual', 111, 101)
+
def test_tiago_no_hand(self):
self.check('tiago_no_hand', 14, 12)