def __init__(self, rigidNode, scaleNode, name, offset, arg=-1):
# node creation
self.node = rigidNode.createChild(name)
scaleNode.addChild(self.node)
# variables
self.frame = Frame.Frame(offset)
path_offset_rigid = '@' + Tools.node_path_rel(self.node, rigidNode)
path_offset_scale = '@' + Tools.node_path_rel(self.node, scaleNode)
# scene creation
self.dofs = self.frame.insert(self.node,
template='Rigid3' + template_suffix,
name='dofs')
if arg == -1:
self.mapping = self.node.createObject(
'RigidScaleToRigidMultiMapping',
template='Rigid3' + template_suffix + ',Vec3' +
template_suffix + ',Rigid3' + template_suffix,
input1=path_offset_rigid,
input2=path_offset_scale,
output='@.',
useGeometricStiffness=geometric_stiffness,
printLog='0')
else:
self.mapping = self.node.createObject(
'RigidScaleToRigidMultiMapping',
template='Rigid3' + template_suffix + ',Vec3' +
template_suffix + ',Rigid3' + template_suffix,
input1=path_offset_rigid,
input2=path_offset_scale,
output='@.',
index='0 ' + str(arg) + ' ' + str(arg),
useGeometricStiffness=geometric_stiffness,
printLog='0')
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 setManually(self, filepath=None, offset=[[0,0,0,0,0,0,1]], voxelSize=0.01, density=1000, generatedDir=None):
if len(offset) == 0:
Sofa.msg_error("RigidScale.API","ShearlessAffineBody should have at least 1 ShearLessAffine")
return
self.framecom = Frame.Frame()
self.bodyOffset = Frame.Frame([0,0,0,0,0,0,1])
path_affine_rigid = '@'+ Tools.node_path_rel(self.affineNode, self.rigidNode)
path_affine_scale = '@'+ Tools.node_path_rel(self.affineNode, self.scaleNode)
if len(offset) == 1: self.frame = [Frame.Frame(offset[0])]
str_position = ""
for p in offset:
str_position = str_position + concat(p) + " "
### scene creation
# rigid dof
self.rigidDofs = self.rigidNode.createObject('MechanicalObject', template='Rigid3'+template_suffix, name='dofs', position=str_position, rest_position=str_position)
# scale dofs
self.scaleDofs = self.scaleNode.createObject('MechanicalObject', template='Vec3'+template_suffix, name='dofs', position=concat([1,1,1]*len(offset)))
positiveNode = self.scaleNode.createChild('positive')
positiveNode.createObject('MechanicalObject', template='Vec3'+template_suffix, name='positivescaleDOFs')
positiveNode.createObject('DifferenceFromTargetMapping', template='Vec3'+template_suffix+',Vec3'+template_suffix, applyRestPosition=1, targets=concat(target_scale))
positiveNode.createObject('UniformCompliance', isCompliance=1, compliance=0)
positiveNode.createObject('UnilateralConstraint')
positiveNode.createObject('Stabilization', name='Stabilization')
# affine dofs
self.affineDofs = self.affineNode.createObject('MechanicalObject', template='Affine', name='parent', showObject=0)
self.affineNode.createObject('RigidScaleToAffineMultiMapping', template='Rigid,Vec3,Affine', input1=path_affine_rigid, input2=path_affine_scale, output='@.', autoInit='1', printLog='0')
if filepath:
self.image = SofaImage.API.Image(self.affineNode, name="image_" + self.name, imageType="ImageUC")
self.shapeFunction = Flexible.API.ShapeFunction(self.affineNode)
if generatedDir is None:
self.image.addMeshLoader(filepath, value=1, insideValue=1, scale=scale3d) # TODO support multiple meshes closingValue=1,
self.image.addMeshToImage(voxelSize)
self.shapeFunction.addVoronoi(self.image, position='@dofs.rest_position')
# mass
self.affineMassNode = self.affineNode.createChild('mass')
self.affineMassNode.createObject('TransferFunction', name='density', template='ImageUC,ImageD', inputImage='@../image.image', param='0 0 1 '+str(density))
self.affineMassNode.createObject('MechanicalObject', template='Vec3'+template_suffix)
self.affineMassNode.createObject('LinearMapping', template='Affine,Vec3'+template_suffix)
self.affineMassNode.createObject('MassFromDensity', name='MassFromDensity', template='Affine,ImageD', image='@density.outputImage', transform='@../image.transform', lumping='0')
self.mass = self.affineNode.createObject('AffineMass', massMatrix='@mass/MassFromDensity.massMatrix')
else:
self.image.addContainer(filename=self.node.name + "_rasterization.raw", directory=generatedDir)
self.shapeFunction.shapeFunction = serialization.importImageShapeFunction(self.affineNode, generatedDir+self.node.name+"_SF_indices.raw", generatedDir+self.node.name+"_SF_weights.raw", 'dofs')
self.mass = serialization.importAffineMass(self.affineNode, generatedDir+self.node.name+"_affinemass.json")
# computation of the object mass center
massInfo = SofaPython.mass.RigidMassInfo()
massInfo.setFromMesh(filepath, density, [1,1,1])
# get the object mass center
self.framecom.rotation = massInfo.inertia_rotation
self.framecom.translation = massInfo.com
else:
print "You need a mesh to create an articulated system"
self.frame = []
for o in offset:
self.frame.append(Frame.Frame(o))
def __insert_registration_node(self):
""" Insert the registration node in the graph, under dofRigid node. """
registration_node = self.createChild(self.nodes["dofRigid"], "RegistrationNode")
param = self.param # shortcut
# merge topologies
repartition = ""
sources_component_name = []
collision_node = [elem.collision for elem in self.rigids.values()]
for i, elem in enumerate(collision_node):
sources_component_name.append(Tools.node_path_rel(registration_node, elem.node) + "/topology")
repartition += "{0} ".format(i) * len(elem.topology.position)
registration_node.createObject('MergeMeshes', name='source_topology', nbMeshes=len(sources_component_name),
**dict({'position' + str(i + 1): '@' + item + '.position' for i, item in
enumerate(sources_component_name)},
**{'triangles' + str(i + 1): '@' + item + '.triangles' for i, item in
enumerate(sources_component_name)}))
registration_node.createObject('MeshTopology', name='topo', src='@./source_topology')
registration_node.createObject('MechanicalObject', name='DOFs')
registration_node.createObject('Triangle')
registration_node.createObject('RigidMapping', template='Rigid3d,Vec3d',
input="@" + Tools.node_path_rel(registration_node, self.nodes["dofRigid"]),
output="@DOFs", rigidIndexPerPoint=repartition)
registration_node.createObject('NormalsFromPoints', name='NormalsFromPoints',
template='Vec3d', position='@DOFs.position',
triangles='@topo.triangles',
invertNormals=self.param.invertSourceNormals)
# Force Field
target_path = Tools.node_path_rel(registration_node, self.nodes["Target"])
registration_node.createObject('ClosestPointRegistrationForceField', name='ICP',
template='Vec3d',
# source
sourceTriangles='@topo.triangles',
sourceNormals='@NormalsFromPoints.normals',
# target
position='@{0}/loader.position'.format(target_path),
triangles='@{0}/loader.triangles'.format(target_path),
normals='@{0}/NormalsFromPoints.normals'.format(target_path),
# Param
cacheSize='4', blendingFactor=param.blendingFactor,
stiffness=param.stiffness,
damping=param.damping,
outlierThreshold=param.outlierThreshold,
normalThreshold=param.normalThreshold,
rejectOutsideBbox=param.rejectOutsideBbox,
drawColorMap='0')
def setFromMesh(self, filepath, density=1000, offset=[0,0,0,0,0,0,1], scale3d=[1,1,1], voxelSize=0.01, numberOfPoints=1, generatedDir=None):
# variables
self.bodyOffset = Frame.Frame(offset)
path_affine_rigid = '@' + Tools.node_path_rel(self.affineNode, self.rigidNode)
path_affine_scale = '@' + Tools.node_path_rel(self.affineNode, self.scaleNode)
massInfo = SofaPython.mass.RigidMassInfo()
massInfo.setFromMesh(filepath, density, scale3d)
self.image = SofaImage.API.Image(self.node, name="image_" + self.name, imageType="ImageUC")
self.image.node.addChild(self.affineNode) # for initialization
self.image.node.addChild(self.rigidNode) # for initialization
self.shapeFunction = Flexible.API.ShapeFunction(self.rigidNode)
if generatedDir is None:
self.image.addMeshLoader(filepath, value=1, insideValue=1, offset=offset, scale=scale3d) # TODO support multiple meshes closingValue=1,
self.image.addMeshToImage(voxelSize)
# rigid dofs
self.sampler = SofaImage.API.Sampler(self.rigidNode)
self.sampler.addImageSampler(self.image, numberOfPoints)
self.rigidDofs = self.sampler.addMechanicalObject('Rigid3'+template_suffix)
else:
self.image.addContainer(filename=self.node.name+"_rasterization.raw", directory=generatedDir)
self.rigidDofs = serialization.importRigidDofs(self.rigidNode, generatedDir+"/"+self.node.name+'_dofs.json')
# scale dofs
self.scaleDofs = self.scaleNode.createObject('MechanicalObject', template='Vec3'+template_suffix, name='dofs', position=concat([1,1,1]*numberOfPoints))
positiveNode = self.scaleNode.createChild('positive')
positiveNode.createObject('MechanicalObject', template='Vec3'+template_suffix, name='positivescaleDOFs')
positiveNode.createObject('DifferenceFromTargetMapping', template='Vec3d,Vec3'+template_suffix, applyRestPosition=1, targets=concat(target_scale))
positiveNode.createObject('UniformCompliance', isCompliance=1, compliance=0)
positiveNode.createObject('UnilateralConstraint')
positiveNode.createObject('Stabilization', name='Stabilization')
# affine dofs
self.affineDofs = self.affineNode.createObject('MechanicalObject', template='Affine', name='dofs')
self.affineNode.createObject('RigidScaleToAffineMultiMapping', template='Rigid,Vec3d,Affine', input1=path_affine_rigid, input2=path_affine_scale, output='@.', autoInit='1', printLog='0')
# shapefunction and mass
if generatedDir is None:
self.shapeFunction.addVoronoi(self.image, position='@dofs.rest_position')
# mass
densityImage = self.image.createTransferFunction(self.affineNode, "density", param='0 0 1 '+str(density))
affineMass = Flexible.API.AffineMass(self.affineNode)
affineMass.massFromDensityImage(self.affineNode, densityImage=densityImage)
self.mass = affineMass.mass
else:
self.shapeFunction.shapeFunction = serialization.importImageShapeFunction(self.affineNode, generatedDir+self.node.name+"_SF_indices.raw", generatedDir+self.node.name+"_SF_weights.raw", 'dofs')
self.mass = serialization.importAffineMass(self.affineNode, generatedDir+self.node.name+"_affinemass.json")
# hack to get the frame position
self.node.init()
for p in self.rigidDofs.position:
p.extend([0,0,0,1])
self.frame.append(Frame.Frame(p))
def setFromMesh(self, filepath, density=1000, offset=[0,0,0,0,0,0,1], scale3d=[1,1,1], voxelSize=0.01, numberOfPoints=1, generatedDir=None):
# variables
self.bodyOffset = Frame.Frame(offset)
path_affine_rigid = '@' + Tools.node_path_rel(self.affineNode, self.rigidNode)
path_affine_scale = '@' + Tools.node_path_rel(self.affineNode, self.scaleNode)
massInfo = SofaPython.mass.RigidMassInfo()
massInfo.setFromMesh(filepath, density, scale3d)
self.image = SofaImage.API.Image(self.node, name="image_" + self.name, imageType="ImageUC")
self.image.node.addChild(self.affineNode) # for initialization
self.image.node.addChild(self.rigidNode) # for initialization
self.shapeFunction = Flexible.API.ShapeFunction(self.rigidNode)
if generatedDir is None:
self.image.addMeshLoader(filepath, value=1, insideValue=1, offset=offset, scale=scale3d) # TODO support multiple meshes closingValue=1,
self.image.addMeshToImage(voxelSize)
# rigid dofs
self.sampler = SofaImage.API.Sampler(self.rigidNode)
self.sampler.addImageSampler(self.image, numberOfPoints)
self.rigidDofs = self.sampler.addMechanicalObject('Rigid3'+template_suffix)
else:
self.image.addContainer(filename=self.node.name+"_rasterization.raw", directory=generatedDir)
self.rigidDofs = serialization.importRigidDofs(self.rigidNode, generatedDir+"/"+self.node.name+'_dofs.json')
# scale dofs
self.scaleDofs = self.scaleNode.createObject('MechanicalObject', template='Vec3'+template_suffix, name='dofs', position=concat([1,1,1]*numberOfPoints))
positiveNode = self.scaleNode.createChild('positive')
positiveNode.createObject('MechanicalObject', template='Vec3'+template_suffix, name='positivescaleDOFs')
positiveNode.createObject('DifferenceFromTargetMapping', template='Vec3d,Vec3'+template_suffix, applyRestPosition=1, targets=concat(target_scale))
positiveNode.createObject('UniformCompliance', isCompliance=1, compliance=0)
positiveNode.createObject('UnilateralConstraint')
# affine dofs
self.affineDofs = self.affineNode.createObject('MechanicalObject', template='Affine', name='dofs')
self.affineNode.createObject('RigidScaleToAffineMultiMapping', template='Rigid,Vec3d,Affine', input1=path_affine_rigid, input2=path_affine_scale, output='@.', autoInit='1', printLog='0')
# shapefunction and mass
if generatedDir is None:
self.shapeFunction.addVoronoi(self.image, position='@dofs.rest_position')
# mass
densityImage = self.image.createTransferFunction(self.affineNode, "density", param='0 0 1 '+str(density))
affineMass = Flexible.API.AffineMass(self.affineNode)
affineMass.massFromDensityImage(self.affineNode, densityImage=densityImage)
self.mass = affineMass.mass
else:
self.shapeFunction.shapeFunction = serialization.importImageShapeFunction(self.affineNode, generatedDir+self.node.name+"_SF_indices.raw", generatedDir+self.node.name+"_SF_weights.raw", 'dofs')
self.mass = serialization.importAffineMass(self.affineNode, generatedDir+self.node.name+"_affinemass.json")
# hack to get the frame position
self.node.init()
for p in self.rigidDofs.position:
p.extend([0,0,0,1])
self.frame.append(Frame.Frame(p))
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(node):
scene = Tools.scene( node )
ode = node.getObject('ode')
ode.stabilization = False
ode.warm_start = False
# ode.debug = True
# ode.propagate_lambdas = True
node.gravity = '0 0 0'
node.dt = 1e-2
num = node.createObject('MinresSolver',
name = 'num',
iterations = 100,
precision = 0)
# resp = node.createObject('DiagonalResponse')
script = node.createObject('PythonScriptController',
filename = __file__,
classname = 'Controller')
style = node.getObject('style')
style.displayFlags = 'showCollisionModels'
# parameters
mass = 1.0
stiff = 1e3
damping = 0.0
# dofs
p = insert_point(scene, 'p', [-1, 0, 0], mass)
sub = p.createChild('sub')
sub.createObject('MechanicalObject',
name = 'dofs',
position = '0 0 0')
sub.createObject('IdentityMapping',
template = 'Vec3d')
compliance = 1/stiff
sub.createObject('UniformCompliance',
template = 'Vec3d',
compliance = compliance,
damping = damping)
shared.dofs = p.getObject('dofs')
shared.mass = mass
shared.stiff = stiff
shared.damping = damping
return node
def createScene(node):
scene = Tools.scene( node )
ode = node.getObject('ode')
ode.stabilization = "no stabilization"
# ode.warm_start = False
# ode.debug = True
node.gravity = '0 0 0'
node.dt = 1e-2
num = node.createObject('MinresSolver',
name = 'num',
iterations = 10,
precision = 0)
style = node.getObject('style')
style.displayFlags = 'showCollisionModels showBehaviorModels'
script = node.createObject('PythonScriptController',
filename = __file__,
classname = 'Controller')
mass = 1
p1 = insert_point(scene, 'p1', [-1, 1, 0], mass)
p2 = insert_point(scene, 'p2', [3, 1, 0], mass)
out = scene.createChild('out')
out.createObject('MechanicalObject',
name = 'dofs',
template = 'Vec1d',
position = '0 0 0')
alpha = 0.2
shared.alpha = alpha
matrix = vec([1 - alpha, 0, 0, alpha, 0, 0,
0, 1 - alpha, 0, 0, alpha, 0,
0, 0, 1 - alpha, 0, 0, alpha] )
value = vec([0, 0, 0])
out.createObject('AffineMultiMapping',
template = 'Vec3d,Vec1d',
input = '@../p1/dofs @../p2/dofs',
output = '@dofs',
matrix = str(matrix),
value = str(value))
out.createObject('UniformCompliance',
template = 'Vec1d',
compliance = 1e-5)
shared.p1 = p1.getObject('dofs')
shared.p2 = p2.getObject('dofs')
def createScene(node):
scene = Tools.scene(node)
ode = node.getObject('ode')
ode.stabilization = "no stabilization"
ode.warm_start = False
# ode.debug = True
# ode.propagate_lambdas = True
node.gravity = '0 0 0'
node.dt = 1e-2
node.createObject('RequiredPlugin', pluginName="SofaLoader")
num = node.createObject('MinresSolver',
name='num',
iterations=100,
precision=0)
# resp = node.createObject('DiagonalResponse')
script = node.createObject('PythonScriptController',
filename=__file__,
classname='Controller')
style = node.getObject('style')
style.displayFlags = 'showCollisionModels'
# parameters
mass = 1.0
stiff = 1e3
damping = 0.0
# dofs
p = insert_point(scene, 'p', [-1, 0, 0], mass)
sub = p.createChild('sub')
sub.createObject('MechanicalObject', name='dofs', position='0 0 0')
sub.createObject('IdentityMapping', template='Vec3d,Vec3d')
compliance = 1 / stiff
sub.createObject('UniformCompliance',
template='Vec3d',
compliance=compliance,
damping=damping)
shared.dofs = p.getObject('dofs')
shared.mass = mass
shared.stiff = stiff
shared.damping = damping
return node
def createScene(node):
scene = Tools.scene( node )
ode = node.getObject('ode')
ode.stabilization = True
ode.warm_start = False
ode.propagate_lambdas = True
# ode.debug = True
node.gravity = '0 -1 0'
num = node.createObject('SequentialSolver',
name = 'num',
iterations = 200,
precision = 0)
resp = node.createObject('DiagonalResponse')
manager = node.getObject('manager')
manager.response = 'CompliantContact'
script = node.createObject('PythonScriptController',
filename = __file__,
classname = 'Controller')
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 = Rigid.Body('rigid')
rigid.collision = path + '/examples/mesh/ground.obj'
rigid.node = rigid.insert( scene )
ground = Rigid.Body('ground')
ground.node = ground.insert( scene )
ground.node.createObject('FixedConstraint', indices = '0')
# blocked joint between ground/rigid
joint = Rigid.Joint('joint')
joint.absolute(Rigid.Frame(), ground.node, rigid.node)
joint.node = joint.insert( scene )
shared.joint = joint
shared.body = rigid
return node
def createScene(node):
scene = Tools.scene(node)
ode = node.getObject('ode')
ode.stabilization = True
ode.warm_start = False
ode.propagate_lambdas = True
# ode.debug = True
node.gravity = '0 -1 0'
num = node.createObject('SequentialSolver',
name='num',
iterations=200,
precision=0)
resp = node.createObject('DiagonalResponse')
manager = node.getObject('manager')
manager.response = 'CompliantContact'
script = node.createObject('PythonScriptController',
filename=__file__,
classname='Controller')
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 = Rigid.Body('rigid')
rigid.collision = path + '/examples/mesh/ground.obj'
rigid.node = rigid.insert(scene)
ground = Rigid.Body('ground')
ground.node = ground.insert(scene)
ground.node.createObject('FixedConstraint', indices='0')
# blocked joint between ground/rigid
joint = Rigid.Joint('joint')
joint.absolute(Rigid.Frame(), ground.node, rigid.node)
joint.node = joint.insert(scene)
shared.joint = joint
shared.body = rigid
return node
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 = "no stabilization"
# ode.warm_start = False
# ode.debug = True
node.gravity = '0 0 0'
node.dt = 1e-2
num = node.createObject('MinresSolver',
name='num',
iterations=10,
precision=0)
style = node.getObject('style')
style.displayFlags = 'showCollisionModels showBehaviorModels'
script = node.createObject('PythonScriptController',
filename=__file__,
classname='Controller')
mass = 1
p1 = insert_point(scene, 'p1', [-1, 1, 0], mass)
p2 = insert_point(scene, 'p2', [3, 1, 0], mass)
out = scene.createChild('out')
out.createObject('MechanicalObject',
name='dofs',
template='Vec1d',
position='0 0 0')
alpha = 0.2
shared.alpha = alpha
matrix = vec([
1 - alpha, 0, 0, alpha, 0, 0, 0, 1 - alpha, 0, 0, alpha, 0, 0, 0,
1 - alpha, 0, 0, alpha
])
value = vec([0, 0, 0])
out.createObject('AffineMultiMapping',
template='Vec3d,Vec1d',
input='@../p1/dofs @../p2/dofs',
output='@dofs',
matrix=str(matrix),
value=str(value))
out.createObject('UniformCompliance', template='Vec1d', compliance=1e-5)
shared.p1 = p1.getObject('dofs')
shared.p2 = p2.getObject('dofs')
def __init__(self, rigidNode, scaleNode, name, offset, arg=-1):
# node creation
self.node = rigidNode.createChild(name)
scaleNode.addChild(self.node)
# variables
self.frame = Frame.Frame(offset)
path_offset_rigid = '@' + Tools.node_path_rel(self.node, rigidNode)
path_offset_scale = '@' + Tools.node_path_rel(self.node, scaleNode)
# scene creation
self.dofs = self.frame.insert(self.node, template='Rigid3'+template_suffix, name='dofs')
if arg==-1:
self.mapping = self.node.createObject('RigidScaleToRigidMultiMapping', template='Rigid3'+template_suffix+',Vec3'+template_suffix+',Rigid3'+template_suffix
, input1=path_offset_rigid, input2=path_offset_scale, output='@.'
, useGeometricStiffness=geometric_stiffness, printLog='0')
else:
self.mapping = self.node.createObject('RigidScaleToRigidMultiMapping', template='Rigid3'+template_suffix+',Vec3'+template_suffix+',Rigid3'+template_suffix
, input1=path_offset_rigid, input2=path_offset_scale, output='@.'
, index='0 '+ str(arg) + ' ' + str(arg), useGeometricStiffness=geometric_stiffness, printLog='0')
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):
# 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 = 1000,
precision = 1e-20)
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 )
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 setMeshLess(self, offset=[[0,0,0,0,0,0,1]], mass=1, rayleigh=0.1, generatedDir=None):
if len(offset) == 0:
Sofa.msg_error("RigidScale.API","ShearlessAffineBody should have at least 1 ShearLessAffine")
return
self.framecom = Frame.Frame()
self.bodyOffset = Frame.Frame([0,0,0,0,0,0,1])
path_affine_rigid = '@' + Tools.node_path_rel(self.affineNode, self.rigidNode)
path_affine_scale = '@' + Tools.node_path_rel(self.affineNode, self.scaleNode)
if len(offset) == 1: self.frame = [Frame.Frame(offset[0])]
str_position = ""
for p in offset:
str_position = str_position + concat(p) + " "
### scene creation
# rigid dof
self.rigidDofs = self.rigidNode.createObject('MechanicalObject', template='Rigid3'+template_suffix, name='dofs', position=str_position, rest_position=str_position)
self.rigidNode.createObject('UniformMass', totalMass=mass, rayleighStiffness=rayleigh);
# scale dofs
self.scaleDofs = self.scaleNode.createObject('MechanicalObject', template='Vec3'+template_suffix, name='dofs', position= concat([1,1,1]*len(offset)))
self.scaleNode.createObject('UniformMass', totalMass=mass, rayleighStiffness=rayleigh);
#positiveNode = self.scaleNode.createChild('positive')
#positiveNode.createObject('MechanicalObject', template='Vec3'+template_suffix, name='positivescaleDOFs')
#target_scale = [0.5,0.5,0.5]
#positiveNode.createObject('DifferenceFromTargetMapping', template='Vec3d,Vec3'+template_suffix, applyRestPosition=1, targets=concat(target_scale))
#positiveNode.createObject('UniformCompliance', isCompliance=1, compliance=0)
#positiveNode.createObject('UnilateralConstraint')
#positiveNode.createObject('Stabilization', name='Stabilization')
# affine dofs
self.affineDofs = self.affineNode.createObject('MechanicalObject', template='Affine', name='parent')
self.affineNode.createObject('RigidScaleToAffineMultiMapping', template='Rigid,Vec3,Affine', input1=path_affine_rigid, input2=path_affine_scale, output='@.', autoInit='1', printLog='0')
self.frame = []
for o in offset:
self.frame.append(Frame.Frame(o))
def setMeshLess(self, offset=[[0,0,0,0,0,0,1]], mass=1, rayleigh=0.1, generatedDir=None):
if len(offset) == 0:
Sofa.msg_error("RigidScale.API","ShearlessAffineBody should have at least 1 ShearLessAffine")
return
self.framecom = Frame.Frame()
self.bodyOffset = Frame.Frame([0,0,0,0,0,0,1])
path_affine_rigid = '@' + Tools.node_path_rel(self.affineNode, self.rigidNode)
path_affine_scale = '@' + Tools.node_path_rel(self.affineNode, self.scaleNode)
if len(offset) == 1: self.frame = [Frame.Frame(offset[0])]
str_position = ""
for p in offset:
str_position = str_position + concat(p) + " "
### scene creation
# rigid dof
self.rigidDofs = self.rigidNode.createObject('MechanicalObject', template='Rigid3'+template_suffix, name='dofs', position=str_position, rest_position=str_position)
self.rigidNode.createObject('UniformMass', totalMass=mass, rayleighStiffness=rayleigh);
# scale dofs
self.scaleDofs = self.scaleNode.createObject('MechanicalObject', template='Vec3'+template_suffix, name='dofs', position= concat([1,1,1]*len(offset)))
self.scaleNode.createObject('UniformMass', totalMass=mass, rayleighStiffness=rayleigh);
#positiveNode = self.scaleNode.createChild('positive')
#positiveNode.createObject('MechanicalObject', template='Vec3'+template_suffix, name='positivescaleDOFs')
#target_scale = [0.5,0.5,0.5]
#positiveNode.createObject('DifferenceFromTargetMapping', template='Vec3d,Vec3'+template_suffix, applyRestPosition=1, targets=concat(target_scale))
#positiveNode.createObject('UniformCompliance', isCompliance=1, compliance=0)
#positiveNode.createObject('UnilateralConstraint')
#positiveNode.createObject('Stabilization', name='Stabilization')
# affine dofs
self.affineDofs = self.affineNode.createObject('MechanicalObject', template='Affine', name='parent')
self.affineNode.createObject('RigidScaleToAffineMultiMapping', template='Rigid,Vec3,Affine', input1=path_affine_rigid, input2=path_affine_scale, output='@.', autoInit='1', printLog='0')
self.frame = []
for o in offset:
self.frame.append(Frame.Frame(o))
def createScene(node):
scene = Tools.scene( node )
style = node.getObject('style')
style.findData('displayFlags').showMappings = True
manager = node.getObject('manager')
manager.response = 'FrictionCompliantContact'
manager.responseParams = 'mu=0' # per object friction coefficient (the friction coef between 2 objects is approximated as the product of both coefs)
ode = node.getObject('ode')
ode.stabilization = True
ode.debug = False
num = node.createObject('SequentialSolver',
name = 'num',
iterations = 100,
precision = 1e-14)
proximity = node.getObject('proximity')
proximity.alarmDistance = 0.5
proximity.contactDistance = 0.2
# plane
plane = Rigid.Body('plane')
plane.visual = dir + '/../mesh/ground.obj'
plane.collision = plane.visual
plane.mass_from_mesh( plane.visual, 10 )
plane.mu = 0.8 # per object friction coefficient
plane.node = plane.insert( scene )
plane.node.createObject('FixedConstraint', indices = '0')
# box
box = Rigid.Body('box')
box.visual = dir + '/../mesh/cube.obj'
box.collision = box.visual
box.dofs.translation = [0, 3, 0]
box.mass_from_mesh( box.visual, 50 )
box.mu = 1 # per object friction coefficient
box.node = box.insert( scene )
def createScene(node):
scene = Tools.scene(node)
style = node.getObject('style')
style.findData('displayFlags').showMappings = True
manager = node.getObject('manager')
manager.response = 'FrictionCompliantContact'
manager.responseParams = 'mu=0' # per object friction coefficient (the friction coef between 2 objects is approximated as the product of both coefs)
ode = node.getObject('ode')
ode.stabilization = True
ode.debug = False
num = node.createObject('SequentialSolver',
name='num',
iterations=100,
precision=1e-14)
proximity = node.getObject('proximity')
proximity.alarmDistance = 0.5
proximity.contactDistance = 0.2
# plane
plane = Rigid.Body('plane')
plane.visual = dir + '/../mesh/ground.obj'
plane.collision = plane.visual
plane.mass_from_mesh(plane.visual, 10)
plane.mu = 0.8 # per object friction coefficient
plane.node = plane.insert(scene)
plane.node.createObject('FixedConstraint', indices='0')
# box
box = Rigid.Body('box')
box.visual = dir + '/../mesh/cube.obj'
box.collision = box.visual
box.dofs.translation = [0, 3, 0]
box.mass_from_mesh(box.visual, 50)
box.mu = 1 # per object friction coefficient
box.node = box.insert(scene)
def __init__(self, parent, name, dofs, dim):
self.node = parent.createChild(name)
self.compliance = 0
self.damping = 0
input = []
dofs_dim = 0
for n in dofs:
input.append( Tools.node_path_rel(self.node, n) )
dofs_dim += len( n.velocity ) * len(n.velocity[0] )
self.matrix = np.zeros( (dim, dofs_dim) )
self.value = np.zeros( dim )
self.dofs = self.node.createObject('MechanicalObject',
name = 'dofs',
template = 'Vec1d',
position = concat( [0] * dofs_dim ) )
template = dofs[0].template
self.map = self.node.createObject('AffineMultiMapping',
name = 'map',
template = '{0}, Vec1d'.format( template ),
input = concat( input ),
output = '@dofs',
matrix = concat( self.matrix.reshape( self.matrix.size ).tolist() ),
value = concat( -self.value ) )
self.ff = self.node.createObject('UniformCompliance',
name = 'ff',
template = 'Vec1d',
compliance = self.compliance,
damping = self.damping )
import Sofa
import math
from Compliant import Rigid, Tools
dir = Tools.path(__file__)
def createScene(node):
scene = Tools.scene(node)
style = node.getObject('style')
style.findData('displayFlags').showMappings = True
manager = node.getObject('manager')
manager.response = 'FrictionCompliantContact'
manager.responseParams = 'mu=0' # per object friction coefficient (the friction coef between 2 objects is approximated as the product of both coefs)
ode = node.getObject('ode')
ode.stabilization = True
ode.debug = False
num = node.createObject('SequentialSolver',
name='num',
iterations=100,
precision=1e-14)
proximity = node.getObject('proximity')
proximity.alarmDistance = 0.5
def insert(self, parent, **kwargs):
node = parent.createChild(self.name)
self.offset_node = []
geometric = kwargs.get('geometric', geometric_stiffness)
# build input data for multimapping
input = []
for b, o in zip(self.body, self.offset):
if o is None:
input.append( '@' + Tools.node_path_rel(node, b) + '/dofs' )
else:
joint = b.createChild( self.name + '-offset' )
self.offset_node.append( joint )
joint.createObject('MechanicalObject',
template = 'Rigid',
name = 'dofs' )
joint.createObject('AssembledRigidRigidMapping',
template = "Rigid,Rigid",
source = '0 ' + concat( o ),
geometricStiffness = geometric)
input.append( '@' + Tools.node_path_rel(node, b) + '/' + joint.name + '/dofs' )
assert len(input) > 0
dofs = node.createObject('MechanicalObject',
template = 'Vec6d',
name = 'dofs',
position = '0 0 0 0 0 0',
velocity = '0 0 0 0 0 0')
mapping = node.createObject('RigidJointMultiMapping',
name = 'mapping',
template = 'Rigid,Vec6d',
input = concat(input),
output = '@dofs',
pairs = "0 0",
geometricStiffness = geometric
)
sub = node.createChild("constraints")
dim = sum(np.array(self.dofs) != 0)
sub.createObject('MechanicalObject',
template = 'Vec1d',
name = 'dofs',
position = concat( np.zeros(dim) ),
velocity = concat( np.zeros(dim) ),
)
mask = [ (1 - d) for d in self.dofs ]
mapping = sub.createObject('MaskMapping',
name = 'mapping',
template = 'Vec6d,Vec1d',
input = '@../',
output = '@dofs',
dofs = concat(mask) )
ff = sub.createObject('UniformCompliance',
name = 'compliance',
template = 'Vec1d',
compliance = self.compliance)
if self.stabilize:
stab = sub.createObject('Stabilization')
self.ff = ff
self.node = node
return node
def createScene(node):
# controller
node.createObject('PythonScriptController',
filename=__file__,
classname='Controller')
node.dt = 0.005
# friction coefficient
shared.mu = 0.5
scene = Tools.scene(node)
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"
bench = node.createObject('Benchmark')
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.1
# ground
ground = Rigid.Body('ground')
ground.node = ground.insert(scene)
# plane
plane = Rigid.Body('plane')
plane.visual = dir + '/../mesh/ground.obj'
plane.collision = plane.visual
plane.mass_from_mesh(plane.visual, 10)
plane.node = plane.insert(scene)
ground.node.createObject('FixedConstraint', indices='0')
# ground-plane joint
frame = Rigid.Frame.Frame()
frame.translation = [8, 0, 0]
joint = Rigid.RevoluteJoint(2)
joint.absolute(frame, ground.node, plane.node)
joint.upper_limit = 0
joint.node = joint.insert(scene)
# box
box = Rigid.Body('box')
box.visual = dir + '/../mesh/cube.obj'
box.collision = box.visual
box.dofs.translation = [0, 3, 0]
box.mass_from_mesh(box.visual, 50)
box.node = box.insert(scene)
shared.plane = plane.node.getObject('dofs')
shared.box = box.node.getObject('dofs')
shared.joint = joint.node.getObject('dofs')
# pid
shared.pid = Control.PID(shared.joint)
shared.pid.pos = -math.atan(shared.mu) # target should trigger slide
shared.pid.basis = [0, 0, 0, 0, 0, 1]
# shared.pid.dofs.externalForce = '-1e7'
scale = 2e5
shared.pid.kp = -5 * scale
shared.pid.kd = -100 * scale
shared.pid.ki = -1e-1 * scale
def createScene(node):
scene = Tools.scene( node )
ode = node.getObject('ode')
ode.stabilization = "pre-stabilization"
ode.warm_start = False
ode.propagate_lambdas = True
# ode.debug = True
node.gravity = '0 -1 0'
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
import Sofa
from Compliant import Rigid, Tools
mesh_path = Tools.path( __file__ )
scale = 1
# parts of the mechanism
parts = [
["Corps","Corps.msh","1.36 0 0.0268 0 0 0 1","0 0 0 0 0 0 1","22.8 751 737", "2.1e+11","0.28","7.8e+3",1291.453/scale,"TetrahedronFEMForceField","Rigid","Vec3d","TLineModel","TPointModel","ExtVec3f","0.obj","Actor_Sensor_NA",],
["Roue","Roue.msh","0 -0.00604 0.354 0 0 0 1","0 0 -0.148 0 0 0 1","105 106 205", "2.1e+11","0.28","7.8e+3",780.336/scale,"TetrahedronFEMForceField","Rigid","Vec3d","TLineModel","TPointModel","ExtVec3f","3.obj","Actor_Sensor_NA"],
["Came","Came.msh","0 0 -0.00768 0 0 0 1","1.085 -0.072 0.33 0 0 0 1","40.5 40.6 0.331", "2.1e+11","0.28","7.8e+3",161.416/scale,"TetrahedronFEMForceField","Rigid","Vec3d","TLineModel","TPointModel","ExtVec3f","2.obj","Actor_Sensor_NA"],
["Piston","Piston.msh","0 0 0.424 0 0 0 1","2.05 0 0.33 0 0 0 1","0.356 14.6 14.7", "2.1e+11","0.28","7.8e+3",132.759/scale,"TetrahedronFEMForceField","Rigid","Vec3d","TLineModel","TPointModel","ExtVec3f","1.obj","Actor_Sensor_NA"]
]
# joint offsets
offset = [
[0, Rigid.Frame().read('0 0 0 0 0 0 1')],
[1, Rigid.Frame().read('0 0 0.148 0 0 0 1')],
[1, Rigid.Frame().read('0.24 -0.145 0.478 0 0 0 1')],
[2, Rigid.Frame().read('-0.845 -0.073 0 0 0 0 1')],
[2, Rigid.Frame().read('0.852 0.072 0 0 0 0 1')],
[3, Rigid.Frame().read('-0.113 0 0 0 0 0 1')],
[3, Rigid.Frame().read('0.15 0 0 0 0 0 1')],
[0, Rigid.Frame().read('2.2 0 0.33 0 0 0 1')]
]
def createGraph(self, root):
# Variable
self.E_t = 0
self.E_t_dt = 0
self.root_node = root
# Sofa parameters
self.root_node.createObject('BackgroundSetting',color='1 1 1')
self.root_node.createObject('VisualStyle', displayFlags='showVisual hideWireframe showBehaviorModels hideForceFields hideInteractionForceFields')
self.root_node.createObject('StaticSolver')
self.root_node.createObject('CGLinearSolver', iterations=500, tolerance=1E-10, threshold=1E-10)
self.root_node.findData('gravity').value = '0 0 0'
# Object to transfer creation
node = self.root_node.createChild('left_humerus')
node.createObject('MeshObjLoader',name='source', filename=source, triangulate=1, translation='0 0 0', rotation='0 0 0', scale3d='1 1 1')
node.createObject('MeshToImageEngine', template='ImageUC', name='rasterizer', src='@source', insideValue='1', voxelSize=voxel_size, padSize=2, rotateImage='false')
node.createObject('ImageContainer', template='ImageUC', name='image', src='@rasterizer', drawBB='false')
node.createObject('ImageSampler', template='ImageUC', name='sampler', src='@image', method=1, param='1 0', clearData=0)
node.createObject('MeshTopology', name='frame_topo', position='@sampler.position')
#================================ Target model ===================================
targetNode = node.createChild('target')
targetNode.createObject('MeshObjLoader', name='target', filename=target, triangulate=1, translation='0 0 0', rotation='0 0 0', scale3d='3 3 3', showObject=0)
targetNode.createObject('MechanicalObject', template='Vec3d', name='DOFs', src='@target', showObject=0)
targetNode.createObject('FixedConstraint', fixAll='1' )
targetVisuNode = targetNode.createChild('visu')
targetVisuNode.createObject('OglModel', template='ExtVec3f', name='visual', src='@../target', color='0.5 0.5 0.5 0.75')
#================================ Rigid frame ====================================
rigidNode = node.createChild('rigid')
rigidNode.createObject('MechanicalObject', template='Rigid3d', name='DOFs', src='@../frame_topo', showObject=0, showObjectScale='0.1')
#=================================== Scale =======================================
scaleNode = node.createChild('scale')
scaleNode.createObject('MechanicalObject', template='Vec3d', name='DOFs', position='1 1 1', showObject=0, showObjectScale='0.1')
#============================= Registration model ================================
objMainNode = rigidNode.createChild('main')
scaleNode.addChild(objMainNode)
# scene creation
loader = objMainNode.createObject('MeshObjLoader',name='source', filename=source, triangulate=1, translation='0 0 0', rotation='0 0 0', scale3d='1 1 1')
objMainNode.createObject('MeshToImageEngine', template='ImageUC', name='rasterizer', src='@source', value=1, insideValue=1, voxelSize=voxel_size, padSize=0, rotateImage='false')
objMainNode.createObject('ImageContainer', template='ImageUC', name='image', src='@rasterizer', drawBB='false')
objMainNode.createObject('MechanicalObject', template='Affine', name='parent', src='@../../frame_topo', showObject=1, showObjectScale='0.1')
objMainNode.createObject('RigidScaleToAffineMultiMapping', template='Rigid,Vec3d,Affine', input1='@../../rigid/DOFs', input2='@../../scale/DOFs', output='@.', index='0 0 0', printLog='0')
objMainNode.createObject('VoronoiShapeFunction', template='ShapeFunctiond,ImageUC', name='SF', position='@parent.rest_position', image='@image.image', transform='@image.transform', nbRef=4, clearData=1, bias=0)
# Contact
objContactNode = objMainNode.createChild('registration')
objContactNode.createObject('MeshTopology', name='topo', src='@../source')
objContactNode.createObject('MechanicalObject', name='DOFs')
objContactNode.createObject('UniformMass', totalMass=1)
objContactNode.createObject('TriangleModel')
objContactNode.createObject('LinearMapping', template='Affine,Vec3d')
# Visual model
objVisuNode = objContactNode.createChild('visual')
objVisuNode.createObject('OglModel', template='ExtVec3f', name='visual', src='@../topo', color='1 0.2 0.2 0.9')
objVisuNode.createObject('IdentityMapping', template='Vec3d,ExtVec3f')
# Registration
objRegistrationNode = objMainNode.createChild('force')
objRegistrationNode.createObject('MechanicalObject', template='Vec3d', name='DOFs', src='@../source')
objRegistrationNode.createObject('LinearMapping', template='Affine,Vec3d')
# registration force field
springs = ""
for i in range(len(loader.position)):
springs += str(i)+' '+str(i)+' '
distanceNode = objRegistrationNode.createChild('registration_constraint')
targetNode.addChild(distanceNode)
distanceNode.createObject('MechanicalObject', template='Vec3d', name='distanceDOFs')
distanceNode.createObject('DifferenceMultiMapping', template='Vec3d,Vec3d', input='@'+Tools.node_path_rel(distanceNode, targetNode)+' @'+Tools.node_path_rel(distanceNode, objRegistrationNode), output='@.', pairs=springs, showObjectScale="0.005")
distanceNode.createObject('UniformCompliance', name='constraint', isCompliance=0, compliance=1E-6, damping=0.1)
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
dofs = particles.createObject('MechanicalObject', template = "Vec3d")
pos = np.zeros( (n, 3) )
vel = np.zeros( (n, 3) )
for i in xrange(n):
pos[i, 1] = 2 + i / 1.5
vel[i, 1] = -1
dofs.position = pos.tolist()
dofs.velocity = vel.tolist()
mass = particles.createObject('UniformMass', template = 'Vec3d')
model = particles.createObject('SphereModel', template = 'Vec3d',
selfCollision = True,
radius = 0.1)
import Sofa
from Compliant import Rigid, Tools, Frame
mesh_path = Tools.path(__file__)
scale = 1
# parts of the mechanism
parts = [[
"Corps",
"Corps.msh",
"1.36 0 0.0268 0 0 0 1",
"0 0 0 0 0 0 1",
"22.8 751 737",
"2.1e+11",
"0.28",
"7.8e+3",
1291.453 / scale,
"TetrahedronFEMForceField",
"Rigid",
"Vec3d",
"TLineModel",
"TPointModel",
"ExtVec3f",
"0.obj",
"Actor_Sensor_NA",
],
[
"Roue", "Roue.msh", "0 -0.00604 0.354 0 0 0 1",
"0 0 -0.148 0 0 0 1", "105 106 205", "2.1e+11", "0.28", "7.8e+3",
def run():
ok = True
info = SofaPython.mass.RigidMassInfo()
# testing axis-aligned known geometric shapes
for m in xrange(len(meshes)):
mesh = meshes[m]
mesh_path = path + meshes[m]
for s in xrange(len(scales)):
scale = scales[s]
if mesh == "cylinder.obj" and scale[0] != scale[1]:
continue
for d in xrange(len(densities)):
density = densities[d]
info.setFromMesh(mesh_path, density, scale)
error = " (" + meshes[m] + ", s=" + Tools.cat(
scale) + " d=" + str(density) + ")"
ok &= EXPECT_TRUE(
almostEqualReal(info.mass, masses[m][s][d]), "mass" +
error + " " + str(info.mass) + "!=" + str(masses[m][s][d]))
ok &= EXPECT_TRUE(
almostEqualLists(info.com, [x * 0.5 for x in scale]),
"com" + error + " " + Tools.cat(info.com) + "!=" +
Tools.cat([x * 0.5 for x in scale]))
ok &= EXPECT_TRUE(
almostEqualLists(info.diagonal_inertia, inertia[m][s][d]),
"inertia" + error + " " + str(info.diagonal_inertia) +
"!=" + str(inertia[m][s][d]))
# testing diagonal inertia extraction from a rotated cuboid
mesh = "cube.obj"
mesh_path = path + mesh
scale = scales[3]
density = 1
theory = sorted(inertia[0][3][0])
for r in rotations:
info.setFromMesh(mesh_path, density, scale, r)
local = sorted(info.diagonal_inertia)
ok &= EXPECT_TRUE(
almostEqualLists(local, theory), "inertia " + str(local) + "!=" +
str(theory) + " (rotation=" + str(r) + ")")
# testing extracted inertia rotation
mesh = "rotated_cuboid_12_35_-27.obj"
mesh_path = path + mesh
density = 1
info.setFromMesh(mesh_path, density)
# theoretical results
scale = [2, 3, 1]
mass = density * scale[0] * scale[1] * scale[2]
inertiat = numpy.empty(3)
inertiat[0] = 1.0 / 12.0 * mass * (
scale[1] * scale[1] + scale[2] * scale[2]) # x
inertiat[1] = 1.0 / 12.0 * mass * (
scale[0] * scale[0] + scale[2] * scale[2]) # y
inertiat[2] = 1.0 / 12.0 * mass * (
scale[0] * scale[0] + scale[1] * scale[1]) # z
# used quaternion in mesh
q = Quaternion.normalized(
Quaternion.from_euler([
12 * math.pi / 180.0, 35 * math.pi / 180.0, -27 * math.pi / 180.0
]))
# corresponding rotation matrices (ie frame defined by columns)
mt = Quaternion.to_matrix(q)
m = Quaternion.to_matrix(info.inertia_rotation)
# matching inertia
idxt = numpy.argsort(inertiat)
idx = numpy.argsort(info.diagonal_inertia)
# checking if each axis/column are parallel (same or opposite for unitary vectors)
for i in xrange(3):
ok &= EXPECT_TRUE(
almostEqualLists(mt[:, idxt[i]].tolist(), m[:, idx[i]].tolist(),
1e-5)
or almostEqualLists(mt[:, idxt[i]].tolist(),
(-m[:, idx[i]]).tolist(), 1e-5),
"wrong inertia rotation")
# print mt[:,idxt]
# print m [:,idx ]
return ok
def createGraph(self, root):
# Variable
self.E_t = 0
self.E_t_dt = 0
self.root_node = root
# Sofa parameters
self.root_node.createObject('BackgroundSetting', color='1 1 1')
self.root_node.createObject(
'VisualStyle',
displayFlags=
'showVisual hideWireframe showBehaviorModels hideForceFields hideInteractionForceFields'
)
self.root_node.createObject('StaticSolver')
self.root_node.createObject('CGLinearSolver',
iterations=500,
tolerance=1E-10,
threshold=1E-10)
self.root_node.findData('gravity').value = '0 0 0'
# Object to transfer creation
node = self.root_node.createChild('left_humerus')
node.createObject('MeshObjLoader',
name='source',
filename=source,
triangulate=1,
translation='0 0 0',
rotation='0 0 0',
scale3d='1 1 1')
node.createObject('MeshToImageEngine',
template='ImageUC',
name='rasterizer',
src='@source',
insideValue='1',
voxelSize=voxel_size,
padSize=2,
rotateImage='false')
node.createObject('ImageContainer',
template='ImageUC',
name='image',
src='@rasterizer',
drawBB='false')
node.createObject('ImageSampler',
template='ImageUC',
name='sampler',
src='@image',
method=1,
param='1 0',
clearData=0)
node.createObject('MeshTopology',
name='frame_topo',
position='@sampler.position')
#================================ Target model ===================================
targetNode = node.createChild('target')
targetNode.createObject('MeshObjLoader',
name='target',
filename=target,
triangulate=1,
translation='0 0 0',
rotation='0 0 0',
scale3d='3 3 3',
showObject=0)
targetNode.createObject('MechanicalObject',
template='Vec3d',
name='DOFs',
src='@target',
showObject=0)
targetNode.createObject('FixedConstraint', fixAll='1')
targetVisuNode = targetNode.createChild('visu')
targetVisuNode.createObject('OglModel',
template='ExtVec3f',
name='visual',
src='@../target',
color='0.5 0.5 0.5 0.75')
#================================ Rigid frame ====================================
rigidNode = node.createChild('rigid')
rigidNode.createObject('MechanicalObject',
template='Rigid3d',
name='DOFs',
src='@../frame_topo',
showObject=0,
showObjectScale='0.1')
#=================================== Scale =======================================
scaleNode = node.createChild('scale')
scaleNode.createObject('MechanicalObject',
template='Vec3d',
name='DOFs',
position='1 1 1',
showObject=0,
showObjectScale='0.1')
#============================= Registration model ================================
objMainNode = rigidNode.createChild('main')
scaleNode.addChild(objMainNode)
# scene creation
loader = objMainNode.createObject('MeshObjLoader',
name='source',
filename=source,
triangulate=1,
translation='0 0 0',
rotation='0 0 0',
scale3d='1 1 1')
objMainNode.createObject('MeshToImageEngine',
template='ImageUC',
name='rasterizer',
src='@source',
value=1,
insideValue=1,
voxelSize=voxel_size,
padSize=0,
rotateImage='false')
objMainNode.createObject('ImageContainer',
template='ImageUC',
name='image',
src='@rasterizer',
drawBB='false')
objMainNode.createObject('MechanicalObject',
template='Affine',
name='parent',
src='@../../frame_topo',
showObject=1,
showObjectScale='0.1')
objMainNode.createObject('RigidScaleToAffineMultiMapping',
template='Rigid,Vec3d,Affine',
input1='@../../rigid/DOFs',
input2='@../../scale/DOFs',
output='@.',
index='0 0 0',
printLog='0')
objMainNode.createObject('VoronoiShapeFunction',
template='ShapeFunctiond,ImageUC',
name='SF',
position='@parent.rest_position',
image='@image.image',
transform='@image.transform',
nbRef=4,
clearData=1,
bias=0)
# Contact
objContactNode = objMainNode.createChild('registration')
objContactNode.createObject('MeshTopology',
name='topo',
src='@../source')
objContactNode.createObject('MechanicalObject', name='DOFs')
objContactNode.createObject('UniformMass', totalMass=1)
objContactNode.createObject('TriangleModel')
objContactNode.createObject('LinearMapping', template='Affine,Vec3d')
# Visual model
objVisuNode = objContactNode.createChild('visual')
objVisuNode.createObject('OglModel',
template='ExtVec3f',
name='visual',
src='@../topo',
color='1 0.2 0.2 0.9')
objVisuNode.createObject('IdentityMapping', template='Vec3d,ExtVec3f')
# Registration
objRegistrationNode = objMainNode.createChild('force')
objRegistrationNode.createObject('MechanicalObject',
template='Vec3d',
name='DOFs',
src='@../source')
objRegistrationNode.createObject('LinearMapping',
template='Affine,Vec3d')
# registration force field
springs = ""
for i in range(len(loader.position)):
springs += str(i) + ' ' + str(i) + ' '
distanceNode = objRegistrationNode.createChild(
'registration_constraint')
targetNode.addChild(distanceNode)
distanceNode.createObject('MechanicalObject',
template='Vec3d',
name='distanceDOFs')
distanceNode.createObject(
'DifferenceMultiMapping',
template='Vec3d,Vec3d',
input='@' + Tools.node_path_rel(distanceNode, targetNode) + ' @' +
Tools.node_path_rel(distanceNode, objRegistrationNode),
output='@.',
pairs=springs,
showObjectScale="0.005")
distanceNode.createObject('UniformCompliance',
name='constraint',
isCompliance=0,
compliance=1E-6,
damping=0.1)
import Sofa
import os, numpy, sys
from Compliant import StructuralAPI
from Compliant import Tools as CompliantTools
from SofaPython import Quaternion, Tools
currentdir = CompliantTools.path(__file__)
N = 10
bodies = []
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")
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.7 # per object friction coefficient (the friction coef between 2 objects is approximated as the product of both coefs)
manager.responseParams = 'mu={0}&compliance={1}&horizontalConeProjection=1'.format(globalMu,
1e-8)
ode = node.getObject('ode')
ode.stabilization = "pre-stabilization"
ode.debug = 2
# (un)comment these to see anisotropy issues with sequential solver
solver = 'ModulusSolver'
solver = 'SequentialSolver'
num = node.createObject(solver,
name = 'num',
iterations = 100,
precision = 1e-14,
anderson = 4)
num.printLog = True
proximity = node.getObject('proximity')
proximity.alarmDistance = 0.5
proximity.contactDistance = 0.2
proximity.useLineLine = True
# plane
plane = Rigid.Body('plane')
plane.dofs.translation = [0, 5, -15]
alpha = math.pi / 5
mu = math.tan( alpha )
print "plane mu:", mu,
if mu < globalMu: print '(should stick)'
else: print '(should slide)'
q = Quaternion.exp( [alpha, 0.0, 0.0] )
plane.dofs.rotation = q
s = 6
plane.scale = [s, s, s]
plane.visual = path + '/../mesh/ground.obj'
plane.collision = plane.visual
plane.mass_from_mesh( plane.visual, 10 )
plane.mu = 0.5 # per object friction coefficient
plane.node = plane.insert( scene )
plane.node.createObject('FixedConstraint', indices = '0')
# box
box = Rigid.Body('box')
box.visual = path + '/../mesh/cube.obj'
box.collision = box.visual
box.dofs.translation = [0, 3, 0]
box.mass_from_mesh( box.visual, 50 )
box.mu = 1 # per object friction coefficient
box.dofs.rotation = q
box.node = box.insert( scene )
def __insert_registration_node(self):
""" Insert the registration node in the graph, under dofRigid node. """
registration_node = self.createChild(self.nodes["dofRigid"],
"RegistrationNode")
param = self.param # shortcut
# merge topologies
repartition = ""
sources_component_name = []
collision_node = [elem.collision for elem in self.rigids.values()]
for i, elem in enumerate(collision_node):
sources_component_name.append(
Tools.node_path_rel(registration_node, elem.node) +
"/topology")
repartition += "{0} ".format(i) * len(elem.topology.position)
registration_node.createObject(
'MergeMeshes',
name='source_topology',
nbMeshes=len(sources_component_name),
**dict(
{
'position' + str(i + 1): '@' + item + '.position'
for i, item in enumerate(sources_component_name)
}, **{
'triangles' + str(i + 1): '@' + item + '.triangles'
for i, item in enumerate(sources_component_name)
}))
registration_node.createObject('MeshTopology',
name='topo',
src='@./source_topology')
registration_node.createObject('MechanicalObject', name='DOFs')
registration_node.createObject('Triangle')
registration_node.createObject(
'RigidMapping',
template='Rigid3d,Vec3d',
input="@" +
Tools.node_path_rel(registration_node, self.nodes["dofRigid"]),
output="@DOFs",
rigidIndexPerPoint=repartition)
registration_node.createObject(
'NormalsFromPoints',
name='NormalsFromPoints',
template='Vec3d',
position='@DOFs.position',
triangles='@topo.triangles',
invertNormals=self.param.invertSourceNormals)
# Force Field
target_path = Tools.node_path_rel(registration_node,
self.nodes["Target"])
registration_node.createObject(
'ClosestPointRegistrationForceField',
name='ICP',
template='Vec3d',
# source
sourceTriangles='@topo.triangles',
sourceNormals='@NormalsFromPoints.normals',
# target
position='@{0}/loader.position'.format(target_path),
triangles='@{0}/loader.triangles'.format(target_path),
normals='@{0}/NormalsFromPoints.normals'.format(target_path),
# Param
cacheSize='4',
blendingFactor=param.blendingFactor,
stiffness=param.stiffness,
damping=param.damping,
outlierThreshold=param.outlierThreshold,
normalThreshold=param.normalThreshold,
rejectOutsideBbox=param.rejectOutsideBbox,
drawColorMap='0')
def setManually(self,
filepath=None,
offset=[[0, 0, 0, 0, 0, 0, 1]],
voxelSize=0.01,
density=1000,
generatedDir=None):
if len(offset) == 0:
Sofa.msg_error(
"RigidScale.API",
"ShearlessAffineBody should have at least 1 ShearLessAffine")
return
self.framecom = Frame.Frame()
self.bodyOffset = Frame.Frame([0, 0, 0, 0, 0, 0, 1])
path_affine_rigid = '@' + Tools.node_path_rel(self.affineNode,
self.rigidNode)
path_affine_scale = '@' + Tools.node_path_rel(self.affineNode,
self.scaleNode)
if len(offset) == 1: self.frame = [Frame.Frame(offset[0])]
str_position = ""
for p in offset:
str_position = str_position + concat(p) + " "
### scene creation
# rigid dof
self.rigidDofs = self.rigidNode.createObject(
'MechanicalObject',
template='Rigid3' + template_suffix,
name='dofs',
position=str_position,
rest_position=str_position)
# scale dofs
self.scaleDofs = self.scaleNode.createObject(
'MechanicalObject',
template='Vec3' + template_suffix,
name='dofs',
position=concat([1, 1, 1] * len(offset)))
positiveNode = self.scaleNode.createChild('positive')
positiveNode.createObject('MechanicalObject',
template='Vec3' + template_suffix,
name='positivescaleDOFs')
positiveNode.createObject('DifferenceFromTargetMapping',
template='Vec3' + template_suffix + ',Vec3' +
template_suffix,
applyRestPosition=1,
targets=concat(target_scale))
positiveNode.createObject('UniformCompliance',
isCompliance=1,
compliance=0)
positiveNode.createObject('UnilateralConstraint')
positiveNode.createObject('Stabilization', name='Stabilization')
# affine dofs
self.affineDofs = self.affineNode.createObject('MechanicalObject',
template='Affine',
name='parent',
showObject=0)
self.affineNode.createObject('RigidScaleToAffineMultiMapping',
template='Rigid,Vec3,Affine',
input1=path_affine_rigid,
input2=path_affine_scale,
output='@.',
autoInit='1',
printLog='0')
if filepath:
self.image = SofaImage.API.Image(self.affineNode,
name="image_" + self.name,
imageType="ImageUC")
self.shapeFunction = Flexible.API.ShapeFunction(self.affineNode)
if generatedDir is None:
self.image.addMeshLoader(
filepath, value=1, insideValue=1
) # TODO support multiple meshes closingValue=1,
self.image.addMeshToImage(voxelSize)
self.shapeFunction.addVoronoi(self.image,
position='@dofs.rest_position')
# mass
self.affineMassNode = self.affineNode.createChild('mass')
self.affineMassNode.createObject('TransferFunction',
name='density',
template='ImageUC,ImageD',
inputImage='@../image.image',
param='0 0 1 ' + str(density))
self.affineMassNode.createObject('MechanicalObject',
template='Vec3' +
template_suffix)
self.affineMassNode.createObject('LinearMapping',
template='Affine,Vec3' +
template_suffix)
self.affineMassNode.createObject(
'MassFromDensity',
name='MassFromDensity',
template='Affine,ImageD',
image='@density.outputImage',
transform='@../image.transform',
lumping='0')
self.mass = self.affineNode.createObject(
'AffineMass',
massMatrix='@mass/MassFromDensity.massMatrix')
else:
self.image.addContainer(filename=self.node.name +
"_rasterization.raw",
directory=generatedDir)
self.shapeFunction.shapeFunction = serialization.importImageShapeFunction(
self.affineNode,
generatedDir + self.node.name + "_SF_indices.raw",
generatedDir + self.node.name + "_SF_weights.raw", 'dofs')
self.mass = serialization.importAffineMass(
self.affineNode,
generatedDir + self.node.name + "_affinemass.json")
# computation of the object mass center
massInfo = SofaPython.mass.RigidMassInfo()
massInfo.setFromMesh(filepath, density, [1, 1, 1])
# get the object mass center
self.framecom.rotation = massInfo.inertia_rotation
self.framecom.translation = massInfo.com
else:
print "You need a mesh to create an articulated system"
self.frame = []
for o in offset:
self.frame.append(Frame.Frame(o))
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):
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.7 # per object friction coefficient (the friction coef between 2 objects is approximated as the product of both coefs)
manager.responseParams = 'mu={0}&compliance={1}&horizontalConeProjection=1'.format(
globalMu, 1e-8)
ode = node.getObject('ode')
ode.stabilization = "pre-stabilization"
ode.debug = 2
# (un)comment these to see anisotropy issues with sequential solver
solver = 'ModulusSolver'
solver = 'SequentialSolver'
num = node.createObject(solver,
name='num',
iterations=100,
precision=1e-14,
anderson=4)
num.printLog = True
proximity = node.getObject('proximity')
proximity.alarmDistance = 0.5
proximity.contactDistance = 0.2
proximity.useLineLine = True
# plane
plane = Rigid.Body('plane')
plane.dofs.translation = [0, 5, -15]
alpha = math.pi / 5
mu = math.tan(alpha)
print "plane mu:", mu,
if mu < globalMu: print '(should stick)'
else: print '(should slide)'
q = Quaternion.exp([alpha, 0.0, 0.0])
plane.dofs.rotation = q
s = 6
plane.scale = [s, s, s]
plane.visual = path + '/../mesh/ground.obj'
plane.collision = plane.visual
plane.mass_from_mesh(plane.visual, 10)
plane.mu = 0.5 # per object friction coefficient
plane.node = plane.insert(scene)
plane.node.createObject('FixedConstraint', indices='0')
# box
box = Rigid.Body('box')
box.visual = path + '/../mesh/cube.obj'
box.collision = box.visual
box.dofs.translation = [0, 3, 0]
box.mass_from_mesh(box.visual, 50)
box.mu = 1 # per object friction coefficient
box.dofs.rotation = q
box.node = box.insert(scene)
def createScene(node):
# controller
node.createObject('PythonScriptController',
filename = __file__,
classname = 'Controller' )
node.dt = 0.005
# friction coefficient
shared.mu = 0.5
scene = Tools.scene( node )
style = node.getObject('style')
style.findData('displayFlags').showMappings = True
manager = node.getObject('manager')
manager.response = 'FrictionCompliantContact'
manager.responseParams = 'mu=' + str(shared.mu)
ode = node.getObject('ode')
ode.stabilization = True
num = node.createObject('SequentialSolver',
name = 'num',
iterations = 100,
precision = 1e-14)
proximity = node.getObject('proximity')
proximity.alarmDistance = 0.5
proximity.contactDistance = 0.1
# ground
ground = Rigid.Body('ground');
ground.node = ground.insert( scene );
# plane
plane = Rigid.Body('plane')
plane.visual = dir + '/mesh/ground.obj'
plane.collision = plane.visual
plane.mass_from_mesh( plane.visual, 10 )
plane.node = plane.insert( scene )
ground.node.createObject('FixedConstraint',
indices = '0')
# ground-plane joint
frame = Rigid.Frame()
frame.translation = [5, 0, 0]
joint = Rigid.RevoluteJoint(2)
joint.absolute(frame, ground.node, plane.node)
joint.node = joint.insert( scene )
# joint limit
limit = joint.node.createChild("limit")
limit.createObject('MechanicalObject', template = 'Vec1d', position = '0')
projection = limit.createObject('ProjectionMapping', template = 'Vec6d, Vec1d' )
projection.set = '0 0 0 0 0 0 -1'
limit.createObject('UniformCompliance', template = 'Vec1d', compliance = '0' )
limit.createObject('UnilateralConstraint');
limit.createObject('Stabilization');
# box
box = Rigid.Body('box')
box.visual = dir + '/mesh/cube.obj'
box.collision = box.visual
box.dofs.translation = [0, 3, 0]
box.mass_from_mesh( box.visual, 50 )
box.node = box.insert( scene )
shared.plane = plane.node.getObject('dofs')
shared.box = box.node.getObject('dofs')
shared.joint = joint.node.getObject('dofs')
# pid
shared.pid = Control.PID(shared.joint)
shared.pid.ref_pos = -math.atan( shared.mu ) # target should trigger slide
shared.pid.basis = [0, 0, 0, 0, 0, 1]
scale = 1e6
shared.pid.kp = - 1 * scale
shared.pid.kd = - 5 * scale
shared.pid.ki = - 0.05 * scale
import Sofa
import math
from Compliant import StructuralAPI, Tools
from Compliant.types import Quaternion, Rigid3
import numpy as np
dir = Tools.path( __file__ )
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',
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 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('BruteForceDetection')
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 createGraph(self, root):
# Variable
self.E_t = 0
self.E_t_dt = 0
self.root_node = root
self.root_node.findData('gravity').value = '0 0 0'
# Sofa parameters
self.root_node.createObject('BackgroundSetting', color='1 1 1')
self.root_node.createObject(
'VisualStyle',
displayFlags=
'showVisual hideWireframe showBehaviorModels hideForceFields showInteractionForceFields'
)
self.root_node.createObject('EulerImplicit',
rayleighStiffness='0.01',
rayleighMass='0.01')
self.root_node.createObject('CGLinearSolver',
iterations=200,
tolerance=1E-6,
threshold=1E-6)
# Object to transfer creation
node = self.root_node.createChild('main')
node.createObject('MeshObjLoader',
name='source',
filename='./mesh/source.obj',
triangulate=1,
translation='0 0 0',
rotation='0 0 0',
scale3d='1 1 1')
node.createObject('MeshToImageEngine',
template='ImageUC',
name='rasterizer',
src='@source',
insideValue='1',
voxelSize=0.01,
padSize=2,
rotateImage='false')
node.createObject('ImageContainer',
template='ImageUC',
name='image',
src='@rasterizer',
drawBB='false')
node.createObject('MeshTopology',
name='frame_topo',
position=listListToStr(dofs_position))
#================================ Target model ===================================
targetNode = node.createChild('target')
targetNode.createObject('MeshObjLoader',
name='loader',
filename='./mesh/target.obj',
rotation='0 0 0',
translation='0 0 0',
scale3d='1 1 1',
showObject=0)
targetNode.createObject('MechanicalObject',
template='Vec3d',
name='DOFs',
src='@loader',
showObject=0)
targetNode.createObject('NormalsFromPoints',
name='normalsFromPoints',
template='Vec3d',
position='@DOFs.position',
triangles='@loader.triangles',
invertNormals=0)
targetNode.createObject('FixedConstraint', fixAll='1', drawSize=0.001)
targetVisuNode = targetNode.createChild('visu')
targetVisuNode.createObject('OglModel',
template='ExtVec3f',
name='visual',
src='@../loader',
color='0.5 0.5 0.5 0.85')
#================================ Rigid frame ====================================
rigidNode = node.createChild('rigid')
rigidNode.createObject('MechanicalObject',
template='Rigid',
name='DOFs',
src='@../frame_topo',
showObject=1,
showObjectScale='0.1')
rigidNode.createObject('RigidMass',
mass="1 1 ",
inertia="1 1 1 1 1 1")
#rigidNode.createObject('FixedConstraint', name='fixed', indices='0')
#=================================== Scale =======================================
scaleNode = node.createChild('scale')
scaleNode.createObject('MechanicalObject',
template='Vec3d',
name='DOFs',
position='1 1 1 1 1 1',
showObject=0,
showObjectScale='0.1')
#=========================== Alignement constraint ===============================
offsetNode = rigidNode.createChild('offset')
offsetNode.createObject('MechanicalObject',
template='Rigid',
name='DOFs',
position='0 0 0 0 0 0 1 0 0 0 0 0 0 1',
showObject=1,
showObjectScale='0.1')
offsetNode.createObject('AssembledRigidRigidMapping',
template='Rigid,Rigid',
source='0 ' + listToStr(offset_position[0]) +
listToStr(joint_orientation) + ' 1 ' +
listToStr(offset_position[1]) +
listToStr(joint_orientation))
# --- old things even if they don't work well are often more stable, just for creating the prototype ...
offsetNode.createObject(
'JointSpringForceField',
template='Rigid',
name='joint',
object1='@.',
object2='@.',
spring=
' BEGIN_SPRING 0 1 FREE_AXIS 0 1 0 0 0 0 KS_T 0 1E10 KS_R 0 1e10 KS_B 3E3 KD 0.1 R_LIM_X 0 0 R_LIM_Y 0 0 R_LIM_Z 0 0 REST_T 0 1 0 END_SPRING'
)
#============================= Registration model ================================
objMainNode = rigidNode.createChild('deformable')
scaleNode.addChild(objMainNode)
# scene creation
loader = objMainNode.createObject('MeshObjLoader',
name='source',
filename='./mesh/source.obj',
triangulate=1,
translation='0 0 0',
rotation='0 0 0',
scale3d='1 1 1')
objMainNode.createObject('MeshToImageEngine',
template='ImageUC',
name='rasterizer',
src='@source',
value=1,
insideValue=1,
voxelSize=0.01,
padSize=0,
rotateImage='false')
objMainNode.createObject('ImageContainer',
template='ImageUC',
name='image',
src='@rasterizer',
drawBB='false')
objMainNode.createObject('MechanicalObject',
template='Affine',
name='parent',
showObject=1,
src='@../../frame_topo',
showObjectScale='0.1')
objMainNode.createObject('RigidScaleToAffineMultiMapping',
template='Rigid,Vec3d,Affine',
input1='@../../rigid/DOFs',
input2='@../../scale/DOFs',
output='@.',
autoInit='1',
printLog='0')
objMainNode.createObject('VoronoiShapeFunction',
template='ShapeFunctiond,ImageUC',
name='SF',
position='@parent.rest_position',
image='@image.image',
transform='@image.transform',
nbRef=4,
clearData=1,
bias=0)
# Behavior Node
objBehaviorNode = objMainNode.createChild('behavior')
objBehaviorNode.createObject('ImageGaussPointSampler',
name='sampler',
indices='@../SF.indices',
weights='@../SF.weights',
transform='@../SF.transform',
method=2,
order=1,
targetNumber=40)
objBehaviorNode.createObject('MechanicalObject', template='F331')
objBehaviorNode.createObject('LinearMapping', template='Affine,F331')
objBehaviorNode.createObject('ProjectiveForceField',
template='F331',
youngModulus=1E6,
poissonRatio=0,
viscosity=0,
isCompliance=0)
# Contact
objContactNode = objMainNode.createChild('registration')
objContactNode.createObject('MeshTopology',
name='topo',
src='@../source')
objContactNode.createObject('MechanicalObject', name='DOFs')
objContactNode.createObject('UniformMass', totalMass=1)
objContactNode.createObject('TriangleModel')
objContactNode.createObject('LinearMapping', template='Affine,Vec3d')
# Visual model
objVisuNode = objContactNode.createChild('visual')
objVisuNode.createObject('OglModel',
template='ExtVec3f',
name='visual',
src='@../topo',
color='1 0.2 0.2 0.8')
objVisuNode.createObject('IdentityMapping', template='Vec3d,ExtVec3f')
# Registration : first attraction field to well positioning the source on the target
objRegistrationNode = objMainNode.createChild('force')
objRegistrationNode.createObject('MeshTopology',
name='topo',
src='@../source')
objRegistrationNode.createObject('MechanicalObject',
template='Vec3d',
name='DOFs')
objRegistrationNode.createObject('LinearMapping',
template='Affine,Vec3d')
# registration force field
springs = ""
for i in range(len(loader.position)):
springs += str(i) + ' ' + str(i) + ' '
distanceNode = objRegistrationNode.createChild(
'registration_constraint')
targetNode.addChild(distanceNode)
distanceNode.createObject('MechanicalObject',
template='Vec3d',
name='distanceDOFs')
distanceNode.createObject(
'DifferenceMultiMapping',
template='Vec3d,Vec3d',
input='@' + Tools.node_path_rel(distanceNode, targetNode) + ' @' +
Tools.node_path_rel(distanceNode, objRegistrationNode),
output='@.',
pairs=springs,
showObjectScale="0.005")
distanceNode.createObject('UniformCompliance',
name='constraint',
isCompliance=0,
compliance=1E-6,
damping=0.1)
# Registration : surface to surface registration forces
surfaceRegistrationNode = objMainNode.createChild('reg_force')
surfaceRegistrationNode.createObject('MeshTopology',
name='topo',
src='@../source')
surfaceRegistrationNode.createObject('MechanicalObject', name='DOFs')
surfaceRegistrationNode.createObject('Triangle')
surfaceRegistrationNode.createObject('LinearMapping',
template='Affine,Vec3d',
assemble=0)
surfaceRegistrationNode.createObject('NormalsFromPoints',
name='normalsFromPoints',
template='Vec3d',
position='@DOFs.position',
triangles='@topo.triangles',
invertNormals=0)
surfaceRegistrationNode.createObject(
'ClosestPointRegistrationForceField',
name='ICP',
template='Vec3d',
sourceTriangles='@topo.triangles',
sourceNormals='@normalsFromPoints.normals',
position='@../../../target/loader.position',
triangles='@../../../target/loader.triangles',
normals='@../../../target/normalsFromPoints.normals',
cacheSize=4,
blendingFactor=1,
stiffness=1E3,
damping=1E-3,
outlierThreshold=0,
normalThreshold=0,
rejectOutsideBbox=0,
drawColorMap='0')
def createScene(node):
# controller
node.createObject('PythonScriptController',
filename = __file__,
classname = 'Controller' )
# time step
node.dt = 0.005
# scene node
scene = Tools.scene( node )
# display flags
style = node.getObject('style')
style.findData('displayFlags').showMappings = True
# collision detection
proximity = node.getObject('proximity')
proximity.alarmDistance = 0.5
proximity.contactDistance = 0.1
# contat manager
manager = node.getObject('manager')
manager.response = 'CompliantContact'
manager.responseParams = 'compliance=0'
# integrator
ode = node.getObject('ode',)
ode.stabilization = True
ode.stabilization_damping = 0
# main solver
num = node.createObject('BenchmarkSolver', name = 'num')
response = node.createObject('LDLTResponse', name = 'response')
iterations = 300
precision = 1e-8
# we need compliantdev for qpsolver
node.createObject('RequiredPlugin',
pluginName = 'CompliantDev')
# benchmarks
shared.pgs = node.createObject('Benchmark', name = 'bench-pgs')
shared.qp = node.createObject('Benchmark', name = 'bench-qp')
# solvers
pgs = node.createObject('SequentialSolver',
name = 'pgs',
iterations = iterations,
precision = precision,
bench = '@./bench-pgs')
qp = node.createObject('QPSolver',
name = 'qp',
iterations = iterations,
precision = precision,
bench = '@./bench-qp',
schur = True)
# plane
plane = Rigid.Body('plane')
plane.visual = dir + '/mesh/ground.obj'
plane.collision = plane.visual
plane.mass_from_mesh( plane.visual, 10 )
plane.node = plane.insert( scene )
plane.node.createObject('FixedConstraint',
indices = '0')
# boxes
n_boxes = 10
for i in xrange(n_boxes):
box = Rigid.Body('box-{0}'.format(i))
box.visual = dir + '/mesh/cube.obj'
box.collision = box.visual
box.dofs.translation = [0, 2.5 * (i + 1), 0]
box.mass_from_mesh( box.visual, 50 )
box.node = box.insert( scene )
## testing Rigid.generate_rigid that computes com, mass and inertia from a mesh
# TODO compute and test principal axes basis by rotating meshes
from SofaTest.Macro import *
from Compliant import Tools
from SofaPython import Quaternion
import SofaPython.mass
import numpy
path = Tools.path( __file__ ) + "/geometric_primitives/"
# cube.obj is unit 1x1x1 cube
# sphere.obj radius=1
# cylinder.obj, height=1 in z, radius=1
# all meshes are centered in (0.5,0.5,0.5)
meshes = ['cube.obj', 'sphere.obj', 'cylinder.obj']
scales = [[1,1,1],[1,1,10],[3.3,3.3,10],[10,5,2]]
densities = [1, 1000, 7.7]
rotations = [[90,0,0],[22.456,0,0],[0,90,0],[0,23.546,0],[0,0,90],[0,0,-63.2],[90,90,0],[90,12.152,0],[25.645,12.36,0],[90,90,90],[-12.356,124.33,-56.1]]
# prepare multi-dimensional structure
masses = []
for x in range(len(meshes)):
row = []
for y in xrange(len(scales)):
col = []
for z in xrange(len(densities)):
## testing Rigid.generate_rigid that computes com, mass and inertia from a mesh
# TODO compute and test principal axes basis by rotating meshes
from SofaTest.Macro import *
from Compliant import Tools
from SofaPython import Quaternion
import SofaPython.mass
import numpy
path = Tools.path(__file__) + "/geometric_primitives/"
# cube.obj is unit 1x1x1 cube
# sphere.obj radius=1
# cylinder.obj, height=1 in z, radius=1
# all meshes are centered in (0.5,0.5,0.5)
meshes = ['cube.obj', 'sphere.obj', 'cylinder.obj']
scales = [[1, 1, 1], [1, 1, 10], [3.3, 3.3, 10], [10, 5, 2]]
densities = [1, 1000, 7.7]
rotations = [[90, 0, 0], [22.456, 0, 0], [0, 90, 0], [0, 23.546, 0],
[0, 0, 90], [0, 0, -63.2], [90, 90, 0], [90, 12.152, 0],
[25.645, 12.36, 0], [90, 90, 90], [-12.356, 124.33, -56.1]]
# prepare multi-dimensional structure
masses = []
for x in range(len(meshes)):
row = []
for y in xrange(len(scales)):
col = []
for z in xrange(len(densities)):
def createGraph(self, root):
# Variable
self.E_t = 0
self.E_t_dt = 0
self.root_node = root
self.root_node.findData('gravity').value = '0 0 0'
# Sofa parameters
self.root_node.createObject('BackgroundSetting',color='1 1 1')
self.root_node.createObject('VisualStyle', displayFlags='showVisual hideWireframe showBehaviorModels hideForceFields showInteractionForceFields')
self.root_node.createObject('EulerImplicit',rayleighStiffness='0.01',rayleighMass='0.01')
self.root_node.createObject('CGLinearSolver', iterations=200, tolerance=1E-6, threshold=1E-6)
# Object to transfer creation
node = self.root_node.createChild('main')
node.createObject('MeshObjLoader',name='source', filename='./mesh/source.obj', triangulate=1, translation='0 0 0', rotation='0 0 0', scale3d='1 1 1')
node.createObject('MeshToImageEngine', template='ImageUC', name='rasterizer', src='@source', insideValue='1', voxelSize=0.01, padSize=2, rotateImage='false')
node.createObject('ImageContainer', template='ImageUC', name='image', src='@rasterizer', drawBB='false')
node.createObject('MeshTopology', name='frame_topo', position=listListToStr(dofs_position))
#================================ Target model ===================================
targetNode = node.createChild('target')
targetNode.createObject('MeshObjLoader', name='loader', filename='./mesh/target.obj', rotation='0 0 0', translation='0 0 0', scale3d='1 1 1', showObject=0)
targetNode.createObject('MechanicalObject', template='Vec3d', name='DOFs', src='@loader', showObject=0)
targetNode.createObject('NormalsFromPoints', name='normalsFromPoints', template='Vec3d', position='@DOFs.position', triangles='@loader.triangles', invertNormals=0)
targetNode.createObject('FixedConstraint', fixAll='1', drawSize=0.001)
targetVisuNode = targetNode.createChild('visu')
targetVisuNode.createObject('OglModel', template='ExtVec3f', name='visual', src='@../loader', color='0.5 0.5 0.5 0.85')
#================================ Rigid frame ====================================
rigidNode = node.createChild('rigid')
rigidNode.createObject('MechanicalObject', template='Rigid', name='DOFs', src='@../frame_topo', showObject=1, showObjectScale='0.1')
rigidNode.createObject('RigidMass', mass="1 1 ", inertia="1 1 1 1 1 1")
#rigidNode.createObject('FixedConstraint', name='fixed', indices='0')
#=================================== Scale =======================================
scaleNode = node.createChild('scale')
scaleNode.createObject('MechanicalObject', template='Vec3d', name='DOFs', position='1 1 1 1 1 1', showObject=0, showObjectScale='0.1')
#=========================== Alignement constraint ===============================
offsetNode = rigidNode.createChild('offset')
offsetNode.createObject('MechanicalObject', template='Rigid', name='DOFs', position='0 0 0 0 0 0 1 0 0 0 0 0 0 1', showObject=1, showObjectScale='0.1')
offsetNode.createObject('AssembledRigidRigidMapping', template='Rigid,Rigid', source='0 '+listToStr(offset_position[0]) + listToStr(joint_orientation) + ' 1 '+listToStr(offset_position[1]) + listToStr(joint_orientation))
# --- old things even if they don't work well are often more stable, just for creating the prototype ...
offsetNode.createObject('JointSpringForceField', template='Rigid', name='joint', object1='@.', object2='@.', spring=' BEGIN_SPRING 0 1 FREE_AXIS 0 1 0 0 0 0 KS_T 0 1E10 KS_R 0 1e10 KS_B 3E3 KD 0.1 R_LIM_X 0 0 R_LIM_Y 0 0 R_LIM_Z 0 0 REST_T 0 1 0 END_SPRING')
#============================= Registration model ================================
objMainNode = rigidNode.createChild('deformable')
scaleNode.addChild(objMainNode)
# scene creation
loader = objMainNode.createObject('MeshObjLoader',name='source', filename='./mesh/source.obj', triangulate=1, translation='0 0 0', rotation='0 0 0', scale3d='1 1 1')
objMainNode.createObject('MeshToImageEngine', template='ImageUC', name='rasterizer', src='@source', value=1, insideValue=1, voxelSize=0.01, padSize=0, rotateImage='false')
objMainNode.createObject('ImageContainer', template='ImageUC', name='image', src='@rasterizer', drawBB='false')
objMainNode.createObject('MechanicalObject', template='Affine', name='parent', showObject=1, src='@../../frame_topo', showObjectScale='0.1')
objMainNode.createObject('RigidScaleToAffineMultiMapping', template='Rigid,Vec3d,Affine', input1='@../../rigid/DOFs', input2='@../../scale/DOFs', output='@.', autoInit='1', printLog='0')
objMainNode.createObject('VoronoiShapeFunction', template='ShapeFunctiond,ImageUC', name='SF', position='@parent.rest_position', image='@image.image', transform='@image.transform', nbRef=4, clearData=1, bias=0)
# Behavior Node
objBehaviorNode = objMainNode.createChild('behavior')
objBehaviorNode.createObject('ImageGaussPointSampler', name='sampler', indices='@../SF.indices', weights='@../SF.weights', transform='@../SF.transform', method=2, order=1, targetNumber=40)
objBehaviorNode.createObject('MechanicalObject', template='F331')
objBehaviorNode.createObject('LinearMapping', template='Affine,F331')
objBehaviorNode.createObject('ProjectiveForceField', template='F331', youngModulus=1E6, poissonRatio=0, viscosity=0, isCompliance=0)
# Contact
objContactNode = objMainNode.createChild('registration')
objContactNode.createObject('MeshTopology', name='topo', src='@../source')
objContactNode.createObject('MechanicalObject', name='DOFs')
objContactNode.createObject('UniformMass', totalMass=1)
objContactNode.createObject('TriangleModel')
objContactNode.createObject('LinearMapping', template='Affine,Vec3d')
# Visual model
objVisuNode = objContactNode.createChild('visual')
objVisuNode.createObject('OglModel', template='ExtVec3f', name='visual', src='@../topo', color='1 0.2 0.2 0.8')
objVisuNode.createObject('IdentityMapping', template='Vec3d,ExtVec3f')
# Registration : first attraction field to well positioning the source on the target
objRegistrationNode = objMainNode.createChild('force')
objRegistrationNode.createObject('MeshTopology', name='topo', src='@../source')
objRegistrationNode.createObject('MechanicalObject', template='Vec3d', name='DOFs')
objRegistrationNode.createObject('LinearMapping', template='Affine,Vec3d')
# registration force field
springs = ""
for i in range(len(loader.position)):
springs += str(i)+' '+str(i)+' '
distanceNode = objRegistrationNode.createChild('registration_constraint')
targetNode.addChild(distanceNode)
distanceNode.createObject('MechanicalObject', template='Vec3d', name='distanceDOFs')
distanceNode.createObject('DifferenceMultiMapping', template='Vec3d,Vec3d', input='@'+Tools.node_path_rel(distanceNode, targetNode)+' @'+Tools.node_path_rel(distanceNode, objRegistrationNode), output='@.', pairs=springs, showObjectScale="0.005")
distanceNode.createObject('UniformCompliance', name='constraint', isCompliance=0, compliance=1E-6, damping=0.1)
# Registration : surface to surface registration forces
surfaceRegistrationNode = objMainNode.createChild('reg_force')
surfaceRegistrationNode.createObject('MeshTopology', name='topo', src='@../source')
surfaceRegistrationNode.createObject('MechanicalObject', name='DOFs')
surfaceRegistrationNode.createObject('Triangle')
surfaceRegistrationNode.createObject('LinearMapping', template='Affine,Vec3d', assemble=0)
surfaceRegistrationNode.createObject('NormalsFromPoints', name='normalsFromPoints', template='Vec3d', position='@DOFs.position', triangles='@topo.triangles', invertNormals=0)
surfaceRegistrationNode.createObject('ClosestPointRegistrationForceField', name='ICP', template='Vec3d'
, sourceTriangles='@topo.triangles', sourceNormals='@normalsFromPoints.normals'
, position='@../../../target/loader.position' , triangles='@../../../target/loader.triangles', normals='@../../../target/normalsFromPoints.normals'
, cacheSize=4, blendingFactor=1, stiffness=1E3, damping=1E-3
, outlierThreshold=0, normalThreshold=0, rejectOutsideBbox=0, drawColorMap='0')
def run():
ok = True
info = SofaPython.mass.RigidMassInfo()
# testing axis-aligned known geometric shapes
for m in xrange(len(meshes)):
mesh = meshes[m]
mesh_path = path + meshes[m]
for s in xrange(len(scales)):
scale = scales[s]
if mesh=="cylinder.obj" and scale[0]!=scale[1]:
continue
for d in xrange(len(densities)):
density=densities[d]
info.setFromMesh( mesh_path, density, scale )
error = " ("+meshes[m]+", s="+Tools.cat(scale)+" d="+str(density)+")"
ok &= EXPECT_TRUE( almostEqualReal(info.mass, masses[m][s][d]), "mass"+error+" "+str(info.mass)+"!="+str(masses[m][s][d]) )
ok &= EXPECT_TRUE( almostEqualLists(info.com,[x*0.5 for x in scale]), "com"+error+" "+Tools.cat(info.com)+"!="+Tools.cat([x*0.5 for x in scale]) )
ok &= EXPECT_TRUE( almostEqualLists(info.diagonal_inertia,inertia[m][s][d]), "inertia"+error+" "+str(info.diagonal_inertia)+"!="+str(inertia[m][s][d]) )
# testing diagonal inertia extraction from a rotated cuboid
mesh = "cube.obj"
mesh_path = path + mesh
scale = scales[3]
density = 1
theory = sorted(inertia[0][3][0])
for r in rotations:
info.setFromMesh( mesh_path, density, scale, r )
local = sorted(info.diagonal_inertia)
ok &= EXPECT_TRUE( almostEqualLists(local,theory), "inertia "+str(local)+"!="+str(theory)+" (rotation="+str(r)+")" )
# testing extracted inertia rotation
mesh = "rotated_cuboid_12_35_-27.obj"
mesh_path = path + mesh
density = 1
info.setFromMesh( mesh_path, density )
# theoretical results
scale = [2,3,1]
mass = density * scale[0]*scale[1]*scale[2]
inertiat = numpy.empty(3)
inertiat[0] = 1.0/12.0 * mass * (scale[1]*scale[1]+scale[2]*scale[2]) # x
inertiat[1] = 1.0/12.0 * mass * (scale[0]*scale[0]+scale[2]*scale[2]) # y
inertiat[2] = 1.0/12.0 * mass * (scale[0]*scale[0]+scale[1]*scale[1]) # z
# used quaternion in mesh
q = Quaternion.normalized( Quaternion.from_euler( [12*math.pi/180.0, 35*math.pi/180.0, -27*math.pi/180.0] ) )
# corresponding rotation matrices (ie frame defined by columns)
mt = Quaternion.to_matrix( q )
m = Quaternion.to_matrix( info.inertia_rotation )
# matching inertia
idxt = numpy.argsort(inertiat)
idx = numpy.argsort(info.diagonal_inertia)
# checking if each axis/column are parallel (same or opposite for unitary vectors)
for i in xrange(3):
ok &= EXPECT_TRUE( almostEqualLists(mt[:,idxt[i]].tolist(),m[:,idx[i]].tolist(),1e-5) or almostEqualLists(mt[:,idxt[i]].tolist(),(-m[:,idx[i]]).tolist(),1e-5), "wrong inertia rotation" )
# print mt[:,idxt]
# print m [:,idx ]
return ok
def createScene(node):
# controller
node.createObject('PythonScriptController',
filename = __file__,
classname = 'Controller' )
node.dt = 0.005
# friction coefficient
shared.mu = 0.5
scene = Tools.scene( node )
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')
node.removeObject(ode)
node.createObject('RequiredPlugin',
pluginName = 'pouf')
ode = node.createObject('pouf_solver')
num = node.createObject('pouf.pgs',
name = 'num',
iterations = 100,
homogenize = True,
precision = 1e-14)
proximity = node.getObject('proximity')
proximity.alarmDistance = 0.5
proximity.contactDistance = 0.1
# ground
ground = Rigid.Body('ground');
ground.node = ground.insert( scene );
# plane
plane = Rigid.Body('plane')
plane.visual = path + '/share/mesh/ground.obj'
plane.collision = plane.visual
plane.mass_from_mesh( plane.visual, 10 )
plane.node = plane.insert( scene )
ground.node.createObject('FixedConstraint',
indices = '0')
# ground-plane joint
frame = Rigid.Frame()
frame.translation = [8, 0, 0]
joint = Rigid.RevoluteJoint(2)
joint.absolute(frame, ground.node, plane.node)
joint.upper_limit = 0
joint.node = joint.insert( scene )
# box
box = Rigid.Body('box')
box.visual = path + '/share/mesh/cube.obj'
box.collision = box.visual
box.dofs.translation = [0, 3, 0]
box.mass_from_mesh( box.visual, 50 )
box.node = box.insert( scene )
shared.plane = plane.node.getObject('dofs')
shared.box = box.node.getObject('dofs')
shared.joint = joint.node.getObject('dofs')
# pid
shared.pid = Control.PID(shared.joint)
shared.pid.pos = -math.atan( shared.mu ) # target should trigger slide
shared.pid.basis = [0, 0, 0, 0, 0, 1]
# shared.pid.dofs.externalForce = '-1e7'
scale = 2e5
shared.pid.kp = - 5 * scale
shared.pid.kd = - 100 * scale
shared.pid.ki = - 1e-1 * scale