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 setup(self,
useCompliant=1,
compliance=1E-6,
showOffset=False,
showOffsetScale=0.1):
# Variable
_isLimited = False
_compliance = 0 if useCompliant else compliance
# Needs to be fixed and used late
boneA_offset = self.boneA.body.addAbsoluteOffset(
self.boneA.name + '_offset_joint_' + self.name, self.frame)
boneB_offset = self.boneB.body.addAbsoluteOffset(
self.boneB.name + '_offset_joint_' + self.name, self.frame)
# joint creation between the offsets
self.joint = StructuralAPI.GenericRigidJoint(self.name,
boneA_offset.node,
boneB_offset.node,
self.mask, _compliance)
# add of the limits
for l in self.limits:
if l != 0:
_isLimited = True
if _isLimited:
self.joint.addLimits(self.limits, _compliance)
# visualization of offset: check that all frames are well placed
boneA_offset.dofs.showObject = showOffset
boneB_offset.dofs.showObject = showOffset
boneA_offset.dofs.showObjectScale = showOffsetScale
boneB_offset.dofs.showObjectScale = showOffsetScale
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 insertJoint(jointModel, rigids, param):
""" create a StructuralAPI.GenericRigidJoint from the jointModel """
frames = list()
for i, offset in enumerate(jointModel.offsets):
if not jointModel.solids[i].id in rigids:
Sofa.msg_warning(
"Compliant.sml", "insertJoint " + jointModel.name +
" failed: " + jointModel.solids[i].id + " is not a rigid body")
return None
rigid = rigids[jointModel.solids[i].id] # shortcut
if rigid is None:
Sofa.msg_warning(
"Compliant.sml",
"in joint {0}, solid {1} is missing, ignored".format(
jointModel.name, jointModel.solids[i].id))
return
if not offset is None:
if offset.isAbsolute():
frames.append(
rigid.addAbsoluteOffset(offset.name, offset.value))
else:
frames.append(rigid.addOffset(offset.name, offset.value))
if not param is None:
frames[-1].dofs.showObject = param.showOffset
frames[
-1].dofs.showObjectScale = SofaPython.units.length_from_SI(
param.showOffsetScale)
else:
frames.append(rigid)
if printLog:
Sofa.msg_info("Compliant.sml", "insertJoint " + jointModel.name)
mask = [1] * 6
limits = [] # mask for limited dofs
isLimited = True # does the joint have valid limits?
for d in jointModel.dofs:
if isLimited:
if d.min == None or d.max == None:
isLimited = False # as soon as a limit is not defined, the limits cannot work
else:
limits.append(d.min)
limits.append(d.max)
mask[d.index] = 0
joint = StructuralAPI.GenericRigidJoint(
jointModel.name,
frames[0].node,
frames[1].node,
mask,
compliance=SofaPython.sml.getValueByTag(param.jointComplianceByTag,
jointModel.tags),
isCompliance=SofaPython.sml.getValueByTag(param.jointIsComplianceByTag,
jointModel.tags))
if isLimited:
joint.addLimits(limits)
return joint
def insertJoint(jointModel, rigids, scale=1, param=None):
""" create a StructuralAPI.GenericRigidJoint from the jointModel """
frames=list()
for i,offset in enumerate(jointModel.offsets):
if not jointModel.solids[i].id in rigids:
Sofa.msg_warning("Compliant.sml","insertJoint "+jointModel.name+" failed: "+jointModel.solids[i].id+" is not a rigid body")
return None
rigid = rigids[jointModel.solids[i].id] # shortcut
if rigid is None:
Sofa.msg_warning("Compliant.sml", "in joint {0}, solid {1} is missing, ignored".format(jointModel.name, jointModel.solids[i].id))
return
if not offset is None:
if offset.isAbsolute():
frames.append( rigid.addAbsoluteOffset(offset.name, StructuralAPI.scaleOffset(scale,offset.value)) )
else:
frames.append( rigid.addOffset(offset.name, StructuralAPI.scaleOffset(scale,offset.value)) )
if not param is None:
frames[-1].dofs.showObject = param.showOffset
frames[-1].dofs.showObjectScale = SofaPython.units.length_from_SI(param.showOffsetScale)
else:
frames.append(rigid)
if printLog:
Sofa.msg_info("Compliant.sml","insertJoint "+jointModel.name)
mask = [1]*6
limits=[] # mask for limited dofs
isLimited = True # does the joint have valid limits?
for d in jointModel.dofs:
if isLimited:
if d.min==None or d.max==None:
isLimited = False # as soon as a limit is not defined, the limits cannot work
else:
limits.append(d.min)
limits.append(d.max)
mask[d.index] = 0
joint = StructuralAPI.GenericRigidJoint(jointModel.name, frames[0].node, frames[1].node, mask,
compliance=SofaPython.sml.getValueByTag(param.jointComplianceByTag, jointModel.tags),
isCompliance=SofaPython.sml.getValueByTag(param.jointIsComplianceByTag, jointModel.tags))
if isLimited:
joint.addLimits(limits)
return joint
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 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(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 rigid_mass_info_offset(rigidModel, density, scale):
'''compute mass info and offset from rigid model'''
if not rigidModel.mass is None and not rigidModel.com is None and not rigidModel.inertia is None:
# sml model has all the info needed
if scale != 1: log_info('scale is not supported in that case')
return model_mass_info(rigidModel), rigidModel.position
elif rigidModel.mesh:
# there is a mesh: compute from it
mi = rigidModel.getRigidMassInfo(density, scale)
offset = StructuralAPI.scaleOffset(scale, rigidModel.position)
return mi, offset
else:
# default
log_warning("using default rigid mass distribution")
mi = SofaPython.mass.RigidMassInfo()
mi.mass = rigidModel.mass if rigidModel.mass is not None else SofaPython.units.mass_from_SI(
1.)
mi.inertia = [scale * scale] * 3
mi.com = scale * rigidModel.position # TODO does this even work?
return mi, None
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 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):
# 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')
