Add foot trajectories to ex 4

exercizes/ex_4_LIPM_to_TSID.py

+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

@@ -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

@@ -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

@@ -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

@@ -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")