fixed offset in y

fixed bug in initial guess saving

added plot on  predicted foot position

python/quadruped_reactive_walking/Controller.py

@@ -98,14 +98,16 @@ class Controller:
         try:
             self.mpc.solve(self.k, m["x_m"], self.guess)  # Closed loop mpc
 
-            # Trajectory tracking
+            ## Trajectory tracking
             #if self.initialized:
             #   self.mpc.solve(self.k, self.mpc_result.x[1], self.guess)
             #else:
             #   self.mpc.solve(self.k, m["x_m"], self.guess)
 
-            ### ONLY IF YOU WANT TO STORE THE FIRST SOLUTION TO WARMSTART THE INITIAL Problem ###
-            #np.save(open('/tmp/init_guess.npy', "wb"), {"xs": self.mpc.ocp.get_results().x, "us": self.mpc.ocp.get_results().u} )
+            # ### ONLY IF YOU WANT TO STORE THE FIRST SOLUTION TO WARMSTART THE INITIAL Problem ###
+            #if not self.initialized:
+            #    np.save(open('/tmp/init_guess.npy', "wb"), {"xs": self.mpc.ocp.get_results().x, "us": self.mpc.ocp.get_results().u} )
+            #    print("Initial guess saved")
 
         except ValueError:
             self.error = True

python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py

@@ -119,10 +119,8 @@ class OCP:
 
     def get_results(self):
         self.results.x = self.ddp.xs.tolist()
-        self.results.a = self.get_croco_acc()
         self.results.u = self.ddp.us.tolist()
         self.results.K = self.ddp.K
-        self.results.solver_time = self.t_solve
         return self.results
 
     def get_croco_forces(self):

python/quadruped_reactive_walking/WB_MPC/ProblemData.py

@@ -9,7 +9,7 @@ class problemDataAbstract:
         self.dt_bldc = 0.0005
         self.r1 = int(self.dt / self.dt_sim)
         self.init_steps = 0
-        self.target_steps =  120
+        self.target_steps =  90
         self.T = self.init_steps + self.target_steps -1
 
         self.robot = erd.load("solo12")
@@ -116,7 +116,7 @@ class ProblemDataFull(problemDataAbstract):
         self.foot_tracking_w = 1e3
         #self.friction_cone_w = 1e3 * 0
         self.control_bound_w = 1e3
-        self.control_reg_w = 1e1
+        self.control_reg_w = 1e0
         self.state_reg_w = np.array([1e-3] * 3 + [5*1e-1]*3)
         self.terminal_velocity_w = np.array([0] * 3 + [1e3] * 3 )
 

python/quadruped_reactive_walking/WB_MPC/Target.py

@@ -29,7 +29,7 @@ class Target:
         pin.updateFramePlacements(pd.model, pd.rdata)
         self.FR_foot0 = pd.rdata.oMf[pd.rfFootId].translation.copy()
         self.A = np.array([0, 0.03, 0.03])
-        self.offset = np.array([0.08, 0.0, 0.06])
+        self.offset = np.array([0.08, 0, 0.06])
         self.freq = np.array([0, 2.5*0, 2.5*0])
         self.phase = np.array([0, 0, np.pi / 2])
 

python/quadruped_reactive_walking/main_solo12_control.py

@@ -180,6 +180,7 @@ def control_loop():
         target.shift_gait()
         if controller.compute(device, qc):
             break
+
         if t <= 10 * params.dt_wbc and check_position_error(device, controller):
             break
 

python/quadruped_reactive_walking/tools/LoggerControl.py

@@ -145,6 +145,13 @@ class LoggerControl:
     def plot(self, save=False, fileName="tmp/"):
         import matplotlib.pyplot as plt
 
+        x_mes = np.concatenate([self.q_mes[:, 3:6], self.v_mes[:, 3:6]], axis = 1)
+
+        x_mpc = [self.ocp_storage["xs"][0][0, :]]
+        [x_mpc.append(x[1, :]) for x in self.ocp_storage["xs"][:-1]]
+        x_mpc = np.array(x_mpc)
+
+        # Feet positions calcuilated by every ocp
         all_ocp_feet_p_log = {
             idx: [
                 get_translation_array(self.pd, x, idx)[0]
@@ -155,12 +162,19 @@ class LoggerControl:
         for foot in all_ocp_feet_p_log:
             all_ocp_feet_p_log[foot] = np.array(all_ocp_feet_p_log[foot])
 
-        x_mes = np.concatenate([self.q_mes[:, 3:6], self.v_mes[:, 3:6]], axis = 1)
-        feet_p_log = {
+        # Measured feet positions
+        m_feet_p_log = {
             idx: 
                 get_translation_array(self.pd, x_mes, idx)[0]
             for idx in self.pd.allContactIds
         }
+
+        # Predicted eet positions
+        feet_p_log = {
+            idx: 
+                get_translation_array(self.pd, x_mpc, idx)[0]
+            for idx in self.pd.allContactIds
+        }
         
         
 
@@ -228,8 +242,9 @@ class LoggerControl:
             plt.subplot(3,1, p+1)
             plt.title('Free foot on ' + legend[p])
             plt.plot(self.target[:, p])
+            plt.plot(m_feet_p_log[self.pd.rfFootId][:, p])
             plt.plot(feet_p_log[self.pd.rfFootId][:, p])
-            plt.legend(["Desired", "Measured"])
+            plt.legend(["Target", "Measured", "Predicted"])
 
         self.plot_controller_times()
         self.plot_OCP_times()