# along with this program; if not, write to the Free Software
# Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
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'
# along with this program; if not, write to the Free Software
# Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
from simulator.SimLucy import SimLucy
from Pose import Pose
from simulator.AXAngle import AXAngle
from simulator.LoadRobotConfiguration import LoadRobotConfiguration
import xml.etree.cElementTree as ET
posesVect = {}
angle = AXAngle()
BalieroLucyMapping = {}
newPose = Pose()
robotConfig = LoadRobotConfiguration()
lucy = SimLucy(True)
root = ET.Element("root")
lucyPersistence = ET.SubElement(root, "Lucy")
configuration = LoadRobotConfiguration()
BalieroLucyMapping['L_Shoulder_Pitch']=2
BalieroLucyMapping['R_Shoulder_Pitch']=1
BalieroLucyMapping['L_Shoulder_Yaw']=4
BalieroLucyMapping['R_Shoulder_Yaw']=3
BalieroLucyMapping['L_Elbow_Yaw']=6
BalieroLucyMapping['R_Elbow_Yaw']=5
BalieroLucyMapping['R_Hip_Yaw']=7
BalieroLucyMapping['L_Hip_Yaw']=8
from simulator.SimLucy import SimLucy
from simulator.AXAngle import AXAngle
from parser.LoadPoses import LoadPoses
import math
import os
import time
print 'Program started'
angle = AXAngle()
angleFix = AXAngle()
angleExecute = AXAngle()
mocapFile = os.getcwd()+"/mocap/cmu_mocap/xml/moon_walk.xml"
lp = LoadPoses(mocapFile)
lucy = SimLucy(True)
poseExecute={}
poseFix={}
poseFix["R_Shoulder_Yaw"] = 0
poseFix["R_Shoulder_Pitch"] = -45
poseFix["R_Hip_Yaw"] = 0
poseFix["R_Hip_Roll"] = 0
poseFix["R_Hip_Pitch"] = -70
poseFix["R_Knee"] = 0
poseFix["R_Ankle_Pitch"] = 9
poseFix["R_Elbow_Yaw"] = 0
poseFix["R_Ankle_Roll"] = 0
poseFix["L_Shoulder_Yaw"] = 0
AP_C = math.pi/10
AP_A = math.pi/60
AP_Phi = -1*math.pi/12
AR_C = 0
AR_A = 0
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)
# along with this program; if not, write to the Free Software
# Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
from simulator.SimLucy import SimLucy
from Pose import Pose
from simulator.AXAngle import AXAngle
from errors.VrepException import VrepException
from simulator.LoadRobotConfiguration import LoadRobotConfiguration
import time
import xml.etree.cElementTree as ET
print "program started"
robotConfig = LoadRobotConfiguration()
lucy = SimLucy(True)
error = False
try:
error, reposeFrame = lucy.getFrame()
while error:
error, reposeFrame = lucy.getFrame()
time.sleep(0.5)
newPose = Pose(reposeFrame)
except VrepException, e:
print str(e)
root = ET.Element("root")
lucyPersistence = ET.SubElement(root, "Lucy")
configuration = LoadRobotConfiguration()
