add step

config/walk_parameters.yaml

@@ -7,13 +7,13 @@ robot:
     PLOTTING: true  # Enable/disable automatic plotting at the end of the experiment
     DEMONSTRATION: false  # Enable/disable demonstration functionalities
     SIMULATION: true  # Enable/disable PyBullet simulation or running on real robot
-    enable_pyb_GUI: false  # Enable/disable PyBullet GUI
+    enable_pyb_GUI: true  # Enable/disable PyBullet GUI
     envID: 0  # Identifier of the environment to choose in which one the simulation will happen
     use_flat_plane: true  # If True the ground is flat, otherwise it has bumps
     predefined_vel: true  # If we are using a predefined reference velocity (True) or a joystick (False)
     N_SIMULATION: 10000  # Number of simulated wbc time steps
     enable_corba_viewer: false  # Enable/disable Corba Viewer
-    enable_multiprocessing: false  # Enable/disable running the MPC in another process in parallel of the main loop
+    enable_multiprocessing: true  # Enable/disable running the MPC in another process in parallel of the main loop
     perfect_estimator: true  # Enable/disable perfect estimator by using data directly from PyBullet
 
     # General control parameters
@@ -22,10 +22,11 @@ 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.015  # Time step of the model predictive control
+    dt_mpc: 0.01  # 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
     save_guess: 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
+    movement: "step" # name of the movement to perform
+    interpolate_mpc: false # 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+
     # 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 save_guess;              // true to save the initial result of the mpc
+  std::string movement;         // Name of the mmovemnet to perform
   bool interpolate_mpc;         // true to interpolate the impedance quantities, otherwise integrate
   int interpolation_type;       // type of interpolation used
   bool kf_enabled;              // Use complementary filter (False) or kalman filter (True) for the estimator

python/CMakeLists.txt

@@ -40,6 +40,7 @@ set(${PY_NAME}_TOOLS_PYTHON
   __init__.py
   LoggerControl.py
   PyBulletSimulator.py
+  Utils.py
   qualisysClient.py
   kinematics_utils.py
   )

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("save_guess", &Params::save_guess)
+        .def_readwrite("movement", &Params::movement)
         .def_readwrite("interpolate_mpc", &Params::interpolate_mpc)
         .def_readwrite("interpolation_type", &Params::interpolation_type)
         .def_readwrite("kf_enabled", &Params::kf_enabled)
@@ -50,6 +51,7 @@ struct ParamsVisitor : public bp::def_visitor<ParamsVisitor<Params>> {
         .def_readwrite("CoM_offset", &Params::CoM_offset)
         .def_readwrite("h_ref", &Params::h_ref)
         .def_readwrite("shoulders", &Params::shoulders)
+        .def_readwrite("max_height", &Params::max_height)
         .def_readwrite("lock_time", &Params::lock_time)
         .def_readwrite("vert_time", &Params::vert_time)
         .def_readwrite("footsteps_init", &Params::footsteps_init)

python/quadruped_reactive_walking/Controller.py

@@ -6,6 +6,7 @@ import pybullet as pyb
 
 from . import WB_MPC_Wrapper
 from .WB_MPC.Target import Target
+from .tools.Utils import init_robot
 
 COLORS = ["#1f77b4", "#ff7f0e", "#2ca02c"]
 
@@ -154,6 +155,7 @@ class Controller:
         self.params = params
         self.gait = np.repeat(np.array([0, 0, 0, 0]).reshape((1, 4)), self.pd.T, axis=0)
         self.q_init = pd.q0
+        init_robot(q_init, params)
 
         self.k = 0
         self.error = False
@@ -163,9 +165,14 @@ class Controller:
         self.result.q_des = self.pd.q0[7:].copy()
         self.result.v_des = self.pd.v0[6:].copy()
 
-        self.target = Target(pd)
-        footsteps = [self.target.footstep(t) for t in range(pd.T)]
-        self.mpc = WB_MPC_Wrapper.MPC_Wrapper(pd, params, footsteps, self.gait)
+        self.target = Target(params)
+        self.footsteps = []
+        for k in range(self.pd.T * self.pd.mpc_wbc_ratio):
+            self.target_footstep = self.target.compute(k).copy()
+            if k % self.pd.mpc_wbc_ratio == 0:
+                self.footsteps.append(self.target_footstep.copy())
+
+        self.mpc = WB_MPC_Wrapper.MPC_Wrapper(pd, params, self.footsteps, self.gait)
         self.mpc_solved = False
         self.k_result = 0
         self.k_solve = 0
@@ -201,20 +208,20 @@ class Controller:
         t_measures = time.time()
         self.t_measures = t_measures - t_start
 
+        self.target_footstep = self.target.compute(self.k + self.pd.T).copy()
+
         if self.k % self.pd.mpc_wbc_ratio == 0:
-            self.target.shift()
             if self.mpc_solved:
                 self.k_solve = self.k
                 self.mpc_solved = False
 
             try:
-                footstep = self.target.footstep(self.pd.T)
                 # self.mpc.solve(self.k, m["x_m"], self.xs_init, self.us_init)
                 if self.initialized:
                     self.mpc.solve(
                         self.k,
                         self.mpc_result.xs[1],
-                        footstep,
+                        self.target_footstep,
                         self.gait,
                         self.xs_init,
                         self.us_init,
@@ -223,7 +230,7 @@ class Controller:
                     self.mpc.solve(
                         self.k,
                         m["x_m"],
-                        footstep,
+                        self.target_footstep,
                         self.gait,
                         self.xs_init,
                         self.us_init,
@@ -250,18 +257,16 @@ class Controller:
             self.result.FF = self.params.Kff_main * np.ones(12)
             self.result.tau_ff[3:6] = self.compute_torque(m)[:]
 
-            # if self.params.interpolate_mpc:
-            #     if self.mpc_result.new_result:
-            #         if self.params.interpolation_type == 3:
-            #             self.interpolator.update(xs[0], xs[1], xs[2])
-            #         # self.interpolator.plot(self.pd.mpc_wbc_ratio, self.pd.dt_wbc)
+            if self.params.interpolate_mpc:
+                if self.mpc_result.new_result:
+                    if self.params.interpolation_type == 3:
+                        self.interpolator.update(xs[0], xs[1], xs[2])
+                    # self.interpolator.plot(self.pd.mpc_wbc_ratio, self.pd.dt_wbc)
 
-            #     t = (self.k - self.k_solve + 1) * self.pd.dt_wbc
-            #     q, v = self.interpolator.interpolate(t)
-            # else:
-            #     q, v = self.integrate_x(m)
-            q = xs[1][:3]
-            v = xs[1][3:]
+                t = (self.k - self.k_solve + 1) * self.pd.dt_wbc
+                q, v = self.interpolator.interpolate(t)
+            else:
+                q, v = self.integrate_x(m)
 
             self.result.q_des[3:6] = q[:]
             self.result.v_des[3:6] = v[:]
@@ -282,8 +287,8 @@ class Controller:
         t_send = time.time()
         self.t_send = t_send - t_mpc
 
-        # self.clamp_result(device)
-        # self.security_check(m)
+        self.clamp_result(device)
+        self.security_check(m)
 
         if self.error:
             self.set_null_control()
@@ -424,9 +429,8 @@ class Controller:
         #         m["x_m"][self.pd.nq :] - self.mpc_result.xs[0][self.pd.nq :],
         #     ]
         # )
-        # x_diff = self.mpc_result.xs[0] - m["x_m"]
-        # tau = self.mpc_result.us[0] + np.dot(self.mpc_result.K[0], x_diff)
-        tau = self.mpc_result.us[0]
+        x_diff = self.mpc_result.xs[0] - m["x_m"]
+        tau = self.mpc_result.us[0] + np.dot(self.mpc_result.K[0], x_diff)
         return tau
 
     def integrate_x(self, m):

python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py

 from tracemalloc import start
+from xxlimited import foo
 
 from .ProblemData import ProblemData
 from .Target import Target

python/quadruped_reactive_walking/WB_MPC/Target.py

+from tracemalloc import take_snapshot
 import numpy as np
 from .ProblemData import ProblemData
 import pinocchio as pin
 
 
 class Target:
-    def __init__(self, pd: ProblemData):
-        self.dt = pd.dt
-        pin.forwardKinematics(pd.model, pd.rdata, pd.q0_reduced, pd.v0_reduced)
-        pin.updateFramePlacements(pd.model, pd.rdata)
-        self.foot_pose = pd.rdata.oMf[pd.rfFootId].translation.copy()
-        self.A = np.array([0, 0.03, 0.03])
-        self.offset = np.array([0.05, -0.02, 0.06])
-        self.freq = np.array([0, 0.5, 0.5])
-        self.phase = np.array([0, np.pi / 2, 0])
-        self.t_offset = 0
-
-    def shift(self):
-        self.t_offset += 1
-
-    def evaluate_circle(self, t):
-        return (
-            self.foot_pose
-            + self.offset
-            + self.A
-            * np.sin(2 * np.pi * self.freq * (self.t_offset + t) * self.dt + self.phase)
+    def __init__(self, params):
+        self.params = params
+        self.dt_wbc = params.dt_wbc
+        self.k_per_step = 160
+
+        self.position = np.array(params.footsteps_under_shoulders).reshape(
+            (3, 4), order="F"
         )
 
-    def footstep(self, t):
+        if params.movement == "circle":
+            self.A = np.array([0, 0.03, 0.03])
+            self.offset = np.array([0.05, -0.02, 0.06])
+            self.freq = np.array([0, 0.5, 0.5])
+            self.phase = np.array([0, np.pi / 2, 0])
+        elif params.movement == "step":
+            self.initial = self.position[:, 1].copy()
+            self.target = self.position[:, 1].copy() + np.array([0.1, 0.0, 0.0])
+
+            self.vert_time = params.vert_time
+            self.max_height = params.max_height
+            self.T = self.k_per_step * self.dt_wbc
+            self.A = np.zeros((6, 3))
+        else:
+            self.target_footstep = self.position + np.array([0.0, 0.0, 0.10])
+
+    def compute(self, k):
         footstep = np.zeros((3, 4))
-        footstep[:, 1] = self.evaluate_circle(t)
+        if self.params.movement == "circle":
+            footstep[:, 1] = self.evaluate_circle(k)
+        elif self.params.movement == "step":
+            footstep[:, 1] = self.evaluate_step(1, k)
+        else:
+            footstep = self.target_footstep.copy()
+
         return footstep
+
+    def evaluate_circle(self, k):
+        return (
+            self.position[:, 1]
+            + self.offset
+            + self.A * np.sin(2 * np.pi * self.freq * k * self.dt_wbc + self.phase)
+        )
+
+    def evaluate_step(self, j, k):
+        n_step = k // self.k_per_step
+        if n_step % 2 == 0:
+            return self.initial.copy() if (n_step % 4 == 0) else self.target.copy()
+
+        if n_step % 4 == 1:
+            initial = self.initial
+            target = self.target
+        else:
+            initial = self.target
+            target = self.initial
+
+        k_step = k % self.k_per_step
+        if k_step == 0:
+            self.update_polynomial(initial, target)
+
+        t = k_step * self.dt_wbc
+        return self.compute_position(j, t)
+
+    def update_polynomial(self, initial, target):
+
+        x0 = initial[0]
+        y0 = initial[1]
+
+        x1 = target[0]
+        y1 = target[1]
+
+        # elapsed time
+        t = 0
+        d = self.T - 2 * self.vert_time
+
+        A = np.zeros((6, 3))
+
+        A[0, 0] = 12 * (x0 - x1) / (2 * (t - d) ** 5)
+        A[1, 0] = 30 * (x1 - x0) * (t + d) / (2 * (t - d) ** 5)
+        A[2, 0] = 20 * (x0 - x1) * (t**2 + d**2 + 4 * t * d) / (2 * (t - d) ** 5)
+        A[3, 0] = 60 * (x1 - x0) * (t * d**2 + t**2 * d) / (2 * (t - d) ** 5)
+        A[4, 0] = 60 * (x0 - x1) * (t**2 * d**2) / (2 * (t - d) ** 5)
+        A[5, 0] = (
+            2 * x1 * t**5
+            - 10 * x1 * t**4 * d
+            + 20 * x1 * t**3 * d**2
+            - 20 * x0 * t**2 * d**3
+            + 10 * x0 * t * d**4
+            - 2 * x0 * d**5
+        ) / (2 * (t - d) ** 5)
+
+        A[0, 1] = 12 * (y0 - y1) / (2 * (t - d) ** 5)
+        A[1, 1] = 30 * (y1 - y0) * (t + d) / (2 * (t - d) ** 5)
+        A[2, 1] = 20 * (y0 - y1) * (t**2 + d**2 + 4 * t * d) / (2 * (t - d) ** 5)
+        A[3, 1] = 60 * (y1 - y0) * (t * d**2 + t**2 * d) / (2 * (t - d) ** 5)
+        A[4, 1] = 60 * (y0 - y1) * (t**2 * d**2) / (2 * (t - d) ** 5)
+        A[5, 1] = (
+            2 * y1 * t**5
+            - 10 * y1 * t**4 * d
+            + 20 * y1 * t**3 * d**2
+            - 20 * y0 * t**2 * d**3
+            + 10 * y0 * t * d**4
+            - 2 * y0 * d**5
+        ) / (2 * (t - d) ** 5)
+
+        A[0, 2] = -self.max_height / ((self.T / 2) ** 6)
+        A[1, 2] = 3 * self.T * self.max_height / ((self.T / 2) ** 6)
+        A[2, 2] = -3 * self.T**2 * self.max_height / ((self.T / 2) ** 6)
+        A[3, 2] = self.T**3 * self.max_height / ((self.T / 2) ** 6)
+
+        self.A = A
+
+    def compute_position(self, j, t):
+        A = self.A.copy()
+
+        t_xy = t - self.vert_time
+        t_xy = max(0.0, t_xy)
+        t_xy = min(t_xy, self.T - 2 * self.vert_time)
+        self.position[:2, j] = (
+            A[5, :2]
+            + A[4, :2] * t_xy
+            + A[3, :2] * t_xy**2
+            + A[2, :2] * t_xy**3
+            + A[1, :2] * t_xy**4
+            + A[0, :2] * t_xy**5
+        )
+
+        self.position[2, j] = (
+            A[3, 2] * t**3 + A[2, 2] * t**4 + A[1, 2] * t**5 + A[0, 2] * t**6
+        )
+
+        return self.position[:, j]

python/quadruped_reactive_walking/WB_MPC_Wrapper.py

@@ -112,6 +112,7 @@ class MPC_Wrapper:
             self.last_available_result.xs = [x0 for _ in range(self.pd.T + 1)]
             p = Process(target=self.MPC_asynchronous)
             p.start()
+
         self.add_new_data(k, x0, footstep, gait, xs, us)
 
     def MPC_asynchronous(self):

python/quadruped_reactive_walking/main_solo12_control.py

@@ -161,7 +161,6 @@ def control_loop():
         put_on_the_floor(device, q_init)
 
     # CONTROL LOOP ***************************************************
-
     t = 0.0
     t_max = (params.N_SIMULATION - 1) * params.dt_wbc
 

python/quadruped_reactive_walking/tools/LoggerControl.py

@@ -100,7 +100,10 @@ class LoggerControl:
             self.mocapOrientationQuat[self.i] = device.baseState[1]
 
         # Controller timings: MPC time, ...
-        self.target[self.i] = controller.target.evaluate_circle(0)
+        if self.i < self.pd.T * self.pd.mpc_wbc_ratio:
+            self.target[self.i] = controller.footsteps[self.i // self.pd.mpc_wbc_ratio][:, 1]
+        else:
+            self.target[self.i] = controller.target.compute(controller.k)[:, 1]
         self.t_mpc[self.i] = controller.t_mpc
         self.t_send[self.i] = controller.t_send
         self.t_loop[self.i] = controller.t_loop

python/quadruped_reactive_walking/tools/Utils.py

+from example_robot_data import load
+import numpy as np
+import pinocchio as pin
+
+
+def init_robot(q_init, params):
+    """Load the solo model and initialize the Gepetto viewer if it is enabled
+
+    Args:
+        q_init (array): the default position of the robot actuators
+        params (object): store parameters
+    """
+    # Load robot model and data
+    solo = load("solo12")
+    q = solo.q0.reshape((-1, 1))
+
+    # Initialisation of the position of footsteps to be under the shoulder
+    # There is a lateral offset of around 7 centimeters
+    fsteps_under_shoulders = np.zeros((3, 4))
+    indexes = [
+        solo.model.getFrameId("FL_FOOT"),
+        solo.model.getFrameId("FR_FOOT"),
+        solo.model.getFrameId("HL_FOOT"),
+        solo.model.getFrameId("HR_FOOT"),
+    ]
+    q[7:, 0] = 0.0
+    pin.framesForwardKinematics(solo.model, solo.data, q)
+    for i in range(4):
+        fsteps_under_shoulders[:, i] = solo.data.oMf[indexes[i]].translation
+    fsteps_under_shoulders[2, :] = 0.0
+
+    # Initial angular positions of actuators
+    q[7:, 0] = q_init
+
+    # Initialisation of model quantities
+    pin.centerOfMass(solo.model, solo.data, q, np.zeros((18, 1)))
+    pin.updateFramePlacements(solo.model, solo.data)
+    pin.crba(solo.model, solo.data, solo.q0)
+
+    # Initialisation of the position of footsteps
+    fsteps_init = np.zeros((3, 4))
+    h_init = 0.0
+    for i in range(4):
+        fsteps_init[:, i] = solo.data.oMf[indexes[i]].translation
+        h = (solo.data.oMf[1].translation - solo.data.oMf[indexes[i]].translation)[2]
+        if h > h_init:
+            h_init = h
+    fsteps_init[2, :] = 0.0
+
+    # Initialisation of the position of shoulders
+    shoulders_init = np.zeros((3, 4))
+    indexes = [4, 12, 20, 28]  # Shoulder indexes
+    for i in range(4):
+        shoulders_init[:, i] = solo.data.oMf[indexes[i]].translation
+
+    # Saving data
+    params.h_ref = h_init
+    # Mass of the whole urdf model (also = to Ycrb[1].mass)
+    params.mass = solo.data.mass[0]
+    # Composite rigid body inertia in q_init position
+    params.I_mat = solo.data.Ycrb[1].inertia.ravel().tolist()
+    params.CoM_offset = (solo.data.com[0][:3] - q[0:3, 0]).tolist()
+    params.CoM_offset[1] = 0.0
+
+    #  Use initial feet pos as reference
+    for i in range(4):
+        for j in range(3):
+            params.shoulders[3 * i + j] = shoulders_init[j, i]
+            params.footsteps_init[3 * i + j] = fsteps_init[j, i]
+            params.footsteps_under_shoulders[3 * i + j] = fsteps_init[j, i]

src/Params.cpp

@@ -24,6 +24,7 @@ Params::Params()
       N_periods(0),
       type_MPC(0),
       save_guess(false),
+      movement(""),
       interpolate_mpc(true),
       interpolation_type(0),
       kf_enabled(false),
@@ -144,6 +145,9 @@ void Params::initialize(const std::string& file_path) {
   assert_yaml_parsing(robot_node, "robot", "save_guess");
   save_guess = robot_node["save_guess"].as<bool>();
 
+  assert_yaml_parsing(robot_node, "robot", "movement");
+  movement = robot_node["movement"].as<std::string>();
+
   assert_yaml_parsing(robot_node, "robot", "interpolate_mpc");
   interpolate_mpc = robot_node["interpolate_mpc"].as<bool>();