def initializeJoints(self): ''' Change control mode of joints to allow for torque control. ''' self.sphericalJoints = [] self.revoluteJoints = [] for joint in range(self.numJoints): jointType = p.getJointInfo(self.humanoidAgent, joint)[2] if jointType == 2: p.setJointMotorControlMultiDof( bodyUniqueId=self.humanoidAgent, jointIndex=joint, controlMode=p.POSITION_CONTROL, targetVelocity=[0,0,0], force=[1,1,1] ) self.sphericalJoints.append(joint) elif jointType == 0: p.setJointMotorControl2( bodyUniqueId=self.humanoidAgent, jointIndex=joint, controlMode=p.VELOCITY_CONTROL, targetVelocity=0, force=1 ) self.revoluteJoints.append(joint)
def loosenModel(self): nJoints = p.getNumJoints(self.cliffordID, physicsClientId=self.physicsClientId) tireIndices = [ self.linkNameToID[name] for name in ['frtire', 'fltire', 'brtire', 'bltire'] ] for i in range(nJoints): if len(p.getJointStateMultiDof(bodyUniqueId=self.cliffordID,jointIndex=i,physicsClientId=self.physicsClientId)[0]) == 4 \ and not self.params["fixedSuspension"]: p.setJointMotorControlMultiDof( bodyUniqueId=self.cliffordID, jointIndex=i, controlMode=p.POSITION_CONTROL, targetPosition=[0, 0, 0, 1], positionGain=0, velocityGain=0, force=[0, 0, 0], physicsClientId=self.physicsClientId) dynamicsData = p.getDynamicsInfo( self.cliffordID, i, physicsClientId=self.physicsClientId) massScale = self.params["massScale"] if i in tireIndices: massScale = massScale * self.params['tireMassScale'] newMass = dynamicsData[0] * massScale newInertia = [ dynamicsData[2][j] * massScale for j in range(len(dynamicsData[2])) ] p.changeDynamics(self.cliffordID, i, mass=newMass, localInertiaDiagonal=newInertia, linearDamping=0.2, angularDamping=0.2, restitution=0, physicsClientId=self.physicsClientId) springJointNames = [ 'brslower2upper', 'blslower2upper', 'frslower2upper', 'flslower2upper' ] springConstant = 0 springDamping = 0 maxSpringForce = 0 for name in springJointNames: #JointInfo = p.getJointInfo(self.cliffordID,self.jointNameToID[name],physicsClientId=self.physicsClientId) #p.setJointMotorControlArray(bodyUniqueId=self.cliffordID, # jointIndices=[self.jointNameToID[name]], # controlMode=p.POSITION_CONTROL, # targetPositions=[0], # positionGains=[springConstant], # velocityGains=[springDamping], # forces=[maxSpringForce],physicsClientId=self.physicsClientId) if not self.params["fixedSuspension"]: p.changeDynamics(self.cliffordID, self.jointNameToID[name], jointLowerLimit=self.params["susLowerLimit"], jointUpperLimit=self.params["susUpperLimit"], physicsClientId=self.physicsClientId)
def disable_body_motors(body_id): for j in range(p.getNumJoints(body_id)): ji = p.getJointInfo(body_id, j) targetPosition = [0] jointType = ji[2] if (jointType == p.JOINT_SPHERICAL): targetPosition = [0, 0, 0, 1] p.setJointMotorControlMultiDof(body_id, j, p.POSITION_CONTROL, targetPosition, targetVelocity=[0, 0, 0], positionGain=0, velocityGain=1, force=[0, 0, 0]) if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE): p.setJointMotorControl2(body_id, j, p.VELOCITY_CONTROL, targetVelocity=0, force=0)
def load_urdf(self): """Load the URDF of the blockstacker into the environment The blockstacker URDF comes with its own dimensions and textures, collidables. """ self.robot = p.loadURDF(Utilities.gen_urdf_path("blockstacker/urdf/blockstacker.urdf"), [0, 0, 0.05], [0, 0, 0.9999383, 0.0111104], useFixedBase=False) p.setJointMotorControlMultiDof(self.robot, self.caster_link, p.POSITION_CONTROL, [0, 0, 0], targetVelocity=[100000, 100000, 100000], positionGain=0, velocityGain=1, force=[0, 0, 0]) p.setJointMotorControlArray(self.robot, self.flywheel_links, p.VELOCITY_CONTROL, targetVelocities=[-2, 2], forces=[1, 1])
def loosenModel(self): nJoints = p.getNumJoints(self.cliffordID, physicsClientId=self.physicsClientId) for i in range(nJoints): if len( p.getJointStateMultiDof( bodyUniqueId=self.cliffordID, jointIndex=i, physicsClientId=self.physicsClientId)[0]) == 4: p.setJointMotorControlMultiDof( bodyUniqueId=self.cliffordID, jointIndex=i, controlMode=p.POSITION_CONTROL, targetPosition=[0, 0, 0, 1], positionGain=0, velocityGain=0, force=[0, 0, 0], physicsClientId=self.physicsClientId) springJointNames = [ 'brslower2upper', 'blslower2upper', 'frslower2upper', 'flslower2upper' ] springConstant = 0 springDamping = 0 maxSpringForce = 0 for name in springJointNames: JointInfo = p.getJointInfo(self.cliffordID, self.jointNameToID[name], physicsClientId=self.physicsClientId) p.setJointMotorControlArray( bodyUniqueId=self.cliffordID, jointIndices=[self.jointNameToID[name]], controlMode=p.POSITION_CONTROL, targetPositions=[0], positionGains=[springConstant], velocityGains=[springDamping], forces=[maxSpringForce], physicsClientId=self.physicsClientId)
leftShoulderRotStart = [frameData[40],frameData[41],frameData[42],frameData[39]] leftShoulderRotEnd = [frameDataNext[40],frameDataNext[41],frameDataNext[42],frameDataNext[39]] leftShoulderRot = p.getQuaternionSlerp(leftShoulderRotStart,leftShoulderRotEnd,frameFraction) leftElbowRotStart = [frameData[43]] leftElbowRotEnd = [frameDataNext[43]] leftElbowRot = [leftElbowRotStart[0]+frameFraction*(leftElbowRotEnd[0]-leftElbowRotStart[0])] #print("chestRot=",chestRot) p.setGravity(0,0,0) kp=1 if (useMotionCapture): p.setJointMotorControlMultiDof(humanoid,chest,p.POSITION_CONTROL, targetPosition=chestRot,positionGain=kp, force=maxForce) p.setJointMotorControlMultiDof(humanoid,neck,p.POSITION_CONTROL,targetPosition=neckRot,positionGain=kp, force=maxForce) p.setJointMotorControlMultiDof(humanoid,rightHip,p.POSITION_CONTROL,targetPosition=rightHipRot,positionGain=kp, force=maxForce) p.setJointMotorControlMultiDof(humanoid,rightKnee,p.POSITION_CONTROL,targetPosition=rightKneeRot,positionGain=kp, force=maxForce) p.setJointMotorControlMultiDof(humanoid,rightAnkle,p.POSITION_CONTROL,targetPosition=rightAnkleRot,positionGain=kp, force=maxForce) p.setJointMotorControlMultiDof(humanoid,rightShoulder,p.POSITION_CONTROL,targetPosition=rightShoulderRot,positionGain=kp, force=maxForce) p.setJointMotorControlMultiDof(humanoid,rightElbow, p.POSITION_CONTROL,targetPosition=rightElbowRot,positionGain=kp, force=maxForce) p.setJointMotorControlMultiDof(humanoid,leftHip, p.POSITION_CONTROL,targetPosition=leftHipRot,positionGain=kp, force=maxForce) p.setJointMotorControlMultiDof(humanoid,leftKnee, p.POSITION_CONTROL,targetPosition=leftKneeRot,positionGain=kp, force=maxForce) p.setJointMotorControlMultiDof(humanoid,leftAnkle, p.POSITION_CONTROL,targetPosition=leftAnkleRot,positionGain=kp, force=maxForce) p.setJointMotorControlMultiDof(humanoid,leftShoulder, p.POSITION_CONTROL,targetPosition=leftShoulderRot,positionGain=kp, force=maxForce) p.setJointMotorControlMultiDof(humanoid,leftElbow, p.POSITION_CONTROL,targetPosition=leftElbowRot,positionGain=kp, force=maxForce) if (useMotionCaptureReset): p.resetJointStateMultiDof(humanoid,chest,chestRot) p.resetJointStateMultiDof(humanoid,neck,neckRot) p.resetJointStateMultiDof(humanoid,rightHip,rightHipRot)
start_orn, useFixedBase=True, flags=humanoid_flag) for i in range(-1, NLINK): p.changeVisualShape(humanoid_id, i, rgbaColor=[1, 1, 1, 0.7]) nframe = sample_data['nframe'] frame_id = p.addUserDebugParameter("frame", 0, nframe, 0) njoint = p.getNumJoints(humanoid_id) for i in range(njoint): p.setJointMotorControlMultiDof(humanoid_id, i, p.POSITION_CONTROL, [0, 0, 0, 1], targetVelocity=[0, 0, 0], positionGain=0, velocityGain=1, force=[0, 0, 0]) joint_list = ['base', 'Rhip', 'Rknee', 'Rankle', 'Lhip', 'Lknee', 'Lankle'] spherical_ind = [1, 3, 4, 6] revolute_ind = [2, 5] target_orn = [[] for _ in range(NLINK)] while (p.isConnected()): target_frame = p.readUserDebugParameter(frame_id) cur_frame = int(np.minimum(target_frame, nframe - 1)) next_frame = int(np.minimum(cur_frame + 1, nframe - 1)) frac = target_frame - cur_frame
controlMode=p.VELOCITY_CONTROL, force=0) p.setJointMotorControl2(linkage, jointInfoList['right_eye_yolk_joint'], controlMode=p.VELOCITY_CONTROL, force=0) p.setJointMotorControl2(linkage, jointInfoList['right_eye_joint'], controlMode=p.VELOCITY_CONTROL, force=0) # Spherical joint control p.setJointMotorControlMultiDof(linkage, jointInfoList['dogbone_joint_far_left'], controlMode=p.POSITION_CONTROL, targetPosition=[0], targetVelocity=[0, 0, 0], positionGain=0, velocityGain=1, force=[0, 0, 0]) p.setJointMotorControlMultiDof(linkage, jointInfoList['dogbone_joint_mid_left'], controlMode=p.POSITION_CONTROL, targetPosition=[0], targetVelocity=[0, 0, 0], positionGain=0, velocityGain=1, force=[0, 0, 0]) p.setJointMotorControlMultiDof(linkage, jointInfoList['dogbone_joint_mid_right'], controlMode=p.POSITION_CONTROL, targetPosition=[0],
def set_joint_motor_control_multi_dof(self, *args, **kwargs): return pb.setJointMotorControlMultiDof(*args, **kwargs, physicsClientId=self.bullet_cid)
targetVelocity=targetVel) p.resetJointStateMultiDof(humanoid2, j, targetValue=targetPosition, targetVelocity=targetVel) for j in range(p.getNumJoints(humanoid)): ji = p.getJointInfo(humanoid, j) targetPosition = [0] jointType = ji[2] if (jointType == p.JOINT_SPHERICAL): targetPosition = [0, 0, 0, 1] p.setJointMotorControlMultiDof(humanoid, j, p.POSITION_CONTROL, targetPosition, targetVelocity=[0, 0, 0], positionGain=0, velocityGain=1, force=[0, 0, 0]) p.setJointMotorControlMultiDof(humanoid3, j, p.POSITION_CONTROL, targetPosition, targetVelocity=[0, 0, 0], positionGain=0, velocityGain=1, force=[31, 31, 31]) p.setJointMotorControlMultiDof(humanoid4, j, p.POSITION_CONTROL, targetPosition,
index0+=1 print("revolute:", targetPosition) print("revolute velocity:", targetVel) p.resetJointStateMultiDof(humanoid,j,targetValue=targetPosition,targetVelocity=targetVel) p.resetJointStateMultiDof(humanoid2,j,targetValue=targetPosition,targetVelocity=targetVel) for j in range (p.getNumJoints(humanoid)): ji = p.getJointInfo(humanoid,j) targetPosition=[0] jointType = ji[2] if (jointType == p.JOINT_SPHERICAL): targetPosition=[0,0,0,1] p.setJointMotorControlMultiDof(humanoid,j,p.POSITION_CONTROL,targetPosition, targetVelocity=[0,0,0], positionGain=0,velocityGain=1,force=[0,0,0]) p.setJointMotorControlMultiDof(humanoid3,j,p.POSITION_CONTROL,targetPosition, targetVelocity=[0,0,0], positionGain=0,velocityGain=1,force=[31,31,31]) p.setJointMotorControlMultiDof(humanoid4,j,p.POSITION_CONTROL,targetPosition, targetVelocity=[0,0,0], positionGain=0,velocityGain=1,force=[1,1,1]) if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE): p.setJointMotorControl2(humanoid,j,p.VELOCITY_CONTROL,targetVelocity=0, force=0) p.setJointMotorControl2(humanoid3,j,p.VELOCITY_CONTROL,targetVelocity=0, force=31) p.setJointMotorControl2(humanoid4,j,p.VELOCITY_CONTROL,targetVelocity=0, force=10) #print(ji) print("joint[",j,"].type=",jointTypes[ji[2]]) print("joint[",j,"].name=",ji[1]) jointIds=[]
info = p.getJointInfo(robot, num) if info[16] != -1: p.setCollisionFilterPair(robot, robot, info[0], info[16], 0) for j in range(p.getNumJoints(robot)): ji = p.getJointInfo(robot, j) print(ji[1], "=", j, " # type=", jointTypes[ji[2]]) targetPosition = [0] jointType = ji[2] if jointType == p.JOINT_SPHERICAL: targetPosition = [0, 0, 0, 1] p.setJointMotorControlMultiDof(robot, j, p.POSITION_CONTROL, targetPosition, targetVelocity=[0, 0, 0], positionGain=0, velocityGain=1, force=[1, 1, 1]) if jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE: p.setJointMotorControl2(robot, j, p.VELOCITY_CONTROL, targetVelocity=0, force=10) # print("joint[", j, "].name=", ji[1], "joint[", j, "].type=", jointTypes[ji[2]]) RightUpLeg = 0 # type= JOINT_SPHERICAL RightLeg = 1 # type= JOINT_SPHERICAL RightFoot = 2 # type= JOINT_SPHERICAL
"../data/human_adult_scan.urdf", #useFixedBase = 1, flags=p.URDF_MAINTAIN_LINK_ORDER | p.URDF_USE_SELF_COLLISION) #| means and here #disable motors for j in range(p.getNumJoints(human_adult_ID)): ji = p.getJointInfo(human_adult_ID, j) targetPosition = [0] jointType = ji[2] if (jointType == p.JOINT_SPHERICAL): targetPosition = [0, 0, 0, 1] p.setJointMotorControlMultiDof(human_adult_ID, j, p.POSITION_CONTROL, targetPosition, targetVelocity=[0, 0, 0], positionGain=0, velocityGain=1, force=[0, 0, 0]) if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE): p.setJointMotorControl2(human_adult_ID, j, p.VELOCITY_CONTROL, targetVelocity=0, force=0) p.resetBasePositionAndOrientation( human_adult_ID, [0, 0, 1.3], p.getQuaternionFromEuler([math.pi / 2, 0, 0])) #for j in range (p.getNumJoints(human_adult_ID)): # info = p.getDynamicsInfo(human_adult_ID, j)
linkInertialFramePositions=linkInertialFramePositions, linkInertialFrameOrientations=linkInertialFrameOrns, linkParentIndices=indices, linkJointTypes=jointTypes, linkJointAxis=axis) ############################################################################### # Next we define the frictional force between the joints of wrecking ball. friction_vec = [joint_friction] * 3 # same all axis control_mode = p.POSITION_CONTROL # set pos control mode for j in range(p.getNumJoints(rope)): p.setJointMotorControlMultiDof(rope, j, control_mode, targetPosition=[0, 0, 0, 1], targetVelocity=[0, 0, 0], positionGain=0, velocityGain=1, force=friction_vec) ############################################################################### # We add the following constraint to keep the cubical hinge fixed. root_robe_c = p.createConstraint(rope, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0], [0, 0, 2]) box_actor = actor.box(centers=np.array([[0, 0, 0]]), directions=np.array([[0, 0, 0]]), scales=(0.02, 0.02, 0.02), colors=np.array([[1, 0, 0]]))
kdPole = p.readUserDebugParameter(kdPoleId) maxForcePole = p.readUserDebugParameter(maxForcePoleId) basePos, baseOrn = p.getBasePositionAndOrientation(pole) baseDof = 7 taus = exPD.computePD(pole, [0, 1], [ basePos[0], basePos[1], basePos[2], baseOrn[0], baseOrn[1], baseOrn[2], baseOrn[3], desiredPosCart, desiredPosPole ], [0, 0, 0, 0, 0, 0, 0, desiredVelCart, desiredVelPole], [0, 0, 0, 0, 0, 0, 0, kpCart, kpPole], [0, 0, 0, 0, 0, 0, 0, kdCart, kdPole], [0, 0, 0, 0, 0, 0, 0, maxForceCart, maxForcePole], timeStep) for j in [0, 1]: p.setJointMotorControlMultiDof(pole, j, controlMode=p.TORQUE_CONTROL, force=[taus[j + baseDof]]) #p.setJointMotorControlArray(pole, [0,1], controlMode=p.TORQUE_CONTROL, forces=taus) if (pd >= 0): link = 0 p.setJointMotorControl2(bodyUniqueId=pole2, jointIndex=link, controlMode=p.PD_CONTROL, targetPosition=desiredPosCart, targetVelocity=desiredVelCart, force=maxForceCart, positionGain=kpCart, velocityGain=kdCart) link = 1 p.setJointMotorControl2(bodyUniqueId=pole2,
basePos, baseOrn = p.getBasePositionAndOrientation(pole) baseDof = 7 taus = exPD.computePD(pole, [0, 1], [ basePos[0], basePos[1], basePos[2], baseOrn[0], baseOrn[1], baseOrn[2], baseOrn[3], desiredPosCart, desiredPosPole ], [0, 0, 0, 0, 0, 0, 0, desiredVelCart, desiredVelPole], [0, 0, 0, 0, 0, 0, 0, kpCart, kpPole], [0, 0, 0, 0, 0, 0, 0, kdCart, kdPole], [0, 0, 0, 0, 0, 0, 0, maxForceCart, maxForcePole], timeStep) for j in [0, 1]: p.setJointMotorControlMultiDof(pole, j, controlMode=p.TORQUE_CONTROL, force=[taus[j + baseDof]]) #p.setJointMotorControlArray(pole, [0,1], controlMode=p.TORQUE_CONTROL, forces=taus) if (pd >= 0): link = 0 p.setJointMotorControl2(bodyUniqueId=pole2, jointIndex=link, controlMode=p.PD_CONTROL, targetPosition=desiredPosCart, targetVelocity=desiredVelCart, force=maxForceCart, positionGain=kpCart, velocityGain=kdCart) link = 1 p.setJointMotorControl2(bodyUniqueId=pole2,
gravzId = p.addUserDebugParameter("grav_y",-20,20,-10) index= 0 spherical_index = -1 for j in range(p.getNumJoints(humanoid)): if index<7: ji = p.getJointInfo(humanoid, j) jointType = ji[2] if (jointType == p.JOINT_SPHERICAL): index+=4 p.resetJointStateMultiDof(humanoid, j, targetValue=[0,0,0,1], targetVelocity=[0,0,0]) #p.changeDynamics(humanoid,j,angularDamping=0, linearDamping=0) spherical_index = j p.setJointMotorControlMultiDof(humanoid, j, controlMode=p.POSITION_CONTROL, targetPosition=[0,0,0,1], positionGain=0.2, targetVelocity=[0,0,0], velocityGain=0, force=[0,0,0]) if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE): index+=1 p.resetJointStateMultiDof(humanoid, j, targetValue=[0], targetVelocity=[0]) p.setJointMotorControlMultiDof(humanoid, j, controlMode=p.POSITION_CONTROL, targetPosition=[0], targetVelocity=[0], force=[0]) p.loadURDF("plane.urdf") p.setRealTimeSimulation(1) while p.isConnected(): gravX = p.readUserDebugParameter(gravxId) gravY = p.readUserDebugParameter(gravyId) gravZ = p.readUserDebugParameter(gravzId)