Commit 0e999961 authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

[nd-trajectory-gen]

1) spline scripts to generate spline outputs from centroidal dyn output.
2) add sample com trajectory as a spline to be loaded (spline1.out)
3) change python-commands.txt
parent 1f25b13b
......@@ -37,11 +37,13 @@ create_topic(robot.ros, robot.device.accelerometer, 'acc');
ent.ctrl_manager.setCtrlMode('all','torque')
robot.base_estimator.set_imu_weight(0.0);
robot.com_traj_gen.playSpline("/local/rbudhira/git/proyan/parametric-curves/spline1.out")
robot.com_traj_gen.playSpline("/local/rbudhira/git/proyan/sot-torque-control/scripts/spline1.out")
sleep(5.0)
robot.com_traj_gen.move(1, 0.05, 1.5)
sleep(2.0*7)
robot.com_traj_gen.startSinusoid(1, -0.05, 1.5)
robot.com_traj_gen.startLinChirp(1, -0.05, 0.3, 0.7, 10)
robot.com_traj_gen.startConstAcc(1, -0.05, 2)
create_topic(robot.ros, robot.inv_dyn.tau_des, 'tau_des');
create_topic(robot.ros, robot.estimator_ft.jointsTorques, 'tau_est');
......
22 serialization::archive 10 0 0 0 0 8 0 0 0 3 5 0 0 3 6 0.0094923001067491169 1.0474389457717114e-06 0.81175839329200339 -4.2699142629326094e-17 1.0165045888355095e-14 9.3680488713610011e-15 3.5248164369244437e-17 2.4909544912854464e-17 2.6330595147804059e-15 -0.00039590008322026903 0.0071087737868908742 0.006589358494181085 0.00080893935729300633 -0.024280302501966115 -0.016392544698387697 -9.5459016232013152e-05 0.014825463937907171 0.010763237686019687 0 0.67333333333333334 3 5 3 6 0.0080194852900977928 -0.018700793024672559 0.79941126830206111 0.0094178215763609362 0.11385080808194102 0.076715464752749277 -0.023368598410155806 -0.26231308871504189 -0.18248221738998713 0.027329698056902371 0.28171940356640318 0.20881641911245977 -0.014489784641465928 -0.14109402236681318 -0.11444064106206972 0.0028455158014958591 0.023126304156425401 0.024091006865118607 0.67333333333333334 1.3399999999999999 3 5 3 6 0.0099385027852336889 1.7131792436079973 0.47314840139259973 -0.007476267062527988 -5.6857258957701893 1.0627545505666873 0.016519198821522609 7.3945918551587759 -1.3053708079905955 -0.01349614807149741 -4.6791142204063414 0.78385533294390441 0.0049111569834245127 1.4260266801482442 -0.22990741032923362 -0.00067831284124881489 -0.16796301524962587 0.026385098742248703 1.3399999999999999 2 3 5 3 6 0.13315197507617293 -3.1610590564825709 0.27764675889308188 -0.31151175133990155 8.3804561222791119 0.98649124251022424 0.30909450028877233 -8.471371488791748 -0.70555215996659526 -0.15032271428728514 4.113355627252643 0.24289937868755268 0.035819779129794282 -0.97686220387102107 -0.03958952413995187 -0.0033460982873162459 0.091792953181109027 0.0023636963543910428 2 2.3534999999999999 3 5 3 6 -0.19439647602941096 10.088918147997555 1.0777349395412881 0.40839605025333892 -19.801903524143736 -0.72611263849453345 -0.32316422759016111 15.50588675539885 0.760739315795389 0.12705173892141372 -6.0864441834950735 -0.38478050796066904 -0.024968016452094122 1.192608223138838 0.094752168162870798 0.0019781357599640836 -0.092783275033376356 -0.0091371098203182344 2.3534999999999999 2.7000000000000002 3 5 3 6 -1.7292684622980234 -35.825335042300047 6.6557571994212159 2.9818825649949492 62.2406725214151 -10.186762171609983 -2.0299714153158708 -42.930936122030616 7.050516889373684 0.68520402872007602 14.645462348145456 -2.4208083339918542 -0.11460438760490754 -2.4692746867553446 0.41233661351217465 0.0075995781954691921 0.16469342287807365 -0.027879658325646872 2.7000000000000002 3.3733333333333335 3 5 3 6 -0.14700606468342639 12.056315534453136 19.450242122946634 0.093419356788384894 -19.619912052518462 -25.142496350697566 0.0058890404496765693 12.084790761610863 13.531561289273164 -0.014539191274376992 -3.5925783029453573 -3.631945236376187 0.0034315648986456039 0.52103862323071404 0.48617984792741975 -0.000250895741870788 -0.02968321403056251 -0.025967657433252356 3.3733333333333335 4.04 3 5 3 6 -1.0994745891880779 26.060143571955109 22.205293876799317 1.3153987277280861 -29.862109332053247 -24.533060064859967 -0.62049739121447411 13.634019099772198 11.233597986954546 0.14583254741766552 -3.1015901622353432 -2.567384874374997 -0.017077656958006893 0.35168955534259272 0.29286855580112547 0.00079725217518245017 -0.015906428707861409 -0.013340271080732435 4.04 4.7000000000000002
from parametriccurves import spline
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):
assert len(x.shape)==1;
p = np.zeros((y.shape[0], deg+1))
for i,y_a in enumerate(y):
p_a , _, _, _, _ = np.polyfit(x, np.asarray(y_a).squeeze(), deg, rcond, full, w, cov);
p[i, :] = p_a
return p
def dd_polyfit(x, y, dy, ddy, deg_y, eps=1e-20):
assert(y.shape[1]==dy.shape[1]==ddy.shape[1])
p = np.zeros((y.shape[0], deg_y+1))
for i in xrange(y.shape[0]):
p_a = dd_polyfit_1d(x, y[i], dy[i], ddy[i], deg_y, eps)
p[i,:] = p_a.squeeze()
return p
def dd_polyfit_1d(x, y, dy, ddy, deg_y, eps=1e-20):
assert len(x.shape)==1;
assert deg_y>=2;
A = np.zeros((x.shape[0]*3, deg_y+1))
for i,t in enumerate(x):
A[i,:] = np.array([t**n for n in xrange(deg_y+1)])
for i,t in enumerate(x):
A[i+len(x),1:] = np.array([n*t**(n-1) for n in xrange(1, deg_y+1)]);
A[i+len(x),0] = 0.0;
for i,t in enumerate(x):
A[i+2*len(x),2:] = np.array([n*(n-1)*t**(n-2) for n in xrange(2,deg_y+1)]);
A[i+2*len(x),0:2] = 0.0;
B = np.concatenate((y, dy, ddy), axis=1).transpose()
x, residual, _, _ = lstsq(A, B)
assert(residual <=eps)
return x
CONTACT_SEQUENCE_WHOLEBODY_FILE="/local/rbudhira/git/jcarpent/reachability/wb_contact_sequence/contact_sequence_trajectory.xml"
CONTACT_SEQUENCE_XML_TAG="ContactSequence"
SPLINE_OUTPUT_FILE="spline1.out"
VERIFY=False
WRITE_OUTPUT =True
cs = ContactSequenceHumanoid(0)
cs.loadFromXML(CONTACT_SEQUENCE_WHOLEBODY_FILE,CONTACT_SEQUENCE_XML_TAG)
traj_times = []
poly_com_list = []; poly_dcom_list = []; poly_ddcom_list = []
poly_L_list = []; poly_dL_list = []
poly_control_list = []
time_vector = np.zeros(len(cs.ms_interval_data))
for spl in cs.ms_interval_data:
x = np.array([])
y_com = np.zeros((3,0)); y_dcom = np.zeros((3,0)); y_ddcom = np.zeros((3,0));
y_L = np.zeros((3,0)); y_dL = np.zeros((3,0));
y_control = np.zeros((3,0));
for t in spl.time_trajectory:
x = np.append(x,t)
if x.size==0: break;
for y in spl.state_trajectory:
y_com = np.append(y_com, y[0:3], axis=1)
y_dcom = np.append(y_dcom, y[3:6], axis=1)
y_L = np.append(y_L, y[6:9], axis=1)
for y in spl.dot_state_trajectory:
y_ddcom = np.append(y_ddcom, y[3:6], axis=1)
y_dL = np.append(y_dL, y[6:9], axis=1)
#for y in spl.control_trajectory:
#y_control = np.append(y_control, y)
traj_times.append(x)
poly_com_list.append(dd_polyfit(x, y_com, y_dcom, y_ddcom, deg_y=5, eps=1e-20))
poly_L_list.append(array_polyfit(x, y_L, deg=4, full=True))
poly_dL_list.append(array_polyfit(x, y_dL, deg=3, full=True))
#Fitting for control vector is the same?
#poly_control_list.append(array_polyfit(x, y_control, deg=3, full=True))
time_vector = np.zeros(len(traj_times)+1)
for ts in xrange(len(traj_times)):
time_vector[ts] = traj_times[ts][0]
time_vector[-1] = traj_times[-1][-1]
com_spline = spline(poly_com_list, time_vector)
if WRITE_OUTPUT:
com_spline.save_spline(SPLINE_OUTPUT_FILE)
###############CONFIRM SPLINE OUTPUT##########################
if VERIFY:
for spl in cs.ms_interval_data:
for j,t in enumerate(spl.time_trajectory):
assert(np.isclose(spl.state_trajectory[j][0:3],com_spline(t)).all())
print j, spl.state_trajectory[j][3:6],com_spline.derivate(t,1)
if not np.isclose(spl.state_trajectory[j][3:6],com_spline.derivate(t,1)).all():
raw_input("Oops")
if not np.isclose(spl.dot_state_trajectory[j][3:6],com_spline.derivate(t,2)).all():
raw_input("Oops again")
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