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")
posesVect[77] = [175, 728, 279, 744, 462, 561, 358, 666, 507, 516, 297, 669, 248, 775, 614, 352, 507, 516]
'''
def getBalieroJointValue(joint_key):
if BalieroLucyMapping[key] not in mirrorPitch:
return poseVectItem[BalieroLucyMapping[key]-1]
else:
return 1023 - poseVectItem[BalieroLucyMapping[key]-1]
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(getBalieroJointValue(key))
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")
