Skip to content

GitLab

Commit 58036e4a authored by Andrea Del Prete's avatar Andrea Del Prete
Browse files

Add foot trajectories to ex 4

parent bd2811f7
Hide whitespace changes
Inline Side-by-side
exercizes/ex_4_LIPM_to_TSID.py 0 → 100644
View file @ 58036e4a
import numpy as np
import LMPC_walking.second_order.plot_utils as plot_utils
from LMPC_walking.second_order.LIPM_to_whole_body import compute_foot_traj, interpolate_lipm_traj
import matplotlib.pyplot as plt
import ex_4_conf as conf
# READ COM-COP TRAJECTORIES COMPUTED WITH LIPM MODEL
data = np.load(conf.DATA_FILE_LIPM)
com_state_x = data['com_state_x']
com_state_y = data['com_state_y']
cop_ref = data['cop_ref']
cop_x = data['cop_x']
cop_y = data['cop_y']
foot_steps = data['foot_steps']
# INTERPOLATE WITH TIME STEP OF CONTROLLER (TSID)
dt_ctrl = conf.dt # time step used by TSID
com, dcom, ddcom, cop, contact_phase, foot_steps_ctrl = \
interpolate_lipm_traj(conf.T_step, conf.nb_steps, conf.dt_mpc, dt_ctrl, conf.h, conf.g,
com_state_x, com_state_y, cop_ref, cop_x, cop_y)
# COMPUTE TRAJECTORIES FOR FEET
N = conf.nb_steps * int(round(conf.T_step/conf.dt_mpc)) # number of time steps for traj-opt
N_ctrl = int((N*conf.dt_mpc)/dt_ctrl) # number of time steps for TSID
foot_steps_RF = foot_steps[::2,:] # assume first foot step corresponds to right foot
x_RF, dx_RF, ddx_RF = compute_foot_traj(foot_steps_RF, N_ctrl, dt_ctrl, conf.T_step, conf.step_height, 'stance')
foot_steps_LF = foot_steps[1::2,:]
x_LF, dx_LF, ddx_LF = compute_foot_traj(foot_steps_LF, N_ctrl, dt_ctrl, conf.T_step, conf.step_height, 'swing')
# SAVE COMPUTED TRAJECTORIES IN NPY FILE FOR TSID
np.savez(conf.DATA_FILE_TSID, com=com, dcom=dcom, ddcom=ddcom,
x_RF=x_RF, dx_RF=dx_RF, ddx_RF=ddx_RF,
x_LF=x_LF, dx_LF=dx_LF, ddx_LF=ddx_LF,
contact_phase=contact_phase)
# PLOT STUFF
time_ctrl = np.arange(0, round(N_ctrl*dt_ctrl, 2), dt_ctrl)
for i in range(3):
plt.figure()
plt.plot(time_ctrl, x_RF[i,:-1], label='x RF '+str(i))
plt.plot(time_ctrl, x_LF[i,:-1], label='x LF '+str(i))
plt.legend()
#for i in range(2):
# plt.figure()
# plt.plot(time_ctrl, dx_RF[i,:-1], label='dx RF '+str(i))
# plt.plot(time_ctrl, dx_LF[i,:-1], label='dx LF '+str(i))
# plt.legend()
#
#for i in range(2):
# plt.figure()
# plt.plot(time_ctrl, ddx_RF[i,:-1], label='ddx RF '+str(i))
# plt.plot(time_ctrl, ddx_LF[i,:-1], label='ddx LF '+str(i))
# plt.legend()
time = np.arange(0, round(N*conf.dt_mpc, 2), conf.dt_mpc)
for i in range(2):
plt.figure()
plt.plot(time_ctrl, cop[i,:-1], label='CoP')
# plt.plot(time_ctrl, foot_steps_ctrl[i,:-1], label='Foot step')
plt.plot(time_ctrl, com[i,:-1], 'g', label='CoM')
if i==0: plt.plot(time, com_state_x[:-1,0], ':', label='CoM TO')
else: plt.plot(time, com_state_y[:-1,0], ':', label='CoM TO')
plt.legend()
#for i in range(2):
# plt.figure()
# plt.plot(time_ctrl, dcom[i,:-1], label='CoM vel')
# vel_fd = (com[i,1:] - com[i,:-1]) / dt_ctrl
# plt.plot(time_ctrl, vel_fd, ':', label='CoM vel fin-diff')
# plt.legend()
#
#for i in range(2):
# plt.figure()
# plt.plot(time_ctrl, ddcom[i,:-1], label='CoM acc')
# acc_fd = (dcom[i,1:] - dcom[i,:-1]) / dt_ctrl
# plt.plot(time_ctrl, acc_fd, ':', label='CoM acc fin-diff')
# plt.legend()
foot_length = conf.lxn + conf.lxp # foot size in the x-direction
foot_width = conf.lyn + conf.lyp # foot size in the y-direciton
plot_utils.plot_xy(time_ctrl, N_ctrl, foot_length, foot_width,
foot_steps_ctrl.T, cop[0,:], cop[1,:],
com[0,:].reshape((N_ctrl+1,1)),
com[1,:].reshape((N_ctrl+1,1)))
plt.plot( com_state_x[:,0], com_state_y[:,0], 'r* ', markersize=15,)
exercizes/ex_4_conf.py
View file @ 58036e4a
......@@ -11,29 +11,20 @@ import os
np.set_printoptions(precision=3, linewidth=200, suppress=True)
LINE_WIDTH = 60
# cost weights in the objective function:
# ---------------------------------------
alpha = 10**(2) # CoP error squared cost weight
beta = 0 # CoM position error squared cost weight
gamma = 10**(-1) # CoM velocity error squared cost weight
h = 0.58 # fixed CoM height (assuming walking on a flat terrain)
g = 9.81 # norm of the gravity vector
foot_step_0 = np.array([0.0, -0.096]) # initial foot step position in x-y
dt_mpc = 0.1 # sampling time interval
T_step = 1.2 # time needed for every step
step_length = 0.001 # fixed step length in the xz-plane
nb_steps = 4 # number of desired walking steps
#N_SIMULATION = 2000 # number of time steps simulated
dt = 0.002 # controller time step
DATA_FILE = 'romeo_walking_traj.npz'
DATA_FILE_LIPM = 'romeo_walking_traj_lipm.npz'
DATA_FILE_TSID = 'romeo_walking_traj_tsid.npz'
# robot parameters
# ----------------------------------------------
filename = str(os.path.dirname(os.path.abspath(__file__)))
path = filename + '/../models/romeo'
urdf = path + '/urdf/romeo.urdf'
srdf = path + '/srdf/romeo_collision.srdf'
lxp = 0.10 # foot length in positive x direction
lxn = 0.05 # foot length in negative x direction
lyp = 0.05 # foot length in positive y direction
lyn = 0.05 # foot length in negative y direction
lz = 0.0 # foot sole height with respect to ankle joint
lz = 0.07 # foot sole height with respect to ankle joint
mu = 0.3 # friction coefficient
fMin = 5.0 # minimum normal force
fMax = 1000.0 # maximum normal force
......@@ -41,6 +32,24 @@ rf_frame_name = "RAnkleRoll" # right foot frame name
lf_frame_name = "LAnkleRoll" # left foot frame name
contactNormal = np.matrix([0., 0., 1.]).T # direction of the normal to the contact surface
# configuration for LIPM trajectory optimization
# ----------------------------------------------
alpha = 10**(2) # CoP error squared cost weight
beta = 0 # CoM position error squared cost weight
gamma = 10**(-1) # CoM velocity error squared cost weight
h = 0.58 # fixed CoM height
g = 9.81 # norm of the gravity vector
foot_step_0 = np.array([0.0, -0.096]) # initial foot step position in x-y
dt_mpc = 0.1 # sampling time interval
T_step = 1.2 # time needed for every step
step_length = 0.1 # fixed step length
step_height = 0.05 # fixed step height
nb_steps = 4 # number of desired walking steps
# configuration for TSID
# ----------------------------------------------
dt = 0.002 # controller time step
w_com = 1.0 # weight of center of mass task
w_foot = 1e-1 # weight of the foot motion task
w_posture = 1e-4 # weight of joint posture task
......@@ -51,20 +60,8 @@ kp_foot = 10.0 # proportional gain of contact constraint
kp_com = 10.0 # proportional gain of center of mass task
kp_posture = 1.0 # proportional gain of joint posture task
# configuration for viewer
# ----------------------------------------------
PRINT_N = 500 # print every PRINT_N time steps
DISPLAY_N = 20 # update robot configuration in viwewer every DISPLAY_N time steps
CAMERA_TRANSFORM = [4.0, -0.2, 0.4, 0.5243823528289795, 0.518651008605957, 0.4620114266872406, 0.4925136864185333]
SPHERE_RADIUS = 0.03
REF_SPHERE_RADIUS = 0.03
COM_SPHERE_COLOR = (1, 0.5, 0, 0.5)
COM_REF_SPHERE_COLOR = (1, 0, 0, 0.5)
RF_SPHERE_COLOR = (0, 1, 0, 0.5)
RF_REF_SPHERE_COLOR = (0, 1, 0.5, 0.5)
LF_SPHERE_COLOR = (0, 0, 1, 0.5)
LF_REF_SPHERE_COLOR = (0.5, 0, 1, 0.5)
filename = str(os.path.dirname(os.path.abspath(__file__)))
path = filename + '/../models/romeo'
urdf = path + '/urdf/romeo.urdf'
srdf = path + '/srdf/romeo_collision.srdf'
\ No newline at end of file
CAMERA_TRANSFORM = [3.578777551651001, 1.2937744855880737, 0.8885031342506409, 0.4116811454296112, 0.5468055009841919, 0.6109083890914917, 0.3978860676288605]
exercizes/ex_4_plan_LIPM_romeo.py
View file @ 58036e4a
......@@ -25,19 +25,14 @@ import LMPC_walking.second_order.motion_model as motion_model
import LMPC_walking.second_order.cost_function as cost_function
import LMPC_walking.second_order.constraints as constraints
import LMPC_walking.second_order.plot_utils as plot_utils
from LMPC_walking.second_order.motion_model import discrete_LIP_dynamics
import matplotlib.pyplot as plt
import ex_4_conf as conf
# Inverted pendulum parameters:
# ----------------------------
foot_length = conf.lxn + conf.lxp # foot size in the x-direction
foot_width = conf.lyn + conf.lyp # foot size in the y-direciton
nb_steps_per_T = int(round(conf.T_step/conf.dt_mpc))
# walking parameters:
# ------------------
N = conf.nb_steps * nb_steps_per_T # number of desired walking intervals
nb_dt_per_step = int(round(conf.T_step/conf.dt_mpc))
N = conf.nb_steps * nb_dt_per_step # number of desired walking intervals
# CoM initial state: [x_0, xdot_0].T
# [y_0, ydot_0].T
......@@ -49,16 +44,16 @@ step_width = 2*np.absolute(y_0[0])
# compute CoP reference trajectory:
# --------------------------------
desiredFoot_steps = reference_trajectories.manual_foot_placement(conf.foot_step_0,
foot_steps = reference_trajectories.manual_foot_placement(conf.foot_step_0,
conf.step_length, conf.nb_steps)
desiredFoot_steps[1:,0] -= conf.step_length
desired_Z_ref = reference_trajectories.create_CoP_trajectory(conf.nb_steps,
desiredFoot_steps, N, nb_steps_per_T)
foot_steps[1:,0] -= conf.step_length
cop_ref = reference_trajectories.create_CoP_trajectory(conf.nb_steps,
foot_steps, N, nb_dt_per_step)
# used in case you want to have terminal constraints
# -------------------------------------------------
x_terminal = np.array([desired_Z_ref[N-1, 0], 0.0]) # CoM terminal constraint in x : [x, xdot].T
y_terminal = np.array([desired_Z_ref[N-1, 1], 0.0]) # CoM terminal constraint in y : [y, ydot].T
x_terminal = np.array([cop_ref[N-1, 0], 0.0]) # CoM terminal constraint in x : [x, xdot].T
y_terminal = np.array([cop_ref[N-1, 1], 0.0]) # CoM terminal constraint in y : [y, ydot].T
nb_terminal_constraints = 4
terminal_index = N-1
......@@ -67,10 +62,10 @@ terminal_index = N-1
[P_ps, P_vs, P_pu, P_vu] = motion_model.compute_recursive_matrices(conf.dt_mpc, conf.g,
conf.h, N)
[Q, p_k] = cost_function.compute_objective_terms(conf.alpha, conf.beta, conf.gamma,
conf.T_step, nb_steps_per_T, N, conf.step_length, step_width,
P_ps, P_pu, P_vs, P_vu, x_0, y_0, desired_Z_ref)
conf.T_step, nb_dt_per_step, N, conf.step_length, step_width,
P_ps, P_pu, P_vs, P_vu, x_0, y_0, cop_ref)
[A_zmp, b_zmp] = constraints.add_ZMP_constraints(N, foot_length, foot_width,
desired_Z_ref, x_0, y_0)
cop_ref, x_0, y_0)
# used in case you want to add both terminal add_ZMP_constraints
# --------------------------------------------------------------
......@@ -83,159 +78,39 @@ b = np.concatenate((b_terminal, b_zmp), axis = 0)
# call quadprog solver:
# --------------------
U = solve_qp(Q, -p_k, A.T, b, nb_terminal_constraints)[0] # uncomment to solve with 4 equality terminal constraints
#U = solve_qp(Q, -p_k, A_zmp.T, b_zmp)[0] # solve only with only CoP inequality constraints
Z_x_total = U[0:N]
Z_y_total = U[N:2*N]
cop_x = U[0:N]
cop_y = U[N:2*N]
# Trajectory optimization: (based on the initial state x_hat_0, y_hat_0)
# -------------------------------------------------------------------------
[X_total, Y_total] = motion_model.compute_recursive_dynamics(P_ps, P_vs, P_pu,
[com_state_x, com_state_y] = motion_model.compute_recursive_dynamics(P_ps, P_vs, P_pu,
P_vu, N, x_0,
y_0, U)
X_total = np.vstack((x_0, X_total))
Y_total = np.vstack((y_0, Y_total))
# ------------------------------------------------------------------------------
# visualize your open-loop trajectory:
# ------------------------------------------------------------------------------
time = np.arange(0, round(N*conf.dt_mpc, 2), conf.dt_mpc)
min_admissible_CoP = desired_Z_ref - np.tile([foot_length/2, foot_width/2],
(N,1))
max_admissible_cop = desired_Z_ref + np.tile([foot_length/2, foot_width/2],
(N,1))
min_admissible_CoP = cop_ref - np.tile([foot_length/2, foot_width/2], (N,1))
max_admissible_cop = cop_ref + np.tile([foot_length/2, foot_width/2], (N,1))
# time vs CoP and CoM in x: 'A.K.A run rabbit run !'
# -------------------------------------------------
#plot_utils.plot_x(time, N, min_admissible_CoP,
# max_admissible_cop, Z_x_total, X_total, desired_Z_ref)
#
## time VS CoP and CoM in y: 'A.K.A what goes up must go down'
## ----------------------------------------------------------
#plot_utils.plot_y(time, N, min_admissible_CoP,
# max_admissible_cop, Z_y_total, Y_total, desired_Z_ref)
#
## plot CoP, CoM in x Vs Cop, CoM in y:
## -----------------------------------
#plot_utils.plot_xy(time, N, foot_length, foot_width,
# desired_Z_ref, Z_x_total, Z_y_total, X_total, Y_total)
dt_ctrl = conf.dt
N_ctrl = int((N*conf.dt_mpc)/dt_ctrl)
(A,B) = discrete_LIP_dynamics(dt_ctrl, conf.g, conf.h)
com = np.empty((3,N_ctrl+1))*np.nan
dcom = np.zeros((3,N_ctrl+1))
ddcom = np.zeros((3,N_ctrl+1))
cop = np.empty((2,N_ctrl+1))*np.nan
#foot_steps = np.empty((2,N_ctrl+1))*np.nan
contact_phase = (N_ctrl+1)*['right']
com[2,:] = conf.h
N_inner = int(N_ctrl/N)
for i in range(N):
com[0,i*N_inner] = X_total[i,0]
com[1,i*N_inner] = Y_total[i,0]
dcom[0,i*N_inner] = X_total[i,1]
dcom[1,i*N_inner] = Y_total[i,1]
if(i>0):
if np.linalg.norm(desired_Z_ref[i,:] - desired_Z_ref[i-1,:]) < 1e-10:
contact_phase[i*N_inner] = contact_phase[i*N_inner-1]
else:
if contact_phase[(i-1)*N_inner]=='right':
contact_phase[i*N_inner] = 'left'
elif contact_phase[(i-1)*N_inner]=='left':
contact_phase[i*N_inner] = 'right'
for j in range(N_inner):
ii = i*N_inner + j
(A,B) = discrete_LIP_dynamics((j+1)*dt_ctrl, conf.g, conf.h)
# foot_steps[:,ii] = desired_Z_ref[i,:].T
cop[0,ii] = Z_x_total[i]
cop[1,ii] = Z_y_total[i]
x_next = A.dot(X_total[i,:]) + B.dot(cop[0,ii])
y_next = A.dot(Y_total[i,:]) + B.dot(cop[1,ii])
com[0,ii+1] = x_next[0]
com[1,ii+1] = y_next[0]
dcom[0,ii+1] = x_next[1]
dcom[1,ii+1] = y_next[1]
ddcom[:2,ii] = conf.g/conf.h * (com[:2,ii] - cop[:,ii])
if(j>0): contact_phase[ii] = contact_phase[ii-1]
# Generate foot trajectories using polynomials with following constraints:
# x(0)=x0, x(T)=x1, dx(0)=dx(T)=0
# x(t) = a + b t + c t^2 + d t^3
# x(0) = a = x0
# dx(0) = b = 0
# dx(T) = 2 c T + 3 d T^2 = 0 => c = -3 d T^2 / (2 T) = -(3/2) d T
# x(T) = x0 + c T^2 + d T^3 = x1
# x0 -(3/2) d T^3 + d T^3 = x1
# -0.5 d T^3 = x1 - x0
# d = 2 (x0-x1) / T^3
# c = -(3/2) T 2 (x0-x1) / (T^3) = 3 (x1-x0) / T^2
def compute_polynomial_traj(x0, x1, T, dt):
a = x0
b = np.zeros_like(x0)
c = 3*(x1-x0) / (T**2)
d = 2*(x0-x1) / (T**3)
N = int(T/dt)
n = x0.shape[0]
x = np.zeros((n,N))
dx = np.zeros((n,N))
ddx = np.zeros((n,N))
for i in range(N):
t = i*dt
x[:,i] = a + b*t + c*t**2 + d*t**3
dx[:,i] = b + 2*c*t + 3*d*t**2
ddx[:,i] = 2*c + 6*d*t
return x, dx, ddx
foot_steps = desiredFoot_steps[::2,:]
#x = np.zeros((3,N_ctrl+1))
#dx = np.zeros((3,N_ctrl+1))
#ddx = np.zeros((3,N_ctrl+1))
#N_step = int(step_time/dt_ctrl)
#for s in range(foot_steps.shape[0]):
# x[0, s*2*N_step :s*2*N_step+N_step] = foot_steps[s,0]
# x[1, s*2*N_step :s*2*N_step+N_step] = foot_steps[s,1]
# x[:2, s*2*N_step+N_step:(s+1)*2*N_step] = compute_polynomial_traj(foot_steps[s,:], foot_steps[s+1,:],
# step_time, dt)
#np.savez('3_step_walking_traj', x=X_total, y=Y_total, z=desired_Z_ref)
np.savez(conf.DATA_FILE, com=com, dcom=dcom, ddcom=ddcom,
contact_phase=contact_phase, foot_steps=foot_steps)
time_ctrl = np.arange(0, round(N_ctrl*dt_ctrl, 2), dt_ctrl)
for i in range(2):
plt.figure()
plt.plot(time_ctrl, cop[i,:-1], label='CoP')
# plt.plot(time_ctrl, foot_steps[i,:-1], label='Foot step')
plt.plot(time_ctrl, com[i,:-1], 'g', label='CoM')
if i==0: plt.plot(time, X_total[:-1,0], ':', label='CoM TO')
else: plt.plot(time, Y_total[:-1,0], ':', label='CoM TO')
plt.legend()
for i in range(2):
plt.figure()
plt.plot(time_ctrl, dcom[i,:-1], label='CoM vel')
vel_fd = (com[i,1:] - com[i,:-1]) / dt_ctrl
plt.plot(time_ctrl, vel_fd, ':', label='CoM vel fin-diff')
plt.legend()
for i in range(2):
plt.figure()
plt.plot(time_ctrl, ddcom[i,:-1], label='CoM acc')
acc_fd = (dcom[i,1:] - dcom[i,:-1]) / dt_ctrl
plt.plot(time_ctrl, acc_fd, ':', label='CoM acc fin-diff')
plt.legend()
#plot_utils.plot_xy(time_ctrl, N_ctrl, foot_length, foot_width,
# foot_steps.T, cop[0,:], cop[1,:],
# com[0,:].reshape((N_ctrl+1,1)),
# com[1,:].reshape((N_ctrl+1,1)))
#plt.plot( X_total[:,0], Y_total[:,0], 'r* ', markersize=15,)
plot_utils.plot_x(time, N, min_admissible_CoP,
max_admissible_cop, cop_x, com_state_x, cop_ref)
# time VS CoP and CoM in y: 'A.K.A what goes up must go down'
# ----------------------------------------------------------
plot_utils.plot_y(time, N, min_admissible_CoP,
max_admissible_cop, cop_y, com_state_y, cop_ref)
# plot CoP, CoM in x Vs Cop, CoM in y:
# -----------------------------------
plot_utils.plot_xy(time, N, foot_length, foot_width,
cop_ref, cop_x, cop_y, com_state_x, com_state_y)
com_state_x = np.vstack((x_0, com_state_x))
com_state_y = np.vstack((y_0, com_state_y))
np.savez(conf.DATA_FILE_LIPM,
com_state_x=com_state_x, com_state_y=com_state_y, cop_ref=cop_ref,
cop_x=cop_x, cop_y=cop_y, foot_steps=foot_steps)
exercizes/ex_4_walking.py
View file @ 58036e4a
......@@ -12,7 +12,7 @@ print "".center(conf.LINE_WIDTH,'#')
print " Test Walking ".center(conf.LINE_WIDTH, '#')
print "".center(conf.LINE_WIDTH,'#'), '\n'
data = np.load(conf.DATA_FILE)
data = np.load(conf.DATA_FILE_TSID)
tsid = TsidBiped(conf)
......@@ -25,21 +25,24 @@ f_LF = matlib.zeros((6, N))
cop_RF = matlib.zeros((2, N))
cop_LF = matlib.zeros((2, N))
foot_steps = np.asmatrix(data['foot_steps'])
contact_phase = data['contact_phase']
com_pos_ref = np.asmatrix(data['com'])
com_vel_ref = np.asmatrix(data['dcom'])
com_acc_ref = np.asmatrix(data['ddcom'])*1.0
com_acc_ref = np.asmatrix(data['ddcom'])
x_RF_ref = np.asmatrix(data['x_RF'])
dx_RF_ref = np.asmatrix(data['dx_RF'])
ddx_RF_ref = np.asmatrix(data['ddx_RF'])
x_LF_ref = np.asmatrix(data['x_LF'])
dx_LF_ref = np.asmatrix(data['dx_LF'])
ddx_LF_ref = np.asmatrix(data['ddx_LF'])
com_acc_des = matlib.empty((3, N))*nan # acc_des = acc_ref - Kp*pos_err - Kd*vel_err
x_rf = tsid.get_placement_RF().translation
offset = matlib.zeros((3,1))
offset[:2,0] = x_rf[:2,0] - foot_steps[0,:].T #tsid.robot.com(tsid.formulation.data())
offset = x_rf - x_RF_ref[:,0]
for i in range(N):
com_pos_ref[:,i] += offset
# foot_steps[:,i] += offset[:2,0]
sampleCom = tsid.trajCom.computeNext()
x_RF_ref[:,i] += offset
x_LF_ref[:,i] += offset
t = 0.0
q, v = tsid.q, tsid.v
......@@ -52,7 +55,7 @@ for i in range(-2000, N):
tsid.remove_contact_LF()
elif i>0:
if contact_phase[i] != contact_phase[i-1]:
print "Changing contact phase from %s to %s"%(contact_phase[i-1], contact_phase[i])
print "Time %.3f Changing contact phase from %s to %s"%(t, contact_phase[i-1], contact_phase[i])
if contact_phase[i] == 'left':
tsid.add_contact_LF()
tsid.remove_contact_RF()
......@@ -61,15 +64,11 @@ for i in range(-2000, N):
tsid.remove_contact_LF()
if i<0:
sampleCom.pos(com_pos_ref[:,0])
tsid.set_com_ref(com_pos_ref[:,0], 0*com_vel_ref[:,0], 0*com_acc_ref[:,0])
else:
sampleCom.pos(com_pos_ref[:,i])
sampleCom.vel(com_vel_ref[:,i])
sampleCom.acc(com_acc_ref[:,i])
tsid.comTask.setReference(sampleCom)
tsid.rightFootTask.setReference(tsid.trajRF.computeNext())
tsid.leftFootTask.setReference(tsid.trajLF.computeNext())
tsid.set_com_ref(com_pos_ref[:,i], com_vel_ref[:,i], com_acc_ref[:,i])
tsid.set_LF_3d_ref(x_LF_ref[:,i], dx_LF_ref[:,i], ddx_LF_ref[:,i])
tsid.set_RF_3d_ref(x_RF_ref[:,i], dx_RF_ref[:,i], ddx_RF_ref[:,i])
HQPData = tsid.formulation.computeProblemData(t, q, v)
......@@ -133,24 +132,24 @@ for i in range(3):
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
(f, ax) = plut.create_empty_figure(3,1)
for i in range(3):
ax[i].plot(time, com_vel[i,:].A1, label='CoM Vel '+str(i))
ax[i].plot(time, com_vel_ref[i,:].A1, 'r:', label='CoM Vel Ref '+str(i))
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('CoM Vel [m/s]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
(f, ax) = plut.create_empty_figure(3,1)
for i in range(3):
ax[i].plot(time, com_acc[i,:].A1, label='CoM Acc '+str(i))
ax[i].plot(time, com_acc_ref[i,:].A1, 'r:', label='CoM Acc Ref '+str(i))
ax[i].plot(time, com_acc_des[i,:].A1, 'g--', label='CoM Acc Des '+str(i))
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('CoM Acc [m/s^2]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
#(f, ax) = plut.create_empty_figure(3,1)
#for i in range(3):
# ax[i].plot(time, com_vel[i,:].A1, label='CoM Vel '+str(i))
# ax[i].plot(time, com_vel_ref[i,:].A1, 'r:', label='CoM Vel Ref '+str(i))
# ax[i].set_xlabel('Time [s]')
# ax[i].set_ylabel('CoM Vel [m/s]')
# leg = ax[i].legend()
# leg.get_frame().set_alpha(0.5)
#
#(f, ax) = plut.create_empty_figure(3,1)
#for i in range(3):
# ax[i].plot(time, com_acc[i,:].A1, label='CoM Acc '+str(i))
# ax[i].plot(time, com_acc_ref[i,:].A1, 'r:', label='CoM Acc Ref '+str(i))
# ax[i].plot(time, com_acc_des[i,:].A1, 'g--', label='CoM Acc Des '+str(i))
# ax[i].set_xlabel('Time [s]')
# ax[i].set_ylabel('CoM Acc [m/s^2]')
# leg = ax[i].legend()
# leg.get_frame().set_alpha(0.5)
(f, ax) = plut.create_empty_figure(2,1)
for i in range(2):
......@@ -161,14 +160,14 @@ for i in range(2):
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
(f, ax) = plut.create_empty_figure(3,2)
ax = ax.reshape((6))
for i in range(6):
ax[i].plot(time, f_LF[i,:].A1, label='Force LF '+str(i))
ax[i].plot(time, f_RF[i,:].A1, label='Force RF '+str(i))
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('Force [N/Nm]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
#(f, ax) = plut.create_empty_figure(3,2)
#ax = ax.reshape((6))
#for i in range(6):
# ax[i].plot(time, f_LF[i,:].A1, label='Force LF '+str(i))
# ax[i].plot(time, f_RF[i,:].A1, label='Force RF '+str(i))
# ax[i].set_xlabel('Time [s]')
# ax[i].set_ylabel('Force [N/Nm]')
# leg = ax[i].legend()
# leg.get_frame().set_alpha(0.5)
plt.show()
exercizes/tsid_biped.py
View file @ 58036e4a
......@@ -46,7 +46,7 @@ class TsidBiped:
H_rf_ref = robot.position(data, self.RF)
# modify initial robot configuration so that foot is on the ground (z=0)
q[2] -= H_rf_ref.translation[2,0]
q[2] -= H_rf_ref.translation[2,0] - conf.lz
formulation.computeProblemData(0.0, q, v)
data = formulation.data()
H_rf_ref = robot.position(data, self.RF)
......@@ -85,23 +85,31 @@ class TsidBiped:
formulation.addMotionTask(self.rightFootTask, self.conf.w_foot, 1, 0.0)
com_ref = robot.com(data)
trajCom = tsid.TrajectoryEuclidianConstant("traj_com", com_ref)
self.trajCom = tsid.TrajectoryEuclidianConstant("traj_com", com_ref)
self.sample_com = self.trajCom.computeNext()
q_ref = q[7:]
trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", q_ref)
postureTask.setReference(trajPosture.computeNext())
self.trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", q_ref)
postureTask.setReference(self.trajPosture.computeNext())
solver = tsid.SolverHQuadProgFast("qp solver")
solver.resize(formulation.nVar, formulation.nEq, formulation.nIn)
self.sampleLF = self.trajLF.computeNext()
self.sample_LF_pos = self.sampleLF.pos()
self.sample_LF_vel = self.sampleLF.vel()
self.sample_LF_acc = self.sampleLF.acc()
self.sampleRF = self.trajRF.computeNext()
self.sample_RF_pos = self.sampleRF.pos()
self.sample_RF_vel = self.sampleRF.vel()
self.sample_RF_acc = self.sampleRF.acc()
self.solver = tsid.SolverHQuadProgFast("qp solver")
self.solver.resize(