Update tests to evaluate MPCs

scripts/crocoddyl_eval/test_1/analyse_control_cycle.py

@@ -11,11 +11,12 @@ import libquadruped_reactive_walking as lqrw
 import crocoddyl_class.MPC_crocoddyl as MPC_crocoddyl
 import crocoddyl
 import utils_mpc
+np.set_printoptions(formatter={'float': lambda x: "{0:0.7f}".format(x)}, linewidth = 450)
 
 ##############
 #  Parameters
 ##############
-iteration_mpc = 2 # Control cycle
+iteration_mpc = 350  # Control cycle
 Relaunch_DDP = True # Compare a third MPC with != parameters
 linear_mpc = True
 params = lqrw.Params()  # Object that holds all controller parameters
@@ -29,7 +30,7 @@ solo = utils_mpc.init_robot(q_init, params)
 ######################
 # Recover Logged data 
 ######################
-file_name = "crocoddyl_eval/logs/chute_mpc_nl_noShoulder.npz"
+file_name = "/home/thomas_cbrs/Desktop/edin/quadruped-reactive-walking/scripts/crocoddyl_eval/logs/explore_weight_acc/data_cost.npz"
 logs = np.load(file_name)
 planner_gait = logs.get("planner_gait")
 planner_xref = logs.get("planner_xref")
@@ -42,14 +43,14 @@ k = int( iteration_mpc * (params.dt_mpc / params.dt_wbc) ) # simulation iteratio
 mpc_osqp = lqrw.MPC(params)
 mpc_osqp.run(0, planner_xref[0] , planner_fsteps[0]) # Initialization of the matrix
 # mpc_osqp.run(1, planner_xref[1] , planner_fsteps[1])
-# mpc_osqp.run(k, planner_xref[k] , planner_fsteps[k])
+mpc_osqp.run(k, planner_xref[k] , planner_fsteps[k])
 
 osqp_xs = mpc_osqp.get_latest_result()[:12,:] # States computed over the whole predicted horizon
 osqp_xs = np.vstack([planner_xref[k,:,0] , osqp_xs.transpose()]).transpose() # Add current state 
 osqp_us = mpc_osqp.get_latest_result()[12:,:] # Forces computed over the whole predicted horizon
 
 # DDP MPC 
-mpc_ddp = MPC_crocoddyl.MPC_crocoddyl(params, mu=0.9, inner=False, linearModel=False)
+mpc_ddp = MPC_crocoddyl.MPC_crocoddyl(params, mu=0.9, inner=False, linearModel=True)
 # Without warm-start :
 # mpc_ddp.warm_start = False
 # mpc_ddp.solve(k, planner_xref[k] , planner_fsteps[k] ) # Without warm-start
@@ -57,8 +58,14 @@ mpc_ddp = MPC_crocoddyl.MPC_crocoddyl(params, mu=0.9, inner=False, linearModel=F
 # Using warm-start logged :
 #( Exactly the function solve from MPC_crocoddyl)
 # But cannot be used because there areis no previous value inside ddp.xs and ddp.us
-mpc_ddp.updateProblem( planner_fsteps[k], planner_xref[k]) # Update dynamics
+planner_xref_offset =  planner_xref[k].copy()
+planner_xref_offset[2,:] += mpc_ddp.offset_com
+mpc_ddp.updateProblem( planner_fsteps[k], planner_xref_offset) # Update dynamics
 
+
+print("\n\n")
+print("model 0 :")
+print(mpc_ddp.problem.runningModels[0].B)
 x_init = []
 u_init = []
 mpc_ddp.ddp.solve(x_init,  u_init, mpc_ddp.max_iteration)
@@ -70,28 +77,30 @@ while planner_gait[k,i,:].any() :
     i +=1
 
 for j in range(mpc_x_f[0,:,:].shape[1] - 1 ) : 
-    u_init.append(mpc_x_f[k_previous,12:,j+1]) # Drag through an iteration (remove first)
+    u_init.append(mpc_x_f[k_previous,12:24,j+1]) # Drag through an iteration (remove first)
 u_init.append(np.repeat(planner_gait[k,i-1,:], 3)*np.array(4*[0.5, 0.5, 5.])) # Add last node with average values, depending on the gait
 
 for j in range(mpc_x_f[0,:,:].shape[1] - 1 ) : 
     x_init.append(mpc_x_f[k_previous,:12,j+1]) # Drag through an iteration (remove first)
 x_init.append(mpc_x_f[k_previous,:12,-1]) # repeat last term 
-x_init.insert(0, planner_xref[k,:, 0]) # With ddp, first value is the initial value 
+x_init.insert(0, planner_xref_offset[:, 0]) # With ddp, first value is the initial value 
 
 # Solve problem
 mpc_ddp.ddp.solve(x_init,  u_init, mpc_ddp.max_iteration)
 
 ddp_xs = mpc_ddp.get_latest_result()[:12,:] # States computed over the whole predicted horizon 
-ddp_xs = np.vstack([planner_xref[k,:,0] , ddp_xs.transpose()]).transpose() # Add current state 
+ddp_xs = np.vstack([planner_xref_offset[:,0] , ddp_xs.transpose()]).transpose() # Add current state 
 ddp_us = mpc_ddp.get_latest_result()[12:,:] # Forces computed over the whole predicted horizon
 
 ######################################
 # Relaunch DDP to adjust the gains 
 ######################################
 
-mpc_ddp_2 = MPC_crocoddyl.MPC_crocoddyl(params, mu=0.9, inner=False  , linearModel=False) # To modify the linear model if wanted, recreate a list with proper model
+mpc_ddp_2 = MPC_crocoddyl.MPC_crocoddyl(params, mu=0.9, inner=False  , linearModel=True) # To modify the linear model if wanted, recreate a list with proper model
 mpc_ddp_2.implicit_integration = False
-mpc_ddp_2.shoulderWeights = 0.
+mpc_ddp_2.offset_com = 0.0
+# mpc_ddp_2.stateWeights = np.sqrt([2.0, 2.0, 20.0, 0.25, 0.25, 1.0, 0.2, 0.2, 0.2, 0.0, 0.0, 0.3])
+# mpc_ddp_2.shoulderWeights = 1.
 # Weight Vector : State 
 # w_x = 0.2
 # w_y = 0.2
@@ -142,12 +151,15 @@ mpc_ddp_2.problem = crocoddyl.ShootingProblem(np.zeros(12),  mpc_ddp_2.ListActio
 mpc_ddp_2.ddp = crocoddyl.SolverDDP(mpc_ddp_2.problem)
 # Run ddp solver
 # Update the dynamic depending on the predicted feet position
+planner_xref_offset =  planner_xref[k]
+planner_xref_offset[2,:] += mpc_ddp_2.offset_com
+
 if Relaunch_DDP :
-    mpc_ddp_2.updateProblem( planner_fsteps[k], planner_xref[k])
+    mpc_ddp_2.updateProblem( planner_fsteps[k], planner_xref_offset)
     mpc_ddp_2.ddp.solve(x_init,  u_init, mpc_ddp_2.max_iteration, isFeasible = False)
     
     ddp_xs_relaunch = mpc_ddp_2.get_latest_result()[:12,:] # States computed over the whole predicted horizon 
-    ddp_xs_relaunch = np.vstack([planner_xref[k,:,0] , ddp_xs_relaunch.transpose()]).transpose() # Add current state 
+    ddp_xs_relaunch = np.vstack([planner_xref_offset[:,0] , ddp_xs_relaunch.transpose()]).transpose() # Add current state 
     ddp_us_relaunch = mpc_ddp_2.get_latest_result()[12:,:] # Forces computed over the whole predicted horizon
 
 #############

scripts/crocoddyl_eval/test_2/unittest_augmented.py

@@ -16,6 +16,9 @@ model = quadruped_walkgen.ActionModelQuadrupedAugmented()
 model.stateWeights = 2*np.ones(12)
 model.heuristicWeights = 2*np.ones(8)
 model.stepWeights = np.ones(8)
+model.shoulderContactWeight = np.array([0.5,0.1,0.9,0.])
+model.shoulderReferencePosition = False
+model.max_fz = 0.5
 
 # Update the dynamic of the model 
 fstep = np.random.rand(12).reshape((3,4))

scripts/crocoddyl_eval/test_2/unittest_step.py

@@ -17,11 +17,36 @@ model.stateWeights = 2*np.ones(12)
 model.heuristicWeights = 2*np.ones(8)
 model.stepWeights = np.ones(8)
 
+model.is_acc_activated = False
+model.acc_limit = np.array([0.09,0.])
+model.acc_weight = 1.88
+
+model.is_vel_activated = True
+model.vel_limit = np.array([0.,0.])
+model.vel_weight = 0.0015
+
+model.is_jerk_activated = True
+model.jerk_weight = 0.000001
+
+
 # Update the dynamic of the model 
 fstep = np.random.rand(12).reshape((3,4))
 xref = np.random.rand(12)
 gait = np.random.randint(2, size=4)
-model.updateModel(fstep, xref , gait)
+position = np.random.rand(12).reshape((3,4))
+velocity = np.random.rand(12).reshape((3,4))
+acceleration = np.random.rand(12).reshape((3,4))
+jerk = np.random.rand(12).reshape((3,4))
+oRh = np.zeros((3,3))
+oTh = np.zeros((3,1))
+oRh[0,0] = 0.88
+oRh[1,1] = 0.88
+oRh[0,1] = -0.2
+oRh[1,0] = 0.2
+oTh[0,0] = 0.1
+oTh[1,0] = 0.09
+Dt = 0.16
+model.updateModel(fstep, xref , gait, position, velocity, acceleration, jerk, oRh, oTh, Dt )
 
 ################################################
 ## CHECK DERIVATIVE WITH NUM_DIFF 
@@ -29,7 +54,7 @@ model.updateModel(fstep, xref , gait)
 
 a = 1
 b = -1 
-N_trial = 50
+N_trial = 1
 epsilon = 10e-6
 
 model_diff = crocoddyl.ActionModelNumDiff(model)
@@ -63,7 +88,12 @@ def run_calcDiff_numDiff(epsilon) :
     fstep = np.random.rand(12).reshape((3,4))
     xref = np.random.rand(12)
     gait = np.random.randint(2, size=4)
-    model.updateModel(fstep, xref , gait)
+    position = np.random.rand(12).reshape((3,4))
+    velocity = np.random.rand(12).reshape((3,4))
+    acceleration = np.random.rand(12).reshape((3,4))
+    jerk = np.random.rand(12).reshape((3,4))
+    Dt = 0.16
+    model.updateModel(fstep, xref , gait, position, velocity, acceleration, jerk, oRh, oTh, Dt )
     model_diff = crocoddyl.ActionModelNumDiff(model)     
    
     # Run calc & calcDiff function : numDiff     
@@ -78,6 +108,11 @@ def run_calcDiff_numDiff(epsilon) :
     Lx_err += np.sum( abs((data.Lx - data_diff.Lx )) )  
 
     Lu +=  np.sum( abs((data.Lu - data_diff.Lu )) >= epsilon  ) 
+    print("\n")
+    print("Lu :")
+    print(data.Lu)
+    print("Lu diff :")
+    print(data_diff.Lu )
     Lu_err += np.sum( abs((data.Lu - data_diff.Lu )) )  
 
     Lxu +=  np.sum( abs((data.Lxu - data_diff.Lxu )) >= epsilon  ) 

scripts/crocoddyl_eval/test_6/compare_mpcs.py

@@ -88,7 +88,6 @@ for j in range(4):
         for t in range(N):
             if np.isnan(data_[t,i,j]):
                 data_[t,i,j] = data_[t-1,i,j]
-
 ##########
 # PLOTS 
 ##########
@@ -166,7 +165,7 @@ for i in range(6):
     
     for j in range(4):
         if MPCs[j]:
-            plt.plot(t_range, data_diff[:,index_error[i],j], color[j], linewidth=3)    
+            plt.plot(t_range, abs(data_diff[:,index_error[i],j]), color[j], linewidth=3)    
 
 # Add mean on graph
 # for i in range(6):
@@ -177,7 +176,7 @@ for i in range(6):
 
     plt.legend(legend, prop={'size': 8})
     plt.ylabel(lgd[i])
-plt.suptitle("Error wrt reference state")
+plt.suptitle("Absolute Error wrt reference state")
 
 plt.figure()
 for i in range(6):
@@ -212,7 +211,7 @@ bars = []
 bars_names = ["Lin", "NL", "Plan", "OSQP"]
 for j in range(4):
     if MPCs[j]:
-        bars.append(bars_names[j])
+        bars.append(MPCs_names[j])
 
 plt.figure()
 for i in range(6):
@@ -256,30 +255,30 @@ plt.suptitle("NORMALIZED RMSE -MEAN: sqrt(  (mes - ref - mean(mes-ref))  **2).me
 ####
 # FF torques & FB torques & Sent torques & Meas torques
 ####
-index12 = [1, 5, 9, 2, 6, 10, 3, 7, 11, 4, 8, 12]
-lgd1 = ["HAA", "HFE", "Knee"]
-lgd2 = ["FL", "FR", "HL", "HR"]
-plt.figure()
-my_axs = []
-for i in range(12):
-    if i == 0:
-        ax = plt.subplot(3, 4, index12[i])
-        my_axs.append(ax)
-    elif i in [1, 2]:
-        ax = plt.subplot(3, 4, index12[i], sharex=my_axs[0])
-        my_axs.append(ax)
-    else:
-        plt.subplot(3, 4, index12[i], sharex=my_axs[0], sharey=my_axs[int(i % 3)])
+# index12 = [1, 5, 9, 2, 6, 10, 3, 7, 11, 4, 8, 12]
+# lgd1 = ["HAA", "HFE", "Knee"]
+# lgd2 = ["FL", "FR", "HL", "HR"]
+# plt.figure()
+# my_axs = []
+# for i in range(12):
+#     if i == 0:
+#         ax = plt.subplot(3, 4, index12[i])
+#         my_axs.append(ax)
+#     elif i in [1, 2]:
+#         ax = plt.subplot(3, 4, index12[i], sharex=my_axs[0])
+#         my_axs.append(ax)
+#     else:
+#         plt.subplot(3, 4, index12[i], sharex=my_axs[0], sharey=my_axs[int(i % 3)])
 
-    for j in range(4):
-        if MPCs[j]:
-            plt.plot(t_range, tau_ff_[:,i,j], color[j], linewidth=3)
+#     for j in range(4):
+#         if MPCs[j]:
+#             plt.plot(t_range, tau_ff_[:,i,j], color[j], linewidth=3)
 
-    plt.xlabel("Time [s]")
-    plt.ylabel(lgd1[i % 3]+" "+lgd2[int(i/3)]+" [Nm]")
-    tmp = lgd1[i % 3]+" "+lgd2[int(i/3)]
-    plt.legend(legend, prop={'size': 8})
-    plt.ylim([-8.0, 8.0])
+#     plt.xlabel("Time [s]")
+#     plt.ylabel(lgd1[i % 3]+" "+lgd2[int(i/3)]+" [Nm]")
+#     tmp = lgd1[i % 3]+" "+lgd2[int(i/3)]
+#     plt.legend(legend, prop={'size': 8})
+#     plt.ylim([-8.0, 8.0])
 
 ####
 # Contact forces (MPC command) & WBC QP output