diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml
index 34a9d98890d28bdf1bc3bde4e5429c932149bcb4..c96eb023a1132d4c488a6cbcb89e70276bb96819 100644
--- a/config/walk_parameters.yaml
+++ b/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+
diff --git a/include/qrw/Params.hpp b/include/qrw/Params.hpp
index e6a150e0e34cdd5152e6769f0dcded4dc2c9910e..f594cfccc2e716ba54c6db867aeff41c14e2107f 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 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+
diff --git a/python/Params.cpp b/python/Params.cpp
index 7008712cbfd146b37f1670d56aefcd7553d1ebdb..33b0327a030ab2250e227c3cec36412afb03791f 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("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)
diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index 41df48b04be4fdcf2a40b67eac85b7d78af8a6a9..a34c03de9429f9f16fb09ce024cf8e1b08f8e738 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/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)
diff --git a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
index 1eddd689455c48efd83577e373d9f4572ad60cff..9767505bb48172abcb880bc8d35a8309737c9684 100644
--- a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
+++ b/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
diff --git a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
index ea0021cf68faa2388786d1783238c70ea0086889..758e8b6bea79dba0e0209a53e64e54aad719889b 100644
--- a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
+++ b/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]
diff --git a/src/Params.cpp b/src/Params.cpp
index 3cdb28362963a71a51a9100723fa876d95bff776..84a1d06a0caf3d282beaa0ee997f5e79d5496e10 100644
--- a/src/Params.cpp
+++ b/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> >();