diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml
index 50d4e5ca723609d24b1037c71634f8a8abb7065b..efa158c674202783d52057c96f72b94cfe8cefab 100644
--- a/config/walk_parameters.yaml
+++ b/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+
diff --git a/include/qrw/Params.hpp b/include/qrw/Params.hpp
index f03660169bfa45002728413e51cfea066d01c2bf..4202a401fb299cbb6109cd8a88a3ec0ecc278d9e 100644
--- a/include/qrw/Params.hpp
+++ b/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
diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
index 8ee1216eacd51158d5d3f5d3062acfde6fdab66f..8739abe9a404e94cb21cbd9b0461469ed9d79064 100644
--- a/python/CMakeLists.txt
+++ b/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
)
diff --git a/python/Params.cpp b/python/Params.cpp
index 0a7b5cd81dfa51d94a78aadc31c94f0e7ba5dc24..60740528acd840e45442523cb8365343d4d1c9c9 100644
--- a/python/Params.cpp
+++ b/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)
diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index 83c259b8c23543ed278c97d5633fd36de39ec986..83108329178d3bc0fbded04385842520ca1b2f0d 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/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):
diff --git a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
index dbce972c0c1ced1ea7888a0fc6f5d78669d552ab..247c74a1e2a6e6e0ca3dcfee4175fd1b8835d5b4 100644
--- a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
+++ b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
@@ -1,4 +1,5 @@
from tracemalloc import start
+from xxlimited import foo
from .ProblemData import ProblemData
from .Target import Target
diff --git a/python/quadruped_reactive_walking/WB_MPC/Target.py b/python/quadruped_reactive_walking/WB_MPC/Target.py
index fa0f055c02f664db8bea4dd60e966c33e158e171..a5b84bd6f97c8bd1e6a4fe9fa458eeef80ef9ae0 100644
--- a/python/quadruped_reactive_walking/WB_MPC/Target.py
+++ b/python/quadruped_reactive_walking/WB_MPC/Target.py
@@ -1,32 +1,138 @@
+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]
diff --git a/python/quadruped_reactive_walking/WB_MPC_Wrapper.py b/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
index 9de9dff2d4dfa9ab5c210303d7fcdfb371df3be8..25b65a510f81fe8c25d8e06529271fe49ac0cddc 100644
--- a/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
+++ b/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):
diff --git a/python/quadruped_reactive_walking/main_solo12_control.py b/python/quadruped_reactive_walking/main_solo12_control.py
index 0e0ce5b30f33ee9477b5fb46ad58c0ef9803a5b7..41e0281876d44da730a5eeeb20944a4b3e0ccef2 100644
--- a/python/quadruped_reactive_walking/main_solo12_control.py
+++ b/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
diff --git a/python/quadruped_reactive_walking/tools/LoggerControl.py b/python/quadruped_reactive_walking/tools/LoggerControl.py
index ce900e6c958b7afd0b4b4d5ab0d9441c4d5ca9c5..06a95e7e9b18a3ab47a18e728af595334eb13081 100644
--- a/python/quadruped_reactive_walking/tools/LoggerControl.py
+++ b/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
diff --git a/python/quadruped_reactive_walking/tools/Utils.py b/python/quadruped_reactive_walking/tools/Utils.py
new file mode 100644
index 0000000000000000000000000000000000000000..1af8310e6872778cfb4693ddd2fb2fb2e0ffb3f3
--- /dev/null
+++ b/python/quadruped_reactive_walking/tools/Utils.py
@@ -0,0 +1,70 @@
+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]
diff --git a/src/Params.cpp b/src/Params.cpp
index fdea1b25e5d787666f2fb382b8580ac644b8e907..e20658378d5d2d85033a8738697f45e2a616715b 100644
--- a/src/Params.cpp
+++ b/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>();