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