From 4486e653a52dded97e8b70f32d205db7e7442cd5 Mon Sep 17 00:00:00 2001
From: Fanny Risbourg <frisbourg@laas.fr>
Date: Mon, 1 Aug 2022 10:30:04 +0200
Subject: [PATCH] add integration
---
config/walk_parameters.yaml | 1 +
include/qrw/Params.hpp | 1 +
python/Params.cpp | 1 +
.../quadruped_reactive_walking/Controller.py | 77 +++++++++++++------
.../WB_MPC/ProblemData.py | 1 -
src/Params.cpp | 4 +
6 files changed, 60 insertions(+), 25 deletions(-)
diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml
index 9807f67c..25b4bdd1 100644
--- a/config/walk_parameters.yaml
+++ b/config/walk_parameters.yaml
@@ -24,6 +24,7 @@ robot:
dt_wbc: 0.001 # Time step of the whole body 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
# Kp_main: [0.0, 0.0, 0.0] # Proportional gains for the PD+
Kp_main: [3,3,3] # 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 6ec37eb4..e6a150e0 100644
--- a/include/qrw/Params.hpp
+++ b/include/qrw/Params.hpp
@@ -92,6 +92,7 @@ class Params {
double dt_mpc; // Time step of the model predictive control
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
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 d8db929f..7008712c 100644
--- a/python/Params.cpp
+++ b/python/Params.cpp
@@ -26,6 +26,7 @@ struct ParamsVisitor : public bp::def_visitor<ParamsVisitor<Params>> {
.def_readwrite("type_MPC", &Params::type_MPC)
.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("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 a2316bca..a041e836 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -44,8 +44,11 @@ class DummyDevice:
def __init__(self):
self.positions = np.zeros(12)
self.velocities = np.zeros(12)
+
+
90
+
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
@@ -105,7 +108,7 @@ class Controller:
self.point_target = self.target.evaluate_in_t(1)[self.pd.rfFootId]
- if self.k % self.pd.r1 == 0:
+ if self.k % int(self.params.dt_mpc / self.params.dt_wbc) == 0:
try:
self.target.update(self.cnt_mpc)
self.target.shift_gait()
@@ -117,13 +120,13 @@ class Controller:
# Trajectory tracking
# if self.initialized:
- # self.mpc.solve(
- # self.k, self.mpc_result.xs[1], self.xs_init, self.us_init)
+ # self.mpc.solve(
+ # self.k, self.mpc_result.xs[1], self.xs_init, self.us_init)
# else:
- # self.mpc.solve(self.k, m["x_m"],
- # self.xs_init, self.us_init)
+ # self.mpc.solve(self.k, m["x_m"],
+ # self.xs_init, self.us_init)
- self.cnt_mpc += 1
+ self.cnt_mpc += 1
except ValueError:
self.error = True
print("MPC Problem")
@@ -135,25 +138,37 @@ class Controller:
self.mpc_result = self.mpc.get_latest_result()
# ## ONLY IF YOU WANT TO STORE THE FIRST SOLUTION TO WARM-START THE INITIAL Problem ###
- #if not self.initialized:
+ # if not self.initialized:
# np.save(open('/tmp/init_guess.npy', "wb"), {"xs": self.mpc_result.xs, "us": self.mpc_result.us} )
# print("Initial guess saved")
# Keep only the actuated joints and set the other to default values
- q_interpolated, v_interpolated = self.interpolate_x(self.cnt_wbc * self.pd.dt_wbc)
- self.q[3:6] = q_interpolated
- self.v[3:6] = v_interpolated
- # self.result.P = np.array(self.params.Kp_main.tolist() * 4)
- # self.result.D = np.array(self.params.Kd_main.tolist() * 4)
self.result.FF = self.params.Kff_main * np.ones(12)
+ actuated_tau_ff = self.mpc_result.us[0] + np.dot(
+ self.mpc_result.K[0],
+ np.concatenate(
+ [
+ pin.difference(
+ self.pd.model,
+ 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 :],
+ ]
+ ),
+ )
+ self.result.tau_ff = np.array([0] * 3 + list(actuated_tau_ff) + [0] * 6)
+
+ if self.params.interpolate_mpc:
+ q, v = self.interpolate_x(self.cnt_wbc * self.pd.dt_wbc)
+ else:
+ q, v = self.integrate_x()
+
+ self.q[3:6] = q
+ self.v[3:6] = v
self.result.q_des = self.q
self.result.v_des = self.v
- actuated_tau_ff = self.mpc_result.us[0] + np.dot(self.mpc_result.K[0],
- np.concatenate([pin.difference(self.pd.model, 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:] ]) )
- self.result.tau_ff = np.array([0] * 3 + list(actuated_tau_ff) + [0] * 6)
self.xs_init = self.mpc_result.xs[1:] + [self.mpc_result.xs[-1]]
self.us_init = self.mpc_result.us[1:] + [self.mpc_result.us[-1]]
@@ -161,8 +176,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()
@@ -294,20 +309,34 @@ class Controller:
q0 = q[0, :]
v1 = v[1, :]
q1 = q[1, :]
-
- if (q1-q0 == 0).any():
+
+ if (q1 - q0 == 0).any():
alpha = np.zeros(len(q0))
else:
- alpha = (v1**2 - v0**2)/(q1 - q0)
+ alpha = (v1**2 - v0**2) / (q1 - q0)
beta = v0
gamma = q0
v_t = beta + alpha * t
- q_t = gamma + beta *t + 1/2 * alpha * t**2
+ q_t = gamma + beta * t + 1 / 2 * alpha * t**2
return q_t, v_t
-
+
+ def integrate_x(self):
+ """
+ Integrate the position and velocity using the acceleration computed from the
+ feedforward torque
+ """
+ q0 = self.result.q_des[3:6]
+ v0 = self.result.v_des[3:6]
+
+ a = pin.aba(self.pd.model, self.pd.rdata, q0, v0, self.result.tau_ff[3:6])
+
+ v = v0 + a * self.params.dt_wbc
+ q = q0 + v * self.params.dt_wbc
+
+ return q, v
def tuple_to_array(self, tup):
a = np.array([element for tupl in tup for element in tupl])
diff --git a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
index b36a893e..0f4e8f9a 100644
--- a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
+++ b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
@@ -6,7 +6,6 @@ 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 = 50
self.T = self.init_steps + self.target_steps -1
diff --git a/src/Params.cpp b/src/Params.cpp
index 2c19abe0..3cdb2836 100644
--- a/src/Params.cpp
+++ b/src/Params.cpp
@@ -23,6 +23,7 @@ Params::Params()
dt_mpc(0.0),
N_periods(0),
type_MPC(0),
+ interpolate_mpc(true),
kf_enabled(false),
Kp_main(3, 0.0),
Kd_main(3, 0.0),
@@ -138,6 +139,9 @@ void Params::initialize(const std::string& file_path) {
assert_yaml_parsing(robot_node, "robot", "perfect_estimator");
perfect_estimator = robot_node["perfect_estimator"].as<bool>();
+ assert_yaml_parsing(robot_node, "robot", "interpolate_mpc");
+ interpolate_mpc = robot_node["interpolate_mpc"].as<bool>();
+
assert_yaml_parsing(robot_node, "robot", "Kp_main");
Kp_main = robot_node["Kp_main"].as<std::vector<double> >();
--
GitLab