AR_Phi = 0
T = 0.5
root = ET.Element("root")
lucyPersistence = ET.SubElement(root, "Lucy")
try:
lucy = SimLucy(True)
pose = {}
poses = {}
poseNumber = 0
configuration = LoadRobotConfiguration()
simTimeMark = lucy.getSimTime()
counter = 0
while lucy.isLucyUp() and counter <= 400:
print "executin pose number: ", counter
frame = ET.SubElement(lucyPersistence, "frame")
frame.set("number" , str(poseNumber))
simTime = lucy.getSimTime()
#Calculate Joint Angles
#Shoulder Pitch
pose["L_Shoulder_Pitch"] = SP_C+SP_A*math.sin(2*math.pi*simTime/T+SP_Phi)
pose["R_Shoulder_Pitch"] = SP_C+SP_A*math.sin(2*math.pi*simTime/T+SP_Phi+math.pi)
#Hip Roll
pose["L_Hip_Roll"] = HR_C+HR_A*math.sin(2*math.pi*simTime/T+HR_Phi)
pose["R_Hip_Roll"] = HR_C+HR_A*math.sin(2*math.pi*simTime/T+HR_Phi+math.pi)
#Hip Pitch
pose["L_Hip_Pitch"] = HP_C+HP_A*math.sin(2*math.pi*simTime/T+HP_Phi)
pose["R_Hip_Pitch"] = HP_C+HP_A*math.sin(2*math.pi*simTime/T+HP_Phi+math.pi)
from simulator.SimLucy import SimLucy
from simulator.AXAngle import AXAngle
from parser.LoadPoses import LoadPoses
from RobotSniffer import RobotSniffer
import math
import os
import time
print 'Program started'
angle = AXAngle()
lucy = SimLucy(True)
sniffer = RobotSniffer(lucy)
sniffer.startSniffing()
while lucy.isLucyUp():
time.sleep(1)
print 'sniffing'
sniffer.stopSniffing()
lucy.stopLucy()
sniffer.generateFile("sniffed_walk1.xml")
print 'Program ended'
sim = Simulator()
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])
