poseFix["L_Shoulder_Pitch"] = -70
poseFix["L_Hip_Yaw"] = 0
poseFix["L_Hip_Roll"] = 0
poseFix["L_Hip_Pitch"] = -50
poseFix["L_Knee"] = 0
poseFix["L_Ankle_Pitch"] = 9
poseFix["L_Elbow_Yaw"] = 0
poseFix["L_Ankle_Roll"] = 0
avoid_joints = ["R_Hip_Yaw", "R_Shoulder_Yaw", "L_Hip_Yaw", "L_Shoulder_Yaw", "L_Hip_Roll", "R_Hip_Roll", "L_Ankle_Roll", "R_Ankle_Roll"]
frameQty=lp.getFrameQty()
while True:
for index in range(frameQty):
pose=lp.getFramePose(index)
for joint in pose.keys():
if joint not in avoid_joints:
#print joint, pose[joint], pose[joint] + poseFix[joint]
#angleExecute.setDegreeValue(pose[joint]+ poseFix[joint])
angleExecute.setDegreeValue(pose[joint]) # TODO REMOVE this is only for moonwalking
poseExecute[joint] = angleExecute.toVrep()
#print joint, poseExecute[joint], angleExecute.getValue()
#print poseExecute['L_Ankle_Pitch'], pose['L_Ankle_Pitch']
lucy.executeFrame(poseExecute)
#print index
lucy.stopLucy()
print 'Program ended'
lucy = SimLucy(True)
#clientID = sim.connectVREP()
pose={}
time_serie_lenght = len(left_ankle_mocap)
root = ET.Element("root")
lucyPersistence = ET.SubElement(root, "Lucy")
configuration = LoadRobotConfiguration()
for j in xrange(time_serie_lenght):
frame = ET.SubElement(lucyPersistence, "frame")
frame.set("number" , str(j))
lucy.isLucyUp()
angle.setDegreeValue(left_knee_mocap[j])
pose["L_Knee"] = angle.toVrep()
angle.setDegreeValue(right_knee_mocap[j])
pose["R_Knee"] = angle.toVrep()
angle.setDegreeValue(255 - right_ankle_mocap[j])
pose["R_Ankle_Pitch"] = angle.toVrep()
angle.setDegreeValue(255 - left_ankle_mocap[j])
pose["L_Ankle_Pitch"] = angle.toVrep()
angle.setDegreeValue(left_hip_mocap[j]-20)
pose["L_Hip_Pitch"] = angle.toVrep()
angle.setDegreeValue(right_hip_mocap[j]-20)
pose["R_Hip_Pitch"] = angle.toVrep()
angle.setDegreeValue(155-left_shoulder_mocap[j])
pose["L_Shoulder_Pitch"] = angle.toVrep()
angle.setDegreeValue(155-right_shoulder_mocap[j])
pose["R_Shoulder_Pitch"] = angle.toVrep()
