# but WITHOUT ANY WARRANTY; without even the implied warranty of
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# 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
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program; if not, write to the Free Software
# Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
import xml.etree.cElementTree as ET
from Pose import Pose
from simulator.AXAngle import AXAngle
from simulator.LoadRobotConfiguration import LoadRobotConfiguration
from simulator.Lucy import SimulatedLucy
posesVect = {}
angle = AXAngle()
BalieroLucyMapping = {}
newPose = Pose()
robotConfig = LoadRobotConfiguration()
lucy = SimulatedLucy(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
# You should have received a copy of the GNU General Public License
# 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
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
import os
import time
import xml.etree.cElementTree as ET
left_ankle_mocap = [108.8880612842577, 108.2441823034163, 107.5839976873114, 106.9271140275009, 106.3127399717629, 105.813247589287, 105.5170796191161, 105.4652624352428, 105.58828300463, 105.7319652402544, 105.8195394181351, 105.9575592162489, 106.2221819047578, 106.4918235998237, 106.5668581680778, 106.3096998532735, 105.7142106931739, 104.9258713808842, 104.1944016529564, 103.7822569194421, 103.8366772141946, 104.2502505717218, 104.7127904301743, 104.9582917585397, 104.8472871430913, 104.2075994625946, 102.8447426022098, 100.8622428701039, 98.76889119020763, 97.10365798914917, 96.04764314510714, 95.43966107682347, 95.08556130008992, 94.95506367442675, 95.07817135358252, 95.34348371855047, 95.46185188911051, 95.16115228578776, 94.41458492169731, 93.41008001957519, 92.31783530989703, 91.20272125129398, 90.1652991855927, 89.46204801729546, 89.41239019365952, 90.21334446924247, 91.89613887336048, 94.45376130661047, 97.99735969279033, 102.7617587075939, 108.7072706057805, 114.8228388252166, 119.1890470989085, 120.2057402975197, 117.6305243430866, 112.9539984565639, 109.0553666595243]
right_ankle_mocap = [95.61582052908075, 95.65119454874024, 95.5984113978611, 95.45164639175356, 95.22559514789724, 94.93812265616884, 94.57492294659441, 94.07885114327387, 93.41406080184815, 92.68031785191104, 92.11800551441694, 91.87060521774664, 91.80173246081969, 91.70373995020086, 91.52588205568887, 91.32576453084602, 91.12600018640653, 90.86615576589304, 90.54719050275439, 90.41825594521555, 90.90961875839322, 92.41965459666804, 95.23212560661749, 99.44108485913475, 104.6317029727236, 109.6326875374181, 113.0461570700667, 114.2462705927577, 113.7269538145393, 112.6275352940777, 112.04174595675, 112.5735603068002, 114.1099174313259, 115.6082579713665, 115.2930277151785, 112.1497769776645, 107.7981307349187, 105.042086003715, 104.7430548395228, 105.8477636639926, 106.87936045759, 106.8847697727609, 105.8393353234152, 104.4577992531807, 103.4711518670009, 103.1374888474591, 103.3607744272528, 103.9691654837471, 104.8296967751942, 105.7816877548587, 106.505118797468, 106.5522462843528, 105.6307602648074, 103.8898950440933, 101.8761656794372, 100.1817633273363, 99.11549295107815]
left_knee_mocap = [103.3442564635671, 103.4700893604482, 103.7727154421756, 104.3456928694319, 105.3343470394194, 106.9732121549355, 109.6271146002103, 113.7812436932816, 119.8467403831764, 127.6427651192389, 136.0030377615223, 143.5576765689042, 149.8415809446355, 155.0769838028895, 159.4471197205436, 162.8093382247915, 164.8018806342181, 165.1414653197376, 163.9155953892795, 161.6396107388366, 158.9990075265206, 156.4950659983592, 154.3217565218735, 152.4948055674035, 150.9677379165379, 149.6560490261107, 148.5360067880646, 147.8009779955496, 147.8144004087617, 148.7898534766773, 150.4969828182749, 152.3435502415842, 153.8291854136046, 154.9337219773246, 156.0351409219606, 157.4800789571467, 159.2658218650785, 161.070230961052, 162.4791242952564, 163.1699706403697, 162.9838955179858, 161.9481511057253, 160.2654438023965, 158.2087914850266, 155.9505290593871, 153.4799856029541, 150.6512102641424, 147.2425618022295, 143.0138200919116, 137.8596548257813, 131.9912291357124, 125.9271088280634, 120.2477451764433, 115.3531986722106, 111.4541367777477, 108.7323073187118, 107.4436678182667]
right_knee_mocap = [155.2987366861989, 155.5711648746035, 155.9049890870594, 156.3306177510397, 156.9047189835861, 157.7131187672177, 158.8546301735333, 160.4135979301568, 162.4314975908274, 164.8211039912188, 167.1801994165631, 168.8282638931335, 169.3420743198938, 168.8941731232591, 167.9437048536077, 166.8171813436008, 165.5438400670859, 163.9199744711556, 161.7075319974956, 158.7887048285939, 155.1385603236602, 150.7460468756555, 145.6556914826537, 140.0437192361357, 134.1871211682299, 128.3836343404731, 122.934884036547, 118.1726248846073, 114.4474761070706, 112.061832141338, 111.2086306568455, 111.9682217471683, 114.3158307318026, 118.0953938202798, 123.0212096847312, 128.7740708401576, 135.1453222782363, 142.0570310094366, 149.3708235990681, 156.6632645791927, 163.2118816568739, 168.2447143611347, 171.2887347493591, 172.3657392577418, 171.9189440772895, 170.5810442748676, 168.9344813455103, 167.3395168234379, 165.8916168143421, 164.5156319136627, 163.1195863560465, 161.715742095185, 160.4585085657667, 159.6116570645375, 159.4496016247759, 160.0894898145728, 161.3757659496063]
left_hip_mocap = [160.3405757740941, 160.0483928242012, 159.718788260206, 159.3549772392398, 158.9624503163585, 158.5441199968649, 158.0921374333452, 157.5828364681954, 156.9982489538532, 156.4063711553857, 156.051405733268, 156.2532066495122, 157.08999235761, 158.2737403708286, 159.3832831963419, 160.1285426652907, 160.4527113062739, 160.5212005664234, 160.6222532543884, 160.9828944779043, 161.6264656552601, 162.3919519409871, 163.0640518184942, 163.4958979327537, 163.667849676463, 163.6916186696673, 163.7783574561317, 164.1733246547903, 165.0742871597065, 166.5479242760456, 168.4481829969073, 170.4226386957434, 172.1116033623366, 173.41523954283, 174.5492341039434, 175.8341292782796, 177.4448680493023, 179.3266994358477, 181.2689532041844, 183.0168002907293, 184.3650337904029, 185.2411729341962, 185.7419387810067, 186.063537503874, 186.3668680415255, 186.6934077376485, 186.9632850672231, 186.9958634813232, 186.5460223851539, 185.4076385383254, 183.5507068293402, 181.1609969323931, 178.5050289231432, 175.7299036001718, 172.8162354060114, 169.7331129863557, 166.6122941475761]
right_hip_mocap = [176.1335991646732, 176.3678820936095, 176.6603883127958, 177.0459056333462, 177.5872331449515, 178.3866236843256, 179.5914842982637, 181.3877052321384, 183.9425768675308, 187.1890257141867, 190.4825459159686, 192.7506616850765, 193.4477799612501, 193.0361198003163, 192.4098538453682, 192.2549582671118, 192.8047583201087, 193.8354629222449, 194.8546087464531, 195.4295636955853, 195.4018769925675, 194.837377855491, 193.8644772261638, 192.5561764229961, 190.8627213405822, 188.6343210441996, 185.7770944424088, 182.4095356540973, 178.8451710830156, 175.4293580268363, 172.4069474710761, 169.9079629665952, 167.9730772703634, 166.5599046756458, 165.5812445391448, 164.9776521232139, 164.7507721340754, 164.9344255217858, 165.5191294934955, 166.3643297539629, 167.1948251361053, 167.7411986491254, 167.9281355789194, 167.942854942157, 168.1202542537324, 168.7329584041697, 169.822147940846, 171.1657130130694, 172.4097848841509, 173.277427600627, 173.7128402173277, 173.896823004066, 174.1581667040972, 174.8314870369608, 176.1055453302945, 177.9161466366643, 179.9704353854289]
left_shoulder_mocap = [13.19406682667959, 13.26935109823979, 13.42006364444384, 13.68029713675935, 14.09337068955072, 14.71659000633349, 15.62829409825128, 16.92068475354689, 18.64301790834815, 20.67538049241524, 22.64808912105225, 24.16940080923494, 25.21328107393301, 26.07180992988619, 27.00547663906984, 28.08580006970992, 29.21779634062759, 30.20710607819693, 30.86097518180626, 31.09484719621922, 30.95893946660259, 30.55734127991727, 29.96138143947834, 29.20508495055056, 28.31068587084732, 27.28542311207367, 26.1031202492135, 24.69664265513098, 22.99885145763202, 21.02626477272542, 18.91480375554735, 16.84841478994881, 14.93797865031913, 13.16710684054805, 11.45602335202243, 9.766331340333204, 8.133050080854055, 6.601277386867615, 5.158353642275115, 3.767212909318263, 2.463527393150931, 1.378848957486517, 0.6741308718789497, 0.4658235869013272, 0.7705087204804503, 1.492538209082785, 2.492345137277816, 3.675879951241495, 5.010571138783577, 6.487343461580593, 8.094236626156414, 9.789079191697217, 11.45795037425654, 12.91579590392064, 13.98540935643197, 14.6131069369042, 14.93761980609455]
right_shoulder_mocap = [24.73162411733119, 24.63126643629743, 24.46249501648602, 24.19888330904315, 23.81066976359862, 23.27201952235513, 22.57276064242151, 21.73166482602448, 20.80205270404036, 19.85142691273589, 18.91256919035805, 17.96797275893746, 17.01302890004031, 16.1000916559498, 15.31206413182259, 14.73254204097721, 14.42450662703004, 14.39941699677051, 14.6139108304858, 15.0225982449954, 15.62437372379964, 16.43399821109551, 17.42516484939584, 18.52180797066586, 19.61875015058211, 20.59695215700605, 21.35224909060195, 21.84185031047098, 22.12455819497798, 22.35950756543336, 22.73632569023634, 23.37391384324175, 24.25508166454617, 25.23355508691186, 26.12164715911637, 26.79943508411489, 27.25086577920365, 27.53301233612717, 27.74320254758278, 27.97853906664163, 28.27580848848543, 28.58748554883578, 28.82851730210235, 28.94468639380529, 28.95262536649455, 28.94552477891864, 29.03864684582436, 29.25350554362183, 29.44848926730484, 29.39422239184857, 28.95105755298729, 28.19527300150052, 27.34463812572243, 26.52860683779747, 25.66008350882146, 24.56371791946592, 23.21972347478187]
print 'Program started'
angle = AXAngle()
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))