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")