def createScene(node):
node.createObject('RequiredPlugin', pluginName="SofaLoader")
# controller
node.createObject('PythonScriptController',
filename=__file__,
classname='Controller')
# friction coefficient
shared.mu = float(random.randint(
0, 10)) / 10.0 # a random mu in [0,1] with 0.1 step
scene = Tools.scene(node)
node.dt = 0.005
style = node.getObject('style')
style.findData('displayFlags').showMappings = True
manager = node.getObject('manager')
manager.response = 'FrictionCompliantContact'
manager.responseParams = 'mu=' + str(
shared.mu) + "&horizontalConeProjection=1"
ode = node.getObject('ode')
ode.stabilization = "pre-stabilization"
num = node.createObject('SequentialSolver',
name='num',
iterations=50,
precision=0)
node.createObject('LDLTResponse')
proximity = node.getObject('proximity')
proximity.alarmDistance = 0.5
proximity.contactDistance = 0.1
# plane
plane = StructuralAPI.RigidBody(node, 'plane')
plane.setManually([0, 0, 0, 0, 0, 0, 1], 1, [1, 1, 1])
plane.node.createObject('FixedConstraint')
cm = plane.addCollisionMesh("mesh/cube.obj", [10, 1, 10])
cm.addVisualModel()
# box
box = StructuralAPI.RigidBody(node, 'box')
box.setFromMesh('mesh/cube.obj', 50, [0, 2.5, 0, 0, 0, 0, 1])
#box.setManually( [0,2.5,0,0,0,0,1], 1, [1,1,1] )
box.dofs.showObject = True
box.dofs.showObjectScale = 5
cm = box.addCollisionMesh("mesh/cube.obj")
cm.addVisualModel()
# keep an eye on dofs
shared.plane = plane.dofs
shared.box = box.dofs
def createScene(node):
scene = Tools.scene(node)
ode = node.getObject('ode')
ode.stabilization = "pre-stabilization"
ode.warm_start = False
ode.constraint_forces = "propagate"
# ode.debug = True
node.gravity = '0 -1 0'
node.createObject('RequiredPlugin', pluginName="SofaLoader")
num = node.createObject('SequentialSolver',
name='num',
iterations=200,
precision=0,
iterateOnBilaterals=True)
resp = node.createObject('DiagonalResponse')
# NB: either iterateOnBilaterals or use a non-diagonal Response
manager = node.getObject('manager')
manager.response = 'CompliantContact'
style = node.getObject('style')
style.displayFlags = 'showBehaviorModels showCollisionModels'
proximity = node.getObject('proximity')
proximity.contactDistance = 0.01
# dofs
p1 = insert_point(scene, 'p1', [-1, 1, 0])
p2 = insert_point(scene, 'p2', [1, 1, 0])
rigid = StructuralAPI.RigidBody(scene, 'rigid')
rigid.setManually([0, 0, 0, 0, 0, 0, 1], 1, [1, 1, 1])
rigid.addCollisionMesh(path + '/examples/mesh/ground.obj')
ground = StructuralAPI.RigidBody(scene, 'ground')
ground.setManually([0, 0, 0, 0, 0, 0, 1], 1, [1, 1, 1])
ground.node.createObject('FixedConstraint')
# blocked joint between ground/rigid
joint = StructuralAPI.FixedRigidJoint("joint", ground.node, rigid.node)
script = node.createObject('PythonScriptController',
filename=__file__,
classname='Controller')
return node
def createScene(node):
scene = Tools.scene( node )
style = node.getObject('style')
style.findData('displayFlags').showMappings = True
manager = node.getObject('manager')
manager.response = 'FrictionCompliantContact'
node.createObject('CompliantAttachButton')
globalMu = 0 # per object friction coefficient (the friction coef between 2 objects is approximated as the product of both coefs)
manager.responseParams = 'mu='+str(globalMu)+"&horizontalConeProjection=1" # perfom an horizontal Coulomb cone projection
ode = node.getObject('ode')
ode.stabilization = "pre-stabilization"
ode.debug = False
num = node.createObject('SequentialSolver',
name = 'num',
iterations = 100,
precision = 1e-14)
node.createObject( "LDLTResponse" )
proximity = node.getObject('proximity')
proximity.alarmDistance = 0.5
proximity.contactDistance = 0.2
proximity.useLineLine = True
# plane
mesh = dir + '/../mesh/ground.obj'
plane = StructuralAPI.RigidBody( node, "plane" )
plane.setManually( [0,0,0,0,0,0,1], 1, [1,1,1] )
#body3.setFromMesh( mesh, 1 )
cm = plane.addCollisionMesh( mesh )
cm.addVisualModel()
plane.node.createObject('FixedConstraint', indices = '0')
cm.triangles.contactFriction = 0.5 # per object friction coefficient
# box
mesh = dir + '/../mesh/cube.obj'
box = StructuralAPI.RigidBody( node, "box" )
box.setFromMesh( mesh, 50, [0, 3, 0, 0,0,0,1] )
cm = box.addCollisionMesh( mesh )
cm.addVisualModel()
cm.triangles.contactFriction = 1 # per object friction coefficient
def createScene(root):
##### global parameters
root.createObject(
'VisualStyle',
displayFlags="showBehavior showWireframe showCollisionModels")
root.dt = 0.01
root.gravity = [0, -10, 0]
root.createObject('RequiredPlugin', pluginName='Compliant')
root.createObject('CompliantAttachButtonSetting')
##### SOLVER
root.createObject('CompliantImplicitSolver',
stabilization=0,
neglecting_compliance_forces_in_geometric_stiffness=0)
root.createObject('SequentialSolver', iterations=100, precision=0)
#root.createObject('LUResponse')
root.createObject('LDLTResponse')
bodies = []
points = []
N = 10
for i in xrange(N):
body = StructuralAPI.RigidBody(root, "body_" + str(i))
body.setManually([i, 0, 0, 0, 0, 0, 1], 1, [1, 1, 1])
body.dofs.showObject = True
body.dofs.showObjectScale = .5
bodies.append(body)
bodies[0].node.createObject('FixedConstraint')
bodies[N - 1].mass.mass = 10
bodies[N - 1].mass.inertia = "10 10 10"
for i in xrange(N - 1):
p0 = bodies[i].addMappedPoint("right", [0.5, 0, 0])
p0.dofs.showObject = True
p0.dofs.showObjectScale = .1
p0.dofs.drawMode = 1
p1 = bodies[i + 1].addMappedPoint("left", [-0.5, 0, 0])
p1.dofs.showObject = True
p1.dofs.showObjectScale = .1
p1.dofs.drawMode = 2
d = p0.node.createChild("d" + str(i))
d.createObject('MechanicalObject',
template='Vec3' + StructuralAPI.template_suffix,
name='dofs',
position='0 0 0')
input = [] # @internal
input.append('@' + Tools.node_path_rel(root, p0.node) + '/dofs')
input.append('@' + Tools.node_path_rel(root, p1.node) + '/dofs')
d.createObject('DifferenceMultiMapping',
name='mapping',
input=Tools.cat(input),
output='@dofs',
pairs="0 0")
p1.node.addChild(d)
d.createObject('UniformCompliance', name='compliance', compliance="0")
def createScene(root):
# root node setup
root.createObject('RequiredPlugin', pluginName='Compliant')
root.createObject('VisualStyle', displayFlags="showBehavior")
root.createObject('CompliantAttachButtonSetting')
# simuation parameters
root.dt = 1e-2
root.gravity = [0, -9.8, 0]
# ode solver
ode = root.createObject(
'CompliantImplicitSolver',
neglecting_compliance_forces_in_geometric_stiffness=False,
stabilization="pre-stabilization")
# numerical solver
root.createObject('SequentialSolver',
name='numsolver',
iterations=250,
precision=1e-14,
iterateOnBilaterals=True)
root.createObject('LDLTResponse', name='response')
#root.createObject('LUResponse', name='response')
# scene node
scene = root.createChild('scene')
# script variables
nbLink = 10
linkSize = 2
# links creation
links = []
# rigid bodies
for i in xrange(nbLink):
body = StructuralAPI.RigidBody(root, "link-{0}".format(i))
body.setManually(offset=[0, -1. * linkSize * i, 0, 0, 0, 0, 1],
inertia_forces=True)
body.dofs.showObject = True
body.dofs.showObjectScale = 0.25 * linkSize
links.append(body)
# attach first link
links[0].setFixed()
# joints creation
for i in xrange(nbLink - 1):
off1 = links[i].addOffset("offset-{0}-{1}".format(i, i + 1),
[0, -0.5 * linkSize, 0, 0, 0, 0, 1])
off2 = links[i + 1].addOffset("offset-{0}-{1}".format(i + 1, i),
[0, 0.5 * linkSize, 0, 0, 0, 0, 1])
StructuralAPI.HingeRigidJoint(2,
"joint-{0}-{1}".format(i, i + 1),
off1.node,
off2.node,
isCompliance=True,
compliance=0)
def createRigidBody(node, name, sleep, pos=[0, 0, 0], size=[1, 1, 1]):
body = StructuralAPI.RigidBody(node, name)
body.setManually([pos[0], pos[1], pos[2], 0, 0, 0, 1], 1, [1, 1, 1])
body.dofs.showObject = True
body.dofs.showObjectScale = 1
body.node.canChangeSleepingState = sleep
collisionModel = body.addCollisionMesh("mesh/cube.obj", size)
return body
def insertRigid(parentNode,
rigidModel,
density,
scale=1,
param=None,
color=[1, 1, 1, 1]):
""" create a StructuralAPI.RigidBody from the rigidModel. The model geometry is scaled with scale.
The rigidMass is computed from:
1) mass, com and inertia if present
2) mesh if possible
3) default to a unit sphere TODO: is it relevant to do so ?
"""
if printLog:
log_info("insertRigid", rigidModel.name)
for mesh in rigidModel.mesh:
if rigidModel.meshAttributes[mesh.id].collision:
log_info("\tcollision mesh:", mesh.name)
rigid = StructuralAPI.RigidBody(parentNode, rigidModel.name)
# mass
mi, offset = rigid_mass_info_offset(rigidModel, density, scale)
if offset is None:
rigid.setManually(mi.com, mi.mass, mi.inertia)
else:
rigid.setFromRigidInfo(mi, offset=offset, inertia_forces=0)
# drawing
if not param is None:
rigid.dofs.showObject = param.showRigid
rigid.dofs.showObjectScale = SofaPython.units.length_from_SI(
param.showRigidScale)
# add collision/visual meshes (visual points to collision when
# possible)
rigid.collisions = {}
rigid.visuals = {}
for mesh in rigidModel.mesh:
if rigidModel.meshAttributes[mesh.id].collision:
rigid.collisions[mesh.id] = rigid.addCollisionMesh(
mesh.source, name_suffix='_' + mesh.name, scale3d=[scale] * 3)
if rigidModel.meshAttributes[mesh.id].visual:
rigid.visuals[mesh.id] = rigid.collisions[
mesh.id].addVisualModel(color=color)
elif rigidModel.meshAttributes[mesh.id].visual:
rigid.visuals[mesh.id] = rigid.addVisualModel(mesh.source,
name_suffix='_' +
mesh.name,
scale3d=[scale] * 3,
color=color)
return rigid
def createFixedRigidBody(node, name, sleep, pos=[0, 0, 0], size=[1, 1, 1]):
body = StructuralAPI.RigidBody(node, name)
body.setManually([pos[0], pos[1], pos[2], 0, 0, 0, 1], 1, [1, 1, 1])
body.dofs.showObject = True
body.dofs.showObjectScale = 1
body.node.createObject('FixedConstraint')
collisionModel = body.addCollisionMesh("mesh/cube.obj", size)
collisionModel.triangles.moving = False
body.node.canChangeSleepingState = sleep
return body
def createRigidBody(node, name, posx, posy=0):
body = StructuralAPI.RigidBody(node, name)
body.setManually([posx, posy, 0, 0, 0, 0, 1], 1, [1, 1, 1])
# enforce rigid visualization
body.dofs.showObject = True
body.dofs.showObjectScale = 1
# add initial velocity to make it move!
body.dofs.velocity = "10 10 10 10 10 10"
# add a collision model to be able to rotate it with the mouse
collisionModel = body.addCollisionMesh("mesh/cube.obj")
return body
def createScene(root):
root.createObject('PythonScriptController',
name="moveController",
filename=__file__,
classname="MoveController")
root.createObject('RequiredPlugin', name='Compliant')
root.createObject('RequiredPlugin', name='ContactMapping')
root.createObject(
'VisualStyle',
name='VisualStyle',
displayFlags="hideMechanicalMappings showCollisionModels")
root.createObject('CompliantImplicitSolver')
#root.createObject('CompliantPseudoStaticSolver',iterations=1,velocityFactor=0)
#root.createObject('CgSolver',iterations="25", precision="1e-6")
root.createObject('SequentialSolver', iterations="100", precision="1e-6")
root.gravity = [[0, 0, 0]]
global bodies
# bodies
for i in xrange(N):
r = StructuralAPI.RigidBody(root, 'P' + str(i))
r.setManually([i, 0, 0, 0, 0, 0, 1], 1, [1, 1, 1])
r.addCollisionMesh('mesh/torus.obj', [.1, .1, .1],
[0, 0, 0, 0, 0, 0, 1], "_" + str(i))
model = r.collision.node.getObject("model")
# define the body index in the name of the selectable component (here a collision model) to be able to retrieve the index from its name
model.name = model.name + "_" + str(i)
r.collision.node.createObject('NormalsFromPoints',
name="NormalsFromPoints",
template="Vec3",
position="@dofs.position",
triangles="@loader.triangles",
quads="@loader.quads")
bodies.append(r)
def createScene(node):
ode = node.createObject('CompliantImplicitSolver')
num = node.createObject('LDLTSolver')
rigid = api.RigidBody(node, 'rigid')
rigid.setManually()
script = Script(node)
script.rigid = rigid
# TODO randomize these
node.dt = 1
omega = 2
rigid.velocity.angular[2] = omega
script.expected_angle = node.dt * omega
script.node = node
return node
def createScene(root):
# root node setup
root.createObject('RequiredPlugin', pluginName='Flexible')
root.createObject('RequiredPlugin', pluginName='Compliant')
root.createObject('VisualStyle', displayFlags="showBehavior")
root.createObject('CompliantAttachButtonSetting')
root.createObject('BackgroundSetting', color='1 1 1')
# simuation parameters
root.dt = 1e-1
root.gravity = [0, 0, 0]
# ode solver
ode = root.createObject(
'CompliantImplicitSolver',
neglecting_compliance_forces_in_geometric_stiffness=False,
stabilization="pre-stabilization")
# numerical solver
# root.createObject('LDLTSolver', name="numsolver")
root.createObject('SequentialSolver',
name='numsolver',
iterations=250,
precision=1e-14,
iterateOnBilaterals=True)
# root.createObject('LDLTResponse', name='response')
# root.createObject('LUResponse', name='response')
# scene node
scene = root.createChild('scene')
# script variables
nbLink = 7
linkSize = 2
# links creation
links = []
# rigid bodies
for i in xrange(nbLink):
body = StructuralAPI.RigidBody(root, "link-{0}".format(i))
body.setManually(offset=[0, -1. * linkSize * i, 0, 0, 0, 0, 1],
inertia_forces=False)
body.dofs.showObject = True
body.dofs.showObjectScale = 0.25 * linkSize
body.addVisualModel("mesh/cylinder.obj",
scale3d=[.3, .1 * linkSize, .3],
offset=[0, -0.5 * linkSize, 0, 0, 0, 0, 1])
links.append(body)
# attach first link
links[0].setFixed()
# joints creation
points = []
for i in xrange(nbLink - 1):
off1 = links[i].addOffset("offset-{0}-{1}".format(i, i + 1),
[0, -0.5 * linkSize, 0, 0, 0, 0, 1])
off2 = links[i + 1].addOffset("offset-{0}-{1}".format(i + 1, i),
[0, 0.5 * linkSize, 0, 0, 0, 0, 1])
j = StructuralAPI.HingeRigidJoint(2,
"joint-{0}-{1}".format(i, i + 1),
off1.node,
off2.node,
isCompliance=False,
compliance=1E-5)
#StructuralAPI.BallAndSocketRigidJoint("joint-{0}-{1}".format(i, i+1), off1.node, off2.node, isCompliance=True, compliance=0)
j.addLimits(0, .8, compliance=1E-15)
points.append(links[i].addAbsoluteMappedPoint(
"point-{0}-{1}".format(i, i + 1),
[0.5, -1. * linkSize * i - 0.25 * linkSize, 0]))
points.append(links[i + 1].addAbsoluteMappedPoint(
"point-{0}-{1}".format(i + 1, i),
[0.5, -1. * linkSize * (i + 1) + 0.25 * linkSize, 0]))
points.append(links[nbLink - 1].addAbsoluteMappedPoint(
"point-{0}".format(nbLink),
[0.5, -1. * linkSize * (nbLink - 1) - 0.5 * linkSize, 0]))
# rod
rodNode = scene.createChild('rod')
rodNode.createObject('MechanicalObject',
name='dofs',
template="Vec3",
position=concat([0.5, 0.5 * linkSize, 0]))
rodNode.createObject('UniformMass', name="mass", totalMass="1")
rodNode.createObject('RestShapeSpringsForceField',
points="0",
stiffness="1E3")
fullrodNode = rodNode.createChild('fullrod')
input = '@' + rodNode.getPathName() + ' '
indexPairs = '0 0 '
edges = ''
for i, p in enumerate(points):
input += '@' + p.node.getPathName() + ' '
indexPairs += str(i + 1) + ' 0 '
edges += str(i) + ' ' + str(i + 1) + ' '
p.node.addChild(fullrodNode)
fullrodNode.createObject('MechanicalObject',
template="Vec3",
showObject=True,
showObjectScale=0.1,
showColor="0 1 0 1",
drawMode=1)
fullrodNode.createObject('SubsetMultiMapping',
template="Vec3,Vec3",
name="mapping",
input=input,
output='@./',
indexPairs=indexPairs)
Lnode = fullrodNode.createChild("L")
Lnode.createObject('MechanicalObject', template="Vec1")
Lnode.createObject('LengthMapping',
template="Vec3,Vec1",
edges=edges,
offset=str(-nbLink * linkSize),
geometricStiffness=1)
Lnode.createObject('UniformCompliance',
compliance=1E-10,
rayleighStiffness=0,
isCompliance='1')
Vnode = fullrodNode.createChild("visual")
Vnode.createObject('VisualModel', edges=edges)
Vnode.createObject('IdentityMapping')
def createFixedRigidBody(node, name, pos):
body = StructuralAPI.RigidBody(node, name)
body.setManually([pos, 0, 0, 0, 0, 0, 1], 1, [1, 1, 1])
body.node.createObject('FixedConstraint')
return body
def createSceneAndController(root):
##### global parameters
root.createObject(
'VisualStyle',
displayFlags="showBehavior showWireframe showCollisionModels")
root.dt = 0.001
root.gravity = [0, -9.8, 0]
root.createObject('RequiredPlugin', pluginName='Compliant')
root.createObject('CompliantAttachButtonSetting')
##### SOLVER
root.createObject('CompliantImplicitSolver', stabilization=1)
root.createObject('SequentialSolver', iterations=100)
root.createObject('LDLTResponse')
##### SIMPLE KINETIC JOINTS
# HINGE
hingeNode = root.createChild('hinge')
hinge_body1 = createFixedRigidBody(hingeNode, "hinge_body1", -30)
hinge_body2 = createRigidBody(hingeNode, "hinge_body2", -30)
hinge = StructuralAPI.HingeRigidJoint(2, "joint", hinge_body1.node,
hinge_body2.node)
hinge.addLimits(-1, 0.75)
hinge.addSpring(100)
# SLIDER
sliderNode = root.createChild('slider')
slider_body1 = createFixedRigidBody(sliderNode, "slider_body1", -25)
slider_body2 = createRigidBody(sliderNode, "slider_body2", -25)
slider = StructuralAPI.SliderRigidJoint(1, "joint", slider_body1.node,
slider_body2.node)
slider.addLimits(-1, 5)
slider.addSpring(100)
# CYLINDRICAL
cylindricalNode = root.createChild('cylindrical')
cylindrical_body1 = createFixedRigidBody(cylindricalNode,
"cylindrical_body1", -20)
cylindrical_body2 = createRigidBody(cylindricalNode, "cylindrical_body2",
-20)
cylindrical = StructuralAPI.CylindricalRigidJoint(1, "joint",
cylindrical_body1.node,
cylindrical_body2.node)
cylindrical.addLimits(-1, 5, -1, 0.75)
cylindrical.addSpring(100, 100)
# BALL AND SOCKET
ballandsocketNode = root.createChild('ballandsocket')
ballandsocket_body1 = createFixedRigidBody(ballandsocketNode,
"ballandsocket_body1", -15)
ballandsocket_body2 = createRigidBody(ballandsocketNode,
"ballandsocket_body2", -15)
ballandsocket = StructuralAPI.BallAndSocketRigidJoint(
"joint", ballandsocket_body1.node, ballandsocket_body2.node)
ballandsocket.addSpring(100)
# soft BALL AND SOCKET
ballandsocketNode = root.createChild('softballandsocket')
ballandsocket_body1 = createFixedRigidBody(ballandsocketNode,
"ballandsocket_body1", -10)
ballandsocket_body2 = createRigidBody(ballandsocketNode,
"ballandsocket_body2", -10)
ballandsocket = StructuralAPI.BallAndSocketRigidJoint(
"joint",
ballandsocket_body1.node,
ballandsocket_body2.node,
compliance=1e-2,
isCompliance=False)
ballandsocket.addSpring(100)
ballandsocketNode = root.createChild('simpleballandsocket')
ballandsocket_body1 = createFixedRigidBody(ballandsocketNode,
"simpleballandsocket_body1", -5)
ballandsocket_body2 = createRigidBody(ballandsocketNode,
"simpleballandsocket_body2", -5)
simpleballandsocket = StructuralAPI.SimpleBallAndSocketRigidJoint(
"joint",
ballandsocket_body1.node,
ballandsocket_body2.node,
compliance=1e-2,
isCompliance=False)
# PLANAR
planarNode = root.createChild('planar')
planar_body1 = createFixedRigidBody(planarNode, "planar_body1", 0)
planar_body2 = createRigidBody(planarNode, "planar_body2", 0)
planar = StructuralAPI.PlanarRigidJoint(2, "joint", planar_body1.node,
planar_body2.node)
planar.addLimits(-0.5, 1, -3, 3)
planar.addSpring(100, 100)
# GIMBAL
# gimbalNode = root.createChild('gimbal')
# gimbal_body1 = createFixedRigidBody(gimbalNode, "gimbal_body1", 0 )
# gimbal_body2 = createRigidBody(gimbalNode, "gimbal_body2", 0 )
# gimbal = StructuralAPI.GimbalRigidJoint( 2, "joint", gimbal_body1.node, gimbal_body2.node )
# gimbal.addLimits(-0.5,1,-3,3)
# gimbal.addSpring( 100,100 )
# FIXED
fixedNode = root.createChild('fixed')
fixed_body1 = createFixedRigidBody(fixedNode, "fixed_body1", 5)
fixed_body2 = createRigidBody(fixedNode, "fixed_body2", 5)
fixed = StructuralAPI.FixedRigidJoint("joint", fixed_body1.node,
fixed_body2.node)
# DISTANCE
distanceNode = root.createChild('distance')
distance_body1 = createFixedRigidBody(distanceNode, "distance_body1", 10)
distance_body2 = createRigidBody(distanceNode, "distance_body2", 10, 4)
distance = StructuralAPI.DistanceRigidJoint("joint", distance_body1.node,
distance_body2.node)
# 6D spring
springNode = root.createChild('6Dspring')
spring_body1 = createFixedRigidBody(springNode, "spring_body1", 15)
spring_body2 = createRigidBody(springNode, "spring_body2", 15)
spring = StructuralAPI.RigidJointSpring(
"joint", spring_body1.node, spring_body2.node,
[100000, 100000, 100000, 100000, 100000, 10000])
# from now work in float
StructuralAPI.template_suffix = "f"
##### MORE COMPLEX EXAMPLE
complexNode = root.createChild('complex')
# rigid body
body1 = StructuralAPI.RigidBody(complexNode, "body1")
body1.setFromMesh("mesh/cube.obj", 1, [5, -7, 0, 0, 0, 0, 1])
body1.dofs.showObject = True
body1.dofs.showObjectScale = 1
# collision model
body1_collisionModel = body1.addCollisionMesh("mesh/cube.obj",
[3.1, 1.1, 2.1])
body1_collisionModel.triangles.group = "1"
# visual model
body1.visualModel = body1.addVisualModel("mesh/cube.obj", [3, 1, 2])
body1.visualModel.model.setColor(1, 0, 0, 1)
# offsets
body1_offset1 = body1.addOffset("offset1", [
2, 1, 3, 0.7325378163287418, 0.4619397662556433, -0.19134171618254486,
0.4619397662556433
])
body1_offset1.dofs.showObject = True
body1_offset1.dofs.showObjectScale = .5
body1_offset2 = body1.addOffset("offset2", [1, 0, 0, 0, 0, 0, 1])
body1_offset2.dofs.showObject = True
body1_offset2.dofs.showObjectScale = .5
##### RIGID BODY 2
# rigid body
body2 = StructuralAPI.RigidBody(complexNode, "body2")
body2.setManually([10, -7, 0, 0, 0, 0, 1], 1, [1, 1, 1])
body2.dofs.showObject = True
body2.dofs.showObjectScale = 1
# collision model
body2.node.createObject("Sphere", group="1")
# visual model
body2.visualModel = body2.addVisualModel("mesh/cube.obj")
body2.visualModel.model.setColor(0, 1, 0, 1)
##### JOINT
joint1 = StructuralAPI.SliderRigidJoint(0, "joint1", body1_offset1.node,
body2.node)
joint1.addLimits(-10, 10)
joint1.addDamper(5)
##### ROTATED INERTIA
mesh = path + "/../Compliant_test/python/geometric_primitives/rotated_cuboid_12_35_-27.obj"
body3 = StructuralAPI.RigidBody(complexNode, "rotated_mesh")
body3.setFromMesh(
mesh, 1, [-3, -5, 0, 0.7071067811865476, 0, 0, 0.7071067811865476])
body3.dofs.showObject = True
body3.dofs.showObjectScale = 1
body3.addCollisionMesh(mesh)
body3.addVisualModel(mesh)
alignedoffset = body3.addOffset("world_axis_aligned",
[0, 0, 0, 0, 0, 0, 1])
alignedoffset.dofs.showObject = True
alignedoffset.dofs.showObjectScale = .5
alignedoffset.addVisualModel("mesh/cube.obj", [0.1, 0.1, 0.1])
cm = alignedoffset.addCollisionMesh("mesh/cube.obj", [0.2, 0.1, 0.1])
cm.addVisualModel()
mappedpoint1 = body3.addMappedPoint("point", [1, 0, 0], isMechanical=False)
mappedpoint1.dofs.showObject = True
mappedpoint1.dofs.showObjectScale = .1
mappedpoint1.dofs.drawMode = 1
global notalignedoffset
notalignedoffset = body3.addOffset("offset", [
1, 0, 0, 0.7325378163287418, 0.4619397662556433, -0.19134171618254486,
0.4619397662556433
],
isMechanical=False)
notalignedoffset.dofs.showObject = True
notalignedoffset.dofs.showObjectScale = .5
mappedpoint2 = notalignedoffset.addMappedPoint("point", [1, 0, 0],
isMechanical=False)
mappedpoint2.dofs.showObject = True
mappedpoint2.dofs.showObjectScale = .1
mappedpoint2.dofs.drawMode = 1
##### COMPLEX SHAPE
mesh = "mesh/dragon.obj"
dragon = StructuralAPI.RigidBody(complexNode, "dragon")
dragon.setFromMesh(mesh, 1, [-10, -5, 0, 0, 0, 0, 1], [.2, .2, .2])
dragon.dofs.showObject = True
dragon.dofs.showObjectScale = 1
dragon.addCollisionMesh(mesh, [.2, .2, .2])
dragon.addVisualModel(mesh, [.2, .2, .2])
def createScene(root):
##### global parameters
root.createObject('VisualStyle', displayFlags="showBehavior showCollisionModels" )
root.dt = 0.01
root.gravity = [0, -10, 0]
root.createObject('RequiredPlugin', pluginName = 'Compliant')
root.createObject('CompliantAttachButton')
root.createObject('DefaultPipeline', name='DefaultCollisionPipeline', depth="6")
root.createObject('BruteForceBroadPhase', name='N2')
root.createObject('BVHNarrowPhase')
root.createObject('DiscreteIntersection')
root.createObject('DefaultContactManager', name="Response", response="CompliantContact", responseParams="compliance=0&restitution=0" )
##### SOLVER
root.createObject('CompliantImplicitSolver', stabilization=1, neglecting_compliance_forces_in_geometric_stiffness=1)
root.createObject('SequentialSolver', iterations=100, precision=0)
#root.createObject('LUResponse')
root.createObject('LDLTResponse')
##### GEAR
gearNode = root.createChild( "GEAR" )
r0 = 0.33
r1 = 0.66
body0 = StructuralAPI.RigidBody( gearNode, "body_0" )
body0.setManually( [0,0,0,0,0,0,1], 1, [1,1,1] )
body0.dofs.showObject = True
body0.dofs.showObjectScale = r0*1.1
body0.dofs.velocity="0 0 0 0 1 0"
body0.node.createObject('Sphere', radius=r0)
body1 = StructuralAPI.RigidBody( gearNode, "body_1" )
body1.setManually( [1,0,0,0,0,0,1], 1, [1,1,1] )
body1.dofs.showObject = True
body1.dofs.showObjectScale = r1*1.1
body1.node.createObject('Sphere', radius=r1)
body0.node.createObject('PartialFixedConstraint', fixedDirections="1 1 1 1 0 1")
body1.node.createObject('PartialFixedConstraint', fixedDirections="1 1 1 1 0 1")
d = body0.node.createChild( "d" )
d.createObject('MechanicalObject', template = 'Vec1'+StructuralAPI.template_suffix, name = 'dofs', position = '0 0 0' )
input = [] # @internal
input.append( '@' + Tools.node_path_rel(root,body0.node) + '/dofs' )
input.append( '@' + Tools.node_path_rel(root,body1.node) + '/dofs' )
d.createObject('GearMultiMapping', name = 'mapping', input = Tools.cat(input), output = '@dofs', pairs = "0 4 0 4", ratio = -r0/r1 )
body1.node.addChild( d )
d.createObject('UniformCompliance', name = 'compliance', compliance="0" )
##### driving belt / chain
beltNode = root.createChild( "BELT" )
r0 = 0.7
r1 = 0.3
body0 = StructuralAPI.RigidBody( beltNode, "body_0" )
body0.setManually( [0,-2,0,0,0,0,1], 1, [1,1,1] )
body0.dofs.showObject = True
body0.dofs.showObjectScale = r0*1.1
body0.dofs.velocity="0 0 0 0 1 0"
body0.node.createObject('Sphere', radius=r0)
body1 = StructuralAPI.RigidBody( beltNode, "body_1" )
body1.setManually( [1.5,-2,0,0,0,0,1], 1, [1,1,1] )
body1.dofs.showObject = True
body1.dofs.showObjectScale = r1*1.1
body1.node.createObject('Sphere', radius=r1)
body0.node.createObject('PartialFixedConstraint', fixedDirections="1 1 1 1 0 1")
body1.node.createObject('PartialFixedConstraint', fixedDirections="1 1 1 1 0 1")
d = body0.node.createChild( "d" )
d.createObject('MechanicalObject', template = 'Vec1'+StructuralAPI.template_suffix, name = 'dofs', position = '0 0 0' )
input = [] # @internal
input.append( '@' + Tools.node_path_rel(root,body0.node) + '/dofs' )
input.append( '@' + Tools.node_path_rel(root,body1.node) + '/dofs' )
d.createObject('GearMultiMapping', name = 'mapping', input = Tools.cat(input), output = '@dofs', pairs = "0 4 0 4", ratio = r0/r1 )
body1.node.addChild( d )
d.createObject('UniformCompliance', name = 'compliance', compliance="0" )
##### angle transmission
angleNode = root.createChild( "ANGLE" )
r0 = 0.49
r1 = 0.49
body0 = StructuralAPI.RigidBody( angleNode, "body_0" )
body0.setManually( [0,-4,0,0,0,0,1], 1, [1,1,1] )
body0.dofs.showObject = True
body0.dofs.showObjectScale = r0*1.1
body0.dofs.velocity="0 0 0 1 0 0"
body0.node.createObject('Sphere', radius=r0)
body1 = StructuralAPI.RigidBody( angleNode, "body_1" )
body1.setManually( [1,-4,0,0,0,0,1], 1, [1,1,1] )
body1.dofs.showObject = True
body1.dofs.showObjectScale = r1*1.1
body1.node.createObject('Sphere', radius=r1)
body0.node.createObject('PartialFixedConstraint', fixedDirections="1 1 1 0 1 1")
body1.node.createObject('PartialFixedConstraint', fixedDirections="1 1 1 1 0 1")
d = body0.node.createChild( "d" )
d.createObject('MechanicalObject', template = 'Vec1'+StructuralAPI.template_suffix, name = 'dofs', position = '0 0 0' )
input = [] # @internal
input.append( '@' + Tools.node_path_rel(root,body0.node) + '/dofs' )
input.append( '@' + Tools.node_path_rel(root,body1.node) + '/dofs' )
d.createObject('GearMultiMapping', name = 'mapping', input = Tools.cat(input), output = '@dofs', pairs = "0 3 0 4", ratio = r0/r1 )
body1.node.addChild( d )
d.createObject('UniformCompliance', name = 'compliance', compliance="0" )
##### rack
rackNode = root.createChild( "RACK" )
body0 = StructuralAPI.RigidBody( rackNode, "body_0" )
body0.setManually( [0,-6,0,0,0,0,1], 1, [1,1,1] )
body0.dofs.showObject = True
body0.dofs.showObjectScale = 0.55
body0.dofs.velocity="0 0 0 0 0 1"
body0.node.createObject('Sphere', radius=0.5)
body1 = StructuralAPI.RigidBody( rackNode, "body_1" )
body1.setManually( [-2,-6.71,0, 0,0,0.7071067811865476,0.7071067811865476], 1, [1,1,1] )
body1.dofs.showObject = True
body1.dofs.showObjectScale = 0.3
body1.node.createObject('Capsule', radii="0.2", heights="5")
body0.node.createObject('PartialFixedConstraint', fixedDirections="1 1 1 1 1 0")
body1.node.createObject('PartialFixedConstraint', fixedDirections="0 1 1 1 1 1")
d = body0.node.createChild( "d" )
d.createObject('MechanicalObject', template = 'Vec1'+StructuralAPI.template_suffix, name = 'dofs', position = '0 0 0' )
input = [] # @internal
input.append( '@' + Tools.node_path_rel(root,body0.node) + '/dofs' )
input.append( '@' + Tools.node_path_rel(root,body1.node) + '/dofs' )
d.createObject('GearMultiMapping', name = 'mapping', input = Tools.cat(input), output = '@dofs', pairs = "0 5 0 0", ratio = 1 )
body1.node.addChild( d )
d.createObject('UniformCompliance', name = 'compliance', compliance="0" )
def insertRigid(parentNode, rigidModel, density, scale=1, param=None):
""" create a StructuralAPI.RigidBody from the rigidModel. The model geometry is scaled with scale.
The rigidMass is computed from:
1) mass, com and inertia if present
2) mesh if possible
3) default to a unit sphere TODO: is it relevant to do so ?
"""
if printLog:
Sofa.msg_info("Compliant.sml","insertRigid "+rigidModel.name)
for mesh in rigidModel.mesh :
if rigidModel.meshAttributes[mesh.id].collision is True:
Sofa.msg_info("Compliant.sml"," collision mesh: "+mesh.name)
rigid = StructuralAPI.RigidBody(parentNode, rigidModel.name)
if not rigidModel.mass is None and not rigidModel.com is None and not rigidModel.inertia is None:
if not 1==scale:
Sofa.msg_info("Compliant.sml","scale is not supported in that case")
# all inertial data is present, let's use it
massinfo = SofaPython.mass.RigidMassInfo()
massinfo.mass = rigidModel.mass # TODO: convert units ?
massinfo.com = rigidModel.com # TODO: convert units ?
if len(rigidModel.inertia)==3 and not rigidModel.inertia_rotation is None:
massinfo.diagonal_inertia = rigidModel.inertia
massinfo.inertia_rotation = rigidModel.inertia_rotation
else:
massinfo.setFromInertia(rigidModel.inertia[0], rigidModel.inertia[1], rigidModel.inertia[2], # Ixx, Ixy, Ixz
rigidModel.inertia[3], rigidModel.inertia[4], # Iyy, Iyz
rigidModel.inertia[5] ) # Izz
rigid.setFromRigidInfo(massinfo, offset=rigidModel.position, inertia_forces = False ) # TODO: handle inertia_forces ?
elif len(rigidModel.mesh)!=0 :
# get inertia from meshes and density
rigid.setFromRigidInfo(rigidModel.getRigidMassInfo(density, scale), offset=StructuralAPI.scaleOffset(scale, rigidModel.position), inertia_forces = False ) # TODO: handle inertia_forces ?
#if not rigidModel.mass is None :
## no density but a mesh let's normalise computed mass with specified mass
#mass= SofaPython.units.mass_from_SI(rigidModel.mass)
#inertia = []
#for inert,m in zip(rigid.mass.inertia, rigid.mass.mass):
#for i in inert:
#inertia.append( i/m[0]*mass)
#rigid.mass.inertia = concat(inertia)
#rigid.mass.mass = mass
else:
# no mesh, get mass/inertia if present, default to a unit sphere
Sofa.msg_warning("Compliant.sml","insertRigid: using default rigidMass")
mass = rigidModel.mass if not rigidModel.mass is None else SofaPython.units.mass_from_SI(1.)
inertia = scale*scale*[1,1,1]
t = scale*rigidModel.position
if not rigidModel.com is None:
t[0] += scale*rigidModel.com[0]
t[1] += scale*rigidModel.com[1]
t[2] += scale*rigidModel.com[2]
rigid.setManually(t, mass, inertia)
if not param is None:
rigid.dofs.showObject = param.showRigid
rigid.dofs.showObjectScale = SofaPython.units.length_from_SI(param.showRigidScale)
# walk around to handle multiple meshes
# @todo: handle them in StructuralAPI ?
rigid.collisions=dict()
rigid.visuals=dict()
for mesh in rigidModel.mesh :
if rigidModel.meshAttributes[mesh.id].collision is True:
rigid.collisions[mesh.id] = rigid.addCollisionMesh(mesh.source,name_suffix='_'+mesh.name, scale3d=[scale]*3)
if rigidModel.meshAttributes[mesh.id].visual is True:
rigid.visuals[mesh.id] = rigid.collisions[mesh.id].addVisualModel()
elif rigidModel.meshAttributes[mesh.id].visual is True:
rigid.visuals[mesh.id] = rigid.addVisualModel(mesh.source,name_suffix='_'+mesh.name, scale3d=[scale]*3)
return rigid
def createScene(root):
#### print HELP
print "\n\n################"
print "'UP' and 'DOWN' to control hinge position between base and arm0"
print "'LEFT' and 'RIGHT' to control hinge velocity between arm0 and arm"
print "'+' and '-' to control finger slider forces"
print "(do no forget to press CTRL)"
print "################\n"
sys.stdout.flush()
##### global parameters
root.createObject('VisualStyle', displayFlags="showBehaviorModels" )
root.dt = 0.001
root.gravity = [0, -9.8, 0]
root.createObject('RequiredPlugin', pluginName = 'Compliant')
root.createObject('CompliantAttachButtonSetting')
##### SOLVER
root.createObject('CompliantImplicitSolver', stabilization=1)
root.createObject('SequentialSolver', iterations=100)
root.createObject('LDLTResponse')
### OBJETS
base = StructuralAPI.RigidBody( root, "base" )
base.setFromMesh( "mesh/PokeCube.obj", 1000, [0,-4,0,0,0,0,1], [1,10,1] )
cm = base.addCollisionMesh( "mesh/PokeCube.obj", [1,10,1] )
cm.triangles.group = "1"
cm.addVisualModel()
base.node.createObject('FixedConstraint')
basearmoffset = base.addOffset( "basearmoffset", [0,4.5,0,0,0,0,1] )
basearmoffset.dofs.showObject=True
basearmoffset.dofs.showObjectScale=1
arm0 = StructuralAPI.RigidBody( root, "arm0" )
arm0.setFromMesh( "mesh/PokeCube.obj", 100, [0,5,0,0,0,0,1], [.9,10,.9] )
cm = arm0.addCollisionMesh( "mesh/PokeCube.obj", [.9,10,.9] )
cm.triangles.group = "1"
vm = cm.addVisualModel()
vm.model.setColor( 1,1,0,1 )
armbaseoffset = arm0.addOffset( "armbaseoffset", [0,-4.5,0,0,0,0,1] )
armbaseoffset.dofs.showObject=True
armbaseoffset.dofs.showObjectScale=1
arm0armoffset = arm0.addOffset( "arm0armoffset", [0,4.5,0,0,0,0,1] )
arm0armoffset.dofs.showObject=True
arm0armoffset.dofs.showObjectScale=1
arm = StructuralAPI.RigidBody( root, "arm" )
arm.setFromMesh( "mesh/PokeCube.obj", 10, [0,14,0,0,0,0,1], [.8,10,.8] )
cm = arm.addCollisionMesh( "mesh/PokeCube.obj", [.8,10,.8] )
cm.triangles.group = "1"
vm = cm.addVisualModel()
vm.model.setColor( 1,0,0,1 )
armarm0offset = arm.addOffset( "armarm0offset", [0,-4.5,0,0,0,0,1] )
armarm0offset.dofs.showObject=True
armarm0offset.dofs.showObjectScale=1
armfingeroffset = arm.addOffset( "armfingeroffset", [0,4.75,0,0,0,0,1] )
armfingeroffset.dofs.showObject=True
armfingeroffset.dofs.showObjectScale=1
fingerL = StructuralAPI.RigidBody( root, "fingerL" )
fingerL.setFromMesh( "mesh/PokeCube.obj", 1, [-1,20,0,0,0,0,1], [.5,3,.5] )
cm = fingerL.addCollisionMesh( "mesh/PokeCube.obj", [.5,3,.5] )
cm.triangles.group = "1"
vm = cm.addVisualModel()
vm.model.setColor( 0,1,0,1 )
fingerLarmoffset = fingerL.addOffset( "fingerLarmoffset", [0,-1.25,0,0,0,0,1] )
fingerLarmoffset.dofs.showObject=True
fingerLarmoffset.dofs.showObjectScale=1
fingerR = StructuralAPI.RigidBody( root, "fingerR" )
fingerR.setFromMesh( "mesh/PokeCube.obj", 1, [1,20,0,0,0,0,1], [.5,3,.5] )
cm = fingerR.addCollisionMesh( "mesh/PokeCube.obj", [.5,3,.5] )
cm.triangles.group = "1"
vm = cm.addVisualModel()
vm.model.setColor( 0,0,1,1 )
fingerRarmoffset = fingerR.addOffset( "fingerRarmoffset", [0,-1.25,0,0,0,0,1] )
fingerRarmoffset.dofs.showObject=True
fingerRarmoffset.dofs.showObjectScale=1
##### LINKS
global hingebasearm0_positionController
global hingearm0arm_velocityController
global sliderfingerLarm_forceController
global sliderfingerRarm_forceController
hingebasearm = StructuralAPI.HingeRigidJoint( 2, "hingebasearm", basearmoffset.node, armbaseoffset.node )
hingebasearm0_positionController = hingebasearm.addPositionController(hingebasearm0_offset,1e-8)
hingebasearm.addLimits(-hingebasearm0_limit,hingebasearm0_limit)
hingearm0arm = StructuralAPI.HingeRigidJoint( 2, "hingearm0arm", arm0armoffset.node, armarm0offset.node )
hingearm0arm_velocityController = hingearm0arm.addVelocityController(hingearm0arm_velocity,0)
#hingearm0arm.addLimits(-.75,.75) # limits with velocity controller are not well handled
sliderfingerLarm = StructuralAPI.SliderRigidJoint( 0, "sliderfingerLarm", armfingeroffset.node, fingerLarmoffset.node )
sliderfingerLarm_forceController = sliderfingerLarm.addForceController(-sliderfinger_force)
sliderfingerLarm.addLimits(-2,-0.25)
sliderfingerRarm = StructuralAPI.SliderRigidJoint( 0, "sliderfingerRarm", armfingeroffset.node, fingerRarmoffset.node )
sliderfingerRarm_forceController = sliderfingerRarm.addForceController(sliderfinger_force)
sliderfingerRarm.addLimits(0.25,2)
root.createObject('PythonScriptController', filename = __file__, classname = 'Controller')
def createScene(node):
scene = Tools.scene(node)
style = node.getObject('style')
style.findData('displayFlags').showMappings = False
style.findData('displayFlags').showVisual = False
style.findData('displayFlags').showCollision = True
manager = node.getObject('manager')
manager.response = 'PenalityCompliantContact'
manager.responseParams = "stiffness=1e5"
node.dt = 2.5e-3
# node.createObject('CompliantAttachButton')
ode = node.getObject('ode')
node.removeObject(ode)
ode = node.createObject("EulerImplicitSolver",
rayleighStiffness=0,
rayleighMass=0)
num = node.createObject('CGLinearSolver',
name='num',
iterations=100,
tolerance=1e-7,
threshold=0)
num.printLog = 1
# num = node.createObject('CgSolver', name = 'num', iterations = 100, precision = 1e-7 )
proximity = node.getObject('proximity')
proximity.alarmDistance = 0.1
proximity.contactDistance = 0
proximity.useLineLine = True
# planes
for i in xrange(4):
mesh = dir + '/../mesh/ground.obj'
plane = StructuralAPI.RigidBody(node, "plane-{}".format(i))
g = Rigid3()
n = np.zeros(3)
n[:] = [0, 1, 0]
r = Quaternion.exp(math.pi / 8 * np.array([1.0, 0.0, 0.0]))
q = Quaternion.exp(i * math.pi / 2 * np.array([0.0, 1.0, 0.0]))
g.orient = q * r
plane.setManually(g, 1, [1, 1, 1])
#body3.setFromMesh( mesh, 1 )
cm = plane.addCollisionMesh(mesh, [10, 1, 10])
cm.triangles.group = "0"
cm.addVisualModel()
plane.node.createObject('FixedConstraint', indices='0')
cm.triangles.contactFriction = 0.5 # per object friction coefficient
# box
particles = node.createChild('particles')
n = 400
for i in xrange(n):
p = particles.createChild('particle-{0}'.format(i))
dofs = p.createObject('MechanicalObject', template="Vec3d")
pos = np.zeros(3)
vel = np.zeros(3)
pos[1] = 2 + i / 1.5
vel[1] = -1
dofs.position = pos.tolist()
dofs.velocity = vel.tolist()
mass = p.createObject('UniformMass', template='Vec3d')
model = p.createObject('SphereModel',
template='Vec3d',
selfCollision=True,
radius=0.1)
def createScene(node):
# global
node.createObject(
'VisualStyle',
displayFlags=
'hideBehaviorModels hideCollisionModels hideMappings hideForceFields')
node.dt = 0.01
node.gravity = '0 -9.81 0'
node.createObject('RequiredPlugin', name='Compliant')
node.createObject('CompliantAttachButtonSetting')
# solvers
compliance = 0
node.createObject('CompliantImplicitSolver',
name='odesolver',
stabilization=1)
#node.createObject('MinresSolver', name='numsolver', iterations='500', precision='1e-14');
node.createObject('LDLTSolver', name='numsolver', schur=0)
# scene
scene = node.createChild('scene')
rigids = []
offsets = []
joints = []
# create rigid bodies
for p in parts:
r = StructuralAPI.RigidBody(scene, p[0])
mesh = mesh_path + '/' + p[3]
density = float(p[2])
offset = p[1]
r.setFromMesh(mesh, density, offset)
cm = r.addCollisionMesh(mesh)
cm.addVisualModel() # visual model similar to collision model
#r.addVisualModel( mesh ) # if the visual model was different from the collision model
r.dofs.showObject = True
r.dofs.showObjectScale = 0.5
rigids.append(r)
# fix first body
rigids[0].node.createObject('FixedConstraint')
# create offsets
for o in joint_offsets:
o = rigids[o[1]].addOffset(o[0], o[2])
o.dofs.showObject = True
o.dofs.showObjectScale = 0.25
offsets.append(o)
## create joints
for l in links:
j = l[3](l[4], l[0], offsets[l[1]].node, offsets[l[2]].node)
j.constraint.compliance.compliance = compliance
joints.append(j)
# just for fun!
#rigids[1].addMotor([0,0,0,0,0,1000])
return node
