Пример #1
0
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'




Пример #2
0
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")






Пример #3
0
#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)