def __init__(self,
parent,
surfaceMeshFileName,
name="VisualModel",
color=[1., 1., 1.],
rotation=[0., 0., 0.],
translation=[0., 0., 0.],
scale=[1., 1., 1.]):
self.node = Node(parent, name)
if surfaceMeshFileName.endswith(".stl"):
self.loader = self.node.createObject('MeshSTLLoader',
name="loader",
filename=surfaceMeshFileName)
elif surfaceMeshFileName.endswith(".obj"):
self.loader = self.node.createObject('MeshObjLoader',
name="loader",
filename=surfaceMeshFileName)
else:
print(
"Extension not handled in STLIB/python/stlib/visuals for file: "
+ str(surfaceMeshFileName))
self.model = self.node.createObject('OglModel',
name="model",
src="@loader",
rotation=rotation,
translation=translation,
scale3d=scale,
color=color,
updateNormals=False)
class VisualModel(SofaObject):
"""VisualModel Prefab
This prefab is creating a VisualModel.
Arguments:
parent
surfaceMeshFileName
name
color
rotation
translation
scale
Content:
Node
{
name : 'Visual'
MeshLoader : 'loader'
OglModel : "model'
}
"""
def __init__(self,
parent,
surfaceMeshFileName,
name="VisualModel",
color=[1., 1., 1.],
rotation=[0., 0., 0.],
translation=[0., 0., 0.],
scale=[1., 1., 1.]):
self.node = Node(parent, name)
if surfaceMeshFileName.endswith(".stl"):
self.loader = self.node.createObject('MeshSTLLoader',
name="loader",
filename=surfaceMeshFileName)
elif surfaceMeshFileName.endswith(".obj"):
self.loader = self.node.createObject('MeshObjLoader',
name="loader",
filename=surfaceMeshFileName)
else:
print(
"Extension not handled in STLIB/python/stlib/visuals for file: "
+ str(surfaceMeshFileName))
self.model = self.node.createObject('OglModel',
name="model",
src="@loader",
rotation=rotation,
translation=translation,
scale3d=scale,
color=color,
updateNormals=False)
def CosseratFinger(rootNode,
cableNode,
translation =[0., 0., 0.],
rotation =[0., 0., 0.],
name ="1"
):
cable = Node(cableNode, name)
cableN = CosseratCable(cable,
name="cable",
trans=translation,
rot=rotation)
slidingPoint = cableN.slidingPoint
cableDofMO = cableN.cableDofMO
finger = Finger(rootNode,
name="Finger"+name,
translation=translation,
rotation=rotation)
mappedPointsNode = cableN.mappedPointsNode
inputFEMCableMO = addConstraintPoints(attachedTo=finger,cstPoints= FEMpos,mappedPointsNode=mappedPointsNode, translation=translation,rotation=rotation)
mappedPointsNode.createObject('DifferenceMultiMapping', name="pointsMulti", input1=inputFEMCableMO, input2=cableDofMO, output=cableN.outputPointMO, direction=cableN.framesMO+".position", color="1 0 0 1")
return cable
def StringSensor(parentNode=None,
name="StringSensor",
cableGeometry=[[1.0, 0.0, 0.0], [0.0, 0.0, 0.0]],
rotation=[0.0, 0.0, 0.0],
translation=[0.0, 0.0, 0.0],
uniformScale=1.0):
"""One line documentation used in the summary
Args:
Structure:
.. sourcecode:: qml
Node : {
name : "StringSensor"
SensorEngine
}
Example:
.. sourcecode::python
def createScene(rootNode):
from stlib.scene import MainHeader
MainHeader(rootNode)
f = Node(rootNode, "Finger")
StringSensor(f)
"""
cable = Node(parentNode, "StringSensor")
# This create a MechanicalObject, a componant holding the degree of freedom of our
# mechanical modelling. In the case of a cable it is a set of positions specifying
# the points where the cable is passing by.
cable.createObject('MechanicalObject',
position=cableGeometry,
rotation=rotation,
translation=translation,
scale=uniformScale)
cable.createObject("SensorEngine",
name="SensorEngine",
indices=range(len(cableGeometry)))
# This create a BarycentricMapping. A BarycentricMapping is a key element as it will create a bi-directional link
# between the cable's DoFs and the parents's ones so that movements of the cable's DoFs will be mapped
# to the finger and vice-versa;
cable.createObject('BarycentricMapping',
name="Mapping",
mapForces=False,
mapMasses=False)
return ss
def __init__(self, parent, translation=[0.0, 0.0, 0.0], rotation=[0.0, 0.0, 0.0], scale=[1.0, 1.0, 1.0], doAddVisualModel=True):
self.node = Node(parent, "ServoMotor")
self.node.addNewData("angle",
"ServoMotor Properties",
"The angular position of the motor (in radians)", "double", 0.0)
self.angle = self.node.findData("angle")
self.dofs = self.node.createObject("MechanicalObject", size=1,
name="dofs",
translation=translation, rotation=rotation, scale3d=scale,
template='Rigid3', showObject=True, showObjectScale=15)
self.servowheel = ServoWheel(self.node)
self.controller = KinematicMotorController(self.node, self.dofs, self.servowheel.dofs,
self.servowheel.node.map,
angleValue=self.angle.getLinkPath())
if doAddVisualModel:
self.addVisualModel()
def __init__(self,
attachedTo=None,
volumeMeshFileName=None,
name="ElasticMaterialObject",
rotation=[0.0, 0.0, 0.0],
translation=[0.0, 0.0, 0.0],
scale=[1.0, 1.0, 1.0],
surfaceMeshFileName=None,
collisionMesh=None,
withConstrain=True,
surfaceColor=[1.0, 1.0, 1.0],
poissonRatio=0.3,
youngModulus=18000,
totalMass=1.0, solver=None):
self.node = Node(attachedTo, name)
ElasticMaterialObject.createPrefab(self,
volumeMeshFileName, name, rotation, translation, scale, surfaceMeshFileName,
collisionMesh, withConstrain, surfaceColor, poissonRatio, youngModulus, totalMass, solver)
def Gripper(parentNode=None):
gripper = Node(parentNode, "Gripper")
#### YOU NEED TO PUT THE GRIPPER CONTENT HERE.
f1 = Finger(gripper)
f2 = Finger(gripper)
f3 = Finger(gripper)
GripperController(gripper, fingers=[f1, f2, f3])
return gripper
def Finger(parentNode=None,
name="Finger",
rotation=[0.0, 0.0, 0.0],
translation=[0.0, 0.0, 0.0],
fixingBox=[0.0, 0.0, 0.0],
pullPointLocation=[0.0, 0.0, 0.0]):
finger = Node(parentNode, name)
#### YOU NEED TO PUT THE FINGER CONTENT HERE.
return finger
def __init__(self,
parent,
surfaceMeshFileName,
name="VisualModel",
color=[1., 1., 1.],
rotation=[0., 0., 0.],
translation=[0., 0., 0.],
scale=[1., 1., 1.]):
self.node = Node(parent, name)
if not self.getRoot().getObject("SofaOpenglVisual", warning=False):
if not self.getRoot().getObject("/Config/SofaOpenglVisual",
warning=False):
Sofa.msg_info(
"Missing RequiredPlugin SofaOpenglVisual in the scene, add it from Prefab VisualModel."
)
self.getRoot().createObject("RequiredPlugin",
name="SofaOpenglVisual")
if surfaceMeshFileName.endswith(".stl"):
self.loader = self.node.createObject('MeshSTLLoader',
name="loader",
filename=surfaceMeshFileName)
elif surfaceMeshFileName.endswith(".obj"):
self.loader = self.node.createObject('MeshObjLoader',
name="loader",
filename=surfaceMeshFileName)
else:
print(
"Extension not handled in STLIB/python/stlib/visuals for file: "
+ str(surfaceMeshFileName))
self.model = self.node.createObject('OglModel',
name="model",
src="@loader",
rotation=rotation,
translation=translation,
scale3d=scale,
color=color,
updateNormals=False)
def Finger(parentNode=None,
name="Finger",
rotation=[0.0, 0.0, 0.0],
translation=[0.0, 0.0, 0.0],
fixingBox=[-5.0, 0.0, 0.0, 10.0, 15.0, 20.0],
pullPointLocation=[0.0, 0.0, 0.0]):
finger = Node(parentNode, name)
eobject = ElasticMaterialObject(finger,
volumeMeshFileName="data/mesh/finger.vtk",
poissonRatio=0.3,
youngModulus=18000,
totalMass=0.5,
surfaceColor=[0.0, 0.8, 0.7],
surfaceMeshFileName="data/mesh/finger.stl",
rotation=rotation,
translation=translation)
FixedBox(eobject.node, atPositions=fixingBox, doVisualization=True)
cable = PullingCable(
eobject.node,
"PullingCable",
pullPointLocation=pullPointLocation,
rotation=rotation,
translation=translation,
cableGeometry=loadPointListFromFile("data/mesh/cable.json"))
FingerController(eobject.node, cable)
CollisionMesh(eobject.node,
name="CollisionMesh",
surfaceMeshFileName="data/mesh/finger.stl",
rotation=rotation,
translation=translation,
collisionGroup=[1, 2])
CollisionMesh(eobject.node,
name="CollisionMeshAuto1",
surfaceMeshFileName="data/mesh/fingerCollision_part1.stl",
rotation=rotation,
translation=translation,
collisionGroup=[1])
CollisionMesh(eobject.node,
name="CollisionMeshAuto2",
surfaceMeshFileName="data/mesh/fingerCollision_part2.stl",
rotation=rotation,
translation=translation,
collisionGroup=[2])
return finger
def createScene(rootNode):
from stlib.scene import MainHeader
MainHeader(rootNode)
f = Node(rootNode, "Finger")
StringSensor(f)
class ElasticMaterialObject(SofaObject):
def __init__(self,
attachedTo=None,
volumeMeshFileName=None,
name="ElasticMaterialObject",
rotation=[0.0, 0.0, 0.0],
translation=[0.0, 0.0, 0.0],
surfaceMeshFileName=None,
collisionMesh=None,
withConstrain=True,
surfaceColor=[1.0, 1.0, 1.0],
poissonRatio=0.3,
youngModulus=18000,
totalMass=1.0,
solver=None):
self.node = Node(attachedTo, name)
ElasticMaterialObject.createPrefab(self, volumeMeshFileName, name,
rotation, translation,
surfaceMeshFileName, collisionMesh,
withConstrain, surfaceColor,
poissonRatio, youngModulus,
totalMass, solver)
@staticmethod
def createPrefab(self,
volumeMeshFileName=None,
name="ElasticMaterialObject",
rotation=[0.0, 0.0, 0.0],
translation=[0.0, 0.0, 0.0],
surfaceMeshFileName=None,
collisionMesh=None,
withConstrain=True,
surfaceColor=[1.0, 1.0, 1.0],
poissonRatio=0.3,
youngModulus=18000,
totalMass=1.0,
solver=None):
if self.node == None:
Sofa.msg_error(
"Unable to create the elastic object because it is not attached to any node. Please fill the attachedTo parameter"
)
return None
if volumeMeshFileName == None:
Sofa.msg_error(
self.node,
"Unable to create an elastic object because there is no volume mesh provided."
)
return None
if volumeMeshFileName.endswith(".msh"):
self.loader = self.node.createObject('MeshGmshLoader',
name='loader',
filename=volumeMeshFileName,
rotation=rotation,
translation=translation)
elif volumeMeshFileName.endswith(".gidmsh"):
self.loader = self.node.createObject('GIDMeshLoader',
name='loader',
filename=volumeMeshFileName,
rotation=rotation,
translation=translation)
else:
self.loader = self.node.createObject('MeshVTKLoader',
name='loader',
filename=volumeMeshFileName,
rotation=rotation,
translation=translation)
if solver == None:
self.integration = self.node.createObject('EulerImplicit',
name='integration')
self.solver = self.node.createObject('SparseLDLSolver',
name="solver")
self.container = self.node.createObject(
'TetrahedronSetTopologyContainer', src='@loader', name='container')
self.dofs = self.node.createObject('MechanicalObject',
template='Vec3d',
name='dofs')
## To be properly simulated and to interact with gravity or inertia forces, an elasticobject
## also needs a mass. You can add a given mass with a uniform distribution for an elasticobject
## by adding a UniformMass component to the elasticobject node
self.mass = self.node.createObject('UniformMass',
totalMass=totalMass,
name='mass')
## The next component to add is a FEM forcefield which defines how the elasticobject reacts
## to a loading (i.e. which deformations are created from forces applied onto it).
## Here, because the elasticobject is made of silicone, its mechanical behavior is assumed elastic.
## This behavior is available via the TetrahedronFEMForceField component.
self.forcefield = self.node.createObject('TetrahedronFEMForceField',
template='Vec3d',
method='large',
name='forcefield',
poissonRatio=poissonRatio,
youngModulus=youngModulus)
if withConstrain:
self.node.createObject('LinearSolverConstraintCorrection',
solverName=self.solver.name)
if collisionMesh:
self.addCollisionModel(collisionMesh, rotation, translation)
if surfaceMeshFileName:
self.addVisualModel(surfaceMeshFileName, surfaceColor, rotation,
translation)
def addCollisionModel(self,
collisionMesh,
rotation=[0.0, 0.0, 0.0],
translation=[0.0, 0.0, 0.0]):
self.collisionmodel = self.node.createChild('CollisionModel')
self.collisionmodel.createObject('MeshSTLLoader',
name='loader',
filename=collisionMesh,
rotation=rotation,
translation=translation)
self.collisionmodel.createObject('TriangleSetTopologyContainer',
src='@loader',
name='container')
self.collisionmodel.createObject('MechanicalObject',
template='Vec3d',
name='dofs')
self.collisionmodel.createObject('Triangle')
self.collisionmodel.createObject('Line')
self.collisionmodel.createObject('Point')
self.collisionmodel.createObject('BarycentricMapping')
def addVisualModel(self, filename, color, rotation, translation):
self.visualmodel = VisualModel(parent=self.node,
surfaceMeshFileName=filename,
color=color,
rotation=rotation,
translation=translation)
## Add a BarycentricMapping to deform the rendering model to follow the ones of the
## mechanical model.
self.visualmodel.mapping = self.visualmodel.node.createObject(
'BarycentricMapping', name='mapping')
def __init__(self, parentNode):
self.node = Node(parentNode, "ServoWheel")
self.dofs = self.node.createObject("MechanicalObject", size=1, template='Rigid3',
showObject=True, showObjectScale=15, name="dofs")
self.node.createObject("RigidRigidMapping", name="map", applyRestPosition=True)
class ServoMotor(SofaObject):
"""A S90 servo motor
This prefab is implementing a S90 servo motor.
https://servodatabase.com/servo/towerpro/sg90
The prefab ServoMotor is composed of:
- a visual model
- a mechanical model composed two rigids. One rigid is for the motor body
while the other is implementing the servo rotating wheel.
- a KinematicMotorController to compute from an input angle the new orientation of the
servo wheel according to its parent frame.
The prefab has the following parameters:
- translation to change default location of the servo (default [0.0,0.0,0.0])
- rotation to change default rotation of the servo (default [0.0,0.0,0.0,1])
- scale to change default scale of the servo (default 1)
- doAddVisualModel to control wether a visual model is added (default True)
The prefab have the following property:
- angle use this to specify the angle of rotation of the servo motor
Example of use in a Sofa scene:
def createScene(root):
...
servo = ServoMotor(root)
## Direct access to the components
servo.node.angle = 1.0
servo.dofs.showObjects = False
## Indirect access to the components
get(servo.node, "dofs.showObjects").value = False
get(servo.node, "servowheel.dofs.showObjects").value = False
"""
def __init__(self, parent, translation=[0.0, 0.0, 0.0], rotation=[0.0, 0.0, 0.0], scale=[1.0, 1.0, 1.0], doAddVisualModel=True):
self.node = Node(parent, "ServoMotor")
self.node.addNewData("angle",
"ServoMotor Properties",
"The angular position of the motor (in radians)", "double", 0.0)
self.angle = self.node.findData("angle")
self.dofs = self.node.createObject("MechanicalObject", size=1,
name="dofs",
translation=translation, rotation=rotation, scale3d=scale,
template='Rigid3', showObject=True, showObjectScale=15)
self.servowheel = ServoWheel(self.node)
self.controller = KinematicMotorController(self.node, self.dofs, self.servowheel.dofs,
self.servowheel.node.map,
angleValue=self.angle.getLinkPath())
if doAddVisualModel:
self.addVisualModel()
def addVisualModel(self):
visual = self.node.createChild("VisualModel")
visual.createObject("MeshSTLLoader", name="loader", filename="../data/mesh/SG90_servomotor.stl")
visual.createObject("MeshTopology", src="@loader")
visual.createObject("OglModel", color=[0.15, 0.45, 0.75, 0.7], writeZTransparent=True)
visual.createObject("RigidMapping", index=0)
def Finger(parentNode=None,
name="Finger",
rotation=[0.0, 0.0, 0.0],
translation=[0.0, 0.0, 0.0],
fixingBox=[0.0, 0.0, 0.0],
pullPointLocation=[0.0, 0.0, 0.0],
youngModulus=18000,
valueType='position'):
finger = Node(parentNode, name)
eobject = ElasticMaterialObject(
finger,
volumeMeshFileName=os.path.join(
templatepath,
"mesh/finger.vtk"), #MISK need to change the relative file
poissonRatio=0.3,
youngModulus=youngModulus,
totalMass=0.5,
surfaceColor=[0.0, 0.8, 0.65],
surfaceMeshFileName=os.path.join(templatepath, "mesh/finger.stl"),
rotation=rotation,
translation=translation)
FixedBox(eobject, atPositions=fixingBox, doVisualization=True)
cable = PullingCable(eobject,
"PullingCable",
pullPointLocation=pullPointLocation,
rotation=rotation,
translation=translation,
cableGeometry=loadPointListFromFile(
os.path.join(templatepath, "mesh/cable.json")),
valueType=valueType)
#Eulalie.C (21/09/18): this feature does not work, either fix it or remove this comment before SoftRobots v19
#FingerController(eobject, cable, valueType) #MISK may change to vary variation based on value type
CollisionMesh(eobject,
name="CollisionMesh",
surfaceMeshFileName=os.path.join(templatepath,
"mesh/finger.stl"),
rotation=rotation,
translation=translation,
collisionGroup=[1, 2])
CollisionMesh(eobject,
name="CollisionMeshAuto1",
surfaceMeshFileName=os.path.join(
templatepath, "mesh/fingerCollision_part1.stl"),
rotation=rotation,
translation=translation,
collisionGroup=[1])
CollisionMesh(eobject,
name="CollisionMeshAuto2",
surfaceMeshFileName=os.path.join(
templatepath, "mesh/fingerCollision_part2.stl"),
rotation=rotation,
translation=translation,
collisionGroup=[2])
return finger