From e8ab9ae5c18290ecc2fd217f2817050a3d0d467f Mon Sep 17 00:00:00 2001
From: Fanny Risbourg <frisbourg@laas.fr>
Date: Mon, 29 Aug 2022 16:01:27 +0200
Subject: [PATCH] Whole body changes
---
config/walk_parameters.yaml | 9 +-
.../quadruped_reactive_walking/Controller.py | 121 +++++++++-------
.../WB_MPC/CrocoddylOCP.py | 58 ++++----
.../WB_MPC/ProblemData.py | 129 ++++--------------
.../WB_MPC/Target.py | 11 +-
.../WB_MPC_Wrapper.py | 74 +++++-----
.../main_solo12_control.py | 7 +-
.../tools/LoggerControl.py | 103 +++++++-------
.../tools/PyBulletSimulator.py | 44 +++++-
.../quadruped_reactive_walking/tools/Utils.py | 3 +
10 files changed, 263 insertions(+), 296 deletions(-)
diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml
index bf5719a0..2ebaf357 100644
--- a/config/walk_parameters.yaml
+++ b/config/walk_parameters.yaml
@@ -25,8 +25,8 @@ robot:
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
save_guess: false # 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
+ movement: "circle" # 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
# Kp_main: [0.0, 0.0, 0.0] # Proportional gains for the PD+
# Kd_main: [0., 0., 0.] # Derivative gains for the PD+
@@ -37,7 +37,8 @@ robot:
# Parameters of Gait
N_periods: 1
- gait: [100, 0, 0, 0, 0] # Initial gait matrix
+ gait: [10, 1, 1, 1, 1,
+ 20, 1, 0, 1, 1] # Initial gait matrix
# Parameters of Joystick
gp_alpha_vel: 0.003 #Â Coefficient of the low pass filter applied to gamepad velocity
@@ -59,7 +60,7 @@ robot:
# Parameters of FootTrajectoryGenerator
max_height: 0.05 # Apex height of the swinging trajectory [m]
lock_time: 0.04 # Target lock before the touchdown [s]
- vert_time: 0.03 # Duration during which feet move only along Z when taking off and landing
+ vert_time: 0.0 # Duration during which feet move only along Z when taking off and landing
# Parameters of MPC with OSQP
# [0.0, 0.0, 20.0, 0.25, 0.25, 10.0, 0.05, 0.05, 0.2, 0.0, 0.0, 0.3]
diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index 0898af1b..ce5d1a5d 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -8,6 +8,7 @@ import quadruped_reactive_walking as qrw
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
COLORS = ["#1f77b4", "#ff7f0e", "#2ca02c"]
@@ -39,10 +40,10 @@ class Interpolator:
self.update(x0, x0)
def update(self, x0, x1, x2=None):
- self.q0 = x0[:12]
- self.q1 = x1[:12]
- self.v0 = x0[12:]
- self.v1 = x1[12:]
+ self.q0 = x0[7:19]
+ self.q1 = x1[7:19]
+ self.v0 = x0[19 + 6 :]
+ self.v1 = x1[19 + 6 :]
if self.type == 0: # Linear
self.alpha = 0.0
self.beta = self.v1
@@ -71,8 +72,8 @@ class Interpolator:
from scipy.interpolate import KroghInterpolator
if x2 is not None:
- self.q2 = x2[:12]
- self.v2 = x2[12:]
+ self.q2 = x2[7:19]
+ self.v2 = x2[19 + 6 :]
self.y = [self.q0, self.v0, self.q1, self.v1, self.q2, self.v2]
self.krog = KroghInterpolator(self.ts, np.array(self.y))
else:
@@ -118,14 +119,14 @@ class Interpolator:
class DummyDevice:
- def __init__(self):
+ def __init__(self, h):
self.imu = self.IMU()
self.joints = self.Joints()
self.base_position = np.zeros(3)
self.base_position[2] = 0.1944
self.b_base_velocity = np.zeros(3)
- self.baseState = tuple()
- self.baseVel = tuple()
+ self.baseState = ((0.0, 0.0, h), (0.0, 0.0, 0.0, 1.0))
+ self.baseVel = ((0.0, 0.0, 0.0), (0.0, 0.0, 0.0))
class IMU:
def __init__(self):
@@ -141,7 +142,7 @@ class DummyDevice:
class Controller:
- def __init__(self, pd, params, q_init, t):
+ def __init__(self, params, q_init, t):
"""
Function that computes the reference control (tau, q_des, v_des and gains)
@@ -152,11 +153,10 @@ class Controller:
"""
self.q_security = np.array([1.2, 2.1, 3.14] * 4)
- self.pd = pd
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
+ self.gait = params.gait
init_robot(q_init, params)
+ self.pd = ProblemData(params)
self.k = 0
self.error = False
@@ -172,19 +172,19 @@ class Controller:
self.target = Target(params)
self.footsteps = []
- for k in range(self.pd.T * self.pd.mpc_wbc_ratio):
+ for k in range(self.params.T * self.params.mpc_wbc_ratio):
self.target_footstep = self.target.compute(k).copy()
- if k % self.pd.mpc_wbc_ratio == 0:
+ if k % self.params.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 = WB_MPC_Wrapper.MPC_Wrapper(
+ self.pd, params, self.footsteps, self.gait
+ )
self.mpc_solved = False
self.k_result = 0
self.k_solve = 0
if self.params.interpolate_mpc:
- self.interpolator = Interpolator(
- params, np.concatenate([self.result.q_des, self.result.v_des])
- )
+ self.interpolator = Interpolator(params, self.pd.x0)
try:
file = np.load("/tmp/init_guess.npy", allow_pickle=True).item()
self.xs_init = list(file["xs"])
@@ -195,9 +195,8 @@ class Controller:
self.us_init = None
print("No initial guess\n")
- device = DummyDevice()
+ device = DummyDevice(params.h_ref)
device.joints.positions = q_init
- self.axs = None
self.compute(device)
def compute(self, device, qc=None):
@@ -217,10 +216,10 @@ class Controller:
self.t_measures = t_measures - t_start
self.target_footstep = self.target.compute(
- self.k + self.pd.T * self.pd.mpc_wbc_ratio
+ self.k + self.params.T * self.params.mpc_wbc_ratio
)
- if self.k % self.pd.mpc_wbc_ratio == 0:
+ if self.k % self.params.mpc_wbc_ratio == 0:
if self.mpc_solved:
self.k_solve = self.k
self.mpc_solved = False
@@ -255,6 +254,7 @@ class Controller:
except ValueError:
self.error = True
print("MPC Problem")
+ self.gait = np.vstack((self.gait[1:, :], self.gait[-1, :]))
t_mpc = time.time()
self.t_mpc = t_mpc - t_measures
@@ -266,7 +266,7 @@ class Controller:
self.mpc_solved = True
self.k_new = self.k
# print(f"MPC solved in {self.k - self.k_solve} iterations")
- # self.axs = self.plot_mpc()
+ # self.plot_mpc()
if not self.initialized and self.params.save_guess:
self.save_guess()
@@ -278,8 +278,8 @@ class Controller:
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
+ # self.interpolator.plot(self.params.mpc_wbc_ratio, self.params.dt_wbc)
+ t = (self.k - self.k_solve + 1) * self.params.dt_wbc
q, v = self.interpolator.interpolate(t)
else:
q, v = self.integrate_x(m)
@@ -287,16 +287,6 @@ class Controller:
self.result.q_des = q[:]
self.result.v_des = v[:]
- if self.axs is not None:
- [
- self.axs[2].scatter(
- y=self.result.tau_ff[3 + i],
- x=(self.k - self.k_solve + 1) * self.pd.dt_wbc,
- marker="+",
- color=COLORS[i],
- )
- for i in range(3)
- ]
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]]
@@ -372,12 +362,12 @@ class Controller:
if self.clamp(self.result.q_des[3 * i + 1], -hip_max, hip_max):
print("Clamping hip n " + str(i))
self.error = set_error
- if self.q_init[7 + 3 * i + 2] >= 0.0 and self.clamp(
+ if self.pd.q0[7 + 3 * i + 2] >= 0.0 and self.clamp(
self.result.q_des[3 * i + 2], knee_min
):
print("Clamping knee n " + str(i))
self.error = set_error
- elif self.q_init[7 + 3 * i + 2] <= 0.0 and self.clamp(
+ elif self.pd.q0[7 + 3 * i + 2] <= 0.0 and self.clamp(
self.result.q_des[3 * i + 2], max_value=-knee_min
):
print("Clamping knee n " + str(i))
@@ -470,7 +460,9 @@ class Controller:
def read_state(self, device):
qj_m = device.joints.positions
vj_m = device.joints.velocities
- x_m = np.concatenate([qj_m, vj_m])
+ bp_m = np.array([e for tup in device.baseState for e in tup])
+ bv_m = np.array([e for tup in device.baseVel for e in tup])
+ x_m = np.concatenate([bp_m, qj_m, bv_m, vj_m])
return {"qj_m": qj_m, "vj_m": vj_m, "x_m": x_m}
def compute_torque(self, m):
@@ -495,31 +487,56 @@ class Controller:
Integrate the position and velocity using the acceleration computed from the
feedforward torque
"""
- q0 = m["qj_m"].copy()
- v0 = m["vj_m"].copy()
- tau = self.result.tau_ff.copy()
+ q0 = m["x_m"].copy()[:19]
+ v0 = m["x_m"].copy()[19:]
+ tau = np.concatenate([np.zeros(6), self.result.tau_ff.copy()])
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
+ q = pin.integrate(self.pd.model, q0, v * self.params.dt_wbc)
- return q, v
+ return q[7:], v[6:]
def plot_mpc(self):
import matplotlib.pyplot as plt
+ legend = ["X", "Y", "Z"]
+ _, axs = plt.subplots(2)
+ [axs[0].plot(np.array(self.mpc_result.xs)[:, axis]) for axis in range(3)]
+ 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].legend(legend)
+ axs[1].set_title("Base velocity")
+
plt.show()
legend = ["Hip", "Shoulder", "Knee"]
- fig, axs = plt.subplots(3)
- [axs[0].plot(np.array(self.mpc_result.xs)[:, joint]) for joint in range(3)]
- axs[0].legend(legend)
+ _, 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])
+ for joint in range(3)
+ ]
+ axs[0, foot].legend(legend)
+ axs[0, foot].set_title("Joint positions for " + self.pd.feet_names[foot])
- [axs[1].plot(np.array(self.mpc_result.xs)[:, 3 + joint]) for joint in range(3)]
- axs[1].legend(legend)
+ [
+ axs[1, foot].plot(
+ np.array(self.mpc_result.xs)[:, 19 + 6 + 3 * foot + joint]
+ )
+ for joint in range(3)
+ ]
+ axs[1, foot].legend(legend)
+ axs[1, foot].set_title("Joint velocity for " + self.pd.feet_names[foot])
- [axs[2].plot(np.array(self.mpc_result.us)[:, joint]) for joint in range(3)]
- axs[2].legend(legend)
+ [
+ axs[2, foot].plot(np.array(self.mpc_result.us)[:, 3 * foot + joint])
+ for joint in range(3)
+ ]
+ axs[2, foot].legend(legend)
+ axs[2, foot].set_title("Joint torques for foot " + self.pd.feet_names[foot])
- return axs
+ plt.show()
diff --git a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
index c3df055d..72f2fffd 100644
--- a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
+++ b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
@@ -10,8 +10,9 @@ from time import time
class OCP:
- def __init__(self, pd: ProblemData, footsteps, gait):
+ def __init__(self, pd: ProblemData, params, footsteps, gait):
self.pd = pd
+ self.params = params
self.max_iter = 1
self.state = crocoddyl.StateMultibody(self.pd.model)
@@ -22,20 +23,19 @@ class OCP:
rm, tm = self.initialize_models(gait, footsteps)
- self.x0 = self.pd.x0_reduced
+ self.x0 = self.pd.x0
- self.problem = crocoddyl.ShootingProblem(
- self.x0, rm, tm)
+ self.problem = crocoddyl.ShootingProblem(self.x0, rm, tm)
self.ddp = crocoddyl.SolverFDDP(self.problem)
def initialize_models(self, gait, footsteps):
models = []
- for t, step in enumerate(gait):
+ for t, step in enumerate(gait[:-1]):
tasks = self.make_task(footsteps[t])
- support_feet = [self.pd.allContactIds[i] for i in np.nonzero(step == 1)[0]]
+ support_feet = [self.pd.feet_ids[i] for i in np.nonzero(step == 1)[0]]
models.append(self.create_model(support_feet, tasks))
- support_feet = [self.pd.allContactIds[i] for i in np.nonzero(gait[-1] == 1)[0]]
+ support_feet = [self.pd.feet_ids[i] for i in np.nonzero(gait[-1] == 1)[0]]
terminal_model = self.create_model(support_feet, is_terminal=True)
return models, terminal_model
@@ -77,9 +77,7 @@ class OCP:
if self.initialized:
tasks = self.make_task(footstep)
- support_feet = [
- self.pd.allContactIds[i] for i in np.nonzero(gait[-1] == 1)[0]
- ]
+ support_feet = [self.pd.feet_ids[i] for i in np.nonzero(gait[-1] == 1)[0]]
self.update_model(self.problem.runningModels[0], tasks, support_feet)
self.problem.circularAppend(
@@ -95,7 +93,7 @@ class OCP:
task = [[], []]
for foot in range(4):
if footstep[:, foot].any():
- task[0].append(self.pd.allContactIds[foot])
+ task[0].append(self.pd.feet_ids[foot])
task[1].append(footstep[:, foot])
return task
@@ -140,12 +138,11 @@ class OCP:
return acc
def update_model(self, model, tasks, support_feet):
- for i in self.pd.allContactIds:
+ for i in self.pd.feet_ids:
name = self.pd.model.frames[i].name + "_contact"
model.differential.contacts.changeContactStatus(name, i in support_feet)
- self.update_tracking_costs(model.differential.costs, tasks)
-
+ self.update_tracking_costs(model.differential.costs, tasks, support_feet)
def create_model(self, support_feet=[], tasks=[], is_terminal=False):
"""
@@ -173,19 +170,16 @@ class OCP:
:param support_feet: list of support feet ids
:return action model for a swing foot phase
"""
- if self.pd.useFixedBase == 0:
- actuation = crocoddyl.ActuationModelFloatingBase(self.state)
- else:
- actuation = crocoddyl.ActuationModelFull(self.state)
+ actuation = crocoddyl.ActuationModelFloatingBase(self.state)
nu = actuation.nu
control = crocoddyl.ControlParametrizationModelPolyZero(nu)
contacts = crocoddyl.ContactModelMultiple(self.state, nu)
- for i in self.pd.allContactIds:
+ for i in self.pd.feet_ids:
name = self.pd.model.frames[i].name + "_contact"
contact = crocoddyl.ContactModel3D(
- self.state, i, np.zeros(3), nu, np.zeros(2)
+ self.state, i, np.zeros(3), nu, self.pd.baumgarte_gains
)
contacts.addContact(name, contact)
if i not in support_feet:
@@ -200,7 +194,9 @@ class OCP:
differential = crocoddyl.DifferentialActionModelContactFwdDynamics(
self.state, actuation, contacts, costs, 0.0, True
)
- model = crocoddyl.IntegratedActionModelEuler(differential, control, self.pd.dt)
+ model = crocoddyl.IntegratedActionModelEuler(
+ differential, control, self.params.dt_mpc
+ )
return model
@@ -222,8 +218,8 @@ class OCP:
"""
nu = model.differential.actuation.nu
costs = model.differential.costs
- for i in self.pd.allContactIds:
- cone = crocoddyl.FrictionCone(self.pd.Rsurf, self.pd.mu, 4, False)
+ for i in self.pd.feet_ids:
+ cone = crocoddyl.FrictionCone(self.pd.Rsurf, self.pd.mu, 4, False, 3)
residual = crocoddyl.ResidualModelContactFrictionCone(
self.state, i, cone, nu
)
@@ -259,15 +255,15 @@ class OCP:
)
costs.addCost("control_bound", control_bound, self.pd.control_bound_w)
- self.update_tracking_costs(costs, tasks)
-
- def update_tracking_costs(self, costs, tasks):
- for i in self.pd.allContactIds:
+ self.update_tracking_costs(costs, tasks, support_feet)
+
+ def update_tracking_costs(self, costs, tasks, support_feet):
+ index = 0
+ for i in self.pd.feet_ids:
name = self.pd.model.frames[i].name + "_foot_tracking"
- index = 0
if i in tasks[0]:
- costs.changeCostStatus(name, True)
costs.costs[name].cost.residual.reference = tasks[1][index]
index += 1
- else:
- costs.changeCostStatus(name, False)
+ costs.changeCostStatus(name, i not in support_feet)
+
+ # print(f"{name} reference: {costs.costs[name].cost.residual.reference} status:{i in tasks[0]}")
diff --git a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
index fd8c7749..50818995 100644
--- a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
+++ b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
@@ -1,20 +1,15 @@
import numpy as np
-import example_robot_data as erd
+from example_robot_data import load
import pinocchio as pin
class problemDataAbstract:
- def __init__(self, param, frozen_names=[]):
- self.dt = param.dt_mpc # OCP dt
- self.dt_wbc = param.dt_wbc
- self.mpc_wbc_ratio = int(self.dt / self.dt_wbc)
- self.init_steps = 0
- self.target_steps = param.gait.shape[0]
- self.T = param.gait.shape[0] - 1
-
- self.robot = erd.load("solo12")
+ def __init__(self, params, frozen_names=[]):
+
+ self.robot = load("solo12")
self.q0 = self.robot.q0
- self.q0[7:] = param.q_init
+ self.q0[:7] = np.array([0.0, 0.0, params.h_ref, 0, 0, 0, 1])
+ self.q0[7:] = params.q_init
self.model = self.robot.model
self.rdata = self.model.createData()
@@ -22,19 +17,16 @@ class problemDataAbstract:
self.visual_model = self.robot.visual_model
self.frozen_names = frozen_names
- if frozen_names:
- self.frozen_idxs = [self.model.getJointId(
- id) for id in frozen_names]
+ if frozen_names != []:
+ self.frozen_idxs = [self.model.getJointId(id) for id in frozen_names]
self.freeze()
self.nq = self.model.nq
self.nv = self.model.nv
self.nx = self.nq + self.nv
self.ndx = 2 * self.nv
- self.nu = (
- 12 - len(frozen_names) + 1 if len(frozen_names) != 0 else 12
- ) # -1 to take into account the freeflyer
- self.ntau = self.nv
+ # -1 to take into account the freeflyer
+ self.nu = 12 - (len(frozen_names) - 1) if len(frozen_names) != 0 else 12
self.effort_limit = np.ones(self.nu) * 2.5
@@ -42,21 +34,10 @@ class problemDataAbstract:
self.x0 = np.concatenate([self.q0, self.v0])
self.u0 = np.zeros(self.nu)
- self.xref = self.x0
- self.uref = self.u0
+ self.baumgarte_gains = np.array([0, 100])
- self.lfFoot, self.rfFoot, self.lhFoot, self.rhFoot = (
- "FL_FOOT",
- "FR_FOOT",
- "HL_FOOT",
- "HR_FOOT",
- )
- self.cnames = [self.lfFoot, self.rfFoot, self.lhFoot, self.rhFoot]
- self.allContactIds = [self.model.getFrameId(f) for f in self.cnames]
- self.lfFootId = self.model.getFrameId(self.lfFoot)
- self.rfFootId = self.model.getFrameId(self.rfFoot)
- self.lhFootId = self.model.getFrameId(self.lhFoot)
- self.rhFootId = self.model.getFrameId(self.rhFoot)
+ self.feet_names = ["FL_FOOT", "FR_FOOT", "HL_FOOT", "HR_FOOT"]
+ self.feet_ids = [self.model.getFrameId(f) for f in self.feet_names]
self.Rsurf = np.eye(3)
@@ -74,97 +55,40 @@ class problemDataAbstract:
class ProblemData(problemDataAbstract):
- def __init__(self, param):
- super().__init__(param)
+ def __init__(self, params):
+ super().__init__(params)
self.useFixedBase = 0
+
# Cost function weights
self.mu = 0.7
- self.foot_tracking_w = 1e2
- self.friction_cone_w = 1e3
+ self.foot_tracking_w = 1e1
+ self.friction_cone_w = 1e4
self.control_bound_w = 1e3
self.control_reg_w = 1e0
self.state_reg_w = np.array(
[0] * 3
+ [1e1] * 3
- + [1e0] * 3
+ + [1e1] * 3
+ [1e-3] * 3
- + [1e0] * 6
+ + [1e1] * 6
+ [0] * 6
+ [1e1] * 3
- + [3 * 1e-1] * 3
+ + [1e-1] * 3
+ [1e1] * 6
)
- self.terminal_velocity_w = np.array([0] * 18 + [1e3] * 18)
- self.control_bound_w = 1e3
+ self.terminal_velocity_w = np.array([0] * self.nv + [1e4] * self.nv)
+ self.force_reg_w = 1e0
- self.x0 = np.array(
- [
- 0.0,
- 0.0,
- 0.2607495,
- 0,
- 0,
- 0,
- 1,
- 0,
- 0.7,
- -1.4,
- 0.0,
- 0.7,
- -1.4,
- 0.0,
- -0.7,
- 1.4,
- 0.0,
- -0.7,
- 1.4,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- ]
- ) # x0 got from PyBullet
-
- self.u0 = np.array(
- [
- -0.02615051,
- -0.25848605,
- 0.51696646,
- 0.0285894,
- -0.25720605,
- 0.51441775,
- -0.02614404,
- 0.25848271,
- -0.51697107,
- 0.02859587,
- 0.25720939,
- -0.51441314,
- ]
- ) # quasi static control
self.xref = self.x0
self.uref = self.u0
class ProblemDataFull(problemDataAbstract):
- def __init__(self, param):
+ def __init__(self, params):
frozen_names = ["root_joint"]
- super().__init__(param, frozen_names)
+ super().__init__(params, frozen_names)
self.useFixedBase = 1
@@ -175,8 +99,7 @@ class ProblemDataFull(problemDataAbstract):
self.control_bound_w = 1e3
self.control_reg_w = 1e0
self.state_reg_w = np.array(
- [1e2] * 3 + [1e-2] * 3 + [1e2] * 6 +
- [1e1] * 3 + [1e0] * 3 + [1e1] * 6
+ [1e2] * 3 + [1e-2] * 3 + [1e2] * 6 + [1e1] * 3 + [1e0] * 3 + [1e1] * 6
)
self.terminal_velocity_w = np.array([0] * 12 + [1e3] * 12)
diff --git a/python/quadruped_reactive_walking/WB_MPC/Target.py b/python/quadruped_reactive_walking/WB_MPC/Target.py
index fbbf86a4..2821cd0d 100644
--- a/python/quadruped_reactive_walking/WB_MPC/Target.py
+++ b/python/quadruped_reactive_walking/WB_MPC/Target.py
@@ -15,10 +15,10 @@ class Target:
)
if params.movement == "circle":
- self.A = np.array([0, 0.03, 0.03])
- self.offset = np.array([0.05, -0.02, 0.06])
+ self.A = np.array([0, 0.03, 0.04])
+ self.offset = np.array([0, 0, 0.05])
self.freq = np.array([0, 0.5, 0.5])
- self.phase = np.array([0, np.pi / 2, 0])
+ self.phase = np.array([0, 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])
@@ -30,7 +30,9 @@ class Target:
self.update_time = -1
else:
- self.target_footstep = self.position + np.array([0.0, 0.0, 0.10])
+ self.target_footstep = self.position
+ self.ramp_length = 100
+ self.target_ramp = np.linspace(0., 0.1, self.ramp_length)
def compute(self, k):
footstep = np.zeros((3, 4))
@@ -40,6 +42,7 @@ class Target:
footstep[:, 1] = self.evaluate_step(1, k)
else:
footstep = self.target_footstep.copy()
+ footstep[2, 1] = self.target_ramp[k] if k < self.ramp_length else self.target_ramp[-1]
return footstep
diff --git a/python/quadruped_reactive_walking/WB_MPC_Wrapper.py b/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
index 25b65a51..c0b8cd21 100644
--- a/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
+++ b/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
@@ -12,10 +12,10 @@ import quadruped_reactive_walking as qrw
class Result:
- def __init__(self, pd):
- self.xs = list(np.zeros((pd.T + 1, pd.nx)))
- self.us = list(np.zeros((pd.T, pd.nu)))
- self.K = list(np.zeros([pd.T, pd.nu, pd.nx]))
+ def __init__(self, pd, params):
+ self.xs = list(np.zeros((params.T + 1, pd.nx)))
+ self.us = list(np.zeros((params.T, pd.nu)))
+ self.K = list(np.zeros([params.T, pd.nu, pd.ndx]))
self.solving_duration = 0.0
self.new_result = False
@@ -29,6 +29,10 @@ class MPC_Wrapper:
def __init__(self, pd, params, footsteps, gait):
self.params = params
self.pd = pd
+ self.T = params.T
+ self.nu = pd.nu
+ self.nx = pd.nx
+ self.ndx = pd.ndx
self.footsteps_plan = footsteps
self.initial_gait = gait
@@ -39,20 +43,20 @@ class MPC_Wrapper:
self.new_data = Value("b", False)
self.running = Value("b", True)
self.in_k = Value("i", 0)
- self.in_x0 = Array("d", [0] * pd.nx)
+ self.in_x0 = Array("d", [0] * self.nx)
self.in_warm_start = Value("b", False)
- self.in_xs = Array("d", [0] * ((pd.T + 1) * pd.nx))
- self.in_us = Array("d", [0] * (pd.T * pd.nu))
+ self.in_xs = Array("d", [0] * ((self.T + 1) * self.nx))
+ self.in_us = Array("d", [0] * (self.T * self.nu))
self.in_footstep = Array("d", [0] * 12)
- self.in_gait = Array("d", [0] * (pd.T * 4))
- self.out_xs = Array("d", [0] * ((pd.T + 1) * pd.nx))
- self.out_us = Array("d", [0] * (pd.T * pd.nu))
- self.out_k = Array("d", [0] * (pd.T * pd.nu * pd.nx))
+ self.in_gait = Array("d", [0] * ((self.T + 1) * 4))
+ self.out_xs = Array("d", [0] * ((self.T + 1) * self.nx))
+ self.out_us = Array("d", [0] * (self.T * self.nu))
+ self.out_k = Array("d", [0] * (self.T * self.nu * self.ndx))
self.out_solving_time = Value("d", 0.0)
else:
- self.ocp = OCP(pd, footsteps, gait)
+ self.ocp = OCP(pd, params, footsteps, gait)
- self.last_available_result = Result(pd)
+ self.last_available_result = Result(pd, params)
self.new_result = Value("b", False)
def solve(self, k, x0, footstep, gait, xs=None, us=None):
@@ -109,7 +113,7 @@ class MPC_Wrapper:
Run the MPC (asynchronous version)
"""
if k == 0:
- self.last_available_result.xs = [x0 for _ in range(self.pd.T + 1)]
+ self.last_available_result.xs = [x0 for _ in range(self.T + 1)]
p = Process(target=self.MPC_asynchronous)
p.start()
@@ -128,7 +132,9 @@ class MPC_Wrapper:
k, x0, footstep, gait, xs, us = self.decompress_dataIn()
if k == 0:
- loop_ocp = OCP(self.pd, self.footsteps_plan, self.initial_gait)
+ loop_ocp = OCP(
+ self.pd, self.params, self.footsteps_plan, self.initial_gait
+ )
loop_ocp.solve(x0, footstep, gait, xs, us)
xs, us, K, solving_time = loop_ocp.get_results()
@@ -154,22 +160,22 @@ class MPC_Wrapper:
with self.in_k.get_lock():
self.in_k.value = k
with self.in_x0.get_lock():
- np.frombuffer(self.in_x0.get_obj()).reshape(self.pd.nx)[:] = x0
+ np.frombuffer(self.in_x0.get_obj()).reshape(self.nx)[:] = x0
with self.in_footstep.get_lock():
np.frombuffer(self.in_footstep.get_obj()).reshape((3, 4))[:, :] = footstep
with self.in_gait.get_lock():
- np.frombuffer(self.in_gait.get_obj()).reshape((self.pd.T, 4))[:, :] = gait
+ np.frombuffer(self.in_gait.get_obj()).reshape((self.T + 1, 4))[:, :] = gait
if xs is None or us is None:
self.in_warm_start.value = False
return
with self.in_xs.get_lock():
- np.frombuffer(self.in_xs.get_obj()).reshape((self.pd.T + 1, self.pd.nx))[
+ np.frombuffer(self.in_xs.get_obj()).reshape((self.T + 1, self.nx))[
:, :
] = np.array(xs)
with self.in_us.get_lock():
- np.frombuffer(self.in_us.get_obj()).reshape((self.pd.T, self.pd.nu))[
+ np.frombuffer(self.in_us.get_obj()).reshape((self.T, self.nu))[
:, :
] = np.array(us)
@@ -181,23 +187,21 @@ class MPC_Wrapper:
with self.in_k.get_lock():
k = self.in_k.value
with self.in_x0.get_lock():
- x0 = np.frombuffer(self.in_x0.get_obj()).reshape(self.pd.nx)
+ x0 = np.frombuffer(self.in_x0.get_obj()).reshape(self.nx)
with self.in_footstep.get_lock():
footstep = np.frombuffer(self.in_footstep.get_obj()).reshape((3, 4))
with self.in_gait.get_lock():
- gait = np.frombuffer(self.in_gait.get_obj()).reshape((self.pd.T, 4))
+ gait = np.frombuffer(self.in_gait.get_obj()).reshape((self.T + 1, 4))
if not self.in_warm_start.value:
return k, x0, footstep, gait, None, None
with self.in_xs.get_lock():
xs = list(
- np.frombuffer(self.in_xs.get_obj()).reshape((self.pd.T + 1, self.pd.nx))
+ np.frombuffer(self.in_xs.get_obj()).reshape((self.T + 1, self.nx))
)
with self.in_us.get_lock():
- us = list(
- np.frombuffer(self.in_us.get_obj()).reshape((self.pd.T, self.pd.nu))
- )
+ us = list(np.frombuffer(self.in_us.get_obj()).reshape((self.T, self.nu)))
return k, x0, footstep, gait, xs, us
@@ -207,17 +211,17 @@ class MPC_Wrapper:
retrieve data in the main control loop from the asynchronous MPC
"""
with self.out_xs.get_lock():
- np.frombuffer(self.out_xs.get_obj()).reshape((self.pd.T + 1, self.pd.nx))[
+ np.frombuffer(self.out_xs.get_obj()).reshape((self.T + 1, self.nx))[
:, :
] = np.array(xs)
with self.out_us.get_lock():
- np.frombuffer(self.out_us.get_obj()).reshape((self.pd.T, self.pd.nu))[
+ np.frombuffer(self.out_us.get_obj()).reshape((self.T, self.nu))[
:, :
] = np.array(us)
with self.out_k.get_lock():
- np.frombuffer(self.out_k.get_obj()).reshape(
- [self.pd.T, self.pd.nu, self.pd.nx]
- )[:, :, :] = np.array(K)
+ np.frombuffer(self.out_k.get_obj()).reshape([self.T, self.nu, self.ndx])[
+ :, :, :
+ ] = np.array(K)
self.out_solving_time.value = solving_time
def decompress_dataOut(self):
@@ -225,14 +229,10 @@ class MPC_Wrapper:
Return the result of the asynchronous MPC (desired contact forces) that is
stored in the shared memory
"""
- xs = list(
- np.frombuffer(self.out_xs.get_obj()).reshape((self.pd.T + 1, self.pd.nx))
- )
- us = list(np.frombuffer(self.out_us.get_obj()).reshape((self.pd.T, self.pd.nu)))
+ xs = list(np.frombuffer(self.out_xs.get_obj()).reshape((self.T + 1, self.nx)))
+ us = list(np.frombuffer(self.out_us.get_obj()).reshape((self.T, self.nu)))
K = list(
- np.frombuffer(self.out_k.get_obj()).reshape(
- [self.pd.T, self.pd.nu, self.pd.nx]
- )
+ np.frombuffer(self.out_k.get_obj()).reshape([self.T, self.nu, self.ndx])
)
solving_time = self.out_solving_time.value
diff --git a/python/quadruped_reactive_walking/main_solo12_control.py b/python/quadruped_reactive_walking/main_solo12_control.py
index 41e02818..2fcfb89b 100644
--- a/python/quadruped_reactive_walking/main_solo12_control.py
+++ b/python/quadruped_reactive_walking/main_solo12_control.py
@@ -8,11 +8,8 @@ from datetime import datetime
import quadruped_reactive_walking as qrw
from .Controller import Controller
from .tools.LoggerControl import LoggerControl
-from .WB_MPC.ProblemData import ProblemData, ProblemDataFull
-
params = qrw.Params() # Object that holds all controller parameters
-pd = ProblemDataFull(params)
repo = git.Repo(search_parent_directories=True)
sha = repo.head.object.hexsha
@@ -134,7 +131,7 @@ def control_loop():
# Default position after calibration
q_init = np.array(params.q_init.tolist())
- controller = Controller(pd, params, q_init, 0.0)
+ controller = Controller(params, q_init, 0.0)
if params.SIMULATION:
device = PyBulletSimulator()
@@ -144,7 +141,7 @@ def control_loop():
qc = QualisysClient(ip="140.93.16.160", body_id=0)
if params.LOGGING or params.PLOTTING:
- loggerControl = LoggerControl(pd, params, log_size=params.N_SIMULATION)
+ loggerControl = LoggerControl(controller.pd, params, log_size=params.N_SIMULATION)
if params.SIMULATION:
device.Init(
diff --git a/python/quadruped_reactive_walking/tools/LoggerControl.py b/python/quadruped_reactive_walking/tools/LoggerControl.py
index 1fb199be..dd514f28 100644
--- a/python/quadruped_reactive_walking/tools/LoggerControl.py
+++ b/python/quadruped_reactive_walking/tools/LoggerControl.py
@@ -55,8 +55,8 @@ class LoggerControl:
self.t_ocp_solve = np.zeros(size)
# MPC
- self.ocp_xs = np.zeros([size, pd.T + 1, pd.nx])
- self.ocp_us = np.zeros([size, pd.T, pd.nu])
+ self.ocp_xs = np.zeros([size, params.T + 1, pd.nx])
+ self.ocp_us = np.zeros([size, params.T, pd.nu])
self.ocp_K = np.zeros([size, self.pd.nu, self.pd.ndx])
self.MPC_equivalent_Kp = np.zeros([size, self.pd.nu])
self.MPC_equivalent_Kd = np.zeros([size, self.pd.nu])
@@ -120,10 +120,14 @@ class LoggerControl:
self.t_ocp_ddp[self.i] = controller.mpc_result.solving_duration
if self.i == 0:
- for i in range(self.pd.T * self.pd.mpc_wbc_ratio):
- self.target[i] = controller.footsteps[i // self.pd.mpc_wbc_ratio][:, 1]
- if self.i + self.pd.T * self.pd.mpc_wbc_ratio < self.log_size:
- self.target[self.i + self.pd.T * self.pd.mpc_wbc_ratio] = controller.target_footstep[:, 1]
+ for i in range(self.params.T * self.params.mpc_wbc_ratio):
+ self.target[i] = controller.footsteps[i // self.params.mpc_wbc_ratio][
+ :, 1
+ ]
+ if self.i + self.params.T * self.params.mpc_wbc_ratio < self.log_size:
+ self.target[
+ self.i + self.params.T * self.params.mpc_wbc_ratio
+ ] = controller.target_footstep[:, 1]
if not self.params.enable_multiprocessing:
self.t_ocp_update[self.i] = controller.mpc.ocp.t_update
@@ -215,41 +219,41 @@ class LoggerControl:
def plot_target(self, save=False, fileName="/tmp"):
import matplotlib.pyplot as plt
- x_mes = np.concatenate([self.q_mes, self.v_mes], axis=1)
-
- horizon = int(self.ocp_xs.shape[0] / self.pd.mpc_wbc_ratio)
- t_scale = np.linspace(
- 0, (horizon) * self.pd.dt, (horizon) * self.pd.mpc_wbc_ratio
- )
-
- x_mpc = [self.ocp_xs[0][0, :]]
- [x_mpc.append(x[1, :]) for x in self.ocp_xs[:-1]]
- x_mpc = np.array(x_mpc)
-
- # Feet positions calcuilated by every ocp
- all_ocp_feet_p_log = {
- idx: [
- get_translation_array(
- self.pd, self.ocp_xs[i * self.pd.mpc_wbc_ratio], idx
- )[0]
- for i in range(horizon)
- ]
- for idx in self.pd.allContactIds
- }
- for foot in all_ocp_feet_p_log:
- all_ocp_feet_p_log[foot] = np.array(all_ocp_feet_p_log[foot])
-
- # Measured feet positions
- m_feet_p_log = {
- idx: get_translation_array(self.pd, x_mes, idx)[0]
- for idx in self.pd.allContactIds
- }
-
- # Predicted feet positions
- feet_p_log = {
- idx: get_translation_array(self.pd, x_mpc, idx)[0]
- for idx in self.pd.allContactIds
- }
+ # x_mes = np.concatenate([self.q_mes, self.v_mes], axis=1)
+
+ # horizon = int(self.ocp_xs.shape[0] / self.params.mpc_wbc_ratio)
+ # t_scale = np.linspace(
+ # 0, (horizon) * self.params.dt_mpc, (horizon) * self.params.mpc_wbc_ratio
+ # )
+
+ # x_mpc = [self.ocp_xs[0][0, :]]
+ # [x_mpc.append(x[1, :]) for x in self.ocp_xs[:-1]]
+ # x_mpc = np.array(x_mpc)
+
+ # # Feet positions calcuilated by every ocp
+ # all_ocp_feet_p_log = {
+ # idx: [
+ # get_translation_array(
+ # self.pd, self.ocp_xs[i * self.params.mpc_wbc_ratio], idx
+ # )[0]
+ # for i in range(horizon)
+ # ]
+ # for idx in self.pd.feet_ids
+ # }
+ # for foot in all_ocp_feet_p_log:
+ # all_ocp_feet_p_log[foot] = np.array(all_ocp_feet_p_log[foot])
+
+ # # Measured feet positions
+ # m_feet_p_log = {
+ # idx: get_translation_array(self.pd, x_mes, idx)[0]
+ # for idx in self.pd.feet_ids
+ # }
+
+ # # Predicted feet positions
+ # feet_p_log = {
+ # idx: get_translation_array(self.pd, x_mpc, idx)[0]
+ # for idx in self.pd.feet_ids
+ # }
# Target plot
legend = ["x", "y", "z"]
@@ -258,26 +262,13 @@ class LoggerControl:
for p in range(3):
axs[p].set_title("Free foot on " + legend[p])
axs[p].plot(self.target[:, p], label="Target")
- axs[p].plot(m_feet_p_log[self.pd.rfFootId][:, p], label="Measured")
- axs[p].plot(feet_p_log[self.pd.rfFootId][:, p], label="Predicted")
+ # axs[p].plot(m_feet_p_log[self.pd.rfFootId][:, p], label="Measured")
+ # axs[p].plot(feet_p_log[self.pd.rfFootId][:, p], label="Predicted")
axs[p].legend()
if save:
plt.savefig(fileName + "/target")
- # legend = ['x', 'y', 'z']
- # plt.figure(figsize=(12, 18), dpi = 90)
- # for p in range(3):
- # plt.subplot(3,1, p+1)
- # plt.title('Free foot on ' + legend[p])
- # plt.plot(t_scale, self.target[:, p], color="tomato")
- # plt.plot(t_scale, m_feet_p_log[self.pd.rfFootId][:, p], color="lightgreen")
- # for i in range(horizon-1):
- # t = np.linspace(i*self.pd.dt, (self.pd.T+ i)*self.pd.dt, self.pd.T+1)
- # y = all_ocp_feet_p_log[self.pd.rfFootId][i][:,p]
- # for j in range(len(y) - 1):
- # plt.plot(t[j:j+2], y[j:j+2], color='royalblue', linewidth = 3, marker='o' ,alpha=max([1 - j/len(y), 0]))
-
def plot_riccati_gains(self, n, save=False, fileName="/tmp"):
import matplotlib.pyplot as plt
diff --git a/python/quadruped_reactive_walking/tools/PyBulletSimulator.py b/python/quadruped_reactive_walking/tools/PyBulletSimulator.py
index 07f59c9c..54ec3898 100644
--- a/python/quadruped_reactive_walking/tools/PyBulletSimulator.py
+++ b/python/quadruped_reactive_walking/tools/PyBulletSimulator.py
@@ -42,7 +42,7 @@ class pybullet_simulator:
# Either use a flat ground or a rough terrain
if use_flat_plane:
- """ self.planeId = pyb.loadURDF("plane.urdf") # Flat plane
+ self.planeId = pyb.loadURDF("plane.urdf") # Flat plane
self.planeIdbis = pyb.loadURDF("plane.urdf") # Flat plane
# Tune position and orientation of plane
@@ -55,7 +55,7 @@ class pybullet_simulator:
self.planeIdbis,
[200.0, 0, -100.0 * np.sin(p_pitch)],
pin.Quaternion(pin.rpy.rpyToMatrix(p_roll, p_pitch, 0.0)).coeffs(),
- ) """
+ )
else:
import random
@@ -269,12 +269,12 @@ class pybullet_simulator:
pyb.setGravity(0, 0, -9.81)
# Load Quadruped robot
- robotStartPos = [0, 0, 0.2607495]
+ robotStartPos = [0, 0, 0]
robotStartOrientation = pyb.getQuaternionFromEuler([0.0, 0.0, 0.0])
pyb.setAdditionalSearchPath(
EXAMPLE_ROBOT_DATA_MODEL_DIR + "/solo_description/robots"
)
- self.robotId = pyb.loadURDF("solo12.urdf", robotStartPos, robotStartOrientation, useFixedBase = 1)
+ self.robotId = pyb.loadURDF("solo12.urdf", robotStartPos, robotStartOrientation)
# Disable default motor control for revolute joints
self.revoluteJointIndices = [0, 1, 2, 4, 5, 6, 8, 9, 10, 12, 13, 14]
@@ -301,6 +301,42 @@ class pybullet_simulator:
forces=jointTorques,
)
+ # Get position of feet in world frame with base at (0, 0, 0)
+ feetLinksID = [3, 7, 11, 15]
+ linkStates = pyb.getLinkStates(self.robotId, feetLinksID)
+
+ # Get minimum height of feet (they are in the ground since base is at 0, 0, 0)
+ z_min = linkStates[0][4][2]
+ i_min = 0
+ i = 1
+ for link in linkStates[1:]:
+ if link[4][2] < z_min:
+ z_min = link[4][2]
+ i_min = i
+ i += 1
+
+ # Set base at (0, 0, -z_min) so that the lowest foot is at z = 0
+ pyb.resetBasePositionAndOrientation(
+ self.robotId,
+ [0.0, 0.0, -z_min],
+ pin.Quaternion(pin.rpy.rpyToMatrix(p_roll, p_pitch, 0.0)).coeffs(),
+ )
+
+ # Progressively raise the base to achieve proper contact (take into account radius of the foot)
+ while (
+ pyb.getClosestPoints(
+ self.robotId,
+ self.planeId,
+ distance=0.005,
+ linkIndexA=feetLinksID[i_min],
+ )
+ )[0][8] < -0.001:
+ z_min -= 0.001
+ pyb.resetBasePositionAndOrientation(
+ self.robotId,
+ [0.0, 0.0, -z_min],
+ pin.Quaternion(pin.rpy.rpyToMatrix(p_roll, p_pitch, 0.0)).coeffs(),
+ )
# Fix the base in the world frame
# pyb.createConstraint(self.robotId, -1, -1, -1, pyb.JOINT_FIXED, [0, 0, 0], [0, 0, 0], [0, 0, robotStartPos[2]])
diff --git a/python/quadruped_reactive_walking/tools/Utils.py b/python/quadruped_reactive_walking/tools/Utils.py
index c58f23a0..7ef8304a 100644
--- a/python/quadruped_reactive_walking/tools/Utils.py
+++ b/python/quadruped_reactive_walking/tools/Utils.py
@@ -62,6 +62,9 @@ def init_robot(q_init, params):
params.CoM_offset = (solo.data.com[0][:3] - q[0:3, 0]).tolist()
params.CoM_offset[1] = 0.0
+ params.mpc_wbc_ratio = int(params.dt_mpc / params.dt_wbc)
+ params.T = params.gait.shape[0] - 1
+
# Â Use initial feet pos as reference
for i in range(4):
for j in range(3):
--
GitLab