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: ",
