From 0f116110954b58fea4eb4d1b4ac1887da5f9d6ec Mon Sep 17 00:00:00 2001
From: Fanny Risbourg <frisbourg@laas.fr>
Date: Tue, 2 Aug 2022 09:40:58 +0200
Subject: [PATCH] debugging
---
config/walk_parameters.yaml | 8 +--
.../quadruped_reactive_walking/Controller.py | 68 +++++++------------
.../WB_MPC/CrocoddylOCP.py | 3 +-
.../WB_MPC/Target.py | 2 +-
4 files changed, 33 insertions(+), 48 deletions(-)
diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml
index 25b4bdd1..34a9d988 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: false # 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
@@ -22,13 +22,13 @@ 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.001 # 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+
+ Kp_main: [1, 1, 1] # Proportional gains for the PD+
# Kd_main: [0., 0., 0.] # Derivative gains for the PD+
- Kd_main: [0.2, 0.2, 0.2] # Derivative gains for the PD+
+ Kd_main: [0.1, 0.1, 0.1] # Derivative gains for the PD+
# Kff_main: 0.0 # Feedforward torques multiplier for the PD+
Kff_main: 1.0 # Feedforward torques multiplier for the PD+
diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index 7d79cf44..ac963fff 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -73,10 +73,9 @@ class Controller:
self.cnt_wbc = 0
self.error = False
self.initialized = False
- self.interpolated = False
self.result = Result(params)
- self.q = self.pd.q0[7:].copy()
- self.v = self.pd.v0[6:].copy()
+ self.result.q_des = self.pd.q0[7:].copy()
+ self.result.v_des = self.pd.v0[6:].copy()
device = DummyDevice()
device.joints.positions = q_init
@@ -90,8 +89,6 @@ class Controller:
self.us_init = None
print("No initial guess\n")
- # self.compute(device)
-
def compute(self, device, qc=None):
"""
Run one iteration of the main control loop
@@ -107,25 +104,14 @@ class Controller:
self.t_measures = t_measures - t_start
self.point_target = self.target.evaluate_in_t(1)[self.pd.rfFootId]
-
if self.k % int(self.params.dt_mpc / self.params.dt_wbc) == 0:
try:
self.target.update(self.cnt_mpc)
self.target.shift_gait()
- self.interpolated = False
self.cnt_wbc = 0
- # Closed-loop
self.mpc.solve(self.k, m["x_m"], self.xs_init, self.us_init)
- # Trajectory tracking
- # if self.initialized:
- # 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.cnt_mpc += 1
except ValueError:
self.error = True
@@ -143,21 +129,8 @@ class Controller:
# print("Initial guess saved")
# 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.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 :],
- ]
- ),
- )
+ actuated_tau_ff = self.compute_torque(m)
self.result.tau_ff = np.array([0] * 3 + list(actuated_tau_ff) + [0] * 6)
if self.params.interpolate_mpc:
@@ -165,10 +138,8 @@ class Controller:
else:
q, v = self.integrate_x(m)
- self.q[3:6] = q
- self.v[3:6] = v
- self.result.q_des = self.q
- self.result.v_des = self.v
+ self.result.q_des[3:6] = q[:]
+ self.result.v_des[3:6] = v[:]
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]]
@@ -293,15 +264,27 @@ class Controller:
device.parse_sensor_data()
qj_m = device.joints.positions
vj_m = device.joints.velocities
- # bp_m = self.tuple_to_array(device.baseState)
- # bv_m = self.tuple_to_array(device.baseVel)
- # if self.pd.useFixedBase == 0:
- # x_m = np.concatenate([bp_m, qj_m, bv_m, vj_m])
- # else:
x_m = np.concatenate([qj_m[3:6], vj_m[3:6]])
return {"qj_m": qj_m, "vj_m": vj_m, "x_m": x_m}
+ def compute_torque(self, m):
+ """
+ Compute the feedforward torque using ricatti gains
+ """
+ x_diff = 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 :],
+ ]
+ )
+ tau = self.mpc_result.us[0] + np.dot(self.mpc_result.K[0], x_diff)
+ return tau
+
def interpolate_x(self, t):
q = np.array(self.mpc_result.xs)[:, : self.pd.nq]
v = np.array(self.mpc_result.xs)[:, self.pd.nq :]
@@ -328,10 +311,11 @@ class Controller:
Integrate the position and velocity using the acceleration computed from the
feedforward torque
"""
- q0 = m["qj_m"][3:6]
- v0 = m["vj_m"][3:6]
+ q0 = m["qj_m"][3:6].copy()
+ v0 = m["vj_m"][3:6].copy()
+ tau = self.result.tau_ff[3:6].copy()
- a = pin.aba(self.pd.model, self.pd.rdata, q0, v0, self.result.tau_ff[3:6])
+ a = pin.aba(self.pd.model, self.pd.rdata, q0, v0, tau)
v = v0 + a * self.params.dt_wbc
q = q0 + v * self.params.dt_wbc
diff --git a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
index 5bdecdd1..4a928456 100644
--- a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
+++ b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
@@ -12,6 +12,7 @@ class OCP:
def __init__(self, pd: ProblemData, target: Target):
self.pd = pd
self.target = target
+ self.max_iter=100
self.state = crocoddyl.StateMultibody(self.pd.model)
self.initialized = False
@@ -108,7 +109,7 @@ class OCP:
self.t_warm_start = t_warm_start - t_update
# self.ddp.setCallbacks([crocoddyl.CallbackVerbose()])
- self.ddp.solve(xs, us, 1, False)
+ self.ddp.solve(xs, us, self.max_iter, False)
t_ddp = time()
self.t_ddp = t_ddp - t_warm_start
diff --git a/python/quadruped_reactive_walking/WB_MPC/Target.py b/python/quadruped_reactive_walking/WB_MPC/Target.py
index 8ca4d2c5..36fef7e0 100644
--- a/python/quadruped_reactive_walking/WB_MPC/Target.py
+++ b/python/quadruped_reactive_walking/WB_MPC/Target.py
@@ -23,7 +23,7 @@ class Target:
self.FR_foot0 = 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 , 0.5*0 ])
+ self.freq = np.array([0, 0.5 , 0.5 ])
self.phase = np.array([0, np.pi / 2, 0])
def patternToId(self, gait):
--
GitLab