def onKeyPressed(self, c): ######### Rate angular ######### if c == "+": # + posA = self.rateAngularDeformMO.findData('rest_position').value posA[5][1] += self.angularRate self.rateAngularDeformMO.findData('rest_position').value = posA if c == "-": # - posA = self.rateAngularDeformMO.findData('rest_position').value posA[5][1] -= self.angularRate self.rateAngularDeformMO.findData('rest_position').value = posA print("============> The rate angular :", posA) ######### Reste rigid position ######### if ord(c) == 19: # up posA = self.rigidBaseMO.findData('rest_position').value qOld = Quat() for i in range(0, 4): qOld[i] = posA[0][i + 3] qNew = Quat.createFromEuler([0., self.angularRate, 0.], 'ryxz') qNew.normalize() qNew.rotateFromQuat(qOld) for i in range(0, 4): posA[0][i + 3] = qNew[i] self.rigidBaseMO.findData('rest_position').value = posA if ord(c) == 21: # down posA = self.rigidBaseMO.findData('rest_position').value qOld = Quat() for i in range(0, 4): qOld[i] = posA[0][i + 3] qNew = Quat.createFromEuler([0., -self.angularRate, 0.], 'ryxz') qNew.normalize() qNew.rotateFromQuat(qOld) for i in range(0, 4): posA[0][i + 3] = qNew[i] self.rigidBaseMO.findData('rest_position').value = posA if ord(c) == 18: # left pos = self.rigidBaseMO.findData('rest_position').value pos[0][0] -= self.rate self.rigidBaseMO.findData('rest_position').value = pos # posA = self.rateAngularDeformMO.findData('position').value # for i in range(len(posA)): # posA[i][2] -= self.angularRate # self.rateAngularDeformMO.findData('position').value = posA if ord(c) == 20: # right pos = self.rigidBaseMO.findData('rest_position').value pos[0][0] += self.rate self.rigidBaseMO.findData('rest_position').value = pos
def __addCables(self): length1 = 10. length2 = 2. lengthTrunk = 195. pullPoint = [[0., length1, 0.], [-length1, 0., 0.], [0., -length1, 0.], [length1, 0., 0.]] direction = Vec3(0., length2-length1, lengthTrunk) direction.normalize() nbCables = 4 for i in range(0, nbCables): theta = 1.57*i q = Quat(0., 0., sin(theta/2.), cos(theta/2.)) position = [[0., 0., 0.]]*20 for k in range(0, 20, 2): v = Vec3(direction[0], direction[1]*17.5 * (k/2)+length1, direction[2]*17.5*(k/2)+21) position[k] = v.rotateFromQuat(q) v = Vec3(direction[0], direction[1]*17.5 * (k/2)+length1, direction[2]*17.5*(k/2)+27) position[k+1] = v.rotateFromQuat(q) cableL = self.node.createChild('cableL'+str(i)) cableL.createObject('MechanicalObject', name='dofs', position=pullPoint[i]+[pos.toList() for pos in position]) cableL.createObject('CableConstraint' if not self.inverseMode else 'CableActuator', template='Vec3', name="cable", hasPullPoint=False, indices=range(0, 21), maxPositiveDisp=70, maxDispVariation=1, minForce=0) cableL.createObject('BarycentricMapping', name='mapping', mapForces=False, mapMasses=False) for i in range(0, nbCables): theta = 1.57*i q = Quat(0., 0., sin(theta/2.), cos(theta/2.)) position = [[0., 0., 0.]]*10 for k in range(0, 9, 2): v = Vec3(direction[0], direction[1]*17.5 * (k/2)+length1, direction[2]*17.5*(k/2)+21) position[k] = v.rotateFromQuat(q) v = Vec3(direction[0], direction[1]*17.5 * (k/2)+length1, direction[2]*17.5*(k/2)+27) position[k+1] = v.rotateFromQuat(q) cableS = self.node.createChild('cableS'+str(i)) cableS.createObject('MechanicalObject', name='dofs', position=pullPoint[i]+[pos.toList() for pos in position]) cableS.createObject('CableConstraint' if not self.inverseMode else 'CableActuator', template='Vec3', name="cable", hasPullPoint=False, indices=range(0, 10), maxPositiveDisp=40, maxDispVariation=1, minForce=0) cableS.createObject('BarycentricMapping', name='mapping', mapForces=False, mapMasses=False)
def getTranslated(points, vec, angle): r = [] q = Quat.createFromEuler([(angle[0] * pi) / 180., (angle[1] * pi) / 180., (angle[2] * pi) / 180.], 'ryxz') v = Vec3(vec[0], vec[1], vec[2]) sol = v.rotateFromQuat(q) for v in points: r.append([v[0] + sol[0], v[1] + sol[1], v[2] + sol[2]]) return r
## ## ## ## ## ## ## ## ## Cosserat Needs ## ## ## ## ## ## ## ## ## length1 = 10. length2 = 2. lengthTrunk = 195. nbBeams = 20 nbFrames = 20 # Dans l'ideal >= nbBeams nbCables = 4 maxSlidingForce = 0 minSlidingForce = -10 pullPoint = [[0., length1, 0.], [-length1, 0., 0.], [0., -length1, 0.], [length1, 0.,0.]] direction = Vec3( lengthTrunk, length2-length1, 0.0) direction.normalize() Ori0 = Quat.createFromEuler([-pi/2., 0., -0.04], 'ryxz') Ori1 = Quat.createFromEuler([-pi/2.04, 0.0, 0.0], 'ryxz') Ori2 = Quat.createFromEuler([-pi/2., 0., 0.04], 'ryxz') Ori3 = Quat.createFromEuler([-pi/1.96, 0., 0.0], 'ryxz') baseOrientation = [Ori0,Ori1,Ori2,Ori3] VecCoord = [] # vector of Vec3 VecFrame = [] # vector of Rigid3d VecCurvAbsOutput = [] # vector of double VecCurvAbsInput = [] # vector of double VecLenght = [] # vector of vetor of double; Cosserat beam length VecPosFem = [] position = [[0., 0., 0.]]*nbBeams for k in range(0, nbBeams, 2): position[k] = Vec3(direction[0]*17.5*(k/2)+21, 0.0, 0.0)
def onEndAnimationStep(self, dt): if self.state == "init": return if self.state == "no-comm": return if (self.nodeActuators.activated): W_R_Dof = [0] * 4 W_R_Ref = [0] * 4 Ndir = [[]] * 3 Angles = [0] * 3 for i in range(0, 4): W_R_Dof[i] = self.nodeDofRigid.dofs.position[0][i + 3] W_R_Ref[ i] = self.nodeTripod.ActuatedArm0.ServoArm.dofs.position[ 0][i + 3] [Ndir[0], Angles[0] ] = Quat.product(Quat(W_R_Ref).getInverse(), Quat(W_R_Dof)).getAxisAngle() for i in range(0, 4): W_R_Dof[i] = self.nodeDofRigid.dofs.position[1][i + 3] W_R_Ref[ i] = self.nodeTripod.ActuatedArm1.ServoArm.dofs.position[ 0][i + 3] [Ndir[1], Angles[1] ] = Quat.product(Quat(W_R_Ref).getInverse(), Quat(W_R_Dof)).getAxisAngle() for i in range(0, 4): W_R_Dof[i] = self.nodeDofRigid.dofs.position[2][i + 3] W_R_Ref[ i] = self.nodeTripod.ActuatedArm2.ServoArm.dofs.position[ 0][i + 3] [Ndir[2], Angles[2] ] = Quat.product(Quat(W_R_Ref).getInverse(), Quat(W_R_Dof)).getAxisAngle() # conversion to degree (centred on 150) # envoie des infos aux servoMoteurs AnglesDeg = [0] * 3 for i in range(3): if (Ndir[i][0] > 0): AnglesDeg[i] = Angles[i] * 180 / 3.14159265359 + 90 else: AnglesDeg[i] = -Angles[i] * 180 / 3.14159265359 + 90 if AnglesDeg[i] < 60: AnglesDeg[i] = 60 if AnglesDeg[i] > 180: AnglesDeg[i] = 180 # The controller board of the real robot receives `AnglesDeg` values if (self.serialport): self.serialport.packetOut = [ int(AnglesDeg[0] + 0), int(AnglesDeg[2] - 0), int(AnglesDeg[1]) ]
def applyAngleToServoWheel(self, angle): rigidparent = RigidDof(self.parentframe) rigidtarget = RigidDof(self.target) rigidtarget.copyFrom(rigidparent) self.dmap.initialPoints = [0.0, 0.0, 0.0]+list(Quat.createFromEuler([angle, 0,0]))
def __addCables(self): length1 = 10. length2 = 2. lengthTrunk = 195. pullPoint = [[0., length1, 0.], [-length1, 0., 0.], [0., -length1, 0.], [length1, 0., 0.]] direction = Vec3(lengthTrunk, length2 - length1, 0.0) direction.normalize() Ori0 = Quat.createFromEuler([-pi / 2., 0., -0.04], 'ryxz') Ori1 = Quat.createFromEuler([-pi / 2.04, 0.0, 0.0], 'ryxz') Ori2 = Quat.createFromEuler([-pi / 2., 0., 0.04], 'ryxz') Ori3 = Quat.createFromEuler([-pi / 1.96, 0., 0.0], 'ryxz') baseOrientation = [Ori0, Ori1, Ori2, Ori3] nbCables = 4 for i in range(0, nbCables): position = [[0., 0., 0.]] * 20 for k in range(0, 20, 2): position[k] = Vec3(direction[0] * 17.5 * (k / 2) + 21, 0.0, 0.0) position[k + 1] = Vec3(direction[0] * 17.5 * (k / 2) + 27, 0.0, 0.0) position = [[0., 0, 0.]] + [pos.toList() for pos in position] #################################################### ### Create the Cosserat cable node #################################################### distance = self.compute_distance(position) curv_abs_output = self.createCurvAbsOutput(distance) curv_abs_input = self.createCurvAbsOutput(distance) frames = self.createFramesList(position) # ############### # RigidBases ############### basePose = frames[0] for l in range(0, 3): basePose[l] = pullPoint[i][l] for l in range(0, 4): basePose[l + 3] = baseOrientation[i][l] rigidBaseNode = self.node.createChild('rigidBase' + str(i)) RigidBaseMO = rigidBaseNode.createObject('MechanicalObject', template='Rigid3d', name="RigidBaseMO", position=basePose, showObject='1', showObjectScale='0.6', showIndices='1') rigidBaseNode.createObject('RestShapeSpringsForceField', name='spring', stiffness="500", angularStiffness="500", external_points="0", mstate="@RigidBaseMO", points="0", template="Rigid3d") ############################################# # Rate of angular Deformation (2 sections) ############################################# ratPosition = [] for l in range(0, len(frames) - 1): ratPosition += [0, 0, 0] longeur = distance # beams size rateAngularDeformNode = self.node.createChild('rateAngularDeform' + str(i)) rateAngularDeformMO = rateAngularDeformNode.createObject( 'MechanicalObject', template='Vec3d', name='rateAngularDeformMO', position=ratPosition) BeamHookeLawForce = rateAngularDeformNode.createObject( 'BeamHookeLawForceField', crossSectionShape='circular', length=longeur, radius='0.5', youngModulus='5e6', showIndices='1') ############## # Frames # ############## mappedFrameNode = rigidBaseNode.createChild('MappedFrames') rateAngularDeformNode.addChild(mappedFrameNode) framesMO = mappedFrameNode.createObject('MechanicalObject', template='Rigid3d', name="FramesMO", position=frames, showObject='1', showObjectScale='1', showIndices=1) inputMO = rateAngularDeformMO.getLinkPath() inputMO_rigid = RigidBaseMO.getLinkPath() outputMO = framesMO.getLinkPath() mappedFrameNode.createObject('DiscretCosseratMapping', curv_abs_input=curv_abs_input, curv_abs_output=curv_abs_output, input1=inputMO, input2=inputMO_rigid, output=outputMO, debug='0', printLog=0)
def Rigidify(targetObject, sourceObject, groupIndices, frames=None, name=None, frameOrientation=None): """ Transform a deformable object into a mixed one containing both rigid and deformable parts. :param targetObject: parent node where to attach the final object. :param sourceObject: node containing the deformable object. The object should be following the ElasticMaterialObject template. :param list groupIndices: array of array indices to rigidify. The length of the array should be equal to the number of rigid component. :param list frames: array of frames. The length of the array should be equal to the number of rigid component. The orientation are given in eulerAngles (in degree) by passing three values or using a quaternion by passing four values. [[rx,ry,rz], [qx,qy,qz,w]] User can also specify the position of the frame by passing six values (position and orientation in degree) or seven values (position and quaternion). [[x,y,z,rx,ry,rz], [x,y,z,qx,qy,qz,w]] If the position is not specified, the position of the rigids will be the barycenter of the region to rigidify. :param str name: specify the name of the Rigidified object, is none provided use the name of the SOurceObject. """ # Deprecation Warning if frameOrientation is not None: Sofa.msg_warning( "The parameter frameOrientations of the function Rigidify is now deprecated. Please use frames instead." ) frames = frameOrientation if frames is None: frames = [[0., 0., 0.]] * len(groupIndices) assert len(groupIndices) == len(frames), "size mismatch." if name is None: name = sourceObject.name sourceObject.init() ero = targetObject.createChild(name) allPositions = sourceObject.container.position allIndices = map(lambda x: x[0], sourceObject.container.points) rigids = [] indicesMap = [] def mfilter(si, ai, pts): tmp = [] for i in ai: if i in si: tmp.append(pts[i]) return tmp # get all the points from the source. sourcePoints = map(Vec3, sourceObject.dofs.position) selectedIndices = [] for i in range(len(groupIndices)): selectedPoints = mfilter(groupIndices[i], allIndices, sourcePoints) if len(frames[i]) == 3: orientation = Quat.createFromEuler(frames[i], inDegree=True) poscenter = getBarycenter(selectedPoints) elif len(frames[i]) == 4: orientation = frames[i] poscenter = getBarycenter(selectedPoints) elif len(frames[i]) == 6: orientation = Quat.createFromEuler( [frames[i][3], frames[i][4], frames[i][5]], inDegree=True) poscenter = [frames[i][0], frames[i][1], frames[i][2]] elif len(frames[i]) == 7: orientation = [ frames[i][3], frames[i][4], frames[i][5], frames[i][6] ] poscenter = [frames[i][0], frames[i][1], frames[i][2]] else: Sofa.msg_error("Do not understand the size of a frame.") rigids.append(poscenter + list(orientation)) selectedIndices += map(lambda x: x, groupIndices[i]) indicesMap += [i] * len(groupIndices[i]) otherIndices = filter(lambda x: x not in selectedIndices, allIndices) Kd = {v: None for k, v in enumerate(allIndices)} Kd.update({v: [0, k] for k, v in enumerate(otherIndices)}) Kd.update({v: [1, k] for k, v in enumerate(selectedIndices)}) indexPairs = [v for kv in Kd.values() for v in kv] freeParticules = ero.createChild("DeformableParts") freeParticules.createObject( "MechanicalObject", template="Vec3", name="dofs", position=[allPositions[i] for i in otherIndices]) rigidParts = ero.createChild("RigidParts") rigidParts.createObject("MechanicalObject", template="Rigid3", name="dofs", reserve=len(rigids), position=rigids) rigidifiedParticules = rigidParts.createChild("RigidifiedParticules") rigidifiedParticules.createObject( "MechanicalObject", template="Vec3", name="dofs", position=[allPositions[i] for i in selectedIndices]) rigidifiedParticules.createObject("RigidMapping", name="mapping", globalToLocalCoords='true', rigidIndexPerPoint=indicesMap) sourceObject.removeObject(sourceObject.solver) sourceObject.removeObject(sourceObject.integration) sourceObject.removeObject(sourceObject.LinearSolverConstraintCorrection) # The coupling is made with the sourceObject. If the source object is from an ElasticMaterialObject # We need to get the owning node form the current python object (this is a hack because of the not yet # Finalized design of stlib. coupling = sourceObject if hasattr(sourceObject, "node"): coupling = sourceObject.node coupling.createObject("SubsetMultiMapping", name="mapping", template="Vec3,Vec3", input=freeParticules.dofs.getLinkPath() + " " + rigidifiedParticules.dofs.getLinkPath(), output=sourceObject.dofs.getLinkPath(), indexPairs=indexPairs) rigidifiedParticules.addChild(coupling) freeParticules.addChild(coupling) return ero
def Gripper(parentNode=None, **kwargs): selfNode = parentNode.createChild("Gripper") translation = kwargs.pop('translation') rotation = kwargs.pop('rotation') color = kwargs.pop('color') #WARNING: Quat createFromEuler will modify the input Euler list!!! rotation_copy = [r for r in rotation] rotBase = Quat.createFromEuler(rotation_copy, inDegree=True) transBase = Vec3(translation) scale = 0.05 dimBase = Vec3([1. * scale, .2 * scale, 2 * scale]) dimLink = Vec3([1. * scale, 2. * scale, .4 * scale]) offsetLink1 = Vec3([0.0, 2.2 * scale, -1.6 * scale]) offsetLink2 = Vec3([0.0, 2.2 * scale, 1.6 * scale]) translation_base = transBase.rotateFromQuat(rotBase).toList() translation_link1 = vadd(transBase.toList(), offsetLink1.rotateFromQuat(rotBase).toList()) translation_link2 = vadd(transBase.toList(), offsetLink2.rotateFromQuat(rotBase).toList()) base = Cube(selfNode, name='base', uniformScale=dimBase.toList(), translation=translation_base, rotation=rotation, color=color) #WARNING: rotateFromQuat is also an in-place operation, why do they do this?! attachOffset_translation1 = [0.0, .2 * scale, -1.6 * scale] attachOffset_translation2 = [0.0, .2 * scale, 1.6 * scale] baseAttaches = base.createChild('baseAttaches') baseAttaches.createObject( 'MechanicalObject', name='attaches', template='Rigid3d', # translation2=attachOffset_translation1+attachOffset_translation2, rotation2=rotation+rotation position=ListToString(attachOffset_translation1) + ' 0 0 0 1 ' + ListToString(attachOffset_translation2) + ' 0 0 0 1') baseAttaches.createObject('RigidRigidMapping') base.getChild('collision').getObject("TTriangleModel").findData( 'group').value = '1 2' base.getChild('collision').getObject("TLineModel").findData( 'group').value = '1 2' base.getChild('collision').getObject("TPointModel").findData( 'group').value = '1 2' baseInput = base.createChild('baseInput') baseInput.createObject('MechanicalObject', name='input', template='Rigid3d', translation=translation_base, rotation=rotation) # baseInput.createObject('JointSpringForceField', object1='@./', object2='@../', # spring='BEGIN_SPRING 0 0 FREE_AXIS 0 0 0 0 0 0 KS_T 20000 30000 KS_R 150000 200000 KD 1 END_SPRING', # name='base_input', template='Rigid3d') baseInput.createObject('AttachConstraint', indices1='0', name='attachConstraint', indices2='0', velocityFactor='1.0', responseFactor='1.0', object1='@./', object2='@../', positionFactor='1.0', constraintFactor='1', twoWay='false') link1 = Cube(selfNode, name='link1', uniformScale=dimLink.toList(), translation=translation_link1, rotation=rotation, color=color) attachOffset_translation = [0.0, -2. * scale, 0] link1Attaches = link1.createChild('link1Attaches') link1Attaches.createObject( 'MechanicalObject', name='attaches', template='Rigid3d', position=ListToString(attachOffset_translation) + ' 0 0 0 1 ') link1Attaches.createObject('RigidRigidMapping') link1.getChild('collision').getObject("TTriangleModel").findData( 'group').value = '1' link1.getChild('collision').getObject("TLineModel").findData( 'group').value = '1' link1.getChild('collision').getObject("TPointModel").findData( 'group').value = '1' link2 = Cube(selfNode, name='link2', uniformScale=dimLink.toList(), translation=translation_link2, rotation=rotation, color=color) attachOffset_translation = [0.0, -2. * scale, 0] link2Attaches = link2.createChild('link2Attaches') link2Attaches.createObject( 'MechanicalObject', name='attaches', template='Rigid3d', position=ListToString(attachOffset_translation) + ' 0 0 0 1 ') link2Attaches.createObject('RigidRigidMapping') link2.getChild('collision').getObject("TTriangleModel").findData( 'group').value = '2' link2.getChild('collision').getObject("TLineModel").findData( 'group').value = '2' link2.getChild('collision').getObject("TPointModel").findData( 'group').value = '2' link1.createObject( 'JointSpringForceField', object1="@../base/baseAttaches", object2="@./link1Attaches", spring= 'BEGIN_SPRING 0 0 FREE_AXIS 1 1 1 0 0 0 KS_T 20000 30000 KS_R 150000 200000 KD 100 REST_T 50 50 0.0 END_SPRING', name='joint_link1', template='Rigid3d') link2.createObject( 'JointSpringForceField', object1="@../base/baseAttaches", object2="@./link2Attaches", spring= 'BEGIN_SPRING 1 0 FREE_AXIS 1 1 1 0 0 0 KS_T 20000 30000 KS_R 150000 200000 KD 100 REST_T 0.0 50 50 END_SPRING', name='joint_link2', template='Rigid3d') # link1.createObject('AttachConstraint', indices1='0', name='attachConstraint', indices2='0', # velocityFactor='1.0', responseFactor='1.0', object1='@./link1Attaches', object2='@../base/baseAttaches', positionFactor='1.0', constraintFactor='1', twoWay='true') # link2.createObject('AttachConstraint', indices1='0', name='attachConstraint', indices2='1', # velocityFactor='1.0', responseFactor='1.0', object1='@./link2Attaches', object2='@../base/baseAttaches', positionFactor='1.0', constraintFactor='1', twoWay='true') GripperController(selfNode, base, link1, link2) return selfNode