def insertJoint(jointModel, rigids, scale=1, param=None):
""" create a StructuralAPI.GenericRigidJoint from the jointModel """
frames=list()
for i,offset in enumerate(jointModel.offsets):
if not jointModel.solids[i].id in rigids:
Sofa.msg_warning("Compliant.sml","insertJoint "+jointModel.name+" failed: "+jointModel.solids[i].id+" is not a rigid body")
return None
rigid = rigids[jointModel.solids[i].id] # shortcut
if rigid is None:
Sofa.msg_warning("Compliant.sml", "in joint {0}, solid {1} is missing, ignored".format(jointModel.name, jointModel.solids[i].id))
return
if not offset is None:
if offset.isAbsolute():
frames.append( rigid.addAbsoluteOffset(offset.name, StructuralAPI.scaleOffset(scale,offset.value)) )
else:
frames.append( rigid.addOffset(offset.name, StructuralAPI.scaleOffset(scale,offset.value)) )
if not param is None:
frames[-1].dofs.showObject = param.showOffset
frames[-1].dofs.showObjectScale = SofaPython.units.length_from_SI(param.showOffsetScale)
else:
frames.append(rigid)
if printLog:
Sofa.msg_info("Compliant.sml","insertJoint "+jointModel.name)
mask = [1]*6
limits=[] # mask for limited dofs
isLimited = True # does the joint have valid limits?
for d in jointModel.dofs:
if isLimited:
if d.min==None or d.max==None:
isLimited = False # as soon as a limit is not defined, the limits cannot work
else:
limits.append(d.min)
limits.append(d.max)
mask[d.index] = 0
joint = StructuralAPI.GenericRigidJoint(jointModel.name, frames[0].node, frames[1].node, mask,
compliance=SofaPython.sml.getValueByTag(param.jointComplianceByTag, jointModel.tags),
isCompliance=SofaPython.sml.getValueByTag(param.jointIsComplianceByTag, jointModel.tags))
if isLimited:
joint.addLimits(limits)
return joint
def rigid_mass_info_offset(rigidModel, density, scale):
'''compute mass info and offset from rigid model'''
if not rigidModel.mass is None and not rigidModel.com is None and not rigidModel.inertia is None:
# sml model has all the info needed
if scale != 1: log_info('scale is not supported in that case')
return model_mass_info(rigidModel), rigidModel.position
elif rigidModel.mesh:
# there is a mesh: compute from it
mi = rigidModel.getRigidMassInfo(density, scale)
offset = StructuralAPI.scaleOffset(scale, rigidModel.position)
return mi, offset
else:
# default
log_warning("using default rigid mass distribution")
mi = SofaPython.mass.RigidMassInfo()
mi.mass = rigidModel.mass if rigidModel.mass is not None else SofaPython.units.mass_from_SI(
1.)
mi.inertia = [scale * scale] * 3
mi.com = scale * rigidModel.position # TODO does this even work?
return mi, None
def insertRigid(parentNode, rigidModel, density, scale=1, param=None):
""" create a StructuralAPI.RigidBody from the rigidModel. The model geometry is scaled with scale.
The rigidMass is computed from:
1) mass, com and inertia if present
2) mesh if possible
3) default to a unit sphere TODO: is it relevant to do so ?
"""
if printLog:
Sofa.msg_info("Compliant.sml","insertRigid "+rigidModel.name)
for mesh in rigidModel.mesh :
if rigidModel.meshAttributes[mesh.id].collision is True:
Sofa.msg_info("Compliant.sml"," collision mesh: "+mesh.name)
rigid = StructuralAPI.RigidBody(parentNode, rigidModel.name)
if not rigidModel.mass is None and not rigidModel.com is None and not rigidModel.inertia is None:
if not 1==scale:
Sofa.msg_info("Compliant.sml","scale is not supported in that case")
# all inertial data is present, let's use it
massinfo = SofaPython.mass.RigidMassInfo()
massinfo.mass = rigidModel.mass # TODO: convert units ?
massinfo.com = rigidModel.com # TODO: convert units ?
if len(rigidModel.inertia)==3 and not rigidModel.inertia_rotation is None:
massinfo.diagonal_inertia = rigidModel.inertia
massinfo.inertia_rotation = rigidModel.inertia_rotation
else:
massinfo.setFromInertia(rigidModel.inertia[0], rigidModel.inertia[1], rigidModel.inertia[2], # Ixx, Ixy, Ixz
rigidModel.inertia[3], rigidModel.inertia[4], # Iyy, Iyz
rigidModel.inertia[5] ) # Izz
rigid.setFromRigidInfo(massinfo, offset=rigidModel.position, inertia_forces = False ) # TODO: handle inertia_forces ?
elif len(rigidModel.mesh)!=0 :
# get inertia from meshes and density
rigid.setFromRigidInfo(rigidModel.getRigidMassInfo(density, scale), offset=StructuralAPI.scaleOffset(scale, rigidModel.position), inertia_forces = False ) # TODO: handle inertia_forces ?
#if not rigidModel.mass is None :
## no density but a mesh let's normalise computed mass with specified mass
#mass= SofaPython.units.mass_from_SI(rigidModel.mass)
#inertia = []
#for inert,m in zip(rigid.mass.inertia, rigid.mass.mass):
#for i in inert:
#inertia.append( i/m[0]*mass)
#rigid.mass.inertia = concat(inertia)
#rigid.mass.mass = mass
else:
# no mesh, get mass/inertia if present, default to a unit sphere
Sofa.msg_warning("Compliant.sml","insertRigid: using default rigidMass")
mass = rigidModel.mass if not rigidModel.mass is None else SofaPython.units.mass_from_SI(1.)
inertia = scale*scale*[1,1,1]
t = scale*rigidModel.position
if not rigidModel.com is None:
t[0] += scale*rigidModel.com[0]
t[1] += scale*rigidModel.com[1]
t[2] += scale*rigidModel.com[2]
rigid.setManually(t, mass, inertia)
if not param is None:
rigid.dofs.showObject = param.showRigid
rigid.dofs.showObjectScale = SofaPython.units.length_from_SI(param.showRigidScale)
# walk around to handle multiple meshes
# @todo: handle them in StructuralAPI ?
rigid.collisions=dict()
rigid.visuals=dict()
for mesh in rigidModel.mesh :
if rigidModel.meshAttributes[mesh.id].collision is True:
rigid.collisions[mesh.id] = rigid.addCollisionMesh(mesh.source,name_suffix='_'+mesh.name, scale3d=[scale]*3)
if rigidModel.meshAttributes[mesh.id].visual is True:
rigid.visuals[mesh.id] = rigid.collisions[mesh.id].addVisualModel()
elif rigidModel.meshAttributes[mesh.id].visual is True:
rigid.visuals[mesh.id] = rigid.addVisualModel(mesh.source,name_suffix='_'+mesh.name, scale3d=[scale]*3)
return rigid
