def __init__(self,
name,
node1,
node2,
stiffnesses=[0, 0, 0, 0, 0, 0],
index1=0,
index2=0):
self.node = node1.createChild(name)
self.dofs = self.node.createObject('MechanicalObject',
template='Vec6' + template_suffix,
name='dofs',
position='0 0 0 0 0 0')
input = [] # @internal
input.append('@' + Tools.node_path_rel(self.node, node1) + '/dofs')
input.append('@' + Tools.node_path_rel(self.node, node2) + '/dofs')
self.mapping = self.node.createObject(
'RigidJointMultiMapping',
name='mapping',
input=concat(input),
output='@dofs',
pairs=str(index1) + " " + str(index2),
geometricStiffness=geometric_stiffness)
self.forcefield = self.node.createObject('DiagonalStiffness',
name='ff',
stiffness=concat(stiffnesses))
node2.addChild(self.node)
def __init__(self, node, masks, limits, compliances):
self.node = node.createChild("limits")
set = []
position = [0] * len(masks)
offset = []
for i in range(len(masks)):
set = set + [0] + masks[i]
offset.append(limits[i])
self.dofs = self.node.createObject('MechanicalObject',
template='Vec1' +
template_suffix,
name='dofs',
position=concat(position))
self.mapping = self.node.createObject('ProjectionMapping',
set=concat(set),
offset=concat(offset))
self.compliance = self.node.createObject(
'DiagonalCompliance',
name='compliance',
compliance=concat(compliances))
# self.type = self.node.createObject('Stabilization')
self.constraint = self.node.createObject('UnilateralConstraint')
def __init__(self, name, node1, node2, mask, compliance=0, index1=0, index2=0, isCompliance=True):
self.node = node1.createChild(name)
self.mask = mask
self.dofs = self.node.createObject(
"MechanicalObject", template="Vec6" + template_suffix, name="dofs", position="0 0 0 0 0 0"
)
input = [] # @internal
input.append("@" + Tools.node_path_rel(self.node, node1) + "/dofs")
if not node2 is None:
input.append("@" + Tools.node_path_rel(self.node, node2) + "/dofs")
self.mapping = self.node.createObject(
"RigidJointMultiMapping",
name="mapping",
input=concat(input),
output="@dofs",
pairs=str(index1) + " " + str(index2),
geometricStiffness=geometric_stiffness,
)
node2.addChild(self.node)
else:
self.mapping = self.node.createObject(
"RigidJointMapping",
name="mapping",
input=concat(input),
output="@dofs",
pairs=str(index1) + " " + str(index2),
geometricStiffness=geometric_stiffness,
)
self.constraint = GenericRigidJoint.Constraint(self.node, mask, compliance, isCompliance)
def __init__(self, node, mask, velocities, compliance):
self.node = node.createChild( "controller" )
position = [0] * len(mask)
self.dofs = self.node.createObject('MechanicalObject', template='Vec1'+template_suffix, name='dofs', position=concat(position))
self.mapping = self.node.createObject('MaskMapping', dofs=concat(mask))
self.compliance = self.node.createObject('UniformCompliance', name='compliance', compliance=compliance, isCompliance=1)
self.type = self.node.createObject('VelocityConstraintValue', velocities=concat(velocities))
def __init__(self,
node,
name,
node1,
node2,
stiffnesses=[0, 0, 0, 0, 0, 0],
index1=0,
index2=0):
self.node = node.createChild(name)
self.dofs = self.node.createObject('MechanicalObject',
template='Vec6d',
name='dofs',
position='0 0 0 0 0 0')
input = [] # @internal
input.append('@' + Tools.node_path_rel(self.node, node1) + '/dofs')
input.append('@' + Tools.node_path_rel(self.node, node2) + '/dofs')
self.mapping = self.node.createObject('RigidJointMultiMapping',
template='Rigid,Vec6d',
name='mapping',
input=concat(input),
output='@dofs',
pairs=str(index1) + " " +
str(index2))
compliances = vec.inv(stiffnesses)
self.compliance = self.node.createObject(
'DiagonalCompliance',
template="Vec6d",
name='compliance',
compliance=concat(compliances),
isCompliance=0)
def insert(self, parent):
self.node = parent.createChild(self.name)
# 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(self.node, b) + '/dofs')
else:
joint = b.createChild(self.name + '-offset')
joint.createObject('MechanicalObject',
template='Rigid',
name='dofs')
joint.createObject('AssembledRigidRigidMapping',
template="Rigid,Rigid",
source='0 ' + str(o))
input.append('@' + Tools.node_path_rel(self.node, b) + '/' +
joint.name + '/dofs')
if len(input) == 0:
print 'warning: empty joint'
return None
dofs = self.node.createObject('MechanicalObject',
template='Vec6',
name='dofs',
position='0 0 0 0 0 0')
map = self.node.createObject('RigidJointMultiMapping',
name='mapping',
template='Rigid3,Vec6',
input=concat(input),
output='@dofs',
pairs="0 0")
sub = self.node.createChild("constraints")
sub.createObject('MechanicalObject', template='Vec1', name='dofs')
mask = [(1 - d) for d in self.dofs]
map = sub.createObject('MaskMapping',
name='mapping',
template='Vec6,Vec1',
input='@../',
output='@dofs',
dofs=concat(mask))
compliance = sub.createObject('UniformCompliance',
name='compliance',
template='Vec1',
compliance=self.compliance)
stab = sub.createObject('Stabilization')
return self.node
def __init__(self, node, mask, threshold):
self.node = node.createChild( "resistance" )
position = [0] * len(mask)
self.dofs = self.node.createObject('MechanicalObject', template='Vec1'+template_suffix, name='dofs', position=concat(position))
self.mapping = self.node.createObject('MaskMapping', dofs=concat(mask))
self.compliance = self.node.createObject('UniformCompliance', name='compliance', compliance=0, isCompliance=1)
self.type = self.node.createObject('VelocityConstraintValue', velocities=concat([0]) )
self.constraint = self.node.createObject('ResistanceConstraint', threshold=threshold)
def __init__(self, node, filepath, scale3d, offset, name_suffix=''):
global idxVisualModel
self.node = node.createChild( "visual"+name_suffix ) # node
r = Quaternion.to_euler(offset[3:]) * 180.0 / math.pi
self.model = self.node.createObject('VisualModel', name="visual"+str(idxVisualModel), fileMesh=filepath,
scale3d=concat(scale3d), translation=concat(offset[:3]) , rotation=concat(r),
useNormals=False, updateNormals=True)
self.mapping = self.node.createObject('RigidMapping', name="mapping")
idxVisualModel+=1
def insert(self, node):
res = node.createChild(self.name)
dofs = self.dofs.insert(res, name="dofs")
mass_node = res
if self.offset != None:
mass_node = res.createChild("mapped_mass")
self.offset.insert(mass_node, name="dofs")
mapping = mass_node.createObject(
"AssembledRigidRigidMapping", template="Rigid", source="0 " + str(self.offset)
)
# mass
mass = mass_node.createObject(
"RigidMass",
template="Rigid",
name="mass",
mass=self.mass,
inertia=concat(self.inertia),
inertia_forces=self.inertia_forces,
)
# visual model
if self.visual != None:
visual_template = "ExtVec3f"
visual = res.createChild("visual")
ogl = visual.createObject(
"OglModel",
template=visual_template,
name="mesh",
fileMesh=self.visual,
color=concat(self.color),
scale3d="1 1 1",
)
visual_map = visual.createObject("RigidMapping", template="Rigid" + ", " + visual_template, input="@../")
# collision model
if self.collision != None:
collision = res.createChild("collision")
collision.createObject("MeshObjLoader", name="loader", filename=self.collision)
collision.createObject("MeshTopology", name="topology", triangles="@loader.triangles")
collision.createObject("MechanicalObject", name="dofs", position="@loader.position")
model = collision.createObject("TriangleModel", name="model", template="Vec3d")
if self.group != None:
model.group = self.group
collision.createObject("RigidMapping", template="Rigid,Vec3d", input="@../", output="@./")
return res
def insertVisual(parentNode, solid, color):
node = parentNode.createChild("node_" + solid.name)
translation = solid.position[:3]
rotation = Quaternion.to_euler(solid.position[3:]) * 180.0 / math.pi
for m in solid.mesh:
Tools.meshLoader(node, m.source, name="loader_" + m.id)
node.createObject("OglModel",
src="@loader_" + m.id,
translation=concat(translation),
rotation=concat(rotation),
color=color)
def __init__(self, node, mask, velocities, compliance):
self.node = node.createChild("controller")
position = [0] * len(mask)
self.dofs = self.node.createObject(
"MechanicalObject", template="Vec1" + template_suffix, name="dofs", position=concat(position)
)
self.mapping = self.node.createObject("MaskMapping", dofs=concat(mask))
self.compliance = self.node.createObject(
"UniformCompliance", name="compliance", compliance=compliance, isCompliance=1
)
self.type = self.node.createObject("VelocityConstraintValue", velocities=concat(velocities))
def __init__(self, node, mask, threshold):
self.node = node.createChild("resistance")
position = [0] * len(mask)
self.dofs = self.node.createObject(
"MechanicalObject", template="Vec1" + template_suffix, name="dofs", position=concat(position)
)
self.mapping = self.node.createObject("MaskMapping", dofs=concat(mask))
self.compliance = self.node.createObject(
"UniformCompliance", name="compliance", compliance=0, isCompliance=1
)
self.type = self.node.createObject("VelocityConstraintValue", velocities=concat([0]))
self.constraint = self.node.createObject("ResistanceConstraint", threshold=threshold)
def __init__(self, node, filepath, scale3d, offset):
self.node = node.createChild("visual") # node
r = Quaternion.to_euler(offset[3:]) * 180.0 / math.pi
self.model = self.node.createObject('VisualModel',
template='ExtVec3f',
name='model',
fileMesh=filepath,
scale3d=concat(scale3d),
translation=concat(offset[:3]),
rotation=concat(r))
self.mapping = self.node.createObject('RigidMapping',
name="mapping")
def setManually(self, offset=[0, 0, 0, 0, 0, 0, 1], mass=1, inertia=[1, 1, 1], inertia_forces=False):
## create the rigid body by manually giving its inertia
self.frame = Frame.Frame(offset)
self.dofs = self.frame.insert(self.node, name="dofs", template="Rigid3" + template_suffix)
self.mass = self.node.createObject(
"RigidMass", name="mass", mass=mass, inertia=concat(inertia), inertia_forces=inertia_forces
)
def __init__(self, node, compliances):
self.node = node.createChild("constraint")
self.dofs = self.node.createObject("MechanicalObject", template="Vec6" + template_suffix, name="dofs")
self.mapping = self.node.createObject("IdentityMapping")
self.compliance = self.node.createObject(
"DiagonalCompliance", name="compliance", compliance=concat(compliances)
)
def __init__(self, node, mask, compliance, isCompliance):
self.node = node.createChild("constraint")
self.dofs = self.node.createObject("MechanicalObject", template="Vec1" + template_suffix, name="dofs")
self.mapping = self.node.createObject("MaskMapping", dofs=concat(mask))
self.compliance = self.node.createObject(
"UniformCompliance", name="compliance", compliance=compliance, isCompliance=isCompliance
)
def insertVisual(parentNode, solid, color):
node = parentNode.createChild("node_"+solid.name)
translation=solid.position[:3]
rotation = Quaternion.to_euler(solid.position[3:]) * 180.0 / math.pi
for m in solid.mesh:
Tools.meshLoader(node, m.source, name="loader_"+m.id)
node.createObject("OglModel",src="@loader_"+m.id, translation=concat(translation),rotation=concat(rotation), color=color)
def __init__(self,
name,
node1,
node2,
compliance=0,
index1=0,
index2=0,
isCompliance=True):
# GenericRigidJoint.__init__(self, name, node1, node2, [1,1,1,0,0,0], compliance, index1, index2) # GenericRigidJoint can work but is way overkill
self.node = node1.createChild(name)
self.dofs = self.node.createObject('MechanicalObject',
template='Vec3' + template_suffix,
name='dofs',
position='0 0 0')
input = [] # @internal
input.append('@' + Tools.node_path_rel(self.node, node1) + '/dofs')
input.append('@' + Tools.node_path_rel(self.node, node2) + '/dofs')
self.mapping = self.node.createObject('DifferenceMultiMapping',
name='mapping',
input=concat(input),
output='@dofs',
pairs=str(index1) + " " +
str(index2))
self.compliance = self.node.createObject('UniformCompliance',
name='compliance',
compliance=compliance,
isCompliance=isCompliance)
node2.addChild(self.node)
def __init__(self,
node,
name,
node1,
node2,
compliance=0,
index1=0,
index2=0,
rest_lenght=-1):
self.node = node.createChild(name)
self.dofs = self.node.createObject('MechanicalObject',
template='Rigid',
name='dofs')
input = [] # @internal
input.append('@' + Tools.node_path_rel(self.node, node1) + '/dofs')
input.append('@' + Tools.node_path_rel(self.node, node2) + '/dofs')
self.mapping = self.node.createObject('SubsetMultiMapping',
template='Rigid,Rigid',
name='mapping',
input=concat(input),
output='@dofs',
indexPairs="0 " + str(index1) +
" 1 " + str(index2))
self.constraint = DistanceRigidJoint.Constraint(
self.node, compliance, rest_lenght)
def __init__(self,
node,
name,
node1,
node2,
mask,
compliance=0,
index1=0,
index2=0):
self.node = node.createChild(name)
self.dofs = self.node.createObject('MechanicalObject',
template='Vec6d',
name='dofs',
position='0 0 0 0 0 0')
input = [] # @internal
input.append('@' + Tools.node_path_rel(self.node, node1) + '/dofs')
input.append('@' + Tools.node_path_rel(self.node, node2) + '/dofs')
self.mapping = self.node.createObject('RigidJointMultiMapping',
template='Rigid,Vec6d',
name='mapping',
input=concat(input),
output='@dofs',
pairs=str(index1) + " " +
str(index2))
self.constraint = GenericRigidJoint.Constraint(self.node, mask,
compliance)
def __init__(self, node, filepath, scale3d, offset, name_suffix=''):
global idxVisualModel
self.node = node.createChild("visual" + name_suffix) # node
r = Quaternion.to_euler(offset[3:]) * 180.0 / math.pi
self.model = self.node.createObject('VisualModel',
name="visual" +
str(idxVisualModel),
fileMesh=filepath,
scale3d=concat(scale3d),
translation=concat(offset[:3]),
rotation=concat(r),
useNormals=False,
updateNormals=True)
self.mapping = self.node.createObject('RigidMapping',
name="mapping")
idxVisualModel += 1
def addSpring(self, stiffness):
mask = [(1 - d) for d in self.mask]
mask = vec.scal(1.0 / stiffness, mask)
return self.node.createObject('DiagonalCompliance',
template="Rigid",
isCompliance="0",
compliance=concat(mask))
def addMotor(self, forces=[0, 0, 0, 0, 0, 0]):
## adding a constant force/torque at the offset location (that could be driven by a controller to simulate a motor)
return self.node.createObject('ConstantForceField',
template='Rigid3' + template_suffix,
name='motor',
points='0',
forces=concat(forces))
def __init__(self,
name,
node1,
node2,
compliances=[0, 0, 0, 0, 0, 0],
index1=0,
index2=0):
self.node = node1.createChild(name)
self.dofs = self.node.createObject('MechanicalObject',
template='Vec6' + template_suffix,
name='dofs',
position='0 0 0 0 0 0')
input = [] # @internal
input.append('@' + Tools.node_path_rel(self.node, node1) + '/dofs')
input.append('@' + Tools.node_path_rel(self.node, node2) + '/dofs')
self.mapping = self.node.createObject(
'RigidJointMultiMapping',
name='mapping',
input=concat(input),
output='@dofs',
pairs=str(index1) + " " + str(index2),
geometricStiffness=geometric_stiffness)
self.constraint = CompleteRigidJoint.Constraint(
self.node, compliances
) # the constraint compliance cannot be in the same branch as eventual limits...
node2.addChild(self.node)
def __init__(self, node, mask, velocities, compliance):
self.node = node.createChild("controller")
position = [0] * len(mask)
self.dofs = self.node.createObject('MechanicalObject',
template='Vec1' +
template_suffix,
name='dofs',
position=concat(position))
self.mapping = self.node.createObject('MaskMapping',
dofs=concat(mask))
self.compliance = self.node.createObject('UniformCompliance',
name='compliance',
compliance=compliance,
isCompliance=1)
self.type = self.node.createObject('VelocityConstraintValue',
velocities=concat(velocities))
def addMotor(self, forces=[0, 0, 0, 0, 0, 0]):
## adding a constant force/torque to the rigid body (that could be driven by a controller to simulate a motor)
return self.node.createObject('ConstantForceField',
template='Rigid',
name='motor',
points='0',
forces=concat(forces))
def addSpring(self, translation_stiffness, rotation_stiffness):
mask = [0] * 6
mask[self.axis] = 1.0 / translation_stiffness
mask[3 + self.axis] = 1.0 / rotation_stiffness
return self.node.createObject('DiagonalCompliance',
template="Rigid",
isCompliance="0",
compliance=concat(mask))
def __init__(self, node, mask, threshold):
self.node = node.createChild("resistance")
position = [0] * len(mask)
self.dofs = self.node.createObject('MechanicalObject',
template='Vec1' +
template_suffix,
name='dofs',
position=concat(position))
self.mapping = self.node.createObject('MaskMapping',
dofs=concat(mask))
self.compliance = self.node.createObject('UniformCompliance',
name='compliance',
compliance=0,
isCompliance=1)
self.type = self.node.createObject('VelocityConstraintValue',
velocities=concat([0]))
self.constraint = self.node.createObject('ResistanceConstraint',
threshold=threshold)
def __init__(self, node, mask, target, compliance, isCompliance=False):
self.node = node.createChild( "controller-mask" )
self.dofs = self.node.createObject('MechanicalObject', template='Vec1'+template_suffix, name='dofs')
self.node.createObject('MaskMapping', dofs=concat(mask))
self.nodeTarget = self.node.createChild( "controller-target" )
self.nodeTarget.createObject('MechanicalObject', template='Vec1'+template_suffix, name='dofs')
self.mapping = self.nodeTarget.createObject('DifferenceFromTargetMapping', targets=concat(target))
self.compliance = self.nodeTarget.createObject('UniformCompliance', name='compliance', compliance=compliance, isCompliance=isCompliance)
def __init__(self, node, mask, forces):
self.node = node.createChild("controller")
size = sum(x != 0 for x in mask)
position = [0] * size
points = numpy.arange(0, size, 1)
self.dofs = self.node.createObject(
"MechanicalObject", template="Vec1" + template_suffix, name="dofs", position=concat(position)
)
self.mapping = self.node.createObject("MaskMapping", dofs=concat(mask))
self.force = self.node.createObject(
"ConstantForceField",
template="Vec1" + template_suffix,
forces=concat(forces),
points=concat(points.tolist()),
)
def addMotor(self, forces=[0, 0, 0, 0, 0, 0]):
## adding a constant force/torque at the offset location (that could be driven by a controller to simulate a motor)
return self.node.createObject(
"ConstantForceField",
template="Rigid3" + template_suffix,
name="motor",
points="0",
forces=concat(forces),
)
def __init__(self, node, filepath, scale3d, offset, name_suffix=""):
self.node = node.createChild("collision" + name_suffix) # node
r = Quaternion.to_euler(offset[3:]) * 180.0 / math.pi
self.loader = SofaPython.Tools.meshLoader(
self.node,
filename=filepath,
name="loader",
scale3d=concat(scale3d),
translation=concat(offset[:3]),
rotation=concat(r),
triangulate=True,
)
self.topology = self.node.createObject("MeshTopology", name="topology", src="@loader")
self.dofs = self.node.createObject("MechanicalObject", name="dofs", template="Vec3" + template_suffix)
self.triangles = self.node.createObject("TriangleModel", name="model")
self.mapping = self.node.createObject("RigidMapping", name="mapping")
self.normals = None
self.visual = None
def __init__(self, node, mask, target, compliance, isCompliance=False):
self.node = node.createChild("controller-mask")
self.dofs = self.node.createObject('MechanicalObject',
template='Vec1' +
template_suffix,
name='dofs')
self.node.createObject('MaskMapping', dofs=concat(mask))
self.nodeTarget = self.node.createChild("controller-target")
self.nodeTarget.createObject('MechanicalObject',
template='Vec1' + template_suffix,
name='dofs')
self.mapping = self.nodeTarget.createObject(
'DifferenceFromTargetMapping', targets=concat(target))
self.compliance = self.nodeTarget.createObject(
'UniformCompliance',
name='compliance',
compliance=compliance,
isCompliance=isCompliance)
def addSpring(self, stiffness1, stiffness2):
axis1 = (self.normal + 1) % 3
axis2 = (self.normal + 2) % 3
if axis1 > axis2:
axis1, axis2 = axis2, axis1
mask = [0] * 6
mask[axis1] = stiffness1
mask[axis2] = stiffness2
return self.node.createObject('DiagonalStiffness',
stiffness=concat(mask))
def setManually(self, offset = [0,0,0,0,0,0,1], mass = 1, inertia = [1,1,1], inertia_forces = False ):
## create the rigid body by manually giving its inertia
self.frame = Rigid.Frame( offset )
self.dofs = self.frame.insert( self.node, name='dofs' )
self.mass = self.node.createObject('RigidMass',
template = 'Rigid',
name = 'mass',
mass = mass,
inertia = concat(inertia),
inertia_forces = inertia_forces )
def __init__(self, node, mask, forces):
self.node = node.createChild("controller")
size = sum(x != 0 for x in mask)
position = [0] * size
points = numpy.arange(0, size, 1)
self.dofs = self.node.createObject('MechanicalObject',
template='Vec1' +
template_suffix,
name='dofs',
position=concat(position))
self.mapping = self.node.createObject('MaskMapping',
dofs=concat(mask))
self.force = self.node.createObject('ConstantForceField',
template='Vec1' +
template_suffix,
forces=concat(forces),
points=concat(points.tolist()))
def __init__(self, node, filepath, scale3d, offset):
self.node = node.createChild("collision") # node
r = Quaternion.to_euler(offset[3:]) * 180.0 / math.pi
self.loader = self.node.createObject("MeshObjLoader",
name='loader',
filename=filepath,
scale3d=concat(scale3d),
translation=concat(
offset[:3]),
rotation=concat(r),
triangulate=1)
self.topology = self.node.createObject('MeshTopology',
name='topology',
src="@loader")
self.dofs = self.node.createObject('MechanicalObject', name='dofs')
self.triangles = self.node.createObject('TriangleModel',
template='Vec3d',
name='model')
self.mapping = self.node.createObject('RigidMapping',
name="mapping")
def __init__(self, node, mask, target, compliance, isCompliance=False):
self.node = node.createChild("controller-mask")
self.dofs = self.node.createObject("MechanicalObject", template="Vec1" + template_suffix, name="dofs")
self.node.createObject("MaskMapping", dofs=concat(mask))
self.nodeTarget = self.node.createChild("controller-target")
self.nodeTarget.createObject("MechanicalObject", template="Vec1" + template_suffix, name="dofs")
self.mapping = self.nodeTarget.createObject("DifferenceFromTargetMapping", targets=concat(target))
self.compliance = self.nodeTarget.createObject(
"UniformCompliance", name="compliance", compliance=compliance, isCompliance=isCompliance
)
def __init__(self, node, compliances):
self.node = node.createChild("constraint")
self.dofs = self.node.createObject('MechanicalObject',
template='Vec6' +
template_suffix,
name='dofs')
self.mapping = self.node.createObject('IdentityMapping')
self.compliance = self.node.createObject(
'DiagonalCompliance',
name='compliance',
compliance=concat(compliances))
def __init__(self, node, mask, compliance):
self.node = node.createChild("constraint")
self.dofs = self.node.createObject('MechanicalObject',
template='Vec1d',
name='dofs')
self.mapping = self.node.createObject('MaskMapping',
dofs=concat(mask))
self.compliance = self.node.createObject('UniformCompliance',
name='compliance',
compliance=compliance)
self.type = self.node.createObject('Stabilization')
def __init__(self, node, masks, limits, compliances):
self.node = node.createChild("limits")
set = []
position = [0] * len(masks)
offset = []
for i in range(len(masks)):
set = set + [0] + masks[i]
offset.append(limits[i])
self.dofs = self.node.createObject(
"MechanicalObject", template="Vec1" + template_suffix, name="dofs", position=concat(position)
)
self.mapping = self.node.createObject("ProjectionMapping", set=concat(set), offset=concat(offset))
self.compliance = self.node.createObject(
"DiagonalCompliance", name="compliance", compliance=concat(compliances)
)
# self.type = self.node.createObject('Stabilization')
self.constraint = self.node.createObject("UnilateralConstraint")
def addSpring(self, stiffness1, stiffness2):
axis1 = (self.normal + 1) % 3
axis2 = (self.normal + 2) % 3
if axis1 > axis2:
axis1, axis2 = axis2, axis1
mask = [0] * 6
mask[axis1] = 1.0 / stiffness1
mask[axis2] = 1.0 / stiffness2
return self.node.createObject('DiagonalCompliance',
template="Rigid",
isCompliance="0",
compliance=concat(mask))
def addSpring(self, stiffness1, stiffness2):
index1 = 3 + (self.axis + 1) % 3
index2 = 3 + (self.axis + 2) % 3
if index1 > index2:
index1, index2 = index2, index1
mask = [0] * 6
mask[index1] = 1.0 / stiffness1
mask[index2] = 1.0 / stiffness2
return self.node.createObject('DiagonalCompliance',
template="Rigid",
isCompliance="0",
compliance=concat(mask))
def __init__(self, node, mask, compliance, isCompliance):
self.node = node.createChild("constraint")
self.dofs = self.node.createObject('MechanicalObject',
template='Vec1' +
template_suffix,
name='dofs')
self.mapping = self.node.createObject('MaskMapping',
dofs=concat(mask))
self.compliance = self.node.createObject('UniformCompliance',
name='compliance',
compliance=compliance,
isCompliance=isCompliance)
def addSpring( self, translation_stiffness=None, rotation_stiffness=None ):
stiffnesses=[]
for i in range(0,3):
if not self.mask[i]:
stiffnesses.append( float(translation_stiffness) )
else:
stiffnesses.append(0)
for i in range(3,6):
if not self.mask[i]:
stiffnesses.append( float(rotation_stiffness) )
else:
stiffnesses.append(0)
return self.node.createObject('DiagonalStiffness', stiffness=concat(stiffnesses))
def setFromRigidInfo(self, info, offset = [0,0,0,0,0,0,1], inertia_forces = False) :
self.framecom = Frame.Frame()
self.framecom.rotation = info.inertia_rotation
self.framecom.translation = info.com
self.frame = Frame.Frame(offset) * self.framecom
self.dofs = self.frame.insert( self.node, name = 'dofs', template="Rigid3"+template_suffix )
self.mass = self.node.createObject('RigidMass',
name = 'mass',
mass = info.mass,
inertia = concat(info.diagonal_inertia),
inertia_forces = inertia_forces )
def __init__(self, node, compliances):
self.node = node.createChild("constraint")
self.dofs = self.node.createObject('MechanicalObject',
template='Vec6d',
name='dofs')
self.mapping = self.node.createObject('IdentityMapping',
template='Vec6d,Vec6d')
self.compliance = self.node.createObject(
'DiagonalCompliance',
template="Vec6d",
name='compliance',
compliance=concat(compliances))
self.type = self.node.createObject('Stabilization')
def __init__(self, name, node1, node2, stiffnesses=[0, 0, 0, 0, 0, 0], index1=0, index2=0):
self.node = node1.createChild(name)
self.dofs = self.node.createObject(
"MechanicalObject", template="Vec6" + template_suffix, name="dofs", position="0 0 0 0 0 0"
)
input = [] # @internal
input.append("@" + Tools.node_path_rel(self.node, node1) + "/dofs")
input.append("@" + Tools.node_path_rel(self.node, node2) + "/dofs")
self.mapping = self.node.createObject(
"RigidJointMultiMapping",
name="mapping",
input=concat(input),
output="@dofs",
pairs=str(index1) + " " + str(index2),
geometricStiffness=geometric_stiffness,
)
compliances = 1.0 / numpy.array(stiffnesses)
self.compliance = self.node.createObject(
"DiagonalCompliance", name="compliance", compliance=concat(compliances), isCompliance=0
)
node2.addChild(self.node)
def insert(self, parent):
res = Joint.insert(self, parent)
if self.lower_limit == None and self.upper_limit == None:
return res
limit = res.createChild('limit')
dofs = limit.createObject('MechanicalObject', template='Vec1')
map = limit.createObject('ProjectionMapping', template='Vec6,Vec1')
limit.createObject('UniformCompliance',
template='Vec1',
compliance='0')
limit.createObject('UnilateralConstraint')
# don't stabilize as we need to detect violated
# constraints first
# limit.createObject('Stabilization');
set = []
position = []
offset = []
if self.lower_limit != None:
set = set + [0] + self.dofs
position.append(0)
offset.append(self.lower_limit)
if self.upper_limit != None:
set = set + [0] + vec.minus(self.dofs)
position.append(0)
offset.append(-self.upper_limit)
map.set = concat(set)
map.offset = concat(offset)
dofs.position = concat(position)
return res
def __init__(self,
name,
node1,
node2,
mask,
compliance=0,
index1=0,
index2=0,
isCompliance=True):
self.node = node1.createChild(name)
self.mask = mask
self.dofs = self.node.createObject('MechanicalObject',
template='Vec6' + template_suffix,
name='dofs',
position='0 0 0 0 0 0')
input = [] # @internal
input.append('@' + Tools.node_path_rel(self.node, node1) + '/dofs')
if not node2 is None:
input.append('@' + Tools.node_path_rel(self.node, node2) + '/dofs')
self.mapping = self.node.createObject(
'RigidJointMultiMapping',
name='mapping',
input=concat(input),
output='@dofs',
pairs=str(index1) + " " + str(index2),
geometricStiffness=geometric_stiffness)
node2.addChild(self.node)
else:
self.mapping = self.node.createObject(
'RigidJointMapping',
name='mapping',
input=concat(input),
output='@dofs',
pairs=str(index1) + " " + str(index2),
geometricStiffness=geometric_stiffness)
self.constraint = GenericRigidJoint.Constraint(self.node, mask,
compliance,
isCompliance)
def addSpring(self, translation_stiffness=None, rotation_stiffness=None):
stiffnesses = []
for i in range(0, 3):
if not self.mask[i]:
stiffnesses.append(float(translation_stiffness))
else:
stiffnesses.append(0)
for i in range(3, 6):
if not self.mask[i]:
stiffnesses.append(float(rotation_stiffness))
else:
stiffnesses.append(0)
return self.node.createObject('DiagonalStiffness',
stiffness=concat(stiffnesses))
def insert(self, parent):
res = Joint.insert(self, parent)
if self.lower_limit == None and self.upper_limit == None:
return res
limit = res.createChild('limit')
dofs = limit.createObject('MechanicalObject', template = 'Vec1')
map = limit.createObject('ProjectionMapping', template = 'Vec6,Vec1' )
limit.createObject('UniformCompliance', template = 'Vec1', compliance = '0' )
limit.createObject('UnilateralConstraint');
# don't stabilize as we need to detect violated
# constraints first
# limit.createObject('Stabilization');
set = []
position = []
offset = []
if self.lower_limit != None:
set = set + [0] + self.dofs
position.append(0)
offset.append(self.lower_limit)
if self.upper_limit != None:
set = set + [0] + vec.minus(self.dofs)
position.append(0)
offset.append(- self.upper_limit)
map.set = concat(set)
map.offset = concat(offset)
dofs.position = concat(position)
return res
def __init__(self, name, node1, node2, compliance=0, index1=0, index2=0, rest_lenght=-1):
self.node = node1.createChild(name)
self.dofs = self.node.createObject("MechanicalObject", template="Rigid3" + template_suffix, name="dofs")
input = [] # @internal
input.append("@" + Tools.node_path_rel(self.node, node1) + "/dofs")
input.append("@" + Tools.node_path_rel(self.node, node2) + "/dofs")
self.mapping = self.node.createObject(
"SubsetMultiMapping",
name="mapping",
input=concat(input),
output="@dofs",
indexPairs="0 " + str(index1) + " 1 " + str(index2),
)
self.constraint = DistanceRigidJoint.Constraint(self.node, compliance, rest_lenght)
node2.addChild(self.node)
def __init__(self, node, masks, limits, compliances):
self.node = node.createChild( "limits" )
set = []
position = [0] * len(masks)
offset = []
for i in range(len(masks)):
set = set + [0] + masks[i]
offset.append(limits[i])
self.dofs = self.node.createObject('MechanicalObject', template='Vec1d', name='dofs', position=concat(position))
self.mapping = self.node.createObject('ProjectionMapping', set=concat(set), offset=concat(offset))
self.compliance = self.node.createObject('DiagonalCompliance', name='compliance', compliance=concat(compliances))
self.type = self.node.createObject('ConstraintValue') # cannot be stabilized for now
self.constraint = self.node.createObject('UnilateralConstraint')
def __init__(self, node, masks, limits, compliance):
self.node = node.createChild( "limits" )
set = []
position = [0] * len(masks)
offset = []
for i in range(len(masks)):
set = set + [0] + masks[i]
offset.append(limits[i])
self.dofs = self.node.createObject('MechanicalObject', template='Vec1'+template_suffix, name='dofs', position=concat(position))
self.mapping = self.node.createObject('ProjectionMapping', set=concat(set), offset=concat(offset))
self.compliance = self.node.createObject('UniformCompliance', name='compliance', compliance=compliance)
# self.type = self.node.createObject('Stabilization')
self.constraint = self.node.createObject('UnilateralConstraint')
def setFromMesh(self, filepath, density = 1000.0, offset = [0,0,0,0,0,0,1], scale3d=[1,1,1], inertia_forces = False ):
## create the rigid body from a mesh (inertia and com are automatically computed)
info = Rigid.generate_rigid(filepath, density, scale3d)
self.framecom = Rigid.Frame()
self.framecom.rotation = info.inertia_rotation
self.framecom.translation = info.com
self.frame = Rigid.Frame(offset) * self.framecom
self.dofs = self.frame.insert( self.node, name = 'dofs' )
self.mass = self.node.createObject('RigidMass',
template = 'Rigid',
name = 'mass',
mass = info.mass,
inertia = concat(info.diagonal_inertia.tolist()),
inertia_forces = inertia_forces )
def __init__(self, name, node1, node2, compliances=[0, 0, 0, 0, 0, 0], index1=0, index2=0):
self.node = node1.createChild(name)
self.dofs = self.node.createObject(
"MechanicalObject", template="Vec6" + template_suffix, name="dofs", position="0 0 0 0 0 0"
)
input = [] # @internal
input.append("@" + Tools.node_path_rel(self.node, node1) + "/dofs")
input.append("@" + Tools.node_path_rel(self.node, node2) + "/dofs")
self.mapping = self.node.createObject(
"RigidJointMultiMapping",
name="mapping",
input=concat(input),
output="@dofs",
pairs=str(index1) + " " + str(index2),
geometricStiffness=geometric_stiffness,
)
self.constraint = CompleteRigidJoint.Constraint(
self.node, compliances
) # the constraint compliance cannot be in the same branch as eventual limits...
node2.addChild(self.node)
def __init__(self, name, node1, node2, compliance=0, index1=0, index2=0, isCompliance=True):
# GenericRigidJoint.__init__(self, name, node1, node2, [1,1,1,0,0,0], compliance, index1, index2) # GenericRigidJoint can work but is way overkill
self.node = node1.createChild(name)
self.dofs = self.node.createObject(
"MechanicalObject", template="Vec3" + template_suffix, name="dofs", position="0 0 0"
)
input = [] # @internal
input.append("@" + Tools.node_path_rel(self.node, node1) + "/dofs")
input.append("@" + Tools.node_path_rel(self.node, node2) + "/dofs")
self.mapping = self.node.createObject(
"DifferenceMultiMapping",
name="mapping",
input=concat(input),
output="@dofs",
pairs=str(index1) + " " + str(index2),
)
self.compliance = self.node.createObject(
"UniformCompliance", name="compliance", compliance=compliance, isCompliance=isCompliance
)
node2.addChild(self.node)
def __init__(self, name, node1, node2, stiffnesses=[0,0,0,0,0,0], index1=0, index2=0):
self.node = node1.createChild( name )
self.dofs = self.node.createObject('MechanicalObject', template = 'Vec6'+template_suffix, name = 'dofs', position = '0 0 0 0 0 0' )
input = [] # @internal
input.append( '@' + Tools.node_path_rel(self.node,node1) + '/dofs' )
input.append( '@' + Tools.node_path_rel(self.node,node2) + '/dofs' )
self.mapping = self.node.createObject('RigidJointMultiMapping', name = 'mapping', input = concat(input), output = '@dofs', pairs = str(index1)+" "+str(index2),
geometricStiffness = geometric_stiffness)
compliances = 1./numpy.array(stiffnesses);
self.compliance = self.node.createObject('DiagonalCompliance', name='compliance', compliance=concat(compliances), isCompliance=0)
node2.addChild( self.node )
def __init__(self, name, node1, node2, compliance=0, index1=0, index2=0, rest_lenght=-1 ):
self.node = node1.createChild( name )
self.dofs = self.node.createObject('MechanicalObject', template='Rigid3'+template_suffix, name='dofs' )
input = [] # @internal
input.append( '@' + Tools.node_path_rel(self.node,node1) + '/dofs' )
input.append( '@' + Tools.node_path_rel(self.node,node2) + '/dofs' )
self.mapping = self.node.createObject('SubsetMultiMapping', name='mapping', input = concat(input), output = '@dofs', indexPairs="0 "+str(index1)+" 1 "+str(index2) )
self.constraint = DistanceRigidJoint.Constraint(self.node, compliance, rest_lenght)
node2.addChild( self.node )
