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:
you have installed SOFA with the STLIB and SoftRobots plugins.
you have basic knowledge of the Python programming language. If this is not the case you can go to PythonTutorials.
you have basic knowledge of scene modelling with SOFA. If not, please complete the FirstStep tutorial first.
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.
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.
rootNode
. For this robot, there will be different objects, including one for the silicone piece (ElasticBody
). Each of them is defined with the function node.createChild()
.-i
option.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.
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)
blueprint.stl
.blueprint.stl
.
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.
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)
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:
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)
RestShapeSpringForceField
, applies in fact elastic spring forces on the points of the mechanical object (the degrees of freedom) included in the FixingBox
, to bring them back to their resting position whenever a modification is computed (due to gravity forces for example). The stiffness of these ‘springs’ is set at a very high level, which produces immediate correction of any change in position: the constrained points cannot move anymore.[30.0,0.0,30.0]
allows to constraint the end of the arm connected to servo 2.ServoMotor
, ServoArm
and the prefab ActuatedArm
that brings the first two together.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:
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).
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:
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)
ActuatedArm
prefab. More particularily, in the ServoMotor
prefab of each ActuatedArm
, a ServoWheel
prefab can be seen. It represents the capability - or degree of freedom - of the servo-arm to rotate around the motor shaft. It is implemented in the file s90servo.py and composed of a Rigid MechanicalObject
, on which the rotations are applied, and then transferred to the ActuatedArm
on which the ServoWheel
is attached.EulerRotation
and translation
parameters of the ActuatedArm
instances, try to constrain different parts of the model.SparseLDLSolver
for the former steps, and here, the CGLinearSolver
(using the Conjugate Gradient method) introduced in the Simulation object. The difference in the results of the simulation, depending on the solver used is negligible in the scenes presented in this tutorial.CGLinearSolver
for the servomotors, it is possible to move the servomotor base in the simulation window by holding the Shift key while left-clicking on the servo base to drag it. This shows the connection with the silicone piece, that follows the mouvement, like it would with the real robot.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())
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())
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 |
---|---|---|
![]() |
Increase | 0 |
![]() |
Decrease | 0 |
![]() |
Increase | 1 |
![]() |
Decrease | 1 |
![]() |
Increase | 2 |
![]() |
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: 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())
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.
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 |
---|---|---|
![]() |
Increase | 0 |
![]() |
Decrease | 0 |
![]() |
Increase | 1 |
![]() |
Decrease | 1 |
![]() |
Increase | 2 |
![]() |
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())
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 |
---|---|---|
![]() |
Increase | 0 |
![]() |
Decrease | 0 |
![]() |
Increase | 1 |
![]() |
Decrease | 1 |
![]() |
Increase | 2 |
![]() |
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())
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())
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
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())
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.
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.↩