Add robotiq_2f85_v4.

PiperOrigin-RevId: 701995260 Change-Id: I20508e0da1b3b9ccfec498c1bc8cb7e2b4b77f87
google-deepmind · Dec 2, 2024 · 5c7ddec · 5c7ddec
1 parent a88bc45
commit 5c7ddec
Show file tree

Hide file tree

Showing 19 changed files with 308 additions and 0 deletions.
diff --git a/LICENSE b/LICENSE
@@ -2784,6 +2784,35 @@ ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 
+================================================================================
+License for contents in the directory 'robotiq_2f85_v4/'
+================================================================================
+
+Copyright (c) 2013, ROS-Industrial
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+
+  Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+  Redistributions in binary form must reproduce the above copyright notice, this
+  list of conditions and the following disclaimer in the documentation and/or
+  other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+
 ================================================================================
 License for contents in the directory 'robotis_op3/'
 ================================================================================

diff --git a/robotiq_2f85_v4/2f85.png b/robotiq_2f85_v4/2f85.png
diff --git a/robotiq_2f85_v4/2f85.xml b/robotiq_2f85_v4/2f85.xml
@@ -0,0 +1,181 @@
+<mujoco model="Dual_wrist_camera">
+  <compiler angle="radian" meshdir="assets" autolimits="true"/>
+
+  <option cone="elliptic" impratio="10"/>
+
+  <asset>
+
+    <material name="metal" rgba="0.58 0.58 0.58 1"/>
+    <material name="silicone" rgba="0.1882 0.1882 0.1882 1"/>
+    <material name="black" rgba="0.149 0.149 0.149 1"/>
+
+    <mesh file="base.stl"/>
+    <mesh file="base_coupling.stl"/>
+    <mesh file="c-a01-85-open.stl"/>
+    <mesh file="driver.stl"/>
+    <mesh file="coupler.stl"/>
+    <mesh file="spring_link.stl"/>
+    <mesh file="follower.stl"/>
+    <mesh file="tongue.stl"/>
+  </asset>
+
+
+  <default>
+    <default class="2f85">
+      <mesh scale="0.001 0.001 0.001"/>
+      <general biastype="affine"/>
+
+      <joint axis="0 0 1"/>
+      <default class="driver">
+        <joint range="0 0.9" armature="0.005" damping="0.1" solimplimit="0.95 0.99 0.001" solreflimit="0.005 1"/>
+      </default>
+      <default class="follower">
+        <joint range="-0.872664 0.9" armature="0.001"  solimplimit="0.95 0.99 0.001" solreflimit="0.005 1"/>
+      </default>
+      <default class="spring_link">
+        <joint range="-0.29670597283 0.9" armature="0.001" stiffness="0.05" springref="2.62" damping="0.00125"/>
+      </default>
+      <default class="coupler">
+        <joint range="-1.57 0" armature="0.001" solimplimit="0.95 0.99 0.001" solreflimit="0.005 1"/>
+      </default>
+
+      <default class="visual">
+        <geom type="mesh" contype="0" conaffinity="0" group="2" material="black"/>
+      </default>
+      <default class="collision">
+        <geom type="mesh" group="3"/>
+        <default class="pad_box1">
+          <geom mass="1e-6" type="box" pos="0.043258  0 0.12"  size="0.002 0.011 0.009375" friction="0.7"
+            solimp="0.95 0.99 0.001" solref="0.004 1" priority="1" rgba="0.55 0.55 0.55 1"/>
+        </default>
+        <default class="pad_box2">
+          <geom mass="1e-6" type="box" pos="0.043258 0 0.13875" size="0.002 0.011 0.009375" friction="0.6"
+            solimp="0.95 0.99 0.001" solref="0.004 1" priority="1" rgba="0.45 0.45 0.45 1"/>
+        </default>
+      </default>
+    </default>
+  </default>
+
+
+  <worldbody>
+    <body name="base" childclass="2f85">
+      <inertial mass="0.777441" pos="0 -2.70394e-05 0.0354675" quat="1 -0.00152849 0 0"
+        diaginertia="0.000260285 0.000225381 0.000152708"/>
+      <geom class="visual" pos="0 0 0.0108" quat="0 0 0 1"   mesh="base"/>
+      <geom class="visual" pos="0 0 0.004" quat="1 -1 0 0"   mesh="base_coupling"/>
+      <geom class="visual" pos="0 0 0.0108" quat="1 0 0 0"  material="metal" mesh="c-a01-85-open"/>
+      <geom class="collision" mesh="base"/>
+      <!-- Left-hand side 4-bar linkage -->
+      <body name="left_driver" pos="-0.0306011 0.00475 0.0657045" quat="1 -1 0 0">
+        <inertial mass="0.00899563" pos="0 0.0177547 0.00107314" quat="0.681301 0.732003 0 0"
+            diaginertia="1.72352e-06 1.60906e-06 3.22006e-07"/>
+        <joint name="left_driver_joint" class="driver"/>
+        <geom class="visual" pos="0.0306011 0.0549045 -0.0047" quat="1 1 0 0"  material="metal" mesh="driver"/>
+        <geom class="collision" pos="0.0306011 0.0549045 -0.0047" quat="1 1 0 0"   mesh="driver"/>
+        <body name="left_coupler" pos="-0.0314249 0.00453223 -0.0102" quat="0 0 0 1">
+          <inertial mass="0.0140974" pos="0 0.00301209 0.0232175" quat="0.705636 -0.0455904 0.0455904 0.705636"
+          diaginertia="4.16206e-06 3.52216e-06 8.88131e-07"/>
+          <geom class="visual" pos="-0.062026 -0.0503723 0.0055" quat="1 -1 0 0"   mesh="coupler"/>
+          <geom class="collision" pos="-0.062026 -0.0503723 0.0055" quat="1 -1 0 0"  mesh="coupler"/>
+        </body>
+      </body>
+      <body name="left_spring_link" pos="-0.0127 -0.012 0.07222" quat="1 -1 0 0">
+        <inertial mass="0.0221642" pos="-8.65005e-09 0.0181624 0.0212658" quat="0.663403 -0.244737 0.244737 0.663403"
+            diaginertia="8.96853e-06 6.71733e-06 2.63931e-06"/>
+        <joint name="left_spring_link_joint" class="spring_link"/>
+        <geom class="visual" pos="0.0127 0.06142 0.01205" quat="1 1 0 0" type="mesh"  mesh="spring_link"/>
+        <geom class="collision" pos="0.0127 0.06142 0.01205" quat="1 1 0 0" type="mesh"  mesh="spring_link"/>
+        <body name="left_follower" pos="-0.0382079 -0.0425003 0.00295" quat="0 -1 -1.90231e-05 0">
+          <inertial mass="0.0125222" pos="0 -0.011046 0.0124786" quat="1 0.1664 0 0"
+          diaginertia="2.67415e-06 2.4559e-06 6.02031e-07"/>
+          <joint name="left_follower" class="follower"/>
+          <geom class="visual" pos="0.0509079 -0.10392 -0.0091" quat="1 -1 0 0" type="mesh"  mesh="follower"/>
+          <geom class="visual" pos="0.0509079 -0.10392 -0.0091" quat="1 -1 0 0" type="mesh" material="metal" mesh="tongue"/>
+          <geom class="collision" pos="0.0509079 -0.10392 -0.0091" quat="1 -1 0 0" type="mesh"  mesh="follower"/>
+          <geom class="collision" pos="0.0509079 -0.10392 -0.0091" quat="1 -1 0 0" type="mesh"  mesh="tongue"/>
+          <body name="left_pad" pos="-0.0377897 -0.103916 -0.0091" quat="1 -1 0 0" >
+            <geom class="pad_box1" name="left_pad1"/>
+            <geom class="pad_box2" name="left_pad2"/>
+          </body>
+        </body>
+      </body>
+      <!-- Right-hand side 4-bar linkage -->
+      <body name="right_driver" pos="0.0306011 -0.00475 0.0657045" quat="0 0 -1 1">
+        <inertial mass="0.00899563" pos="2.96931e-12 0.0177547 0.00107314" quat="0.681301 0.732003 0 0"
+        diaginertia="1.72352e-06 1.60906e-06 3.22006e-07"/>
+        <joint name="right_driver_joint" class="driver"/>
+        <geom class="visual" pos="0.0306011 0.0549045 -0.0047" quat="1 1 0 0" material="metal" mesh="driver"/>
+        <geom class="collision" pos="0.0306011 0.0549045 -0.0047" quat="1 1 0 0"  mesh="driver"/>
+        <body name="right_coupler" pos="-0.0314249 0.00453223 -0.0102" quat="0 0 0 1">
+          <inertial mass="0.0140974" pos="0 0.00301209 0.0232175" quat="0.705636 -0.0455904 0.0455904 0.705636"
+          diaginertia="4.16206e-06 3.52216e-06 8.88131e-07"/>
+          <geom class="visual" pos="-0.062026 -0.0503723 0.0055" quat="1 -1 0 0"   mesh="coupler"/>
+          <geom class="collision" pos="-0.062026 -0.0503723 0.0055" quat="1 -1 0 0"   mesh="coupler"/>
+        </body>
+      </body>
+      <body name="right_spring_link" pos="0.0127 0.012 0.07222" quat="0 0 -1 1">
+        <inertial mass="0.0221642" pos="-8.65005e-09 0.0181624 0.0212658" quat="0.663403 -0.244737 0.244737 0.663403"
+        diaginertia="8.96853e-06 6.71733e-06 2.63931e-06"/>
+        <joint name="right_spring_link_joint" class="spring_link"/>
+        <geom class="visual" pos="0.0127 0.06142 0.01205" quat="1 1 0 0"   mesh="spring_link"/>
+        <geom class="collision" pos="0.0127 0.06142 0.01205" quat="1 1 0 0"   mesh="spring_link"/>
+        <body name="right_follower" pos="-0.0382079 -0.0425003 0.00295" quat="0 -1 0 0">
+          <inertial mass="0.0125222" pos="0 -0.011046 0.0124786" quat="1 0.1664 0 0"
+          diaginertia="2.67415e-06 2.4559e-06 6.02031e-07"/>
+          <joint name="right_follower_joint" class="follower"/>
+          <geom class="visual" pos="0.0509079 -0.10392 -0.0091" quat="1 -1 0 0"  material="metal" mesh="tongue"/>
+          <geom class="visual" pos="0.0509079 -0.10392 -0.0091" quat="1 -1 0 0"   mesh="follower"/>
+          <geom class="collision" pos="0.0509079 -0.10392 -0.0091" quat="1 -1 0 0"   mesh="tongue"/>
+          <geom class="collision" pos="0.0509079 -0.10392 -0.0091" quat="1 -1 0 0"   mesh="follower"/>
+          <body name="right_pad" pos="-0.0377897 -0.103916 -0.0091"   quat="1 -1 0 0" >
+            <geom class="pad_box1" name="right_pad1"/>
+            <geom class="pad_box2" name="right_pad2"/>
+          </body>
+        </body>
+      </body>
+    </body>
+  </worldbody>
+
+  <contact>
+    <exclude body1="base" body2="left_driver"/>
+    <exclude body1="base" body2="right_driver"/>
+    <exclude body1="base" body2="left_spring_link"/>
+    <exclude body1="base" body2="right_spring_link"/>
+    <exclude body1="right_coupler" body2="right_follower"/>
+    <exclude body1="left_coupler" body2="left_follower"/>
+  </contact>
+
+  <!--
+    This adds stability to the model by having a tendon that distributes the forces between both
+    joints, such that the equality constraint doesn't have to do that much work in order to equalize
+    both joints. Since both joints share the same sign, we split the force between both equally by
+    setting coef=0.485
+  -->
+  <tendon>
+    <fixed name="split">
+      <joint joint="right_driver_joint" coef="0.485"/>
+      <joint joint="left_driver_joint" coef="0.485"/>
+    </fixed>
+  </tendon>
+
+  <equality>
+    <connect anchor="-0.0179014 -0.00651468 0.0044" body1="right_follower" body2="right_coupler" solimp="0.95 0.99 0.001" solref="0.005 1"/>
+    <connect anchor="-0.0179014 -0.00651468 0.0044" body1="left_follower" body2="left_coupler" solimp="0.95 0.99 0.001" solref="0.005 1"/>
+    <joint joint1="right_driver_joint" joint2="left_driver_joint" polycoef="0 1 0 0 0" solimp="0.95 0.99 0.001"
+    solref="0.005 1"/>
+  </equality>
+
+  <!--
+    The general actuator below is a customized position actuator (with some damping) where
+    gainprm[0] != kp (see http://mujoco.org/book/modeling.html#position).
+    The reason why gainprm[0] != kp is because the control input range has to be re-scaled to
+    [0, 255]. The joint range is currently set at [0, 0.8], the control range is [0, 255] and
+    kp = 100. Tau = Kp * scale * control_input - Kp * error, max(Kp * scale * control_input) = 0.8,
+    hence scale = 0.8 * 100 / 255
+  -->
+  <actuator>
+    <general class="2f85" name="fingers_actuator" tendon="split" forcerange="-5 5" ctrlrange="0 255"
+      gainprm="0.3137255 0 0" biasprm="0 -100 -10"/>
+  </actuator>
+
+</mujoco>
diff --git a/robotiq_2f85_v4/LICENSE b/robotiq_2f85_v4/LICENSE
@@ -0,0 +1,23 @@
+Copyright (c) 2013, ROS-Industrial
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+
+  Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+  Redistributions in binary form must reproduce the above copyright notice, this
+  list of conditions and the following disclaimer in the documentation and/or
+  other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
diff --git a/robotiq_2f85_v4/README.md b/robotiq_2f85_v4/README.md
@@ -0,0 +1,35 @@
+# Robotiq 2F-85 Description (MJCF)
+
+Requires MuJoCo 2.2.2 or later.
+
+## Overview
+
+This package contains a simplified robot description (MJCF) of the [Robotiq 85mm
+2-Finger Adaptive
+Gripper](https://robotiq.com/products/2f85-140-adaptive-robot-gripper) developed
+by [Robotiq](https://robotiq.com/). It is derived from the [publicly available
+URDF
+description](https://github.com/ros-industrial/robotiq/tree/kinetic-devel/robotiq_2f_85_gripper_visualization).
+
+<p float="left">
+  <img src="2f85.png" width="400">
+</p>
+
+## URDF → MJCF derivation steps
+
+1. Downloaded gripper step file containing CAD information: https://blog.robotiq.com/hubfs/support-files/2F85_Opened_20190924-Sep-06-2024-02-25-46-4707-PM.step
+2. Downloaded fingertips step file: https://blog.robotiq.com/hubfs/support-files/C-083-PAD_SURMOULE_85_defeature_20191025-Sep-06-2024-02-26-08-4297-PM.step
+3. Created assembly file using onshape and joint limits from robotiq_2f85 XML model.
+4. Used onshape-to-robot to export assembly file to URDF : https://github.com/Rhoban/onshape-to-robot
+5. Converted URDF to MJCF by loading the model in simulate and saved the XML.
+7. Used robotiq_2f85 XML as inspiration. Kept new XML very similar to the robotiq_2f85.
+8. Changed value of tendon coef from 0.5 to 0.485 to make sure that fingertips touch.
+7. Used inertia and mass from robotiq_2f85 model.
+9. Added `scene.xml` which includes the robot, with a textured groundplane, skybox, and haze.
+10. Added hanging box to `scene.xml`.
+
+## License
+Here is the term and condition link:
+https://robotiq.com/fr/termes
+Have no idea don't speak the language
+
diff --git a/robotiq_2f85_v4/assets/base.stl b/robotiq_2f85_v4/assets/base.stl
diff --git a/robotiq_2f85_v4/assets/base_coupling.stl b/robotiq_2f85_v4/assets/base_coupling.stl
diff --git a/robotiq_2f85_v4/assets/c-a01-85-open.stl b/robotiq_2f85_v4/assets/c-a01-85-open.stl
diff --git a/robotiq_2f85_v4/assets/coupler.stl b/robotiq_2f85_v4/assets/coupler.stl
diff --git a/robotiq_2f85_v4/assets/driver.stl b/robotiq_2f85_v4/assets/driver.stl
diff --git a/robotiq_2f85_v4/assets/follower.stl b/robotiq_2f85_v4/assets/follower.stl
diff --git a/robotiq_2f85_v4/assets/pad.stl b/robotiq_2f85_v4/assets/pad.stl
diff --git a/robotiq_2f85_v4/assets/robotiq_fts300.stl b/robotiq_2f85_v4/assets/robotiq_fts300.stl
diff --git a/robotiq_2f85_v4/assets/robotiq_fts300_base.stl b/robotiq_2f85_v4/assets/robotiq_fts300_base.stl
diff --git a/robotiq_2f85_v4/assets/robotiq_fts300_coupling.stl b/robotiq_2f85_v4/assets/robotiq_fts300_coupling.stl
diff --git a/robotiq_2f85_v4/assets/robotiq_fts300_top.stl b/robotiq_2f85_v4/assets/robotiq_fts300_top.stl
diff --git a/robotiq_2f85_v4/assets/spring_link.stl b/robotiq_2f85_v4/assets/spring_link.stl
diff --git a/robotiq_2f85_v4/assets/tongue.stl b/robotiq_2f85_v4/assets/tongue.stl
diff --git a/robotiq_2f85_v4/scene.xml b/robotiq_2f85_v4/scene.xml
@@ -0,0 +1,40 @@
+<mujoco model="2f85 scene">
+  <include file="2f85.xml"/>
+
+  <!-- Add some fluid viscosity to prevent the hanging box from jiggling forever -->
+  <option viscosity="0.1"/>
+
+  <statistic center="0 0 0.05" extent="0.3"/>
+
+  <visual>
+    <headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0"/>
+    <rgba haze="0.15 0.25 0.35 1"/>
+    <global azimuth="60" elevation="-20"/>
+  </visual>
+
+  <asset>
+    <texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072"/>
+    <texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3"
+      markrgb="0.8 0.8 0.8" width="300" height="300"/>
+    <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2"/>
+  </asset>
+
+  <worldbody>
+    <light pos="0 0 1"/>
+    <light pos="0 -0.2 1" dir="0 0.2 -0.8" directional="true"/>
+    <geom name="floor" size="0 0 0.05" type="plane" material="groundplane"/>
+    <site size="0.002" pos="0 0 0.2" name="anchor"/>
+    <body name="object" pos="0 0 0.15">
+      <freejoint/>
+      <geom type="box" size="0.015 0.015 0.015" rgba=".5 .7 .5 1" friction=".5" priority="1"/>
+      <site size="0.002" pos="0.015 0.015 0.015" name="hook"/>
+    </body>
+  </worldbody>
+
+  <tendon>
+    <spatial limited="true" range="0 0.02" width="0.001">
+      <site site="hook"/>
+      <site site="anchor"/>
+    </spatial>
+  </tendon>
+</mujoco>