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