Tutorial: making a soft gripper

You can find more tutorial files listed here: https://project.inria.fr/softrobot/install-get-started-2/tutorial/

In this tutorial, we will see how to use our framework to create and use a mechanical simulation of a soft robot. As an example, we choose to model the soft gripper made by Taimoor Hassan and his co-authors in [Manti et al. 15] and [Hassan et al. 15]. As visible in the picture, the soft gripper is a “single actuator for three fingers" system made of a soft material. The use of a naturally compliant material facilitates object pretension.

soft_gripper_1.png

 

Our approach attempts to provide a precise mechanical modeling of the simulated robot. This includes the geometry of its different parts as well as its deformation law and the associated properties. In this tutorial, we assume these are known and simply need to be added into our simulation framework.   

This video is presenting all the different steps of the tutorial:
 


 

We would like to thank Taimoor Hassan and his co-authors for letting us use their robot design in this tutorial.

Step 1: Volumetric mesh generation

You can download the file associated with this step at:

http://sofamacbuilder.lille.inria.fr/defrost/scenes/Tutorial/step1-VolumetricMeshGeneretion/MeshTetraGeneration.py.
(Of course, it is more appealing to test it with SOFA on the virtual machine !)

In order to simulate a mechanical object using Finite Element Modeling (FEM), a discrete version of its volume is required. This volume, referred as volumetric mesh will serve as a domain for FEM computation. The aim of this tutorial step is to explain how to create such a volumetric mesh from a closed surface description made of triangles. Several tools exist for volume meshing such as GID (http://www.gidhome.com/), CGAL (http://www.cgal.org/) or Gmsh (http://geuz.org/gmsh/). In the following, we will use CGAL, as a dedicated plugin is available within the SOFA framework.

A. First generic example


CGAL creates a 3D volumetric mesh composed of tetrahedra from any closed surface. The SOFA Python file to perform such a procedure should contain at least:

1. The directive that will dynamically link the plugin to SOFA executable:
rootNode.createObject('RequiredPlugin', pluginName='CGALPlugin')

2. We then create a node object that will gather the input and the processing through
node = rootNode.createChild('node')

3. The closed 3D surface which should be described in any 3D format supported by SOFA (OBJ, VTK STL,...) node.createObject('Mesh',name='mesh',filename=path+'bunny.obj')

4. The CGAL method that will process the surface to compute the volumetric mesh:
node.createObject('MeshGenerationFromPolyhedron',inputPoints='@mesh.position', inputTriangles='@mesh.triangles', drawTetras='1')

5. The parameters used for this component are related to the input data: inputPoints, inputTriangles or inputQuads describe the composition of the input mesh

6. The resulting volumetric mesh is then exported in a VTK file by:
node.createObject('Mesh', position='@gen.outputPoints', tetrahedra='@gen.outputTetras')
node.createObject('VTKExporter', filename='bunny', edges='0', tetras='1', exportAtBegin='1')

The first line creates a Mesh object that will be defined by the vertices and the tetrahedra that were computed by CGAL and the second line allows to export the resulting volumetric mesh at the beginning of the SOFA simulation (please note that the .vtu file extension is automatically added at the end of the filename).

By adding the usual SOFA Python directives, the complete scene file is available at the following location on the virtual image hard drive. Running this scene file with SOFA using the Stanford Bunny 3D model (https://graphics.stanford.edu/data/3Dscanrep/ ) leads to the following result:

 

B. Additional information

Using CGAL with default parameters often creates a very fine mesh composed of many vertices. This may involve a large computational fingerprint that is not compatible with real-time control of the soft robot. The MeshGenerationFromPolyhedron component allows to tune the tetrahedra budget in order to optimize the tradeoff between simulation speed and accuracy. The fine-tuning of these parameters requires some background on Delaunay Triangulation (https://en.wikipedia.org/wiki/Delaunay_triangulation) but some rules of thumb may be provided. There are four parameters that can be modified to change the coarseness of the volumetric mesh:

  • facetSize: this parameter provides an upper bound for the radii of the surface Delaunay ball; a larger value may lead to larger tetrahedra.
  • facetApproximation: the approximation error between the boundary and the subdivision surface. It provides an upper bound for the distance between the circumcenter of a surface facet and the center of a surface Delaunay ball of this facet.
  • cellRatio: This parameter controls the shape of mesh cells. Actually, it is an upper bound for the ratio between the circumradius of a mesh tetrahedron and its shortest edge. There is a theoretical bound for this parameter: the Delaunay refinement process is guaranteed to terminate for values larger than 2.
  • cellSize: This parameter controls the size of mesh tetrahedra. It provides an upper bound on the circumradius of the mesh tetrahedra.

Starting from the previous SOFA scene file, we add these four parameters to the MeshGenerationFromPolyhedron component leading to:


node.createObject('MeshGenerationFromPolyhedron',
                                inputPoints='@mesh.position',
                                inputTriangles='@mesh.triangles',
                                drawTetras='1',facetSize="1.5",    
                                facetApproximation="0.15"
                                cellRatio="2", cellSize="1.5")

Instead of the 26k vertices for the tetrahedral mesh, a coarser mesh is obtained with only 600 vertices. In order to refine the mesh, we can decrease the value of these parameters. For instance, with:


node.createObject('MeshGenerationFromPolyhedron'
                                inputPoints='@mesh.position'
                                inputTriangles='@mesh.triangles',
                                drawTetras='1',
                                facetSize="0.35",
                                facetApproximation="0.1"
                                cellRatio="2", cellSize="0.5" )

the resulting mesh is composed of 2250 vertices, which provides good accuracy and is within the acceptable range for real-time computations: our models are usually composed of 3-4k vertices.

 

C. Gripper model

 

Given these principles, a volumetric mesh has been created for a finger of the soft gripper. The Python file MeshTetraGeneration.py is launched in SOFA. The resulting volumetric mesh of the finger is composed of ~350 vertices allowing fast computation and enough accuracy given the simplicity of the surface geometry.

Finger_0.png Finger_1.png

 

Values used to generate this volumetric mesh are:  cellSize=10, facetSize=4, cellRatio=2 and facetApproximation=1.

    

Step 2: Modeling and simulating deformations

You can download the file associated with this step at:
http://sofamacbuilder.lille.inria.fr/defrost/scenes/Tutorial/step2-ModelingAndSimulatingDeformations/Finger.py

In the previous step we showed how to generate a volumetric representation of the object. We will now explain how this geometric representation can be mechanically simulated. In SOFA This implies to provide a mechanical model of the deformation and a solving method.  In the scene, you can add an ODE (Ordinary Differential Equation) solver for time integration which will allow the finger position to be updated at each timestep. For example, using a first order Euler Implicit solver is obtained by:  

finger.createObject('EulerImplicit', name='odesolver', firstOrder='1')

The next component to add is a FEM forcefield which defines how the object reacts to a loading (i.e. which deformations are created from forces applied onto it). Here, because the finger is made of silicone, its mechanical behavior is assumed elastic. This behavior is available via the TetrahedronFEMForceField component that you need to add in the finger node by adding:

finger.createObject('TetrahedronFEMForceField', template='Vec3d', name='FEM',
                                    method='large', poissonRatio='0.3',  youngModulus='18000')

Poisson’s ratio and Young Modulus are material dependant parameters and should be provided by the user. To be properly simulated and to interact with gravity or inertia forces, an object also needs a mass. You can add a given mass with a uniform distribution for an object by adding a UniformMass component to the finger node:    

finger.createObject('UniformMass', totalmass='0.5')

Reload your scene into SOFA and click on the “Animate” button. You can now interact with the finger object by pressing SHIFT and clicking on the mouse left button.

FingerRepos.png FingerEtire.png

Step 3: Actuating the robot with cable

You can download the file associated with this step at: 

http://sofamacbuilder.lille.inria.fr/defrost/scenes/Tutorial/step3-ActuatingTheRobotWithCable/Finger.py

In the previous step, we showed how to model and simulate a soft robot with a finger shape and made of a deformable material (silicone). In this step, we will explain how to actuate it using a cable attached to the fingertip. The cable can then be used to pull or release the fingertip.

 

 

In SOFA, actuation is represented through constraints. To model cables, the SoftRobot plugin provides a CableConstraint behavior component.

In the previously created scene, locate the end of the createScene() function then add a child node named "cable" to the finger node with the createChild function.

       

        cable = finger.createChild('cable')
 

It is now possible to add different behaviors to this node. In SOFA, behaviors are implemented as the composition of simple components that are added to the scene nodes. The first component to add is of type MechanicalObject. The MechanicalObject component is important in SOFA as it stores the state (position, velocity,..) of the points simulated models. In our case, these points are used to sample the geometrical curve of the cable inside the soft finger model.

These initial points are specified using the position argument passed to the createObject function.
 

 

       cable.createObject('MechanicalObject',   position=(

                               "-17.5 12.5 2.5 " +" -32.5 12.5 2.5 " + " -47.5 12.5 2.5 "  + " -62.5 12.5 2.5 " +

                               "-77.5 12.5 2.5" +  "-83.5 12.5 4.5" "-85.5 12.5 6.5" +"-85.5 12.5 8.5" +

                               "-83.5 12.5 10.5" + "-77.5 12.5 12.5" + "-62.5 12.5 12.5" + "-47.5 12.5 12.5" +

                               "-32.5 12.5 12.5" + "-17.5 12.5 12.5" ))
 

The second component to add to the cable node is of type CableConstraint. It is created in the following way:

     cable.createObject('CableConstraint',
                                              name="aCableActuator",

                                              indices='0 1 2 3 4 5 6 7 8 9 10 11 12 13',

                                              pullPoint="0 12.5 12.5")
 

where indices allows to select a subpart of the points defined in the mechanical object and the sequence of the crossing points of the cable. Here we use all of them and we start from point 0 to the point 13. The last point of the list is supposed to be anchored in the model while the others will be sliding points. The pullPoint parameter is the fixed point from which the cable is pulled.
 

If you try the SOFA scene now, you should see pink lines illustrating the cable location but the mechanical behavior of the cable is not connected to the finger's one. To create a bidirectional link between the behavior of the cable and the finger's one, we use a component called BarycentricMapping. This component will map the constraints between the cable’s mechanical DoFs and the one of the finger. One can do this by adding the following line:      

    
       cable.createObject('BarycentricMapping')     
 

It is now time to add some interactivity to your simulated cable. This is done by adding a Controller component to the cable node. With SOFA, it is possible to implement controllers in Python using a component called a PythonScriptController, resulting in the following line to your scene:
 

      cable.createObject('PythonScriptController',

filename="controller.py",

classname="controller")
 

The desired interaction is implemented in a separated file called controller.py which contains:
 

      #!/usr/bin/env python

      # -*- coding: utf-8 -*-

      import Sofa

 

      class controller(Sofa.PythonScriptController):

               def initGraph(self, node):

                        self.node = node

 

               def onKeyPressed(self,c):

                        inputvalue = self.node.getObject('aCable').findData('inputValue')

                        if (c == "+"):

                                displacement = inputvalue.value[0][0] + 1.

                                inputvalue.value = str(displacement)

                        elif (c == "-"):

                                displacement = inputvalue.value[0][0] - 1.

                                inputvalue.value = str(displacement)
 

At this step, pressing CTRL and +/-  changes the displacement of the CableConstraint and results in pulling/releasing the fingertip.

 

Step 4: Control of the fingertip position

You can download the file associated with this step at:
http://sofamacbuilder.lille.inria.fr/defrost/scenes/Tutorial/step4-EnablingDirectManipulationOfTheFingertipPosition/Finger.py

In the previous steps, we showed how to model and simulate a soft robot with the example of the soft finger, made of a deformable material (silicone). We also explained how to add a cable to this model to actuate it and how to add an interactive script to simulate pulling of this cable. The same kind of method could be used to simulate pneumatic actuation (see the examples). This approach simulates a control in the actuator space. To change the fingertip position, one needs to pull the cable as a puppeteer.

In this step, we will see how to add a control of the effector. This is made possible through our approach published in [Duriez 2013] and [Largillière et al. 2015] by inverting the simulated model using an optimisation. With this approach, from the fingertip position, the inverse formulation computes how much the cable should be pulled to satisfy the target position.

 

 

To activate the inverse solver, you first need to replace the line:
 

 rootNode.createObject('GenericConstraintSolver', tolerance="1e-5", maxIterations="100")
 

with this one:
 

 rootNode.createObject('QPInverseProblemSolver')
 

To be able to manipulate the fingertip, you need to add two elements: one is the goal to be reached by the fingertip while the other is the element of the robot that should reach this target. Locate the end of the createScene() function. You can add a yellow sphere materializing the goal to reach by adding the following code:
 

     goal = rootNode.createChild('goal')

     goal.createObject('EulerImplicit', firstOrder='1')

     goal.createObject('CGLinearSolver', iterations='100', tolerance="1e-5",  threshold="1e-5")

     goal.createObject('MechanicalObject', name='goalMO',  position='-120  7.36479  7.14143')

     goal.createObject('Sphere', radius='5', group='3')

     goal.createObject('UncoupledConstraintCorrection')

Now, you can add an effector constraint that will connect the goal position to the mechanical model.

     effector = finger.createChild('effector')
     effector.createObject('MechanicalObject', name="effectorPoint", position=("-103.071  7.36479  7.14143"))                 

     effector.createObject('PositionEffector', template='Vec3d', indices="0", effectorGoal="@../../goal/goalMO.position")
     effector.createObject('BarycentricMapping', mapForces="false"mapMasses="false")         

 

Step 5: setting actuator limits

You can download the file associated with this step at:
http://sofamacbuilder.lille.inria.fr/defrost/scenes/Tutorial/step5-ActuatorLimits/Finger.py

In the previous steps, we showed how to control the deformation of the soft robot by actuator control (direct simulation) or effector control (inverse simulation). We can additionnally put some limits on the actuators: maximum force, maximum displacement... For a cable, we assume that the force can not be negative: it pulls but can not push.

finger_6.png

To implement this behavior you need to replace the lines that create the  CableActuor object with this one setting the maxPositiveDisp parameter to the maximum allowed value for the actuator:

cable.createObject('CableActuator', name="aCable", 
                        indices='0 1 2 3 4 5 6 7 8 9 10 11 12 13', 
                        pullPoint="0.0 12.5 12.5",
                        maxPositiveDisp="40")


How to use the demo ?

After clicking on "Animate", you can select the orange sphere (SHIFT + click left) and move it around.

If you try to impose a displacement on the effector that exceeds the maximum displacement allowed on the cable, the optimization finds the minimal reachable distance to the goal. You can edit this maximum allowed displacement from the SOFA GUI. First click on the scene graph (left side of the GUI) in the component "CableActuator"; see Property 2/2 and edit the field maxPositiveDisp (for instance put 20 instead of 40: it means that the maximum displacement in the pulling direction is now 20mm). Then click on Update and retry. You should obtain this:

finger_7.png

Step 6: Complete soft gripper

You can download the file associated with this step at:
http://sofamacbuilder.lille.inria.fr/defrost/scenes/Tutorial/step6-SoftGripper/SoftGripper_directControl.py

In the previous steps, we showed the simulation of a single finger made of elastic material. But the orginal soft gripper robot is composed of three fingers actuated with cables. In the design of Taimoor Hassan et al., a single cable is used for the three fingers. We reproduce it in the simulation,  by applying the same control input to three cable actuators placed on the three fingers

In the file SoftGripper_directControl.py, we put 3 finger models by copy/paste of the previous simulation

PythonScriptController is placed at the root node of the graph to interactively change the inputs of 3 the CableConstraint components. Here, we use a control of the actuators (direct simulation).

How to use the demo ?

Launch the file SoftGripper_directControl.py on SOFA, click the button Animate  and then press Ctrl + to close the gripper and Ctrl - to open the gripper.

Step 7: Grasping simulation

You can download the file associated with this step at:
http://sofamacbuilder.lille.inria.fr/defrost/scenes/Tutorial/step7-SoftGripperGraspingSimulation/SoftGripper.py (Here, it is strongly recommended to test it on the virtual machine !!)

The simulation in SOFA reproduces grasping using a combination of constraints for frictional contact (using Coulomb's friction) and for actuators. To do this, we need to modify several elements of the simulation.

 

The simulation in SOFA allows to reproduce grasping using a combination of constraints for frictional contact (using Coulomb's friction) and for actuators. To do this, we need to modify several elements of the simulation (see SoftGripper.py)

a. Activate collision and direct simulation

In this example, we will simulate the collision response of the gripper with its environment and particularly an object that will be grasped. The first step is to activate the modules for collision detection and response in SOFA. These modules are described in more details in the documentation of SOFA. The user can modify the coefficient of friction in RuleBasedContactManager (mu=0.8). GenericConstraintSolver is able to solve, in a unified system, the constraints for collision response and the constraints (direct, non inverse) of the actuators. This is what this example illustrates:

          ##################################
          # Direct simulation
          ##################################
           rootNode.createObject('FreeMotionAnimationLoop')
           rootNode.createObject('GenericConstraintSolver', printLog='0', tolerance="1e-6", maxIterations="1000")
           rootNode.createObject('CollisionPipeline', verbose="0")
           rootNode.createObject('BruteForceDetection', name="N2")
           rootNode.createObject('RuleBasedContactManager', name="Response", response="FrictionContact", rules="0 * FrictionContact?mu=0.8" )
           rootNode.createObject('LocalMinDistance', name="Proximity", alarmDistance="4", contactDistance="3", angleCone="0.01")
      

b. Add an object that will be grasped

We add a rigid object and simulate its dynamics with a 6DoFs mechanical object. It computes the physics at the center of gravity. The mass and inertia of the object are computed by UniformMass. The component UncoupledConstraintCorrection provides the physics-based response after collision (in practice, it will use the parameters of the component UniformMass):

          ############# Grasped Object ##########
          # mechanics
           cube =directSimuNode.createChild('cube')
           cube.createObject('EulerImplicit', name='odesolver')
           cube.createObject('CGLinearSolver', name='linearSolver')
           cube.createObject('MechanicalObject', template="Rigid", scale="6", dx="60.0", dy="10", dz="8", rx="10")
           cube.createObject('UniformMass', mass='0.03 10 1000 0 0 0 1000 0 0 0 1000')
           cube.createObject('UncoupledConstraintCorrection')
      

Then, we will need to add a collision model to detect the contacts between this cube and the soft gripper. First, we load the mesh that is used for collision and create a mechanical object with it. Then, we add the collision primitives (Triangle, Line and Point) that will be tested. Finally, the collision model is linked to the behavior of the center of gravity by a RigidMapping:

          #collision
           cubeCollis = cube.createChild('cubeCollis')
           cubeCollis.createObject('MeshObjLoader', name="loader", filename="mesh/smCube27.obj", triangulate="true",  scale="6")
           cubeCollis.createObject('Mesh', src="@loader")
           cubeCollis.createObject('MechanicalObject')
           cubeCollis.createObject('Triangle')
           cubeCollis.createObject('Line')
           cubeCollis.createObject('Point')
           cubeCollis.createObject('RigidMapping')
      

In the same way, we can load an other mesh and use a mapping for the visualization:

          #visualization
           cubeVisu = cube.createChild('cubeVisu')
           cubeVisu.createObject('OglModel', name="Visual", fileMesh="mesh/smCube27.obj", color="0.0 0.5 0.5 1.0", scale="6.2")
           cubeVisu.createObject('RigidMapping')

      

c. Add a fixed floor

Now, we add a collision surface and a collision plane so that we can place the object before and after grasping:

          ################### Fix Floor #############
           planeNode = directSimuNode.createChild('Plane')
           planeNode.createObject('MeshObjLoader', name='loader', filename="mesh/floorFlat.obj", triangulate="true", rotation="0 0 270", scale =10, translation="38 0 0")
           planeNode.createObject('Mesh', src="@loader")
           planeNode.createObject('MechanicalObject', src="@loader")
           planeNode.createObject('Triangle',simulated="0", moving="0")
           planeNode.createObject('Line',simulated="0", moving="0")
        planeNode.createObject('Point',simulated="0", moving="0")
           planeNode.createObject('OglModel',name="Visual", fileMesh="mesh/floorFlat.obj", color="1 0 0 1",rotation="0 0 270", scale =10, translation="38.1 0 0")
      

d. Add collision models to the gripper

We need to put a collision surface to each finger of the soft gripper. In the graph, the collision model is a child of the node that contains the mechanical object (here, the FEM model of each finger). This surface is defined by a mesh (here finger.stl). The primitives of collision are also defined (Triangle, Line, Point). The surface motion is linked to the motion of the FEM model using a BarycentricMapping:

      
           #finger1/finger1Collis
           finger1Collis = finger1.createChild('collis')
           finger1Collis.createObject('MeshSTLLoader', name='loader', filename=path+'finger.stl', rotation="0 0 25", translation="150 0 0")
           finger1Collis.createObject('TriangleSetTopologyContainer', src='@loader', name='container')
           finger1Collis.createObject('MechanicalObject', name='collisMO', template='Vec3d')
           finger1Collis.createObject('Triangle')
           finger1Collis.createObject('Line')
           finger1Collis.createObject('Point')
           finger1Collis.createObject('BarycentricMapping')
      

e. Control the gripper

The user can interactively control the gripper using keyboard events. The definition of the controls is done using a python script.

      ####################################
       # Control of the robot with the keyboard
       rootNode.createObject('PythonScriptController', filename="controllerGripper.py", classname="controller")
       ####################################
      

To close/open the gripper, press the following keys:

  • CTRL +
  • CTRL -

Note that with MacOS, you may have to use CMD instead of CTRL . The actuator is controlled in force. The key CTRL + increases the force applied on the cable. To move the gripper, press the following keys:

  • CTRL ↑ to move it up
  • CTRL ↓ to move it down
  • CTRL ← to move it left
  • CTRL → to move it right