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'
posesVect[61] = [254,807,279,744,462,561,358,666,467,527,287,686,203,757,649,377,480,458]
posesVect[62] = [239,792,279,744,462,561,358,666,473,519,283,685,222,761,627,377,482,459]
posesVect[63] = [223,776,279,744,462,561,358,666,482,510,286,684,245,766,607,376,486,485]
posesVect[64] = [209,762,279,744,462,561,358,666,490,504,291,683,262,771,595,369,491,500]
posesVect[65] = [195,748,279,744,462,561,358,666,496,505,293,680,262,775,596,363,496,505]
posesVect[66] = [184,737,279,744,462,561,358,666,501,510,293,676,254,777,605,357,501,510]
posesVect[67] = [175,728,279,744,462,561,358,666,507,516,297,669,248,775,614,352,507,516]
print "program started"
for i in range(len(posesVect)):
frame = ET.SubElement(lucyPersistence, "frame")
frame.set("number" , str(i))
poseVectItem = posesVect[i]
for key in robotConfig.getJointsName():
xmlJoint = ET.SubElement(frame, key)
joint_id = configuration.loadJointId(key)
angle.setValue(poseVectItem[BalieroLucyMapping[key]-1])
xmlJointAngle = xmlJoint.set("angle" , str(angle.toDegrees()))
newPose.setValue(key, angle.toVrep()) #hay que pasarlo a vrep value
lucy.executePose(newPose)
lucy.stopLucy()
tree = ET.ElementTree(root)
tree.write("baliero.xml")
#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()
#lucy.executeFrame(pose)
