wrong interpolation

config/walk_parameters.yaml

@@ -22,9 +22,10 @@ robot:
     # q_init: [0.0, 0.764, -1.407, 0.0, 0.76407, -1.4, 0.0, 0.76407, -1.407, 0.0, 0.764, -1.407]  # h_com = 0.218
     q_init: [0.0, 0.7, -1.4, 0.0, 0.7, -1.4, 0.0, -0.7, 1.4, 0.0, -0.7, 1.4]  # Initial articular positions
     dt_wbc: 0.001  # Time step of the whole body control
-    dt_mpc: 0.001  # Time step of the model predictive control
+    dt_mpc: 0.015  # Time step of the model predictive control
     type_MPC: 3  # Which MPC solver you want to use: 0 for OSQP MPC, 1, 2, 3 for Crocoddyl MPCs
-    interpolate_mpc: false # true to interpolate the impedance quantities between nodes of the MPC
+    interpolate_mpc: true # true to interpolate the impedance quantities between nodes of the MPC
+    interpolation_type: 3 # 0,1,2,3 decide which kind of interpolation is used
 #     Kp_main: [0.0, 0.0, 0.0]  # Proportional gains for the PD+
     Kp_main: [1, 1, 1]  # Proportional gains for the PD+
 #     Kd_main: [0., 0., 0.]  # Derivative gains for the PD+

include/qrw/Params.hpp

@@ -93,6 +93,7 @@ class Params {
   int N_periods;                // Number of gait periods in the MPC prediction horizon
   int type_MPC;                 // Which MPC solver you want to use: 0 for OSQP MPC, 1, 2, 3 for Crocoddyl MPCs
   bool interpolate_mpc;         // true to interpolate the impedance quantities, otherwise integrate
+  int interpolation_type;         // true to interpolate the impedance quantities, otherwise integrate
   bool kf_enabled;              // Use complementary filter (False) or kalman filter (True) for the estimator
   std::vector<double> Kp_main;  // Proportional gains for the PD+
   std::vector<double> Kd_main;  // Derivative gains for the PD+

python/Params.cpp

@@ -27,6 +27,7 @@ struct ParamsVisitor : public bp::def_visitor<ParamsVisitor<Params>> {
         .def_readwrite("use_flat_plane", &Params::use_flat_plane)
         .def_readwrite("predefined_vel", &Params::predefined_vel)
         .def_readwrite("interpolate_mpc", &Params::interpolate_mpc)
+        .def_readwrite("interpolation_type", &Params::interpolation_type)
         .def_readwrite("kf_enabled", &Params::kf_enabled)
         .def_readwrite("Kp_main", &Params::Kp_main)
         .def_readwrite("Kd_main", &Params::Kd_main)

python/quadruped_reactive_walking/Controller.py

@@ -24,8 +24,8 @@ class Result:
 
 
 class Interpolation:
-    def __init__(self):
-        pass
+    def __init__(self, params):
+        self.params = params
 
     def load_data(self, q, v):
         self.v0 = v[0, :]
@@ -34,43 +34,47 @@ class Interpolation:
         self.q1 = q[1, :]
 
     def interpolate(self, t):
-        # Perfect match, but wrong
-        # if (self.q1-self.q0 == 0).any():
-        # alpha = np.zeros(len(self.q0))
-        # else:
-        # alpha = 2 * 1/2* (self.v1**2 - self.v0**2)/(self.q1 - self.q0)
+        # Linear
+        if self.params.interpolation_type == 0:
+            beta = self.v1
+            gamma = self.q0
 
-        # beta = self.v0
-        # gamma = self.q0
+            v_t = beta
+            q_t = gamma + beta * t
 
-        # v_t = beta + alpha * t
-        # q_t = gamma + beta * t + alpha * t**2
+        # Linear Wrong
+        if self.params.interpolation_type == 1:
+            beta = self.v1
+            gamma = self.q0
 
-        # Linear
-        beta = self.v1
-        gamma = self.q0
+            v_t = beta
+            q_t = gamma + beta * t
 
-        v_t = beta
-        q_t = gamma + beta * t
+        # Perfect match, but wrong
+        if self.params.interpolation_type == 2:
+            if (self.q1-self.q0 == 0).any():
+                alpha = np.zeros(len(self.q0))
+            else:
+                alpha = (self.v1**2 - self.v0**2)/(self.q1 - self.q0)
 
-        # Linear Wrong
-        # beta = self.v1
-        # gamma = self.q0
+            beta = self.v0
+            gamma = self.q0
 
-        # v_t = self.v0 + self.v1*(self.v1 - self.v0)/(self.q1 - self.q0) * t
-        # q_t = self.q0 + self.v1 * t
+            v_t = beta + alpha * t
+            q_t = gamma + beta * t + alpha * t**2     
 
         # Quadratic
-        # if (self.q1-self.q0 == 0).any():
-        #     alpha = np.zeros(len(self.q0))
-        # else:
-        #     alpha = self.v1*(self.v1 - self.v0)/(self.q1 - self.q0)
+        if self.params.interpolation_type == 3:
+            if (self.q1-self.q0 == 0).any():
+                alpha = np.zeros(len(self.q0))
+            else:
+                alpha = self.v1 *(self.v1- self.v0)/(self.q1 - self.q0)
 
-        # beta = self.v0
-        # gamma = self.q0
+            beta = self.v0
+            gamma = self.q0
 
-        # v_t = beta + alpha * t
-        # q_t = gamma + beta * t + 1/2 * alpha * t**2
+            v_t = beta + alpha * t
+            q_t = gamma + beta * t + 1/2 * alpha * t**2
 
         return q_t, v_t
 
@@ -119,7 +123,6 @@ class DummyDevice:
             self.velocities = np.zeros(12)
 
 
-
 class Controller:
     def __init__(self, pd, target, params, q_init, t):
         """Function that runs a simulation scenario based on a reference velocity profile, an environment and
@@ -144,7 +147,7 @@ class Controller:
         self.cnt_wbc = 0
         self.error = False
         self.initialized = False
-        self.interpolator = Interpolation()
+        self.interpolator = Interpolation(params)
         self.result = Result(params)
         self.result.q_des = self.pd.q0[7:].copy()
         self.result.v_des = self.pd.v0[6:].copy()
@@ -203,7 +206,8 @@ class Controller:
             # Keep only the actuated joints and set the other to default values
             self.result.FF = self.params.Kff_main * np.ones(12)
             actuated_tau_ff = self.compute_torque(m)
-            self.result.tau_ff = np.array([0] * 3 + list(actuated_tau_ff) + [0] * 6)
+            self.result.tau_ff = np.array(
+                [0] * 3 + list(actuated_tau_ff) + [0] * 6)
 
             if self.params.interpolate_mpc:
                 # load the data to be interpolated only once per mpc solution
@@ -211,9 +215,11 @@ class Controller:
                     x = np.array(self.mpc_result.xs)
                     self.interpolator.load_data(
                         x[:, : self.pd.nq], x[:, self.pd.nq:])
-                    
-                q, v = self.interpolator.interpolate((self.cnt_wbc +1) * self.pd.dt_wbc)
-                #self.interpolator.plot_interpolation(self.pd.r1, self.pd.dt_wbc)
+
+                q, v = self.interpolator.interpolate(
+                    (self.cnt_wbc + 1) * self.pd.dt_wbc)
+
+                self.interpolator.plot_interpolation(self.pd.r1, self.pd.dt_wbc)
             else:
                 q, v = self.integrate_x(m)
 
@@ -358,7 +364,7 @@ class Controller:
                     m["x_m"][: self.pd.nq],
                     self.mpc_result.xs[0][: self.pd.nq],
                 ),
-                m["x_m"][self.pd.nq :] - self.mpc_result.xs[0][self.pd.nq :],
+                m["x_m"][self.pd.nq:] - self.mpc_result.xs[0][self.pd.nq:],
             ]
         )
         tau = self.mpc_result.us[0] + np.dot(self.mpc_result.K[0], x_diff)

python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py

@@ -12,7 +12,7 @@ class OCP:
     def __init__(self, pd: ProblemData, target: Target):
         self.pd = pd
         self.target = target
-        self.max_iter=100
+        self.max_iter=1
 
         self.state = crocoddyl.StateMultibody(self.pd.model)
         self.initialized = False

python/quadruped_reactive_walking/WB_MPC/ProblemData.py

@@ -6,6 +6,7 @@ class problemDataAbstract:
     def __init__(self, param, frozen_names = []):
         self.dt = param.dt_mpc # OCP dt
         self.dt_wbc = param.dt_wbc
+        self.r1 = int(self.dt/self.dt_wbc)
         self.init_steps = 0
         self.target_steps =  150
         self.T = self.init_steps + self.target_steps -1
@@ -117,7 +118,7 @@ class ProblemDataFull(problemDataAbstract):
         #self.friction_cone_w = 1e3 * 0
         self.control_bound_w = 1e3
         self.control_reg_w = 1e0
-        self.state_reg_w = np.array([1e-2] * 3 + [1e0]*3)
+        self.state_reg_w = np.array([1e-5] * 3 + [1e0]*3)
         self.terminal_velocity_w = np.array([0] * 3 + [1e3] * 3 )
 
         self.q0_reduced = self.q0[10 : 13]

src/Params.cpp

@@ -24,6 +24,7 @@ Params::Params()
       N_periods(0),
       type_MPC(0),
       interpolate_mpc(true),
+      interpolation_type(0),
       kf_enabled(false),
       Kp_main(3, 0.0),
       Kd_main(3, 0.0),
@@ -142,6 +143,9 @@ void Params::initialize(const std::string& file_path) {
   assert_yaml_parsing(robot_node, "robot", "interpolate_mpc");
   interpolate_mpc = robot_node["interpolate_mpc"].as<bool>();
 
+  assert_yaml_parsing(robot_node, "robot", "interpolation_type");
+  interpolation_type = robot_node["interpolation_type"].as<int>();
+
   assert_yaml_parsing(robot_node, "robot", "Kp_main");
   Kp_main = robot_node["Kp_main"].as<std::vector<double> >();