This tutorial describes how to use SOFA and the SoftRobots plugin in order to simulate a soft robot. The first part is dedicated to the set up of the simulation environment and the modelling of a virtual soft robot. Then, the tutorial deals with the control of the real robot associated with the simulation.
Once completed, the knowledge acquired from this tutorial on the robot called “Tripod” can be applied to other soft robots, thanks to the high modularity of SOFA.

Tutorial prequisites:

The robot “Tripod” considered here (see Figure 1 below) is actuated by three servomotors connected to a deformable silicone material. Each of them controls the deformation of one ‘arm’ of the silicone piece. By combining the effects of the three servomotors, is is possible to control the shape of the whole deformable part, which benefits from a theoritically infinite number of degrees of freedom.

Figure 1: Photo of the Tipod robot.

Reminder of SOFA’s GUI:

Once a scene is loaded in SOFA, you can click on the [Animate] button in the SOFA GUI to start the simulation. Once you have clicked anywhere on the 3D window, interactions are possible with the model on the 3D window and with the robot: depending on what was programmed, you can control the simulation to manipulate the robot, or interact with it by pressing Ctrl+Keys.

Note that with MacOS, you may have to use cmd instead of Ctrl.

Moreover, please note that, in order to send data to the robot, it can be necessary to have administrator rights.

STEP 1: Building the Mechanical Model for the soft part & its Visual Model

At the end of this step, you will be able to:

Reminders of the First Steps tutorial

The first two steps aim at modelling the deformable silicone piece of the robot. The objects composing it are added one by one with the function node.createObject(). Just like in the FirstStep tutorial, a mechanical model of the piece is first created, called ElasticBody. As the silicone piece is soft and can be deformed through constraints, it is necessary to describe its entire volume (and not only its center of gravity as it is usually done for rigid objects). Based on its shape, it is discretized: here it is decomposed into tetrahedral elements, linking all the points - or nodes - of the volume. This discretization produces the following types of elements: tetrahedrons, triangles, lines, and points. These elements are listed into a MechanicalObject named “dofs”. The mesh file describing the discretization into tetrahedral elements is “tripod_mid.stl”. The data provided by the meshes must be organized and stored into different data fields (one for the positions of the nodes, one for the triangular elements, …) before SOFA uses them for computation. This formatting is done by means of code blocks called loaders. The loader used here, MeshSTLLoader is designed for STL files.
The mass distribution of the piece is implemented, as well as a time integration scheme (implicit Euler method) and a solver (here the quick resolution method using the LDL matrix decomposition).
Then, a Visual model of the piece is built, using the same mesh file as for the mechanical model. Because it was decided (for the sake of simplicity) to use the same meshing for both models, the loader is introduced in the rootNode.
Finally, the two representations are linked together by a mapping, which allows to adapt the visual model to the deformations of the mechanical model of the soft piece. As the same mesh is used for both the mechanical and the visual model, the mapping is simply mirroring the mechanical model to adapt the visual one. This is why we use an IdentityMapping here.

The resulting simulation looks like this:
 Try the scene in SOFA.
 Write it yourself.
 Show/Hide the code.
# -*- coding: utf-8 -*-
'''
Step 1:
We are introducing basic mechanical modeling, the new components bring
time integration and a mechanical object to the scene .
'''
import Sofa
from stlib3.scene import Scene

def createScene(rootNode):

    pluginList = ["Sofa.Component.IO.Mesh",
                  "Sofa.Component.LinearSolver.Direct",
                  "Sofa.Component.Mass",
                  "Sofa.Component.ODESolver.Backward",
                  "Sofa.Component.StateContainer",
                  "Sofa.Component.Visual",
                  "Sofa.GL.Component.Rendering3D",
                  "Sofa.GUI.Component"]

    # Setting the gravity, assuming the length unit is in millimeters
    scene = Scene(rootNode, gravity=[0.0, -9810, 0.0], plugins=pluginList, iterative=False)
    scene.addMainHeader()
    scene.addObject('DefaultAnimationLoop')
    scene.addObject('DefaultVisualManagerLoop')

    # Setting the timestep in seconds
    rootNode.dt = 0.01

    # Graphic modelling of the legends associated to the servomotors
    blueprint = Sofa.Core.Node("Blueprints")
    blueprint.addObject('MeshSTLLoader', name='loader', filename='data/mesh/blueprint.stl')
    blueprint.addObject('OglModel', src='@loader')
    scene.Settings.addChild(blueprint)

    # Tool to load the mesh file of the silicone piece. It will be used for both the mechanical and the visual models.
    scene.Modelling.addObject('MeshSTLLoader', name='loader', filename='data/mesh/tripod_mid.stl')

    # Basic mechanical modelling of the silicone piece
    elasticbody = scene.Modelling.addChild('MechanicalModel')
    elasticbody.addObject('MechanicalObject', name='dofs',
                             position=scene.Modelling.loader.position.getLinkPath(),
                             showObject=True, showObjectScale=5.0,
                             rotation=[90.0, 0.0, 0.0])
    elasticbody.addObject('UniformMass')

    # Visual object
    visual = scene.Modelling.addChild('VisualModel')
    # The mesh used for the Visual object is the same as the one for the MechanicalObject, and has been introduced in the rootNode
    visual.addObject('OglModel', name='renderer',
                        src=scene.Modelling.loader.getLinkPath(),
                        color=[1.0, 1.0, 1.0, 0.5])

    # A mapping applies the deformations computed on the mechanical model (the input parameter)
    # to the visual model (the output parameter).
    visual.addObject('IdentityMapping',
                             input=elasticbody.dofs.getLinkPath(),
                             output=visual.renderer.getLinkPath())
                             
    scene.Simulation.addChild(scene.Modelling)                          

Exploring the scene

Remarks

Figure 2: Legends of the servomotors’ positions, as described in the file blueprint.stl.

STEP 2: Modelling the possible deformations of the soft material

At the end of this step, you will be able to:

Unlike the rigid objects modelled in the FirstSteps tutorial, the silicone piece is deformable, and as such, requires additionnal components describing the behavior of the material when it is submitted to mechanical constraints.
In order to implement the deformation behaviour, the MechanicalObject must be connected to one or multiple ForceFields. These Forcefieds are in charge of computing the internal forces that will guide the deformation of the soft piece. Many different mechanical behaviors exist and it is important to understand the one that best approximates the behavior of the real object. In particular, it is important to know how soft or stiff the material is, as well as if it has an elastic behaviour or a more complex one (hyperelastic, plastic, etc…). In our case, it has been chosen to implement a law of elastic deformation, modelled using the Finite Element Method (FEM). Its parameters are the Young modulus, and the Poisson ratio.

elasticbody.createObject("TetrahedronFEMForceField", youngModulus=250, poissonRatio=0.45)

In SOFA, the ElasticMaterialObject from stlib3.physics.deformable provides a ready to use prefabricated object to easily add such an object in our scene. It defines the whole mechanical model of a deformable elastic object.

ElasticMaterialObject.createPrefab(node,
                                    volumeMeshFileName,
                                    name,
                                    rotation,
                                    translation,
                                    surfaceColor,
                                    poissonRatio,
                                    youngModulus,
                                    totalMass,
                                    solver)

However, before using this prefabricated object, let’s first build our own, based on a corotational Finite Element Method with a tetrahedral volumetric representation of the shape. The mesh tripod_mid.gidmsh used to store the shape of the deformable object was built with the GiD mesh generator. Starting from the model obtained in the last step, the parameters of the elastic deformation are added to the silicone piece with a ForceField component.

 Try the scene in SOFA.
 Write it yourself.
 Show/Hide the code.
# -*- coding: utf-8 -*-
'''
Step 2: Introducing elastic material modelling
'''
import Sofa
from stlib3.scene import Scene

def createScene(rootNode):

    pluginList = ["Sofa.Component.IO.Mesh",
                  "Sofa.Component.LinearSolver.Direct",
                  "Sofa.Component.Mass",
                  "Sofa.Component.ODESolver.Backward",
                  "Sofa.Component.SolidMechanics.FEM.Elastic",
                  "Sofa.Component.StateContainer",
                  "Sofa.Component.Topology.Container.Dynamic",
                  "Sofa.Component.Visual",
                  "Sofa.GL.Component.Rendering3D",
                  "Sofa.GUI.Component"]

    scene = Scene(rootNode, gravity=[0.0, -9810, 0.0],
                  plugins=pluginList, iterative=False)
    scene.addMainHeader()
    scene.addObject('DefaultAnimationLoop')
    scene.addObject('DefaultVisualManagerLoop')
    scene.dt = 0.01
        
    # It is possible to visualize the 'forcefields' by doing
    scene.VisualStyle.displayFlags = 'showForceFields'
    
    # Change the stiffness of the spring while interacting with the simulation
    scene.Settings.mouseButton.stiffness = 1.0
    
    # Graphic modelling of the legends associated to the servomotors
    blueprint = Sofa.Core.Node("Blueprints")
    blueprint.addObject('MeshSTLLoader', name='loader', filename='data/mesh/blueprint.stl')
    blueprint.addObject('OglModel', src='@loader')
    scene.Modelling.addChild(blueprint)

    # To simulate an elastic object, we need:
    # - a deformation law (here linear elasticity)
    # - a solving method (here FEM)
    # - as we are using FEM we need a space discretization (here tetrahedron)
    elasticbody = scene.Modelling.addChild('ElasticBody')

    # Specific loader for the mechanical model
    elasticbody.addObject('GIDMeshLoader',
                          name='loader',
                          filename='data/mesh/tripod_low.gidmsh')
    elasticbody.addObject('TetrahedronSetTopologyContainer',
                          src='@loader',
                          name='tetras')
                          
    mechanicalmodel = elasticbody.addChild("MechanicalModel")                      
    mechanicalmodel.addObject('MechanicalObject',
                          name='dofs',
                          position=elasticbody.loader.position.getLinkPath(),
                          rotation=[90.0, 0.0, 0.0],
                          showObject=True,
                          showObjectScale=5.0)
    mechanicalmodel.addObject('UniformMass', 
                          name="mass", 
                          totalMass=0.032)

    # ForceField components
    mechanicalmodel.addObject('TetrahedronFEMForceField',
                          name="linearElasticBehavior",
                          youngModulus=250,
                          poissonRatio=0.45)
    
    # Visual model
    visual = Sofa.Core.Node("VisualModel")
    # Specific loader for the visual model
    visual.addObject('MeshSTLLoader',
                     name='loader',
                     filename='data/mesh/tripod_mid.stl',
                     rotation=[90, 0, 0])
    visual.addObject('OglModel',
                     src=visual.loader.getLinkPath(),
                     name='renderer',
                     color=[1.0, 1.0, 1.0, 0.5])
    scene.Modelling.ElasticBody.addChild(visual)

    visual.addObject('BarycentricMapping',
                     input=mechanicalmodel.dofs.getLinkPath(),
                     output=visual.renderer.getLinkPath())
   
    scene.Simulation.addChild(elasticbody)                 

Exploring the scene

STEP 3: Adding externals constraints

Figure 3: Interaction with the soft material with the central part fixed.

At the end of this step, you will be able to:

In this step, the prefab FixingBox is described, that allows to fix the position of some areas of a physical object in space. (ROI stands for Region Of Interest.) It will be used in this step to prevent the falling of the silicone piece under gravity, by constraining the position of its central part. The prefab object is called via the following function:

FixingBox(ParentNode, ApplyTo, translation, scale)

In parallel, in order to lighten the code and ease the reading, the introduction of a function ElasticBody(parent) is proposed in this step, bringing together all the components of the silicone piece model. This function also uses the prefab ElasticMaterialObject, that implements the mechanical model of an elastic material (including a mesh loader, mass definition, time integration and solver tools, and the description of the Force Fields).
Such a function is defined at the beginning of the scene, and can be called as often as wished in the description of the scene (i.e. in the function createScene(rootNode)).

 Try the scene in SOFA.
 Write it yourself.
 Show/Hide the code.
# -*- coding: utf-8 -*-
'''
Step 3:
In this step, the use of code blocks using functions is introduced, as reusable elements of the code.
'''
import Sofa
from stlib3.scene import Scene

# A prefab to fix some part of an object at its rest position.
from fixingbox import FixingBox

# A prefab to fix
from elasticbody import ElasticBody

from blueprint import Blueprint

def createScene(rootNode):

    pluginList = ["Sofa.Component.Engine.Select",
                  "Sofa.Component.IO.Mesh",
                  "Sofa.Component.LinearSolver.Direct",
                  "Sofa.Component.Mass",
                  "Sofa.Component.ODESolver.Backward",
                  "Sofa.Component.SolidMechanics.FEM.Elastic",
                  "Sofa.Component.SolidMechanics.Spring",
                  "Sofa.Component.StateContainer",
                  "Sofa.Component.Topology.Container.Dynamic",
                  "Sofa.Component.Visual",
                  "Sofa.GL.Component.Rendering3D",
                  "Sofa.GUI.Component"]

    scene = Scene(rootNode, gravity=[0.0, -9810, 0.0],
                  plugins=pluginList,
                  iterative=False)
    scene.addMainHeader()
    scene.addObject('DefaultAnimationLoop')
    scene.addObject('DefaultVisualManagerLoop')
    scene.Settings.mouseButton.stiffness = 10
    scene.VisualStyle.displayFlags = 'showBehavior'
    scene.dt = 0.01

     # Add the blueprint prefab
    scene.Modelling.addChild(Blueprint())

    # Add the elasticobject prefab        
    scene.Modelling.addChild(ElasticBody(rotation=[90,0,0],color=[1.0,1.0,1.0,0.5]))

    # Instanciating the FixingBox prefab into the graph, constraining the mechanical object of the ElasticBody.
    box = FixingBox(scene.Modelling,
                    scene.Modelling.ElasticBody.MechanicalModel,
                    translation=[10.0, 0.0, 0.0],
                    scale=[30., 30., 30.])
    # Changing the property of the Box ROI so that the constraint area appears on screen.
    box.BoxROI.drawBoxes = True

    scene.Simulation.addChild(scene.Modelling.ElasticBody)    
    scene.Simulation.addChild(scene.Modelling.FixingBox)

Exploring the scene

STEP 4: Adding actuators and connecting to deformable part

Figure 4: Tripod assembly.

At the end of this step, you will be able to:

STEP 4-1: Adding actuators

At the end of this step, you will be able to:

It is now time to add the actuators that will deform the elastic piece. On the real robot, this is done by 3 servomotors actuating servo-arms attached to the silicone piece. On SOFA, two prefabricated objects have been implemented and brought together in a third one named ActuatedArm. The two elements composing it can be seen on Figure 5 below. The prefabs are described in a python file, that contains all the elements of the model of the object. They are imported at the beginning of the code file of the scene, and can then be used in the scene. The lines to import the ServoArm and the ActuatedArm prefab are the following:

from actuatedarm import ActuatedArm
from tripod import ElasticBody

actuatedarm and tripod (as well as s90servo that is used in the actuatedarm file) are the names of the python scripts that describe the prefabs. They are located in the folder ‘details’ of the Tripod tutorial (same folder as the scripts for the separate steps of the tutorial).

Figure 5: Display of the servomotor (left) from SG90_servomotor.stl and servo-arm mounted on the servomotor (right) from SG90_servoarm.stl.

The prefab ActuatedArm is building up the ensemble composed of a servomotor and a servo-arm, and includes a Fixing Box situated at the tip of the servo-arm, that attaches the silicone piece to the servo-arm.

ActuatedArm(parent, name, translation, eulerRotation, attachingTo)

It uses the prefabs ServoMotor and ServoArm, that are described as rigid objects and implemented in the files s90servo.py and actuatedarm.py respectively. By opening those files, you will see that the prefabs are actually defined as classes. At the end of the file, you can find a small scene in the function createScene() that provides an example of use of the prefab.

ServoMotor(parent, translation, rotation, scale, doAddVisualModel)

ServoArm(parent, mappingInput, name)
# The mappingInput corresponds to the rigid mechanical object that will control the orientation of the servo-arm, in our case it will be the ServoWheel prefab (see 'Exploring the scene' for more details)

The mechanical model of the ServoMotor and the ServoArm is described in their prefab, but it doesn’t include a time integration scheme nor a solver. In the previous steps, these were added in the object’s node. However, in this step, a different organization is proposed: the mechanical & visual models of the objects are created on one side, grouped in a node called Tripod; and a node ‘Simulation’ is built on the other side, that implements time integration and a solving tool, and links them to their corresponding object. This linking is done thanks to the object Scene from the file tutorial.py located in in the same directory, that is also imported at the beginning of the code file.
Just like for the ElasticBody, a function Tripod() is introduced, now that all the pieces of the robot have been modelled:

Tripod(parent, name="Tripod", radius, numMotors, angleShift)
 Try the scene in SOFA.
 Write it yourself.
 Show/Hide the code.
# -*- coding: utf-8 -*-
"""
Step 4-1: Adding the ActuatedArm prefab.
This prefab is defining the servomotor, the servo-arm and the constraint that attaches the end of the arm to the deformable object.
"""
import Sofa
from tutorial import *
from splib3.numerics import sin, cos, to_radians
from actuatedarm import ActuatedArm
from elasticbody import ElasticBody
from blueprint import Blueprint


def Tripod(name="Tripod", radius=60, numMotors=3, angleShift=180.0):
    def __getTransform(index, numstep, angleShift, radius, dist):
        fi = float(index)
        fnumstep = float(numstep)
        angle = fi * 360 / fnumstep
        angle2 = fi * 360 / fnumstep + angleShift
        eulerRotation = [0, angle, 0]
        translation = [dist * sin(to_radians(angle2)), -1.35, dist * cos(to_radians(angle2))]

        return translation, eulerRotation
    
    self = Sofa.Core.Node(name)
    self.actuatedarms = []
    for i in range(0, numMotors):
        name = "ActuatedArm" + str(i)
        translation, eulerRotation = __getTransform(i, numMotors, angleShift, radius, radius)
        arm = ActuatedArm(name=name, translation=translation, rotation=eulerRotation)
        
        # Add limits to angle that correspond to limits on real robot
        arm.ServoMotor.minAngle = -2.0225
        arm.ServoMotor.maxAngle = -0.0255
        self.actuatedarms.append(arm)
        self.addChild(arm)
            
    self.addChild(ElasticBody(translation=[0.0, 30, 0.0],rotation=[90,0,0],color=[1.0,1.0,1.0,0.5]))
    return self  


def createScene(rootNode):
    from splib3.animation import animate
    from stlib3.scene import Scene
    from fixingbox import FixingBox
    import math

    pluginList = ["ArticulatedSystemPlugin",
                  "Sofa.Component.AnimationLoop",
                  "Sofa.Component.Constraint.Lagrangian.Correction",
                  "Sofa.Component.Constraint.Lagrangian.Solver",
                  "Sofa.Component.Constraint.Projective",
                  "Sofa.Component.Engine.Select",
                  "Sofa.Component.IO.Mesh",
                  "Sofa.Component.LinearSolver.Direct",
                  "Sofa.Component.Mass",
                  "Sofa.Component.ODESolver.Backward",
                  "Sofa.Component.SolidMechanics.FEM.Elastic",
                  "Sofa.Component.SolidMechanics.Spring",
                  "Sofa.Component.StateContainer",
                  "Sofa.Component.Topology.Container.Constant",
                  "Sofa.Component.Topology.Container.Dynamic",
                  "Sofa.Component.Visual",
                  "Sofa.GL.Component.Rendering3D",
                  "Sofa.GUI.Component"]

    scene = Scene(rootNode, gravity=[0.0, -9810, 0.0], iterative=False,
                  plugins=pluginList)
    scene.addMainHeader()
    scene.addObject('DefaultVisualManagerLoop')
    scene.addObject('FreeMotionAnimationLoop')
    scene.addObject('GenericConstraintSolver', maxIterations=50, tolerance=1e-5)
    scene.Simulation.addObject('GenericConstraintCorrection')
    scene.Settings.mouseButton.stiffness = 10
    rootNode.dt = 0.01
    scene.VisualStyle.displayFlags = "showBehavior"

    scene.Modelling.addChild(Blueprint())

    tripod = scene.Modelling.addChild(Tripod())
        
    FixingBox(scene.Modelling, tripod.ElasticBody.MechanicalModel, scale=[10, 10, 10], translation=[0., 25, 0.])
    scene.Modelling.FixingBox.BoxROI.drawBoxes = True

    scene.Simulation.addChild(scene.Modelling.Tripod)
    scene.Simulation.addChild(scene.Modelling.FixingBox)

    # Add animations
    def myanimate(targets, factor):
        for arm in targets:
            arm.angleIn.value = -factor * math.pi / 4
    animate(myanimate, {"targets": [tripod.ActuatedArm0, tripod.ActuatedArm1, tripod.ActuatedArm2]}, duration=1)

Exploring the scene

Remarks

STEP 4-2: Rigidify parts of the deformable piece

Figure 6: Deformable piece with the extremities rigidified.

At the end of this step, you will be able to:

 Try the scene in SOFA.
 Write it yourself.
 Show/Hide the code.
# -*- coding: utf-8 -*-
"""
Step 4-2: Rigidify extremity of deformable part to be able to fix it to the actuated arms
"""
from splib3.numerics import to_radians
from stlib3.physics.mixedmaterial import Rigidify
from stlib3.components import addOrientedBoxRoi
from splib3.numerics import vec3
from splib3.numerics.quat import Quat
from tutorial import *
from actuatedarm import ActuatedArm
from elasticbody import ElasticBody
from blueprint import Blueprint


def Tripod(name="Tripod", radius=60, numMotors=3, angleShift=180.0):
    def __getTransform(index, numstep, angleShift, radius, dist):
        fi = float(index)
        fnumstep = float(numstep)
        angle = fi * 360 / fnumstep
        angle2 = fi * 360 / fnumstep + angleShift
        eulerRotation = [0, angle, 0]
        translation = [dist * sin(to_radians(angle2)), -1.35, dist * cos(to_radians(angle2))]
        return translation, eulerRotation

    def __rigidify(self, radius=60, numMotors=3, angleShift=180.0):
        deformableObject = self.ElasticBody.MechanicalModel
        self.ElasticBody.init()
        dist = radius
        numstep = numMotors
        groupIndices = []
        frames = []
        for i in range(0, numstep):
            translation, eulerRotation = __getTransform(i, numstep, angleShift, radius, dist)

            box = addOrientedBoxRoi(self, position=[list(i) for i in deformableObject.dofs.rest_position.value],
                                    name="BoxROI" + str(i),
                                    translation=vec3.vadd(translation, [0.0, 25.0, 0.0]),
                                    eulerRotation=eulerRotation, scale=[45, 15, 30])

            box.drawBoxes = False
            box.init()
            groupIndices.append([ind for ind in box.indices.value])
            frames.append(vec3.vadd(translation, [0.0, 25.0, 0.0]) + list(
                Quat.createFromEuler([0, float(i) * 360 / float(numstep), 0], inDegree=True)))

        # Rigidify the deformable part at extremity to attach arms
        rigidifiedstruct = Rigidify(self, deformableObject, groupIndices=groupIndices, frames=frames,
                                    name="RigidifiedStructure")

    self = Sofa.Core.Node(name)
    self.actuatedarms = []
    for i in range(0, numMotors):
        name = "ActuatedArm" + str(i)
        translation, eulerRotation = __getTransform(i, numMotors, angleShift, radius, radius)
        arm = ActuatedArm(name=name, translation=translation, rotation=eulerRotation)

        # Add limits to angle that correspond to limits on real robot
        arm.ServoMotor.minAngle = -2.0225
        arm.ServoMotor.maxAngle = -0.0255
        self.actuatedarms.append(arm)
        self.addChild(arm)

    self.addChild(ElasticBody(translation=[0.0, 30, 0.0], rotation=[90, 0, 0], color=[1.0, 1.0, 1.0, 0.5]))

    __rigidify(self, radius, numMotors, angleShift)
    return self


def createScene(rootNode):
    from splib3.animation import animate
    from stlib3.scene import Scene
    from fixingbox import FixingBox
    import math

    pluginList = ["ArticulatedSystemPlugin",
                  "Sofa.Component.AnimationLoop",
                  "Sofa.Component.Constraint.Lagrangian.Correction",
                  "Sofa.Component.Constraint.Lagrangian.Solver",
                  "Sofa.Component.Constraint.Projective",
                  "Sofa.Component.Engine.Select",
                  "Sofa.Component.IO.Mesh",
                  "Sofa.Component.LinearSolver.Direct",
                  "Sofa.Component.Mapping.MappedMatrix",
                  "Sofa.Component.Mass",
                  "Sofa.Component.ODESolver.Backward",
                  "Sofa.Component.SolidMechanics.FEM.Elastic",
                  "Sofa.Component.SolidMechanics.Spring",
                  "Sofa.Component.StateContainer",
                  "Sofa.Component.Topology.Container.Constant",
                  "Sofa.Component.Topology.Container.Dynamic",
                  "Sofa.Component.Visual",
                  "Sofa.GL.Component.Rendering3D",
                  "Sofa.GUI.Component"]

    scene = Scene(rootNode, gravity=[0.0, -9810, 0.0], iterative=False,
                  plugins=pluginList)
    scene.addMainHeader()
    scene.addObject('DefaultVisualManagerLoop')
    scene.addObject('FreeMotionAnimationLoop')
    scene.addObject('GenericConstraintSolver', maxIterations=50, tolerance=1e-5)
    scene.Simulation.addObject('GenericConstraintCorrection')
    scene.Settings.mouseButton.stiffness = 10
    scene.VisualStyle.displayFlags = "showBehavior"
    scene.dt = 0.01

    scene.Modelling.addChild(Blueprint())
    tripod = scene.Modelling.addChild(Tripod())
    tripod.BoxROI0.drawBoxes = True
    tripod.BoxROI1.drawBoxes = True
    tripod.BoxROI2.drawBoxes = True

    # Use this to activate some rendering on the rigidified object ######################################
    setData(tripod.RigidifiedStructure.RigidParts.dofs, showObject=True, showObjectScale=10, drawMode=2)
    setData(tripod.RigidifiedStructure.RigidParts.RigidifiedParticules.dofs, showObject=True, showObjectScale=1,
            drawMode=1, showColor=[1., 1., 0., 1.])
    setData(tripod.RigidifiedStructure.DeformableParts.dofs, showObject=True, showObjectScale=1, drawMode=2)
    #####################################################################################################

    scene.Simulation.addChild(tripod)

    FixingBox(scene.Modelling, tripod.ElasticBody.MechanicalModel, scale=[10, 10, 10], translation=[0., 25, 0.])

    def myanimate(targets, factor):
        for arm in targets:
            arm.ServoMotor.angleIn = -factor * math.pi / 4.

    animate(myanimate, {"targets": [tripod.ActuatedArm0, tripod.ActuatedArm1, tripod.ActuatedArm2]}, duration=1)

    # Temporary additions to have the system correctly built in SOFA
    # Will no longer be required in SOFA v23.12
    scene.Simulation.addObject('MechanicalMatrixMapper',
                               template='Vec3,Rigid3',
                               name="RigidAndDeformableCoupling",
                               object1=tripod.RigidifiedStructure.DeformableParts.dofs.getLinkPath(),
                               object2=tripod.RigidifiedStructure.RigidParts.dofs.getLinkPath(),
                               nodeToParse=tripod.RigidifiedStructure.DeformableParts.MechanicalModel.getLinkPath())

STEP 4-3: Attach the acutators to the deformable piece

Figure 7: Deformable piece with the extremities rigidified and attached to the servoarms.

At the end of this step, you will be able to:

 Try the scene in SOFA.
 Write it yourself.
 Show/Hide the code.
# -*- coding: utf-8 -*-
"""
Step 4-3: Fix arms to deformable part
"""
from splib3.numerics import to_radians
from stlib3.physics.mixedmaterial import Rigidify
from stlib3.components import addOrientedBoxRoi
from splib3.numerics import vec3
from splib3.numerics.quat import Quat
from tutorial import *
from actuatedarm import ActuatedArm
from elasticbody import ElasticBody
from blueprint import Blueprint


def Tripod(name="Tripod", radius=60, numMotors=3, angleShift=180.0):
    def __getTransform(index, numstep, angleShift, radius, dist):
        fi = float(index)
        fnumstep = float(numstep)
        angle = fi * 360 / fnumstep
        angle2 = fi * 360 / fnumstep + angleShift
        eulerRotation = [0, angle, 0]
        translation = [dist * sin(to_radians(angle2)), -1.35, dist * cos(to_radians(angle2))]
        return translation, eulerRotation

    def __rigidify(self, radius=60, numMotors=3, angleShift=180.0):
        deformableObject = self.ElasticBody.MechanicalModel
        self.ElasticBody.init()
        dist = radius
        numstep = numMotors
        groupIndices = []
        frames = []
        for i in range(0, numstep):
            translation, eulerRotation = __getTransform(i, numstep, angleShift, radius, dist)

            box = addOrientedBoxRoi(self, position=[list(i) for i in deformableObject.dofs.rest_position.value],
                                    name="BoxROI" + str(i),
                                    translation=vec3.vadd(translation, [0.0, 25.0, 0.0]),
                                    eulerRotation=eulerRotation, scale=[45, 15, 30])

            box.drawBoxes = False
            box.init()
            groupIndices.append([ind for ind in box.indices.value])
            frames.append(vec3.vadd(translation, [0.0, 25.0, 0.0]) + list(
                Quat.createFromEuler([0, float(i) * 360 / float(numstep), 0], inDegree=True)))

        # Rigidify the deformable part at extremity to attach arms
        rigidifiedstruct = Rigidify(self, deformableObject, groupIndices=groupIndices, frames=frames,
                                    name="RigidifiedStructure")

    def __attachToActuatedArms(self, numstep):
         rigidParts = self.RigidifiedStructure.RigidParts
         for arm in self.actuatedarms:
            arm.ServoMotor.Articulation.ServoWheel.addChild(rigidParts)

         rigidParts.addObject('SubsetMultiMapping',
                              input=[self.actuatedarms[0].ServoMotor.Articulation.ServoWheel.getLinkPath(),
                                     self.actuatedarms[1].ServoMotor.Articulation.ServoWheel.getLinkPath(),
                                     self.actuatedarms[2].ServoMotor.Articulation.ServoWheel.getLinkPath()],
                              output='@./', indexPairs=[0, 1, 1, 1, 2, 1])

    self = Sofa.Core.Node(name)
    self.actuatedarms = []
    for i in range(0, numMotors):
        name = "ActuatedArm" + str(i)
        translation, eulerRotation = __getTransform(i, numMotors, angleShift, radius, radius)
        arm = ActuatedArm(name=name, translation=translation, rotation=eulerRotation)

        # Add limits to angle that correspond to limits on real robot
        arm.ServoMotor.minAngle = -2.0225
        arm.ServoMotor.maxAngle = -0.0255
        self.actuatedarms.append(arm)
        self.addChild(arm)

    self.addChild(ElasticBody(translation=[0.0, 30, 0.0], rotation=[90,0,0], color=[1.0,1.0,1.0,0.5]))
    __rigidify(self, radius, numMotors, angleShift)
    __attachToActuatedArms(self, numMotors)
    return self


def createScene(rootNode):
    from splib3.animation import animate
    from stlib3.scene import Scene
    import math

    pluginList = ["ArticulatedSystemPlugin",
                  "Sofa.Component.AnimationLoop",
                  "Sofa.Component.Constraint.Lagrangian.Correction",
                  "Sofa.Component.Constraint.Lagrangian.Solver",
                  "Sofa.Component.Constraint.Projective",
                  "Sofa.Component.Engine.Select",
                  "Sofa.Component.IO.Mesh",
                  "Sofa.Component.LinearSolver.Direct",
                  "Sofa.Component.Mapping.MappedMatrix",
                  "Sofa.Component.Mass",
                  "Sofa.Component.ODESolver.Backward",
                  "Sofa.Component.SolidMechanics.FEM.Elastic",
                  "Sofa.Component.SolidMechanics.Spring",
                  "Sofa.Component.StateContainer",
                  "Sofa.Component.Topology.Container.Constant",
                  "Sofa.Component.Topology.Container.Dynamic",
                  "Sofa.Component.Visual",
                  "Sofa.GL.Component.Rendering3D",
                  "Sofa.GUI.Component"]

    scene = Scene(rootNode, gravity=[0.0, -9810, 0.0], iterative=False,
                  plugins=pluginList)
    scene.addMainHeader()
    scene.addObject('DefaultVisualManagerLoop')
    scene.addObject('FreeMotionAnimationLoop')
    scene.addObject('GenericConstraintSolver', maxIterations=50, tolerance=1e-5)
    scene.Simulation.addObject('GenericConstraintCorrection')
    scene.Settings.mouseButton.stiffness = 10
    scene.Simulation.TimeIntegrationSchema.rayleighStiffness = 0.05
    scene.VisualStyle.displayFlags = "showBehavior"
    scene.dt = 0.01

    # Add the blueprint
    scene.Modelling.addChild(Blueprint())

    # Add the tripod
    tripod = scene.Modelling.addChild(Tripod())
    tripod.BoxROI0.drawBoxes = True
    tripod.BoxROI1.drawBoxes = True
    tripod.BoxROI2.drawBoxes = True

    scene.Simulation.addChild(tripod)

    def myanimate(targets, factor):
        for arm in targets:
            arm.angleIn = -factor * math.pi / 4.

    animate(myanimate, {"targets": [tripod.ActuatedArm0, tripod.ActuatedArm1, tripod.ActuatedArm2]}, duration=1)

    # Temporary additions to have the system correctly built in SOFA
    # Will no longer be required in SOFA v23.12
    for i in range(3):
        scene.Simulation.addObject('MechanicalMatrixMapper',
                                   name="ArmAndDeformableCoupling" + str(i),
                                   template='Vec1,Vec3',
                                   object1=tripod["ActuatedArm" + str(i) + ".ServoMotor.Articulation.dofs"].getLinkPath(),
                                   object2=tripod["RigidifiedStructure.DeformableParts.dofs"].getLinkPath(),
                                   skipJ2tKJ2=False if i == 0 else True,
                                   nodeToParse=tripod.RigidifiedStructure.DeformableParts.MechanicalModel.getLinkPath())

STEP 5: Adding controllers

Figure 8: Tripod initialisation animation.

At the end of this step, you will be able to:

The servomotors have a default angular position, that corresponds to an angle of 180°. To interactively change this default position, a dedicated object will be added, called a Controller. Controllers allow to implement custom behavior and end-user interaction directly, using python.

In this step we are adding such a controller, in order to be able to control the position of each servo-arm with keyboard keys. On the real robot, the initial position considered is the one when the silicone piece is laying flat, which means that the servomotors are at an angle of 90°.
The keys involved in the control and the effect they trigger are described below:
The following combinations allow to control the angular position of the servomotors, separately:

Keyboard keys1 Effect on angle For Servo
Ctrl + Cursor up Increase 0
Ctrl + Cursor down Decrease 0
Ctrl + Cursor left Increase 1
Ctrl + Cursor right Decrease 1
Ctrl + Key plus Increase 2
Ctrl + Key minus Decrease 2

SOFA allows a default animation management: this is what was being used up to now. In this step, we want to add a more specific animation that updates the scene at each timestep, depending on the keys pressed by the user. The control of this animation is done thanks to a python script controller (class CONTROLLER_NAME(Sofa.Core.Controller)) that is added in our scene file. It uses the function (or method) onKeypressedEvent() that is included in SOFA by default and that triggers an action if a designated key is pressed. The controller is implemented such as, after each key press, the designated servomotor moves from a stepsize value of 0.1 rad (that is a little less than 6°) by changing the value of the attribute ServoMotor.angleIn.
Moreover, another animation is added in the function setupanimation(actuators, step, angularstep, factor), in order to move with one keystroke the three servomotors from their default angular position to the initial position of the real robot. It is triggered by the following keystroke: Ctrl + A The animation is implemented, using the function animate(cb, params,duration) from the STLIB plugin, and the function setupanimation(actuators, step, angularstep, factor). The animate function calls setupanimation and provides the structure of the animation: a small variation of the parameters (step value) is computed each time that the steupanimation function is called; the animate function is a recursive function, that calls itself over and over again, as long as the duration value hasn’t been reached.
The controller is added as another node in the scene.

 Try the scene in SOFA.
 Write it yourself.
 Show/Hide the code.
# -*- coding: utf-8 -*-
"""
Step 5: Adding a controller.
The controller will connect user actions to the simulated behaviour.
"""
import Sofa
from stlib3.scene import Scene                   #< Prefab for the scene
from tripod import Tripod                        #< Prefab for the Tripod
from tripodcontroller import TripodController    #< Implementation of a controller that modify the Tripod


class MyController(Sofa.Core.Controller):
    def __init__(self, *args, **kwargs):
        # These are needed (and the normal way to override from a python class)
        Sofa.Core.Controller.__init__(self, *args, **kwargs)

        def onKeypressedEvent(self, key):
            print("Key Pressed")


def createScene(rootNode):

    pluginList = ["ArticulatedSystemPlugin",
                  "Sofa.Component.AnimationLoop",
                  "Sofa.Component.Constraint.Lagrangian.Correction",
                  "Sofa.Component.Constraint.Lagrangian.Solver",
                  "Sofa.Component.Constraint.Projective",
                  "Sofa.Component.Engine.Select",
                  "Sofa.Component.IO.Mesh",
                  "Sofa.Component.LinearSolver.Direct",
                  "Sofa.Component.Mapping.MappedMatrix",
                  "Sofa.Component.Mass",
                  "Sofa.Component.ODESolver.Backward",
                  "Sofa.Component.SolidMechanics.FEM.Elastic",
                  "Sofa.Component.SolidMechanics.Spring",
                  "Sofa.Component.StateContainer",
                  "Sofa.Component.Topology.Container.Constant",
                  "Sofa.Component.Topology.Container.Dynamic",
                  "Sofa.Component.Visual",
                  "Sofa.GL.Component.Rendering3D",
                  "Sofa.GUI.Component"]

    scene = Scene(rootNode, gravity=[0., -9810., 0.], dt=0.01, iterative=False, plugins=pluginList)
    scene.addMainHeader()
    scene.addObject('DefaultVisualManagerLoop')
    scene.addObject('FreeMotionAnimationLoop')
    scene.addObject('GenericConstraintSolver', maxIterations=50, tolerance=1e-5)
    scene.Simulation.addObject('GenericConstraintCorrection')
    scene.Simulation.TimeIntegrationSchema.rayleighStiffness = 0.005
    scene.Settings.mouseButton.stiffness = 10
    scene.VisualStyle.displayFlags = "showBehavior"

    tripod = Tripod()
    scene.Modelling.addChild(tripod)
    scene.Modelling.addObject(TripodController(name="TripodController",actuators=[tripod.ActuatedArm0, tripod.ActuatedArm1, tripod.ActuatedArm2]))
    scene.Simulation.addChild(tripod)

    # Temporary additions to have the system correctly built in SOFA
    # Will no longer be required in SOFA v23.12
    scene.Simulation.addObject('MechanicalMatrixMapper',
                                 name="deformableAndFreeCenterCoupling",
                                 template='Vec3,Rigid3',
                                 object1=tripod["RigidifiedStructure.DeformableParts.dofs"].getLinkPath(),
                                 object2=tripod["RigidifiedStructure.FreeCenter.dofs"].getLinkPath(),
                                 nodeToParse=tripod["RigidifiedStructure.DeformableParts.MechanicalModel"].getLinkPath())

    for i in range(3):
        scene.Simulation.addObject('MechanicalMatrixMapper',
                                   name="deformableAndArm{i}Coupling".format(i=i),
                                   template='Vec1,Vec3',
                                   object1=tripod["ActuatedArm" + str(i) + ".ServoMotor.Articulation.dofs"].getLinkPath(),
                                   object2=tripod["RigidifiedStructure.DeformableParts.dofs"].getLinkPath(),
                                   skipJ2tKJ2=True,
                                   nodeToParse=tripod["RigidifiedStructure.DeformableParts.MechanicalModel"].getLinkPath())

Exploring the scene

Remark

When the scene is loaded and animated, it can be interesting to display the execution time’s distribution between the different components of the simulation. For that purpose, activate the Log Time option in the Stats panel of the simulation window. The steps duration statistics appear then in the terminal window. A screenshot of it can be seen below in Figure 9. If you’re using the distributed binaries or have compiled SOFA with QtCharts and QtWebEngine installed, you can activate the Display AdvanceTimer profiler option in the Stats panel of the simulation window. A window with the steps duration statistics will pop up. A screenshot of it can be seen below in Figure 10.

The most time consuming process - and thus the one requiring the greatest computing resources - is related to the computation of the Mechanical behaviour, with more than half of the resources allocated to the solving tools. This highlights the complexity of the system and explains why the mesh cannot be endlessly tightened: the simulation would take a great amount of time to compute, too much for any real time application.

Figure 9: Steps Duration Statistics, as seen in the terminal window with which the tutorial was run
Figure 10: Steps Duration Statistics, as seen in the AdvanceTimer Profiler window

STEP 6: Adding collision models

Figure 11: Tripod simulation with collision between a sphere, the deformable part and top of the three servomotors

At the end of this step, you will be able to:

By default SOFA doesn’t handle collisions as they are very expensive to compute. To activate collisions you need to define specifically the geometries for which collisions are checked and how they are handled. In this step we are adding a rigid Sphere object falling on the robot, as well as the description of the contact management between the ball and the silicone piece.
(This scene is defined for the simulation only, the interaction with the real robot has not been added.)

A new controller, called JumpController, is also added to change rapidely the servo motors angles so the robot can play with the falling ball.

The same keystrokes as in the previous steps are used, adding two new intermediate positions for a more dynamical response.

Keyboard keys Effect on angle For Servo
Ctrl + Cursor up Increase 0
Ctrl + Cursor down Decrease 0
Ctrl + Cursor left Increase 1
Ctrl + Cursor right Decrease 1
Ctrl + Key plus Increase 2
Ctrl + Key minus Decrease 2
 Try the scene in SOFA.
 Write it yourself.
 Show/Hide the code.
# -*- coding: utf-8 -*-
"""
Step 6: we are showing how to add collision detection
and contact management in our project.

The important component to see in the graph are:
- the ones added by the addContact function
- the one in the "Collision" part of the tripod object
- the one in the "Collision" part of the Sphere object
"""
import Sofa
from splib3.constants import Key
from stlib3.physics.rigid import Sphere
from stlib3.scene.contactheader import ContactHeader
from stlib3.scene import Scene
from tripod import Tripod
from tripodcontroller import TripodController


class JumpController(Sofa.Core.Controller):
    """This controller has two roles:
       - if the user presses up/left/right/down/plus/minus, the servomotor angle
         is changed.
       - if the user presses A, an animation is started to move the servomotor to the initial position
         of the real robot.
    """
    def __init__(self, *args, **kwargs):
        # These are needed (and the normal way to override from a python class)
        Sofa.Core.Controller.__init__(self, *args, **kwargs)
        self.stepsize = 0.1
        self.actuators = kwargs["actuators"]

    def onKeypressedEvent(self, event):
        key = event['key']
        self.animateTripod(key)

    def animateTripod(self, key):
        apos = None
        if key == Key.Z:
            apos = -3.14/4
        if key == Key.Q:
            apos = -3.14/3

        if apos is not None:
            for actuator in self.actuators:
                actuator.ServoMotor.angleIn = apos


def createScene(rootNode):

    pluginList = ["ArticulatedSystemPlugin",
                  "Sofa.Component.AnimationLoop",
                  "Sofa.Component.Collision.Detection.Algorithm",
                  "Sofa.Component.Collision.Detection.Intersection",
                  "Sofa.Component.Collision.Geometry",
                  "Sofa.Component.Collision.Response.Contact",
                  "Sofa.Component.Constraint.Lagrangian.Correction",
                  "Sofa.Component.Constraint.Lagrangian.Solver",
                  "Sofa.Component.Constraint.Projective",
                  "Sofa.Component.Engine.Select",
                  "Sofa.Component.IO.Mesh",
                  "Sofa.Component.LinearSolver.Direct",
                  "Sofa.Component.Mapping.MappedMatrix",
                  "Sofa.Component.Mass",
                  "Sofa.Component.ODESolver.Backward",
                  "Sofa.Component.SolidMechanics.FEM.Elastic",
                  "Sofa.Component.SolidMechanics.Spring",
                  "Sofa.Component.StateContainer",
                  "Sofa.Component.Topology.Container.Constant",
                  "Sofa.Component.Topology.Container.Dynamic",
                  "Sofa.Component.Visual",
                  "Sofa.GL.Component.Rendering3D",
                  "Sofa.GUI.Component"]

    scene = Scene(rootNode, gravity=[0., -9810., 0.], dt=0.01, plugins=pluginList, iterative=False)

    ContactHeader(rootNode, alarmDistance=15, contactDistance=0.5, frictionCoef=0.2)

    # Adding contact handling
    scene.addMainHeader()
    scene.addObject('DefaultVisualManagerLoop')
    scene.Simulation.addObject('GenericConstraintCorrection')
    scene.VisualStyle.displayFlags = "showCollisionModels"
    scene.Simulation.TimeIntegrationSchema.rayleighStiffness = 0.005
    scene.Settings.mouseButton.stiffness = 10

    tripod = scene.Modelling.addChild(Tripod())
    tripod.addCollision()


    # The regular controller that is being used for the last 2 steps
    controller = scene.addObject(TripodController(name="TripodController", actuators=[tripod.ActuatedArm0, tripod.ActuatedArm1, tripod.ActuatedArm2]))
    # You can set the animation from the python script by adding this call
    controller.initTripod('A')

    # The additionnal controller that add two predefined positions for the three servomotors
    scene.addObject(JumpController(name="JumpController", actuators=[tripod.ActuatedArm0, tripod.ActuatedArm1, tripod.ActuatedArm2]))

    sphere = Sphere(scene.Modelling, translation=[0.0, 50.0, 0.0],
                       uniformScale=13.,
                       totalMass=0.032,
                       isAStaticObject=True)
    sphere.addObject('UncoupledConstraintCorrection')

    scene.Simulation.addChild(sphere)
    scene.Simulation.addChild(tripod)

    # Temporary additions to have the system correctly built in SOFA
    # Will no longer be required in SOFA v23.12
    scene.Simulation.addObject('MechanicalMatrixMapper',
                                 name="deformableAndFreeCenterCoupling",
                                 template='Vec3,Rigid3',
                                 object1=tripod["RigidifiedStructure.DeformableParts.dofs"].getLinkPath(),
                                 object2=tripod["RigidifiedStructure.FreeCenter.dofs"].getLinkPath(),
                                 nodeToParse=tripod["RigidifiedStructure.DeformableParts.MechanicalModel"].getLinkPath())

    for i in range(3):
        scene.Simulation.addObject('MechanicalMatrixMapper',
                                   name="deformableAndArm{i}Coupling".format(i=i),
                                   template='Vec1,Vec3',
                                   object1=tripod["ActuatedArm" + str(i) + ".ServoMotor.Articulation.dofs"].getLinkPath(),
                                   object2=tripod["RigidifiedStructure.DeformableParts.dofs"].getLinkPath(),
                                   skipJ2tKJ2=True,
                                   nodeToParse=tripod["RigidifiedStructure.DeformableParts.MechanicalModel"].getLinkPath())

Exploring the scene

STEP 7: Connecting to the physical robot

At the end of this step, you will be able to:

It is now time to connect our simulated robot to the real one. This requires the addition of two elements. The first one, SerialPortBridgeGeneric, is a component that defines how the communication through the USB/Serial port is handled; it is an object of the rootNode.
The second one, SerialPortController, is another Controller, reading the angular position of the simulated servomotors (in one of their data field) and sends them via the USB communication cable. The angular position of the simulated servomotors is stored in actuators[i].ServoMotor.angleIn in radian, and is transfered to the field serialport.packetOut of the controller board.
Because the data are now sent to the real robot, it is necessary to implement a limitation of the possible angular positions to be reached: between 60 and 180 degrees. Any angle outside this interval is limited to the interval’s extrem value instead.

The keystrokes implemented are the same as for the previous steps, adding one to start sending data to the robot.

Keyboard keys Effect on angle For Servo
Ctrl + Cursor up Increase 0
Ctrl + Cursor down Decrease 0
Ctrl + Cursor left Increase 1
Ctrl + Cursor right Decrease 1
Ctrl + Key plus Increase 2
Ctrl + Key minus Decrease 2
 Try the scene in SOFA.
 Write it yourself.
 Show/Hide the code.
# -*- coding: utf-8 -*-
"""
Step 7: Here we are showing how to setup a communication with the servomotors
"""
import Sofa
from tutorial import *
from splib3.animation import animate
from splib3.constants import Key
from math import floor, pi
from tripod import Tripod
from tripodcontroller import TripodController, setupanimation


class TripodControllerWithCom(TripodController):
    """This controller has three roles:
       - if the user presses up/left/right/down/plus/minus, the servomotor angle
         is changed.
       - if the user presses A, an animation is started to move the servomotor to the initial position
         of the real robot.
       - if thr user presses B start the communication with controller card, send
         servomotor commands
    """

    def __init__(self, *args, **kwargs):
        TripodController.__init__(self, *args, **kwargs)
        self.serialportctrl = kwargs['serialportctrl']

    def initTripod(self, key):
        if key == Key.A and self.serialportctrl.state == "init":
            self.serialportctrl.state = "no-comm"
            animate(setupanimation, {"actuators": self.actuators, "step": 35.0, "angularstep": -1.4965}, duration=0.2)

        # Inclusion of the keystroke to start data sending = establishing communication ('comm')
        if key == Key.B and self.serialportctrl.state == "no-comm":
            self.serialportctrl.state = "comm"


# Description of how the communication is handled
# CHANGE HERE the serialport that correspond to your computer
# def SerialPortBridgeGeneric(rootNode, serialport='/dev/cu.usbserial-1420'):
# def SerialPortBridgeGeneric(rootNode, serialport='COM3'):
def SerialPortBridgeGeneric(rootNode, serialport='/dev/ttyUSB0'):
    return rootNode.addObject('SerialPortBridgeGeneric', port=serialport, baudRate=115200, size=3, listening=True,
                              header=255)


# Data sending controller
class SerialPortController(Sofa.Core.Controller):
    def __init__(self, *args, **kwargs):
        Sofa.Core.Controller.__init__(self, *args, **kwargs)
        self.name = "serialportcontroller"
        self.actuatedarms = kwargs['inputs']
        self.serialport = kwargs['serialport']
        self.serialport.packetOut = [150, 150, 150]
        self.state = "init"

    def onAnimateEndEvent(self, event):
        # Data sending if the robot is initializing or in the no-communication state
        if self.state == "init":
            return

        if self.state == "no-comm":
            return

        # Vector storing the simulated servomotors' angular position
        angles = []

        for arm in self.actuatedarms:
            # Conversion of the angle values from radians to degrees
            angleDegree = arm.ServoMotor.angleOut.value * 360 / (2.0 * pi)
            angleByte = int(floor(angleDegree)) + 179

            # Limitation of the angular position's command
            if angleByte < 60:
                angleByte = 60
            if angleByte > 180:
                angleByte = 180

            # Filling the list of the 3 angle values
            angles.append(angleByte)
        # The controller board of the real robot receives `angles` values
        self.serialport.packetOut = angles


def createScene(rootNode):
    from stlib3.scene import Scene

    pluginList = ["ArticulatedSystemPlugin",
                  "Sofa.Component.AnimationLoop",
                  "Sofa.Component.Constraint.Lagrangian.Correction",
                  "Sofa.Component.Constraint.Lagrangian.Solver",
                  "Sofa.Component.Constraint.Projective",
                  "Sofa.Component.Engine.Select",
                  "Sofa.Component.IO.Mesh",
                  "Sofa.Component.LinearSolver.Direct",
                  "Sofa.Component.Mapping.MappedMatrix",
                  "Sofa.Component.Mass",
                  "Sofa.Component.ODESolver.Backward",
                  "Sofa.Component.SolidMechanics.FEM.Elastic",
                  "Sofa.Component.SolidMechanics.Spring",
                  "Sofa.Component.StateContainer",
                  "Sofa.Component.Topology.Container.Constant",
                  "Sofa.Component.Topology.Container.Dynamic",
                  "Sofa.Component.Visual",
                  "Sofa.GL.Component.Rendering3D",
                  "Sofa.GUI.Component",
                  "SoftRobots"]

    scene = Scene(rootNode, gravity=[0., -9810., 0.], dt=0.01, iterative=False,
                  plugins=pluginList)

    # Adding contact handling
    scene.addMainHeader()
    scene.addObject('DefaultVisualManagerLoop')
    scene.addObject('FreeMotionAnimationLoop')
    scene.addObject('GenericConstraintSolver', maxIterations=50, tolerance=1e-5)
    scene.Simulation.addObject('GenericConstraintCorrection')
    scene.VisualStyle.displayFlags = "showBehavior"
    scene.Settings.mouseButton.stiffness = 10

    tripod = scene.Modelling.addChild(Tripod())

    serial = SerialPortBridgeGeneric(scene)

    # The real robot receives data from the 3 actuators
    serialportctrl = scene.addObject(
        SerialPortController(name="SerialPortController", inputs=tripod.actuatedarms, serialport=serial))

    # The simulation's control with keystrokes is still available
    controller = scene.addObject(
        TripodControllerWithCom(scene, actuators=tripod.actuatedarms, serialportctrl=serialportctrl))
    # You can set the animation from the python script by adding this call
    controller.initTripod('A')

    scene.Simulation.addChild(tripod)

    # Temporary additions to have the system correctly built in SOFA
    # Will no longer be required in SOFA v23.12
    scene.Simulation.addObject('MechanicalMatrixMapper',
                                 name="deformableAndFreeCenterCoupling",
                                 template='Vec3,Rigid3',
                                 object1=tripod["RigidifiedStructure.DeformableParts.dofs"].getLinkPath(),
                                 object2=tripod["RigidifiedStructure.FreeCenter.dofs"].getLinkPath(),
                                 nodeToParse=tripod["RigidifiedStructure.DeformableParts.MechanicalModel"].getLinkPath())

    for i in range(3):
        scene.Simulation.addObject('MechanicalMatrixMapper',
                                   name="deformableAndArm{i}Coupling".format(i=i),
                                   template='Vec1,Vec3',
                                   object1=tripod["ActuatedArm" + str(i) + ".ServoMotor.Articulation.dofs"].getLinkPath(),
                                   object2=tripod["RigidifiedStructure.DeformableParts.dofs"].getLinkPath(),
                                   skipJ2tKJ2=True,
                                   nodeToParse=tripod["RigidifiedStructure.DeformableParts.MechanicalModel"].getLinkPath())

Exploring the scene

STEP 8.1: Inverse control

In the previous steps we where controlling the robot by directly specifying the angle of the ServorMotor object. In this step we will use SOFA to inverse the model and adding an effector to the simulation so that it becomes possible to specify the effector’s position and let the simulation compute the angular positions to apply to reach the effectors’s position.

The same keystrokes as in the previous steps are used, adding a new one to start the inverse resolution.

 Try the scene in SOFA.
 Write it yourself.
 Show/Hide the code.
# -*- coding: utf-8 -*-
"""
Step 8: Here we are showing how to setup the inverse control
"""
import Sofa
from tutorial import *
from tripod import Tripod
from tripodcontroller import SerialPortController, SerialPortBridgeGeneric, InverseController, DirectController


def EffectorGoal(position):
    self = Sofa.Core.Node('Goal')
    self.addObject('EulerImplicitSolver', firstOrder=True)
    self.addObject('CGLinearSolver', iterations=100, threshold=1e-5, tolerance=1e-5)
    self.addObject('MechanicalObject', name='goalMO', template='Rigid3', position=position+[0., 0., 0., 1.], showObject=True, showObjectScale=10)
    self.addObject('UncoupledConstraintCorrection')

    spheres = self.addChild('Spheres')
    spheres.addObject('MechanicalObject', name='mo', position=[[0, 0, 0],  [10, 0, 0],   [0, 10, 0],   [0, 0, 10]])
    spheres.addObject('SphereCollisionModel', radius=5, group=1)
    spheres.addObject('RigidMapping')
    return self


class GoalController(Sofa.Core.Controller):
    """This controller moves the goal position when the inverse control is activated
    """

    def __init__(self, *args, **kwargs):
        Sofa.Core.Controller.__init__(self, *args, **kwargs)
        self.name = "GoalController"
        self.activated = False
        self.time = 0
        self.dy = 0.1
        goalNode = args[1]
        self.mo = goalNode.goalMO
        self.dt = goalNode.getRoot().dt.value

    def onKeyPressed(self, key):
        if key == Key.I:
            self.activated = True

    def onAnimateBeginEvent(self, e):
        if self.activated:
            self.time = self.time+self.dt

        if self.time >= 1:
            self.time = 0;
            self.dy = -self.dy

        pos = [self.mo.position[0][0], self.mo.position[0][1], self.mo.position[0][2]]
        pos[1] += self.dy
        self.mo.position = [[pos[0], pos[1], pos[2], 0, 0, 0, 1]]


def addInverseComponents(arms, freecenter, goalNode, use_orientation):
    actuators=[]
    for arm in arms:
        actuator = arm.ServoMotor.Articulation.addChild('actuator')
        actuators.append(actuator)
        actuator.activated = False
        actuator.addObject('JointActuator', name='JointActuator', template='Vec1',
                                                index=0, applyForce=True,
                                                minAngle=-1.5, maxAngle=1.5, maxAngleVariation=0.1)

    effector = freecenter.addChild("Effector")
    freecenter.dofs.showObject=True
    effector.activated = False
    actuators.append(effector)
    if goalNode is None:
        effector.addObject('PositionEffector', name='effector', template='Rigid3',
                               useDirections=[1, 1, 1, 0, 0, 0],
                               indices=0, effectorGoal=[10, 40, 0], limitShiftToTarget=True,
                               maxShiftToTarget=5)
    elif use_orientation:
        effector.addObject('PositionEffector', name='effector', template='Rigid3',
                               useDirections=[0, 1, 0, 1, 0, 1],
                               indices=0, effectorGoal=goalNode.goalMO.position.getLinkPath())
    else:
        effector.addObject('PositionEffector', name='effector', template='Rigid3',
                               useDirections=[1, 1, 1, 0, 0, 0],
                               indices=0, effectorGoal=goalNode.goalMO.position.getLinkPath(),
                               limitShiftToTarget=True, maxShiftToTarget=5)
    return actuators


def createScene(rootNode):
    from stlib3.scene import Scene

    pluginList = ["ArticulatedSystemPlugin",
                  "Sofa.Component.AnimationLoop",
                  "Sofa.Component.Collision.Geometry",
                  "Sofa.Component.Constraint.Lagrangian.Correction",
                  "Sofa.Component.Constraint.Projective",
                  "Sofa.Component.Engine.Select",
                  "Sofa.Component.IO.Mesh",
                  "Sofa.Component.LinearSolver.Direct",
                  "Sofa.Component.LinearSolver.Iterative",
                  "Sofa.Component.Mapping.MappedMatrix",
                  "Sofa.Component.Mass",
                  "Sofa.Component.ODESolver.Backward",
                  "Sofa.Component.SolidMechanics.FEM.Elastic",
                  "Sofa.Component.SolidMechanics.Spring",
                  "Sofa.Component.StateContainer",
                  "Sofa.Component.Topology.Container.Constant",
                  "Sofa.Component.Topology.Container.Dynamic",
                  "Sofa.Component.Visual",
                  "Sofa.GL.Component.Rendering3D",
                  "Sofa.GUI.Component",
                  "SoftRobots",
                  "SoftRobots.Inverse"]

    scene = Scene(rootNode, gravity=[0., -9810., 0.], dt=0.01, iterative=False, plugins=pluginList)

    # Adding contact handling
    scene.addMainHeader()
    scene.addObject('DefaultVisualManagerLoop')

    # Inverse Solver
    scene.addObject('FreeMotionAnimationLoop')
    scene.addObject('QPInverseProblemSolver', name='QP', printLog=False)
    scene.Simulation.addObject('GenericConstraintCorrection')
    scene.Settings.mouseButton.stiffness = 10
    scene.VisualStyle.displayFlags = "showBehavior showCollision"

    tripod = scene.Modelling.addChild(Tripod())

    # Serial port bridge
    serial = SerialPortBridgeGeneric(rootNode)

    # Choose here to control position or orientation of end-effector
    orientation = False
    if orientation:
        # inverse in orientation
        goalNode = EffectorGoal([0, 50, 50])
    else:
        # inverse in position
        goalNode = EffectorGoal([0, 40, 0])
    scene.Modelling.addChild(goalNode)

    actuators = addInverseComponents(tripod.actuatedarms, tripod.RigidifiedStructure.FreeCenter, goalNode, orientation)

    # The real robot receives data from the 3 actuators
    # serialportctrl = scene.addObject(SerialPortController(scene, inputs=tripod.actuatedarms, serialport=serial))
    invCtr = scene.addObject(InverseController(scene, goalNode, actuators, tripod.ActuatedArm0.ServoMotor.Articulation.ServoWheel.RigidParts,
                                                tripod, serial,
                                                [tripod.ActuatedArm0, tripod.ActuatedArm1, tripod.ActuatedArm2]))

    # The regular controller that is being used for the last 2 steps but with small additions
    scene.addObject(DirectController(scene, tripod.actuatedarms, invCtr))

    scene.Simulation.addChild(tripod)

    # Temporary additions to have the system correctly built in SOFA
    # Will no longer be required in SOFA v23.12
    scene.Simulation.addObject('MechanicalMatrixMapper',
                                 name="deformableAndFreeCenterCoupling",
                                 template='Vec3,Rigid3',
                                 object1=tripod["RigidifiedStructure.DeformableParts.dofs"].getLinkPath(),
                                 object2=tripod["RigidifiedStructure.FreeCenter.dofs"].getLinkPath(),
                                 nodeToParse=tripod["RigidifiedStructure.DeformableParts.MechanicalModel"].getLinkPath())

    for i in range(3):
        scene.Simulation.addObject('MechanicalMatrixMapper',
                                   name="deformableAndArm{i}Coupling".format(i=i),
                                   template='Vec1,Vec3',
                                   object1=tripod["ActuatedArm" + str(i) + ".ServoMotor.Articulation.dofs"].getLinkPath(),
                                   object2=tripod["RigidifiedStructure.DeformableParts.dofs"].getLinkPath(),
                                   skipJ2tKJ2=True,
                                   nodeToParse=tripod["RigidifiedStructure.DeformableParts.MechanicalModel"].getLinkPath())

STEP 8.2: Defining a motion path for the maze

Figure 12: Maze with a little ball.

In the previous steps we where controlling the robot in inverse mode interactively. We want now to make the robot follow a predefined path. For that we will play with a maze. Let’s try to define a path that will move a little ball inside the maze. Open the file mazeplanning.json, and add new points, then press ctrl+r to reload the scene and verify the results in simulation. Tips: To make the trajectory work well on the robot, try to emphasize the movements. Sometimes the ball rolls better in the simulation than in reality.

 Try the scene in SOFA.
 Write it yourself.
 Show/Hide the code.
import Sofa
import Sofa.Core
from stlib3.scene.contactheader import ContactHeader
from mazecontroller import MazeController
import json


class Maze(Sofa.Prefab):
    prefabParameters = [
        {'name': 'translation', 'type': 'Vec3d', 'help': '', 'default': [0, 5, 0]},
        {'name': 'rotation', 'type': 'Vec3d', 'help': '', 'default': [-90, 0, 0]}
    ]

    prefabData = [
        {'name': 'index', 'type': 'int', 'help': 'index of rigid to attach to', 'default': 0},
    ]

    def __init__(self, *args, **kwargs):
        Sofa.Prefab.__init__(self, *args, **kwargs)

    def init(self):
        self.addObject("MeshSTLLoader", name="loader", filename="data/mesh/maze/maze.stl",
                       translation=self.translation.value, rotation=self.rotation.value)
        self.addObject("MeshTopology", src='@loader')
        self.addObject("MechanicalObject")
        self.addObject("TriangleCollisionModel")
        self.addObject("LineCollisionModel")
        self.addObject("PointCollisionModel")


class Sphere(Sofa.Prefab):
    prefabParameters = [
        {'name': 'position', 'type': 'Vec3d', 'help': '', 'default': [3, 50, -8]},
        {'name': 'withSolver', 'type': 'bool', 'help': '', 'default': False}
    ]

    def __init__(self, *args, **kwargs):
        Sofa.Prefab.__init__(self, *args, **kwargs)

    def init(self):
        if self.withSolver.value:
            self.addObject('EulerImplicitSolver')
            self.addObject('SparseLDLSolver', template="CompressedRowSparseMatrixd")
            self.addObject('GenericConstraintCorrection')
        self.addObject("MechanicalObject", position=self.position.value)
        self.addObject("UniformMass", totalMass=1e-4)
        self.addObject('SphereCollisionModel', radius=2)


def createScene(rootNode):
    rootNode.gravity = [0., -9810., 0.]
    rootNode.dt = 0.01

    pluginList = ["Sofa.Component.AnimationLoop",
                  "Sofa.Component.Collision.Detection.Algorithm",
                  "Sofa.Component.Collision.Detection.Intersection",
                  "Sofa.Component.Collision.Geometry",
                  "Sofa.Component.Collision.Response.Contact",
                  "Sofa.Component.Constraint.Lagrangian.Correction",
                  "Sofa.Component.Constraint.Lagrangian.Solver",
                  "Sofa.Component.IO.Mesh", "Sofa.Component.LinearSolver.Direct",
                  "Sofa.Component.LinearSolver.Iterative", "Sofa.Component.Mass",
                  "Sofa.Component.ODESolver.Backward",
                  "Sofa.Component.SolidMechanics.Spring",
                  "Sofa.Component.StateContainer",
                  "Sofa.Component.Topology.Container.Constant",
                  "Sofa.Component.Visual"]

    rootNode.addObject('RequiredPlugin', pluginName=pluginList)

    ContactHeader(rootNode, alarmDistance=15, contactDistance=0.5, frictionCoef=0)
    rootNode.addObject('VisualStyle', displayFlags=['showCollisionModels', 'showBehavior'])
    rootNode.addObject('DefaultVisualManagerLoop')

    effector = rootNode.addChild('Effector')
    effector.addObject('EulerImplicitSolver', firstOrder=True)
    effector.addObject('CGLinearSolver', iterations=100, threshold=1e-5, tolerance=1e-5)
    effector.addObject('MechanicalObject', template='Rigid3', name='goalMO', position=[0, 40, 0, 0, 0, 0, 1],
                       showObject=True, showObjectScale=10)
    effector.addObject('RestShapeSpringsForceField', points=0, angularStiffness=1e5, stiffness=1e5)
    effector.addObject('UncoupledConstraintCorrection', compliance='1e-10  1e-10  0 0 1e-10  0 1e-10 ')

    # Open maze planning from JSON file
    data = json.load(open('mazeplanning.json'))
    effector.addObject(MazeController(effector, data["anglePlanningTable"], True))

    maze = effector.addChild(Maze())
    maze.addObject("RigidMapping", index=0)

    rootNode.addChild(Sphere(withSolver=True))

    return

STEP 8.3: Inverse control to follow a predefined motion path

Figure 13: Tripod with maze on top.

We can now include the maze trajectory withing the tripod robot scene to make a full simulation and compare it with the real-world example. Run the scene step8-maze.py, and press ctrl+a to set the Tripod in the starting configuration, then press ctrl+i to start the inverse resolution.

 Try the scene in SOFA.
 Write it yourself.
 Show/Hide the code.
# -*- coding: utf-8 -*-
"""
Step 8: Here we are showing how to setup the inverse control
"""
import Sofa
from tutorial import *
from tripod import Tripod
from tripodcontroller import SerialPortController, SerialPortBridgeGeneric, InverseController, DirectController
from maze import Maze, Sphere
from mazecontroller import MazeController


def EffectorGoal(node, position):
    goal = node.addChild('Goal')
    goal.addObject('EulerImplicitSolver', firstOrder=True)
    goal.addObject('CGLinearSolver', iterations=100, threshold=1e-12, tolerance=1e-10)
    goal.addObject('MechanicalObject', name='goalMO', template='Rigid3', position=position+[0., 0., 0., 1.], showObject=True, showObjectScale=10)
    goal.addObject('RestShapeSpringsForceField', points=0, angularStiffness=1e5, stiffness=1e5)
    goal.addObject('UncoupledConstraintCorrection')
    return goal


class GoalController(Sofa.Core.Controller):
    """This controller moves the goal position when the inverse control is activated
    """

    def __init__(self, *args, **kwargs):
        Sofa.Core.Controller.__init__(self, *args, **kwargs)
        self.name = "GoalController"
        self.activated = False
        self.time = 0
        self.dy = 0.1
        goalNode = args[1]
        self.mo = goalNode.goalMO
        self.dt = goalNode.getRoot().dt.value

    def onKeyPressed(self, key):
        if key == Key.I:
            self.activated = True

    def onAnimateBeginEvent(self, e):

        if self.activated:
            self.time = self.time+self.dt

        if self.time >= 1:
            self.time = 0;
            self.dy = -self.dy

        pos = [self.mo.position[0][0], self.mo.position[0][1], self.mo.position[0][2]]
        pos[1] += self.dy
        self.mo.position = [[pos[0], pos[1], pos[2], 0, 0, 0, 1]]


def addInverseComponents(arms, freecenter, goalNode, use_orientation):
    actuators=[]
    for arm in arms:
        actuator = arm.ServoMotor.Articulation.addChild('actuator')
        actuators.append(actuator)
        actuator.activated = False
        actuator.addObject('JointActuator', name='JointActuator', template='Vec1',
                                                index=0, applyForce=True,
                                                minAngle=-1.0, maxAngle=1.0, maxAngleVariation=0.1)

    effector = freecenter.addChild("Effector")
    effector.activated = False
    actuators.append(effector)
    if goalNode is None:
        effector.addObject('PositionEffector', name='effector', template='Rigid3',
                               useDirections=[1, 1, 1, 0, 0, 0],
                               indices=0, effectorGoal=[10, 40, 0], limitShiftToTarget=True,
                               maxShiftToTarget=5)
    elif use_orientation:
        effector.addObject('PositionEffector', name='effectorY', template='Rigid3',
                            useDirections=[0, 1, 0, 0, 0, 0],
                            indices=0, effectorGoal=goalNode.goalMO.getLinkPath() + '.position',
                            limitShiftToTarget=True, maxShiftToTarget=5)
        effector.addObject('PositionEffector', name='effectorRXRZ', template='Rigid3', 
                            useDirections=[0, 0, 0, 1, 0, 1],
                            indices=0, effectorGoal=goalNode.goalMO.getLinkPath() + '.position')
    else:
        effector.addObject('PositionEffector', name='effector', template='Rigid3',
                               useDirections=[1, 1, 1, 0, 0, 0],
                               indices=0, effectorGoal=goalNode.goalMO.getLinkPath() + '.position',
                               limitShiftToTarget=True, maxShiftToTarget=5)
    return actuators


def createScene(rootNode):
    from stlib3.scene import Scene
    import json

    pluginList = ["ArticulatedSystemPlugin",
                  "Sofa.Component.AnimationLoop",
                  "Sofa.Component.Collision.Detection.Algorithm",
                  "Sofa.Component.Collision.Detection.Intersection",
                  "Sofa.Component.Collision.Geometry",
                  "Sofa.Component.Collision.Response.Contact",
                  "Sofa.Component.Constraint.Lagrangian.Correction",
                  "Sofa.Component.Constraint.Projective",
                  "Sofa.Component.Engine.Select",
                  "Sofa.Component.IO.Mesh",
                  "Sofa.Component.LinearSolver.Direct",
                  "Sofa.Component.LinearSolver.Iterative",
                  "Sofa.Component.Mapping.MappedMatrix",
                  "Sofa.Component.Mass",
                  "Sofa.Component.ODESolver.Backward",
                  "Sofa.Component.SolidMechanics.FEM.Elastic",
                  "Sofa.Component.SolidMechanics.Spring",
                  "Sofa.Component.StateContainer",
                  "Sofa.Component.Topology.Container.Constant",
                  "Sofa.Component.Topology.Container.Dynamic",
                  "Sofa.Component.Visual",
                  "Sofa.GL.Component.Rendering3D",
                  "Sofa.GUI.Component",
                  "SoftRobots",
                  "SoftRobots.Inverse"]

    scene = Scene(rootNode, gravity=[0., -9810, 0.], dt=0.01, iterative=False, plugins=pluginList)
    ContactHeader(rootNode, alarmDistance=15, contactDistance=0.5, frictionCoef=0)
    scene.removeObject(scene.GenericConstraintSolver)

    goalNode = EffectorGoal(rootNode, [0, 30, 0])

    # Open maze planning from JSON file
    data = json.load(open('mazeplanning.json'))
    goalNode.addObject(MazeController(goalNode, data["anglePlanningTable"], False))

    # Adding contact handling
    scene.addMainHeader()
    scene.addObject('DefaultVisualManagerLoop')

    # Inverse Solver
    scene.addObject('QPInverseProblemSolver', name='QP', printLog=False, epsilon=0.01)
    scene.Simulation.addObject('GenericConstraintCorrection')
    scene.VisualStyle.displayFlags = "showBehavior showCollisionModels"

    tripod = scene.Modelling.addChild(Tripod())
    actuators = addInverseComponents(tripod.actuatedarms, tripod.RigidifiedStructure.FreeCenter, goalNode, True)
    maze = tripod.RigidifiedStructure.FreeCenter.addChild(Maze())
    maze.addObject("RigidMapping", index=0)
    scene.Simulation.addChild(Sphere())

    # Serial port bridge
    serial = SerialPortBridgeGeneric(rootNode)

    # The real robot receives data from the 3 actuators
    # serialportctrl = scene.addObject(SerialPortController(scene, inputs=tripod.actuatedarms, serialport=serial))
    invCtr = scene.addObject(InverseController(scene, goalNode, actuators, tripod.ActuatedArm0.ServoMotor.Articulation.ServoWheel.RigidParts,
                                                tripod, serial,
                                                [tripod.ActuatedArm0, tripod.ActuatedArm1, tripod.ActuatedArm2]))

    # The regular controller that is being used for the last 2 steps but with small additions
    scene.addObject(DirectController(scene, tripod.actuatedarms, invCtr))

    scene.Simulation.addChild(tripod)

    # Temporary additions to have the system correctly built in SOFA
    # Will no longer be required in SOFA v23.12
    scene.Simulation.addObject('MechanicalMatrixMapper',
                                 name="deformableAndFreeCenterCoupling",
                                 template='Vec3,Rigid3',
                                 object1=tripod["RigidifiedStructure.DeformableParts.dofs"].getLinkPath(),
                                 object2=tripod["RigidifiedStructure.FreeCenter.dofs"].getLinkPath(),
                                 nodeToParse=tripod["RigidifiedStructure.DeformableParts.MechanicalModel"].getLinkPath())

    for i in range(3):
        scene.Simulation.addObject('MechanicalMatrixMapper',
                                   name="deformableAndArm{i}Coupling".format(i=i),
                                   template='Vec1,Vec3',
                                   object1=tripod["ActuatedArm" + str(i) + ".ServoMotor.Articulation.dofs"].getLinkPath(),
                                   object2=tripod["RigidifiedStructure.DeformableParts.dofs"].getLinkPath(),
                                   skipJ2tKJ2=True,
                                   nodeToParse=tripod["RigidifiedStructure.DeformableParts.MechanicalModel"].getLinkPath())

Conclusion

Congratulation, you completed this tutorial. You are strongly encouraged to pursue with the other tutorials.

If you have any comments or suggestions, please submit issues on our github/SoftRobots page.


  1. All keyboard keys come from Kai Noack’s ‘Free Keyboard Graphics and Key Icons for Screencasts’, that can be found at this adress https://github.com/q2apro/keyboard-keys-speedflips/. Many thanks to him for publishing it in the free domain.