def applyMMMRotationToURDFJoint(urdf_body_id, joint_index, rx, ry, rz, inverse=False): quat_0 = p.getQuaternionFromEuler([rx,rz,-ry]) quat_tf = p.getQuaternionFromEuler([math.pi/2,0,0]) translation, quat_tf_inv = p.invertTransform([0,0,0], quat_tf) quat_tf_urdf = p.getQuaternionFromEuler([-math.pi/2,0,-math.pi]) translation, quat_tf_urdf_inv = p.invertTransform([0,0,0], quat_tf_urdf) translation, quat_tmp = p.multiplyTransforms([0,0,0], quat_tf_urdf, [0,0,0], quat_tf) translation, quat_tmp = p.multiplyTransforms([0,0,0], quat_tmp, [0,0,0], quat_0) translation, quat_tmp = p.multiplyTransforms([0,0,0], quat_tmp, [0,0,0], quat_tf_inv) translation, quat_final = p.multiplyTransforms([0,0,0], quat_tmp, [0,0,0], quat_tf_urdf_inv) if inverse: translation, quat_final = p.invertTransform([0,0,0], quat_final) p.resetJointStateMultiDof(urdf_body_id, joint_index, quat_final)
def initializeStaticTarget(self): ''' Convert the motion file into a format that matches the observations collected from the agent in order to compare and compute rewards. ''' JointFrameMapIndices = [ 0, #root [9, 10, 11, 8], #chest [13, 14, 15, 12], #neck [17, 18, 19, 16], #rHip 20, #rKnee [22, 23, 24, 21], #rAnkle [26, 27, 28, 25], #rShoulder 29, #rElbow 0, #rWrist [31, 32, 33, 30], #lHip 34, #lKnee [36, 37, 38, 35], #lAnkle [40, 41, 42, 39], #lShoulder 43, #lElbow 0, #lWrist ] processedTargetMotion = [] targetFrames = self.targetMotion['Frames'] for frameIndex in range(len(targetFrames)-1): # Calculate First Frame targetPos_orig = [targetFrames[frameIndex][i] for i in [1, 2, 3]] targetOrn_orig = targetFrames[frameIndex][5:8] + [targetFrames[frameIndex][4]] # transform the position and orientation to have z-axis upward y2zPos = [0, 0, 0.0] y2zOrn = p.getQuaternionFromEuler([1.57, 0, 0]) basePos, baseOrn = p.multiplyTransforms(y2zPos, y2zOrn, targetPos_orig, targetOrn_orig) # set the agent's root position and orientation p.resetBasePositionAndOrientation( self.targetDummyID, posObj=basePos, ornObj=baseOrn ) # Set joint positions for joint in self.targetDummy.revoluteJoints: p.resetJointState( self.targetDummyID, jointIndex=joint, targetValue=targetFrames[frameIndex][JointFrameMapIndices[joint]] ) for joint in self.targetDummy.sphericalJoints: p.resetJointStateMultiDof( self.targetDummyID, jointIndex=joint, targetValue=[targetFrames[frameIndex][i] for i in JointFrameMapIndices[joint]] ) currentFrame = self.targetDummy.collectObservations() processedTargetMotion.append(currentFrame) return processedTargetMotion
def applyMMMRotationToURDFJoint(urdf_body_id, joint_index, rx, ry, rz, inverse=False): q = generateQuaternionFromMMMRxRyRz(rx, ry, rz) quat_tf_urdf = p.getQuaternionFromEuler([-math.pi/2, math.pi, 0]) translation, quat_tf_urdf_inv = p.invertTransform([0,0,0], quat_tf_urdf) t, q = p.multiplyTransforms([0,0,0], quat_tf_urdf, [0,0,0], q) t, q = p.multiplyTransforms([0,0,0], q, [0,0,0], quat_tf_urdf_inv) if inverse: t, q = p.invertTransform([0,0,0], q) p.resetJointStateMultiDof(urdf_body_id, joint_index, q)
def set_body_velocities(walker): walker.advance() pos_2, ori_2 = p.getBasePositionAndOrientation(walker.body_id) joint_position_list_2 = get_joint_positions_as_list(walker) walker.regress() walker.regress() pos_1, ori_1 = p.getBasePositionAndOrientation(walker.body_id) joint_position_list_1 = get_joint_positions_as_list(walker) walker.advance() # compute base velocities via central differences assuming a timestep of 0.01 [s] baseLinearVelocity = (np.array(pos_2) - np.array(pos_1)) / 0.02 quaternion_derivative = (np.array(ori_2) - np.array(ori_1)) / 0.02 dummy, ori_now = p.getBasePositionAndOrientation(walker.body_id) dummy, inverse_ori_now = p.invertTransform([0, 0, 0], ori_now) omega4vec = quaternion_multiplication(2 * quaternion_derivative, inverse_ori_now) baseAngularVelocity = [omega4vec[0], omega4vec[1], omega4vec[2]] p.resetBaseVelocity(walker.body_id, linearVelocity=baseLinearVelocity, angularVelocity=baseAngularVelocity) # compute joint velocities via central differences assuming a timestep of 0.01 [s] joint_position_list_now = get_joint_positions_as_list(walker) for i in range(len(joint_position_list_now)): joint_info = p.getJointInfo(walker.body_id, i) if joint_info[2] == p.JOINT_SPHERICAL: omega = quaternion_and_its_derivative_to_angular_velocity( joint_position_list_now[i], (np.array(joint_position_list_2[i]) - np.array(joint_position_list_1[i])) / 0.02) p.resetJointStateMultiDof(walker.body_id, i, targetValue=joint_position_list_now[i], targetVelocity=omega) elif joint_info[2] == p.JOINT_REVOLUTE: omega_scalar = (joint_position_list_2[i] - joint_position_list_1[i]) / 0.02 p.resetJointState(walker.body_id, i, targetValue=joint_position_list_now[i], targetVelocity=omega_scalar)
def initializeStaticTarget(self): ''' Convert the motion file into a format that matches the observations collected from the agent in order to compare and compute rewards. ''' JointFrameMapIndices = [ 0, #root [9, 10, 11, 8], #chest [13, 14, 15, 12], #neck [17, 18, 19, 16], #rHip 20, #rKnee [22, 23, 24, 21], #rAnkle [26, 27, 28, 25], #rShoulder 29, #rElbow 0, #rWrist [31, 32, 33, 30], #lHip 34, #lKnee [36, 37, 38, 35], #lAnkle [40, 41, 42, 39], #lShoulder 43, #lElbow 0, #lWrist ] processedTargetMotion = [] targetFrames = self.targetMotion['Frames'] for frameIndex in range(len(targetFrames)-1): # Set joint positions for joint in self.targetDummy.revoluteJoints: p.resetJointState( self.targetDummyID, jointIndex=joint, targetValue=targetFrames[frameIndex][JointFrameMapIndices[joint]] ) for joint in self.targetDummy.sphericalJoints: p.resetJointStateMultiDof( self.targetDummyID, jointIndex=joint, targetValue=[targetFrames[frameIndex][i] for i in JointFrameMapIndices[3]] ) currentFrame = self.targetDummy.collectObservations() processedTargetMotion.append(currentFrame) return processedTargetMotion
def set_pose(robot, pose): num_joints = pybullet.getNumJoints(robot) root_pos = get_root_pos(pose) root_rot = get_root_rot(pose) pybullet.resetBasePositionAndOrientation(robot, root_pos, root_rot) for j in range(num_joints): j_info = pybullet.getJointInfo(robot, j) j_state = pybullet.getJointStateMultiDof(robot, j) j_pose_idx = j_info[3] j_pose_size = len(j_state[0]) j_vel_size = len(j_state[1]) if (j_pose_size > 0): j_pose = pose[j_pose_idx:(j_pose_idx + j_pose_size)] j_vel = np.zeros(j_vel_size) pybullet.resetJointStateMultiDof(robot, j, j_pose, j_vel) return
def HomePos(self): p.setRealTimeSimulation(0) # Reset dumb joint positions for dumb_idx in range(self.dumbJointNum): p.resetJointState(self.linkage, self.dumbJointIds[dumb_idx], targetValue=self.dumbJointHome) # Reset actuated joint positions for act_idx in range(self.actJointNum): p.resetJointState(self.linkage, self.actJointIds[act_idx], targetValue=self.actJointHome) # Update position list self.actJointPos = [self.actJointHome] * self.actJointNum # Reset spherical joints for spher_idx in range(self.spherJointNum): p.resetJointStateMultiDof( self.linkage, self.spherJointIds[spher_idx], targetValue=self.spherJointHome[spher_idx])
def reset_joint_state_multi_dof(self, *args, **kwargs): return pb.resetJointStateMultiDof(*args, **kwargs, physicsClientId=self.bullet_cid)
ji = p.getJointInfo(humanoid, j) targetPosition = [0] jointType = ji[2] if (jointType == p.JOINT_SPHERICAL): targetPosition = [ startPose[index0 + 1], startPose[index0 + 2], startPose[index0 + 3], startPose[index0 + 0] ] targetVel = [ startVel[index0 + 0], startVel[index0 + 1], startVel[index0 + 2] ] index0 += 4 print("spherical position: ", targetPosition) print("spherical velocity: ", targetVel) p.resetJointStateMultiDof(humanoid, j, targetValue=targetPosition, targetVelocity=targetVel) p.resetJointStateMultiDof(humanoid2, j, targetValue=targetPosition, targetVelocity=targetVel) if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE): targetPosition = [startPose[index0]] targetVel = [startVel[index0]] index0 += 1 print("revolute:", targetPosition) print("revolute velocity:", targetVel) p.resetJointStateMultiDof(humanoid, j, targetValue=targetPosition, targetVelocity=targetVel)
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) p.resetJointStateMultiDof(humanoid,rightKnee,rightKneeRot) p.resetJointStateMultiDof(humanoid,rightAnkle,rightAnkleRot) p.resetJointStateMultiDof(humanoid,rightShoulder,rightShoulderRot) p.resetJointStateMultiDof(humanoid,rightElbow, rightElbowRot) p.resetJointStateMultiDof(humanoid,leftHip, leftHipRot) p.resetJointStateMultiDof(humanoid,leftKnee, leftKneeRot) p.resetJointStateMultiDof(humanoid,leftAnkle, leftAnkleRot) p.resetJointStateMultiDof(humanoid,leftShoulder, leftShoulderRot) p.resetJointStateMultiDof(humanoid,leftElbow, leftElbowRot) p.stepSimulation() #time.sleep(1./240.)
def playReferenceMotion(self, motionFile): motionFile = os.path.join(os.path.dirname(__file__), motionFile) with open(motionFile, "r") as motion_file: motion = json.load(motion_file) ''' Joint indices should correspond to the following: 0 - root Type: FIXED 1 - chest Type: SPHERICAL 2 - neck Type: SPHERICAL 3 - right_hip Type: SPHERICAL 4 - right_knee Type: REVOLUTE 5 - right_ankle Type: SPHERICAL 6 - right_shoulder Type: SPHERICAL 7 - right_elbow Type: REVOLUTE 8 - right_wrist Type: FIXED 9 - left_hip Type: SPHERICAL 10 - left_knee Type: REVOLUTE 11 - left_ankle Type: SPHERICAL 12 - left_shoulder Type: SPHERICAL 13 - left_elbow Type: REVOLUTE 14 - left_wrist Type: FIXED ''' JointFrameMapIndices = [ 0, #root [9, 10, 11, 8], #chest [13, 14, 15, 12], #neck [17, 18, 19, 16], #rHip 20, #rKnee [22, 23, 24, 21], #rAnkle [26, 27, 28, 25], #rShoulder 29, #rElbow 0, #rWrist [31, 32, 33, 30], #lHip 34, #lKnee [36, 37, 38, 35], #lAnkle [40, 41, 42, 39], #lShoulder 43, #lElbow 0, #lWrist ] for frame in motion['Frames']: basePos1 = [frame[i] for i in [1, 2, 3]] baseOrn1 = frame[5:8] + [frame[4]] # transform the position and orientation to have z-axis upward y2zPos = [0, 0, 0.0] y2zOrn = p.getQuaternionFromEuler([1.57, 0, 0]) basePos, baseOrn = p.multiplyTransforms(y2zPos, y2zOrn, basePos1, baseOrn1) # set the agent's root position and orientation p.resetBasePositionAndOrientation( self.humanoidAgent, posObj=basePos, ornObj=baseOrn ) # Set joint positions for joint in self.revoluteJoints: p.resetJointState( self.humanoidAgent, jointIndex=joint, targetValue=frame[JointFrameMapIndices[joint]] ) for joint in self.sphericalJoints: p.resetJointStateMultiDof( self.humanoidAgent, jointIndex=joint, targetValue=[frame[i] for i in JointFrameMapIndices[3]] ) for i in range(16): p.stepSimulation() time.sleep(0.1/240)
def prepare_man_on_qolo(body_id): sdl = p.getVisualShapeData(body_id) for i in range(len(sdl)): if i < 26: p.changeVisualShape(body_id, sdl[i][1], rgbaColor=[0, 0, 0.45, 1]) else: p.changeVisualShape(body_id, sdl[i][1], rgbaColor=[0.45, 0.45, 0.45, 1]) belly_x_rot = 0.09 p.resetBasePositionAndOrientation( body_id, [0, 0, 1.345], p.getQuaternionFromEuler([np.pi / 2 + belly_x_rot, 0, 0])) p.resetJointStateMultiDof(body_id, 0, p.getQuaternionFromEuler([-belly_x_rot, 0, 0])) leg_z_rot = 0.04 leg_x_rot = -0.23 leg_y_rot = -0.35 p.resetJointStateMultiDof( body_id, 2, p.getQuaternionFromEuler([leg_x_rot, leg_y_rot, -leg_z_rot])) p.resetJointStateMultiDof( body_id, 3, p.getQuaternionFromEuler([leg_x_rot, -leg_y_rot, leg_z_rot])) shin_rot = 0.4 p.resetJointState(body_id, 4, shin_rot) p.resetJointState(body_id, 5, shin_rot) foot_y_rot = 0.45 foot_x_rot = -0.2 p.resetJointStateMultiDof( body_id, 6, p.getQuaternionFromEuler([foot_x_rot, foot_y_rot, 0])) p.resetJointStateMultiDof( body_id, 7, p.getQuaternionFromEuler([foot_x_rot, -foot_y_rot, 0])) neck_x_rot = 0.45 p.resetJointStateMultiDof(body_id, 14, p.getQuaternionFromEuler([neck_x_rot, 0, 0])) head_x_rot = -0.45 p.resetJointStateMultiDof(body_id, 15, p.getQuaternionFromEuler([head_x_rot, 0, 0])) arm_x_rot = 0.02 arm_z_rot = 0.25 arm_y_rot = 0.0 p.resetJointStateMultiDof( body_id, 8, p.getQuaternionFromEuler([arm_x_rot, arm_y_rot, -arm_z_rot])) p.resetJointStateMultiDof( body_id, 9, p.getQuaternionFromEuler([arm_x_rot, -arm_y_rot, arm_z_rot])) forearm_rot = -0.35 p.resetJointState(body_id, 10, forearm_rot) p.resetJointState(body_id, 11, forearm_rot) hand_x_rot = -0.09 hand_z_rot = -0.09 hand_y_rot = 0.0 p.resetJointStateMultiDof( body_id, 12, p.getQuaternionFromEuler([hand_x_rot, hand_y_rot, -hand_z_rot])) p.resetJointStateMultiDof( body_id, 13, p.getQuaternionFromEuler([hand_x_rot, -hand_y_rot, hand_z_rot]))
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 cur_basePos = np.array(sample_data['pos']['base'][cur_frame]) next_basePos = np.array(sample_data['pos']['base'][next_frame]) target_basePos = cur_basePos + frac * (next_basePos - cur_basePos) for i in range(len(joint_list)): cur_orn = sample_data['orn'][joint_list[i]][cur_frame] next_orn = sample_data['orn'][joint_list[i]][next_frame] if i in JOINT_SPHERICAL: target_orn[i] = p.getQuaternionSlerp(cur_orn, next_orn, frac) elif i in JOINT_REVOLUTE: target_orn[i] = cur_orn + frac * (next_orn - cur_orn) print("{}".format(target_basePos)) p.resetBasePositionAndOrientation(humanoid_id, target_basePos, target_orn[0]) for i in spherical_ind: p.resetJointStateMultiDof(humanoid_id, i, targetValue=target_orn[i]) for i in revolute_ind: th = target_orn[i] p.resetJointState(humanoid_id, i, targetValue=th) p.stepSimulation()
import pybullet as p import time physicsClient = p.connect(p.GUI) human_adult_ID = p.loadSDF("../data/human_adult.sdf") time.sleep(1.0) #raw_input("Press Enter to continue...") for i in range(2000): for j in range(p.getNumJoints(human_adult_ID[0])): p.resetJointStateMultiDof(human_adult_ID[0], j, targetValue=[i / 10.0, i / 8.0, i / 10.0], targetVelocity=[0, 0, 0]) p.resetBaseVelocity(human_adult_ID[0], [0.1, 0.1, 0.1]) p.stepSimulation() time.sleep(1. / 240.) p.disconnect()
p.resetJointState(linkage, jointInfoList['linear_motor_rod_joint_mid_right'], targetValue=0) p.resetJointState(linkage, jointInfoList['linear_motor_rod_joint_far_right'], targetValue=0) p.resetJointState(linkage, jointInfoList['skull_joint'], targetValue=0) p.resetJointState(linkage, jointInfoList['left_eye_yolk_joint'], targetValue=0) p.resetJointState(linkage, jointInfoList['left_eye_joint'], targetValue=0) p.resetJointState(linkage, jointInfoList['right_eye_yolk_joint'], targetValue=0) p.resetJointState(linkage, jointInfoList['right_eye_joint'], targetValue=0) p.resetJointStateMultiDof(linkage, jointInfoList['dogbone_joint_far_left'], targetValue=[0, 0, 0.00427]) p.resetJointStateMultiDof(linkage, jointInfoList['dogbone_joint_mid_left'], targetValue=[0, 0, 0.00427]) p.resetJointStateMultiDof(linkage, jointInfoList['dogbone_joint_mid_right'], targetValue=[0, 0, -0.00427]) p.resetJointStateMultiDof(linkage, jointInfoList['dogbone_joint_far_right'], targetValue=[0, 0, -0.00427]) # Close kinematic loops using constraints # Need to transform coordinates to the principal axes of inertia # [16.47e-3, 12.81e-3, 19.77e-3] [30.25e-3, 0, 0] far left # [16.47e-3, -15.69e-3, 19.77e-3] [30.25e-3, 0, 0] mid left ****
sphereRadius = 0.5 colSphereId = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.11, 0.11, 0.2]) mass = 0 #visualShapeId = p.createVisualShape(p.GEOM_SPHERE, radius=sphereRadius) baseOrientation = [0, 0, 0, 1] basePosition = [0, 0, -3.0] planeID = p.createMultiBody(mass, colSphereId, -1, basePosition, baseOrientation) #time.sleep(1.0) #raw_input("Press Enter to continue...") for i in range(20000): for j in range(3): p.resetJointStateMultiDof(human_adult_ID, j, p.getQuaternionFromEuler([0, 0, 0])) p.resetJointStateMultiDof( human_adult_ID, 3, p.getQuaternionFromEuler([i / 100.0, i / 150.0, i / 350.0])) #for j in range(p.getNumJoints(human_adult_ID)): # p.resetJointStateMultiDof(human_adult_ID, # j, # targetValue=[i/10.0,i/8.0,i/10.0], # targetVelocity=[0.01,0.01,0.01]) p.resetBasePositionAndOrientation(human_adult_ID, [0.1, 0.1, 0.1], p.getQuaternionFromEuler([0, 0, 0])) p.stepSimulation() time.sleep(1. / 240.) #p.getNumJoints(humanoid) #p.resetJointStateMultiDof(humanoid, j, targetValue=targetPosition, targetVelocity=targetVel)
humanoid = p.loadURDF("spherical_joint_limit.urdf",[0,0,2], useFixedBase=True) gravxId = p.addUserDebugParameter("grav_x",-20,20,0.3) gravyId = p.addUserDebugParameter("grav_y",-20,20,0.3) 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.resetBasePositionAndOrientation(humanoid4, startLocations[3],[0,0,0,1]) index0=7 for j in range (p.getNumJoints(humanoid)): ji = p.getJointInfo(humanoid,j) targetPosition=[0] jointType = ji[2] if (jointType == p.JOINT_SPHERICAL): targetPosition=[startPose[index0+1],startPose[index0+2],startPose[index0+3],startPose[index0+0]] targetVel = [startVel[index0+0],startVel[index0+1],startVel[index0+2]] index0+=4 print("spherical position: ",targetPosition) print("spherical velocity: ",targetVel) p.resetJointStateMultiDof(humanoid,j,targetValue=targetPosition,targetVelocity=targetVel) p.resetJointStateMultiDof(humanoid2,j,targetValue=targetPosition,targetVelocity=targetVel) if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE): targetPosition=[startPose[index0]] targetVel = [startVel[index0]] 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)
leftKneeRotStart = [frameData[34]] leftKneeRotEnd = [frameDataNext[34]] leftKneeRot = [leftKneeRotStart[0] + frameFraction * (leftKneeRotEnd[0] - leftKneeRotStart[0])] leftAnkleRotStart = [frameData[36], frameData[37], frameData[38], frameData[35]] leftAnkleRotEnd = [frameDataNext[36], frameDataNext[37], frameDataNext[38], frameDataNext[35]] leftAnkleRot = p.getQuaternionSlerp(leftAnkleRotStart, leftAnkleRotEnd, frameFraction) 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]) ] p.resetJointStateMultiDof(kinematic, chest, chestRot) p.resetJointStateMultiDof(kinematic, neck, neckRot) p.resetJointStateMultiDof(kinematic, rightHip, rightHipRot) p.resetJointStateMultiDof(kinematic, rightKnee, rightKneeRot) p.resetJointStateMultiDof(kinematic, rightAnkle, rightAnkleRot) p.resetJointStateMultiDof(kinematic, rightShoulder, rightShoulderRot) p.resetJointStateMultiDof(kinematic, rightElbow, rightElbowRot) p.resetJointStateMultiDof(kinematic, leftHip, leftHipRot) p.resetJointStateMultiDof(kinematic, leftKnee, leftKneeRot) p.resetJointStateMultiDof(kinematic, leftAnkle, leftAnkleRot) p.resetJointStateMultiDof(kinematic, leftShoulder, leftShoulderRot) p.resetJointStateMultiDof(kinematic, leftElbow, leftElbowRot) p.stepSimulation() time.sleep(timeStep)
# leftToeBaseRotEnd = leftToeBaseFrames[frameNext] # leftToeBaseRot = p.getQuaternionSlerp(leftToeBaseRotStart, leftToeBaseRotEnd, frameFraction) # rightLegRotEnd = rightLegFrames[frameNext] # rightLegRot = p.getQuaternionSlerp(rightLegRotStart, rightLegRotEnd, frameFraction) # rightFootRotEnd = rightFootFrames[frameNext] # rightFootRot = p.getQuaternionSlerp(rightFootRotStart, rightFootRotEnd, frameFraction) # rightToeBaseRotEnd = rightToeBaseFrames[frameNext] # rightToeBaseRot = p.getQuaternionSlerp(rightToeBaseRotStart, rightToeBaseRotEnd, frameFraction) # leftForeArmRotEnd = leftForeArmFrames[frameNext] # leftForeArmRot = p.getQuaternionSlerp(leftForeArmRotStart, leftForeArmRotEnd, frameFraction) # rightForeArmRotEnd = rightForeArmFrames[frameNext] # rightForeArmRot = p.getQuaternionSlerp(rightForeArmRotStart, rightForeArmRotEnd, frameFraction) # headRotEnd = headFrames[frameNext] # headRot = p.getQuaternionSlerp(headRotStart, headRotEnd, frameFraction) p.resetJointStateMultiDof(robot, Spine, spineRotStart) p.resetJointStateMultiDof(robot, Spine1, spine1RotStart) p.resetJointStateMultiDof(robot, Spine2, spine2RotStart) p.resetJointStateMultiDof(robot, RightShoulder, rightShoulderRotStart) p.resetJointStateMultiDof(robot, RightArm, rightArmRotStart) p.resetJointStateMultiDof(robot, RightForeArm, rightForeArmRotStart) p.resetJointStateMultiDof(robot, RightHand, rightHandRotStart) p.resetJointStateMultiDof(robot, LeftShoulder, leftShoulderRotStart) p.resetJointStateMultiDof(robot, LeftArm, leftArmRotStart) p.resetJointStateMultiDof(robot, LeftForeArm, leftForeArmRotStart) p.resetJointStateMultiDof(robot, LeftHand, leftHandRotStart) p.resetJointStateMultiDof(robot, Neck, neckRotStart) p.resetJointStateMultiDof(robot, Head, headRotStart) p.resetJointStateMultiDof(robot, LeftUpLeg, leftUpLegRotStart) # p.resetJointStateMultiDof(robot, LeftUpLeg, convert_bvh_to_bullet(leftUpLegFrames[0])) print("frame: ", frame, "bvh: ", leftUpLegFrames[frame], "result: ",