Commit 0db3bdd5 authored by Rohan Budhiraja's avatar Rohan Budhiraja Committed by andreadelprete
Browse files

[data][lf-traj] change foot frame from sole center to ankle joint

parent 08fd2185
22 serialization::archive 10 0 0 0 0 3 0 0 0 3 6 0 0 3 7 0.024490399999999999 0.105 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 2 3 6 3 7 0.024490399999999999 0.105 -1713.1800523357015 0 0 4473.3034699812306 0 0 -4845.1974936801344 0 0 2786.3546648210572 0 0 -897.25879512320421 0 0 153.40546879128775 0 0 -10.879820481634702 2 2.7000000000000002 3 6 3 7 0.024490399999999999 0.105 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 2.7000000000000002 4.7000000000000002
22 serialization::archive 10 0 0 0 0 3 0 0 0 3 6 0 0 3 7 0.01 0.095000000000000001 0.105 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 2 3 6 3 7 0.01 0.095000000000000001 -1713.075052335691 0 0 4473.3034699812197 0 0 -4845.1974936801307 0 0 2786.3546648210572 0 0 -897.25879512320444 0 0 153.40546879128777 0 0 -10.879820481634704 2 2.7000000000000002 3 6 3 7 0.01 0.095000000000000001 0.105 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 2.7000000000000002 4.7000000000000002
22 serialization::archive 10 0 0 0 0 3 0 0 0 3 6 0 0 3 7 0.024490399999999999 0.105 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 2 3 6 3 7 0.024490399999999999 0.105 -2569.7700785035231 0 0 6709.9552049718041 0 0 -7267.7962405201797 0 0 4179.531997231581 0 0 -1345.888192684806 0 0 230.1082031869316 0 0 -16.319730722452054 2 2.7000000000000002 3 6 3 7 0.024490399999999999 0.105 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 2.7000000000000002 4.7000000000000002
22 serialization::archive 10 0 0 0 0 3 0 0 0 3 6 0 0 3 7 0.01 0.095000000000000001 0.105 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 2 3 6 3 7 0.01 0.095000000000000001 -2569.6650785035381 0 0 6709.9552049718332 0 0 -7267.7962405201979 0 0 4179.5319972315865 0 0 -1345.8881926848069 0 0 230.10820318693172 0 0 -16.319730722452061 2 2.7000000000000002 3 6 3 7 0.01 0.095000000000000001 0.105 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 2.7000000000000002 4.7000000000000002
22 serialization::archive 10 0 0 0 0 3 0 0 0 3 6 0 0 3 7 0.024490399999999999 0.105 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 2 3 6 3 7 0.024490399999999999 0.105 -3426.360104671403 0 0 8946.6069399624612 0 0 -9690.3949873602687 0 0 5572.7093296421144 0 0 -1794.5175902464084 0 0 306.81093758257549 0 0 -21.759640963269405 2 2.7000000000000002 3 6 3 7 0.024490399999999999 0.105 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 2.7000000000000002 4.7000000000000002
22 serialization::archive 10 0 0 0 0 3 0 0 0 3 6 0 0 3 7 0.01 0.095000000000000001 0.105 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 2 3 6 3 7 0.01 0.095000000000000001 -3426.2551046713452 0 0 8946.6069399623957 0 0 -9690.3949873602432 0 0 5572.7093296421108 0 0 -1794.5175902464082 0 0 306.81093758257543 0 0 -21.759640963269401 2 2.7000000000000002 3 6 3 7 0.01 0.095000000000000001 0.105 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 2.7000000000000002 4.7000000000000002
22 serialization::archive 10 0 0 0 0 3 0 0 0 3 6 0 0 3 7 0.024490399999999999 0.105 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 2 3 6 3 7 0.024490399999999999 0.105 -4282.9501308392064 0 0 11183.258674953024 0 0 -12112.993734200318 0 0 6965.8866620526414 0 0 -2243.1469878080106 0 0 383.51367197821935 0 0 -27.199551204086756 2 2.7000000000000002 3 6 3 7 0.024490399999999999 0.105 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 2.7000000000000002 4.7000000000000002
22 serialization::archive 10 0 0 0 0 3 0 0 0 3 6 0 0 3 7 0.01 0.095000000000000001 0.105 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 2 3 6 3 7 0.01 0.095000000000000001 -4282.8451308392032 0 0 11183.258674953024 0 0 -12112.993734200318 0 0 6965.8866620526414 0 0 -2243.1469878080106 0 0 383.51367197821935 0 0 -27.199551204086756 2 2.7000000000000002 3 6 3 7 0.01 0.095000000000000001 0.105 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 2.7000000000000002 4.7000000000000002
22 serialization::archive 10 0 0 0 0 3 0 0 0 3 6 0 0 3 7 0.024490399999999999 0.105 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 2 3 6 3 7 0.024490399999999999 0.105 -5139.5401570070462 0 0 13419.910409943608 0 0 -14535.592481040359 0 0 8359.0639944631621 0 0 -2691.7763853696119 0 0 460.21640637386321 0 0 -32.639461444904107 2 2.7000000000000002 3 6 3 7 0.024490399999999999 0.105 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 2.7000000000000002 4.7000000000000002
22 serialization::archive 10 0 0 0 0 3 0 0 0 3 6 0 0 3 7 0.01 0.095000000000000001 0.105 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 2 3 6 3 7 0.01 0.095000000000000001 -5139.4351570070721 0 0 13419.910409943652 0 0 -14535.592481040381 0 0 8359.0639944631657 0 0 -2691.7763853696119 0 0 460.21640637386315 0 0 -32.6394614449041 2 2.7000000000000002 3 6 3 7 0.01 0.095000000000000001 0.105 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 2.7000000000000002 4.7000000000000002
......@@ -10,7 +10,7 @@ import numpy as np
Z_AMP=0.06
TIME_TO_LIFT=(2,2.7)
TOTAL_TRAJ_TIME=4.7
LF_INIT=np.array([0.0244904,0.105,0.])
LF_INIT=np.array([0.01,0.095,0.105])
####CONFIG######################
......@@ -163,7 +163,7 @@ def foot_lift0(initPos, z_amp=Z_AMP, timeToLift=TIME_TO_LIFT,
A[5,2:] = np.array([n*(n-1)*(s0**(n-2)) for n in xrange(2,7)])
A[6,2:] = np.array([n*(n-1)*(s1**(n-2)) for n in xrange(2,7)])
B = np.matrix([[0.0,z_amp,0.0,0.,0.,0.,0.]]).transpose()
B = np.matrix([[initPos[2],z_amp+initPos[2],initPos[2],0.,0.,0.,0.]]).transpose()
poly_coeff_begin=np.zeros((3, 7))
poly_coeff_s =np.zeros((3, 7))
......@@ -173,6 +173,9 @@ def foot_lift0(initPos, z_amp=Z_AMP, timeToLift=TIME_TO_LIFT,
arr[0][0] = initPos[0]
arr[1][0] = initPos[1]
for arr in [poly_coeff_begin, poly_coeff_end]:
arr[2][0] = initPos[2]
time_vector = np.array([0., s0, s1, totalTrajTime])
poly_coeff_s[2,:] = np.array(solve(A,B)).squeeze()
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment