diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml
index 952737187aff256964bb4ec61ebbb775e10719eb..11c1c53cf3495585ca460893281c1f9bba282519 100644
--- a/config/walk_parameters.yaml
+++ b/config/walk_parameters.yaml
@@ -11,7 +11,7 @@ robot:
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
+ N_SIMULATION: 5000 # Number of simulated wbc time steps
enable_corba_viewer: false # Enable/disable Corba Viewer
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
@@ -25,17 +25,18 @@ robot:
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
- movement: "circle" # name of the movement to perform
+ movement: "step" # name of the movement to perform
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
+ closed_loop: false # true to close the loop on the MPC
Kp_main: [3, 3, 3] # Proportional gains for the PD+
Kd_main: [0.3, 0.3, 0.3] # Derivative gains for the PD+
Kff_main: 1.0 # Feedforward torques multiplier for the PD+
# Parameters of Gait
N_periods: 1
- gait: [5, 1, 1, 1, 1,
- 45, 1, 0, 1, 1]
+ gait: [16, 1, 1, 1, 1,
+ 16, 1, 0, 1, 1]
# Parameters of Joystick
gp_alpha_vel: 0.003 #Â Coefficient of the low pass filter applied to gamepad velocity
@@ -55,7 +56,7 @@ robot:
k_feedback: 0.03 # Value of the gain for the feedback heuristic
# Parameters of FootTrajectoryGenerator
- max_height: 0.05 # Apex height of the swinging trajectory [m]
+ max_height: 0.03 # Apex height of the swinging trajectory [m]
lock_time: 0.04 # Target lock before the touchdown [s]
vert_time: 0.0 # Duration during which feet move only along Z when taking off and landing
diff --git a/include/qrw/Params.hpp b/include/qrw/Params.hpp
index 4202a401fb299cbb6109cd8a88a3ec0ecc278d9e..45d92909e8a4dc93c5d5cd0a16639f7b11b28be1 100644
--- a/include/qrw/Params.hpp
+++ b/include/qrw/Params.hpp
@@ -96,6 +96,7 @@ class Params {
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 closed_loop; // true to close the MPC loop
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+
diff --git a/python/Params.cpp b/python/Params.cpp
index 60740528acd840e45442523cb8365343d4d1c9c9..81da7557b104da140e60f8d81a63a0c50080851a 100644
--- a/python/Params.cpp
+++ b/python/Params.cpp
@@ -30,6 +30,7 @@ struct ParamsVisitor : public bp::def_visitor<ParamsVisitor<Params>> {
.def_readwrite("movement", &Params::movement)
.def_readwrite("interpolate_mpc", &Params::interpolate_mpc)
.def_readwrite("interpolation_type", &Params::interpolation_type)
+ .def_readwrite("closed_loop", &Params::closed_loop)
.def_readwrite("kf_enabled", &Params::kf_enabled)
.def_readwrite("Kp_main", &Params::Kp_main)
.def_readwrite("Kd_main", &Params::Kd_main)
diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index df97a879b0be0223d612e23320013939bd9545ac..04f747484a6f8e5351e79d6f954fce1ecb0bb4c7 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -9,6 +9,7 @@ from . import WB_MPC_Wrapper
from .WB_MPC.Target import Target
from .tools.Utils import init_robot, quaternionToRPY
from .WB_MPC.ProblemData import ProblemData, ProblemDataFull
+from .tools.kinematics_utils import get_translation_array
COLORS = ["#1f77b4", "#ff7f0e", "#2ca02c"]
@@ -170,7 +171,11 @@ class Controller:
self.result.q_des = self.pd.q0[7:].copy()
self.result.v_des = self.pd.v0[6:].copy()
- self.target = Target(params)
+ pin.forwardKinematics(self.pd.model, self.pd.rdata, self.pd.q0, np.zeros(18))
+ pin.updateFramePlacements(self.pd.model, self.pd.rdata)
+ foot_pose = self.pd.rdata.oMf[self.pd.feet_ids[1]].translation.copy()
+
+ self.target = Target(params, foot_pose)
self.footsteps = []
for k in range(self.params.T * self.params.mpc_wbc_ratio):
self.target_footstep = self.target.compute(k).copy()
@@ -224,37 +229,36 @@ class Controller:
self.k_solve = self.k
self.mpc_solved = False
- try:
- self.mpc.solve(
- self.k,
- m["x_m"],
- self.target_footstep.copy(),
- self.gait,
- self.xs_init,
- self.us_init,
- )
- # if self.initialized:
- # self.mpc.solve(
- # self.k,
- # self.mpc_result.xs[1],
- # self.target_footstep.copy(),
- # self.gait,
- # self.xs_init,
- # self.us_init,
- # )
- # else:
- # self.mpc.solve(
- # self.k,
- # m["x_m"],
- # self.target_footstep.copy(),
- # self.gait,
- # self.xs_init,
- # self.us_init,
- # )
- except ValueError:
- self.error = True
- print("MPC Problem")
- self.gait = np.vstack((self.gait[1:, :], self.gait[-1, :]))
+ if self.params.closed_loop or not self.initialized:
+ try:
+ self.mpc.solve(
+ self.k,
+ m["x_m"],
+ self.target_footstep.copy(),
+ self.gait,
+ self.xs_init,
+ self.us_init,
+ )
+ except ValueError:
+ self.error = True
+ print("MPC Problem")
+ else:
+ try:
+ self.mpc.solve(
+ self.k,
+ self.mpc_result.xs[1],
+ self.target_footstep.copy(),
+ self.gait,
+ self.xs_init,
+ self.us_init,
+ )
+ except ValueError:
+ self.error = True
+ print("MPC Problem")
+ if self.params.movement == "step":
+ self.gait = np.vstack((self.gait[1:, :], self.gait[0, :]))
+ else:
+ self.gait = np.vstack((self.gait[1:, :], self.gait[-1, :]))
t_mpc = time.time()
self.t_mpc = t_mpc - t_measures
@@ -293,8 +297,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()
@@ -510,7 +514,10 @@ class Controller:
axs[0].legend(legend)
axs[0].set_title("Base position")
- [axs[1].plot(np.array(self.mpc_result.xs)[:, 19 + axis]) for axis in range(3)]
+ [
+ axs[1].plot(np.array(self.mpc_result.xs)[:, 19 + axis])
+ for axis in range(3)
+ ]
axs[1].legend(legend)
axs[1].set_title("Base velocity")
@@ -519,11 +526,15 @@ class Controller:
_, axs = plt.subplots(3, 4, sharex=True)
for foot in range(4):
[
- axs[0, foot].plot(np.array(self.mpc_result.xs)[:, 7 + 3 * foot + joint])
+ axs[0, foot].plot(
+ np.array(self.mpc_result.xs)[:, 7 + 3 * foot + joint]
+ )
for joint in range(3)
]
axs[0, foot].legend(legend)
- axs[0, foot].set_title("Joint positions for " + self.pd.feet_names[foot])
+ axs[0, foot].set_title(
+ "Joint positions for " + self.pd.feet_names[foot]
+ )
[
axs[1, foot].plot(
@@ -539,6 +550,8 @@ class Controller:
for joint in range(3)
]
axs[2, foot].legend(legend)
- axs[2, foot].set_title("Joint torques for foot " + self.pd.feet_names[foot])
+ axs[2, foot].set_title(
+ "Joint torques for foot " + self.pd.feet_names[foot]
+ )
plt.show()
diff --git a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
index caa644d451c404e0cc00abe3b3c8254d533bc4f7..9543ec261584c12e10f85a396de70ab2b05c3285 100644
--- a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
+++ b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
@@ -13,7 +13,7 @@ class OCP:
def __init__(self, pd: ProblemData, params, footsteps, gait):
self.pd = pd
self.params = params
- self.max_iter = 500 if params.save_guess else 1
+ self.max_iter = 1000 if params.save_guess else 1
self.state = crocoddyl.StateMultibody(self.pd.model)
self.initialized = False
diff --git a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
index ca63488d46d4689c0e4031494c358ddec2ddd014..48e91ad3fced5470e132f0ff65e035a3d195d469 100644
--- a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
+++ b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
@@ -62,7 +62,11 @@ class ProblemData(problemDataAbstract):
# Cost function weights
self.mu = 0.7
- self.foot_tracking_w = 2. * 1e1
+
+ if params.movement == "step":
+ self.foot_tracking_w = 2.0 * 1e3
+ else:
+ self.foot_tracking_w = 1.5 * 1e1
self.friction_cone_w = 1e4
self.control_bound_w = 1e3
self.control_reg_w = 1e0
diff --git a/python/quadruped_reactive_walking/WB_MPC/Target.py b/python/quadruped_reactive_walking/WB_MPC/Target.py
index ce90c06f94f41cfbde9728ecb85cb248331f78fb..f087bf9e51cec3363b5ea55c1b891cfe0764c8e6 100644
--- a/python/quadruped_reactive_walking/WB_MPC/Target.py
+++ b/python/quadruped_reactive_walking/WB_MPC/Target.py
@@ -2,10 +2,11 @@ from tracemalloc import take_snapshot
import numpy as np
from .ProblemData import ProblemData
import pinocchio as pin
+from scipy.interpolate import KroghInterpolator
class Target:
- def __init__(self, params):
+ def __init__(self, params, foot_pose):
self.params = params
self.dt_wbc = params.dt_wbc
self.k_per_step = 160
@@ -20,15 +21,21 @@ class Target:
self.freq = np.array([0.5, 0., 0.5])
self.phase = np.array([-np.pi/2-0.5, 0., -np.pi/2])
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.p0 = foot_pose
+ self.p1 = foot_pose.copy() + np.array([0.025, 0.0, 0.03])
+ self.v1 = np.array([0.5, 0., 0.])
+ self.p2 = foot_pose.copy() + np.array([0.05, 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))
+ self.ts = np.repeat(np.linspace(0, self.T, 3), 2)
self.update_time = -1
+
+ def interpolate(self, t):
+ if self.type == 3:
+ q = self.krog(t)
+ v = self.krog.derivative(t)
+ return q, v
else:
self.target_footstep = self.position
self.ramp_length = 100
@@ -40,6 +47,7 @@ class Target:
footstep[:, 1] = self.evaluate_circle(k)
elif self.params.movement == "step":
footstep[:, 1] = self.evaluate_step(1, k)
+ footstep[2, 1] += 0.015
else:
footstep = self.target_footstep.copy()
footstep[2, 1] = self.target_ramp[k] if k < self.ramp_length else self.target_ramp[-1]
@@ -56,89 +64,32 @@ class Target:
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()
+ return self.p0.copy() if (n_step % 4 == 0) else self.p2.copy()
if n_step % 4 == 1:
- initial = self.initial
- target = self.target
+ initial = self.p0
+ target = self.p2
+ velocity = self.v1
else:
- initial = self.target
- target = self.initial
+ initial = self.p2
+ target = self.p0
+ velocity = -self.v1
+
k_step = k % self.k_per_step
if n_step != self.update_time:
- self.update_polynomial(initial, target)
+ self.update_interpolator(initial, target, velocity)
self.update_time = n_step
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
- )
+ return self.compute_position(t)
- self.position[2, j] = (
- A[3, 2] * t**3 + A[2, 2] * t**4 + A[1, 2] * t**5 + A[0, 2] * t**6
- )
+ def update_interpolator(self, initial, target, velocity):
+ self.y = [initial, np.zeros(3), self.p1, velocity, target, np.zeros(3)]
+ self.krog = KroghInterpolator(self.ts, np.array(self.y))
- return self.position[:, j]
+ def compute_position(self, t):
+ p = self.krog(t)
+ # v = self.krog.derivative(t)
+ return p
+
diff --git a/python/quadruped_reactive_walking/tools/LoggerControl.py b/python/quadruped_reactive_walking/tools/LoggerControl.py
index 0370937b3c573e0e1212a4fef76e978497bd60ae..65d6068bc8c5cc86448f7db9882eea8ee5558072 100644
--- a/python/quadruped_reactive_walking/tools/LoggerControl.py
+++ b/python/quadruped_reactive_walking/tools/LoggerControl.py
@@ -230,6 +230,14 @@ class LoggerControl:
}
# Target plot
+ _, axs = plt.subplots(3, sharex=True)
+ legend = ["x", "y", "z"]
+ for p in range(3):
+ axs[p].set_title("Base position on " + legend[p])
+ axs[p].plot([self.pd.xref[p]] * self.log_size)
+ axs[p].plot(self.q[:, p])
+ axs[p].legend(["Target", "Measured"])
+
_, axs = plt.subplots(3, sharex=True)
legend = ["x", "y", "z"]
for p in range(3):
@@ -270,7 +278,9 @@ class LoggerControl:
def plot_controller_times(self):
import matplotlib.pyplot as plt
- t_range = np.array([k * self.params.dt_mpc for k in range(self.tstamps.shape[0])])
+ t_range = np.array(
+ [k * self.params.dt_mpc for k in range(self.tstamps.shape[0])]
+ )
plt.figure()
plt.plot(t_range, self.t_measures, "r+")
@@ -287,7 +297,9 @@ class LoggerControl:
def plot_OCP_times(self):
import matplotlib.pyplot as plt
- t_range = np.array([k * self.params.dt_mpc for k in range(self.tstamps.shape[0])])
+ t_range = np.array(
+ [k * self.params.dt_mpc for k in range(self.tstamps.shape[0])]
+ )
plt.figure()
plt.plot(t_range, self.t_ocp_update, "r+")
diff --git a/src/Params.cpp b/src/Params.cpp
index e20658378d5d2d85033a8738697f45e2a616715b..21a1632ee5093dc587c51343e337dc0c5c852f38 100644
--- a/src/Params.cpp
+++ b/src/Params.cpp
@@ -27,6 +27,7 @@ Params::Params()
movement(""),
interpolate_mpc(true),
interpolation_type(0),
+ closed_loop(true),
kf_enabled(false),
Kp_main(3, 0.0),
Kd_main(3, 0.0),
@@ -154,6 +155,9 @@ void Params::initialize(const std::string& file_path) {
assert_yaml_parsing(robot_node, "robot", "interpolation_type");
interpolation_type = robot_node["interpolation_type"].as<int>();
+ assert_yaml_parsing(robot_node, "robot", "closed_loop");
+ closed_loop = robot_node["closed_loop"].as<bool>();
+
assert_yaml_parsing(robot_node, "robot", "Kp_main");
Kp_main = robot_node["Kp_main"].as<std::vector<double> >();