Commit 08fd2185 authored by Rohan Budhiraja's avatar Rohan Budhiraja Committed by andreadelprete
Browse files

[data] lf foot reference trajectory for 2,3,4,5,6 cm step (zero initial and final accelerations)

parent bf7c3951
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.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.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.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.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
......@@ -50,7 +50,7 @@ create_topic(robot.ros, robot.lf_traj_gen.x, 'lf_ref_traj_gen');
ent.ctrl_manager.setCtrlMode('all','torque')
robot.base_estimator.set_imu_weight(1.0);
SOT_TORQUE_INSTALL_DIR = '/local/rbudhira/git/sot/sot-torque-control'
SOT_TORQUE_INSTALL_DIR = '/home/adelpret/devel/openrobots/src/sot-torque-control'
robot.com_traj_gen.setSpline(SOT_TORQUE_INSTALL_DIR+"/data/traj_com_y_0.08/com_spline.curve", -1)
robot.lf_force_traj_gen.setSpline(SOT_TORQUE_INSTALL_DIR+"/data/traj_com_y_0.08/lf_force.curve", -1)
......
from parametriccurves import spline, spline6
from locomote import ContactSequenceHumanoid
from numpy import polyfit
from numpy.linalg import lstsq, solve
from pinocchio.utils import *
import numpy as np
####################FOOT TRAJECTORY
Z_AMP=0.06
TIME_TO_LIFT=(2,2.7)
TOTAL_TRAJ_TIME=4.7
LF_INIT=np.array([0.0244904,0.105,0.])
####CONFIG######################
CONTACT_SEQUENCE_WHOLEBODY_FILE="../data/traj_com_y_0.08/contact_sequence_trajectory.xml"
CONTACT_SEQUENCE_XML_TAG="ContactSequence"
......@@ -7,19 +22,13 @@ RF_FORCE_OUTPUT_FILE ="../data/traj_com_y_0.08/rf_force.curve"
LF_FORCE_OUTPUT_FILE ="../data/traj_com_y_0.08/lf_force.curve"
RH_FORCE_OUTPUT_FILE ="../data/traj_com_y_0.08/rh_force.curve"
LH_FORCE_OUTPUT_FILE ="../data/traj_com_y_0.08/lh_force.curve"
LF_SPLINE_OUTPUT_FILE="../data/traj_com_y_0.08/lf_spline"+ str(int(Z_AMP*100))+".curve"
VERIFY=False
WRITE_OUTPUT =False
#PLOT =True
####CONFIG######################
WRITE_OUTPUT =True
PLOT =True
##########################################
from parametriccurves import spline, spline6
from locomote import ContactSequenceHumanoid
from numpy import polyfit
from numpy.linalg import lstsq
from pinocchio.utils import *
import numpy as np
def array_polyfit(x, y, deg, rcond=None, full=False, w=None, cov=False, eps=1e-18):
assert len(x.shape)==1;
p = np.zeros((y.shape[0], deg+1))
......@@ -60,7 +69,6 @@ def dd_polyfit_1d(x, y, dy, ddy, deg_y, eps=1e-20):
assert(residual <=eps)
return x
cs = ContactSequenceHumanoid(0)
cs.loadFromXML(CONTACT_SEQUENCE_WHOLEBODY_FILE,CONTACT_SEQUENCE_XML_TAG)
......@@ -139,6 +147,42 @@ lf_force = spline6(poly_control_list_lf, time_vector)
rh_force = spline6(poly_control_list_rh, time_vector)
lh_force = spline6(poly_control_list_lh, time_vector)
def foot_lift0(initPos, z_amp=Z_AMP, timeToLift=TIME_TO_LIFT,
totalTrajTime=TOTAL_TRAJ_TIME):
s = timeToLift[1]-timeToLift[0]
s0=timeToLift[0]
s1 = timeToLift[1]
A = np.zeros((7, 7))
A[0,:] = np.array([s0**n for n in xrange(7)])
A[1,:] = np.array([(0.5*(s0+s1))**n for n in xrange(7)])
A[2,:] = np.array([s1**n for n in xrange(7)])
A[3,1:] = np.array([n*(s0**(n-1)) for n in xrange(1,7)])
A[4,1:] = np.array([n*(s1**(n-1)) for n in xrange(1,7)])
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()
poly_coeff_begin=np.zeros((3, 7))
poly_coeff_s =np.zeros((3, 7))
poly_coeff_end= np.zeros((3, 7))
for arr in [poly_coeff_begin, poly_coeff_s, poly_coeff_end]:
arr[0][0] = initPos[0]
arr[1][0] = initPos[1]
time_vector = np.array([0., s0, s1, totalTrajTime])
poly_coeff_s[2,:] = np.array(solve(A,B)).squeeze()
return ([poly_coeff_begin, poly_coeff_s, poly_coeff_end], time_vector)
lf_traj = foot_lift0(initPos=LF_INIT, z_amp=Z_AMP, timeToLift=TIME_TO_LIFT,
totalTrajTime=TOTAL_TRAJ_TIME)
lf_spline = spline(lf_traj[0], lf_traj[1])
print "Trajectories created"
if WRITE_OUTPUT:
com_spline.save_to_file(COM_SPLINE_OUTPUT_FILE)
......@@ -146,7 +190,7 @@ if WRITE_OUTPUT:
lf_force.save_to_file(LF_FORCE_OUTPUT_FILE)
rh_force.save_to_file(RH_FORCE_OUTPUT_FILE)
lh_force.save_to_file(LH_FORCE_OUTPUT_FILE)
lf_spline.save_to_file(LF_SPLINE_OUTPUT_FILE)
###############CONFIRM SPLINE OUTPUT##########################
if VERIFY:
......@@ -163,3 +207,12 @@ if VERIFY:
raw_input("Oops")
if not np.isclose(spl.dot_state_trajectory[j][3:6],com_spline.derivate(t,2)).all():
raw_input("Oops again")
if PLOT:
import matplotlib.pyplot as plt
N = 1000
lf_spline_data = np.array([lf_spline((t/(1.0*N))*lf_spline.max())[2] \
for t in xrange(N)]).squeeze()
time_data = np.array([(t/(1.0*N))*lf_spline.max() for t in xrange(N)]).squeeze()
plt.plot(time_data, lf_spline_data)
plt.show()
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