From 593c56dc8f45fc9501091fe945e35b5992a1d5b9 Mon Sep 17 00:00:00 2001
From: odri <odri@furano.laas.fr>
Date: Fri, 2 Sep 2022 17:16:08 +0200
Subject: [PATCH] [EXPE] com circle
---
config/walk_parameters.yaml | 16 ++--
.../quadruped_reactive_walking/Controller.py | 50 +++++++++++--
.../WB_MPC/CrocoddylOCP.py | 33 ++++++---
.../WB_MPC/ProblemData.py | 20 ++---
.../WB_MPC/Target.py | 49 ++++++++++---
.../WB_MPC_Wrapper.py | 43 ++++++-----
.../tools/LoggerControl.py | 73 +++++++++++++++----
7 files changed, 202 insertions(+), 82 deletions(-)
diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml
index c4e6cf0b..06e62a66 100644
--- a/config/walk_parameters.yaml
+++ b/config/walk_parameters.yaml
@@ -6,14 +6,14 @@ robot:
LOGGING: true # Enable/disable logging during the experiment
PLOTTING: true # Enable/disable automatic plotting at the end of the experiment
DEMONSTRATION: false # Enable/disable demonstration functionalities
- SIMULATION: true # Enable/disable PyBullet simulation or running on real robot
- enable_pyb_GUI: true # Enable/disable PyBullet GUI
+ SIMULATION: false # Enable/disable PyBullet simulation or running on real robot
+ enable_pyb_GUI: false # Enable/disable PyBullet GUI
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: 5000 # Number of simulated wbc time steps
+ N_SIMULATION: 10000 # 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
+ enable_multiprocessing: true # Enable/disable running the MPC in another process in parallel of the main loop
perfect_estimator: false # Enable/disable perfect estimator by using data directly from PyBullet
# General control parameters
@@ -25,18 +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: "" # name of the movement to perform
+ movement: "base_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
- closed_loop: false # true to close the loop on the MPC
- Kp_main: [3, 3, 3] # Proportional gains for the PD+
+ closed_loop: true # true to close the loop on the MPC
+ Kp_main: [2, 2, 2] # 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: [16, 1, 1, 1, 1,
- 16, 1, 0, 1, 1]
+ 16, 1, 1, 1, 1]
# Parameters of Joystick
gp_alpha_vel: 0.003 #Â Coefficient of the low pass filter applied to gamepad velocity
diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index 17a11a4b..e8a32a82 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -28,6 +28,7 @@ class Result:
self.v_des = np.zeros(12)
self.tau_ff = np.zeros(12)
+
class DummyDevice:
def __init__(self, h):
self.imu = self.IMU()
@@ -86,13 +87,21 @@ class Controller:
self.target = Target(params, foot_pose)
self.footsteps = []
+ self.base_refs = []
for k in range(self.params.T * self.params.mpc_wbc_ratio):
- self.target_footstep = self.target.compute(k).copy()
+ if params.movement == "base_circle":
+ self.target_base = self.target.compute(k).copy()
+ self.target_footstep = np.zeros((3, 4))
+ else:
+ self.target_footstep = self.target.compute(k).copy()
+ self.target_base = np.zeros(3)
+
if k % self.params.mpc_wbc_ratio == 0:
+ self.base_refs.append(self.target_base.copy())
self.footsteps.append(self.target_footstep.copy())
self.mpc = WB_MPC_Wrapper.MPC_Wrapper(
- self.pd, params, self.footsteps, self.gait
+ self.pd, params, self.footsteps, self.base_refs, self.gait
)
self.mpc_solved = False
self.k_result = 0
@@ -109,6 +118,11 @@ class Controller:
self.us_init = None
print("No initial guess\n")
+ self.filter_q = qrw.Filter()
+ self.filter_q.initialize(params)
+ self.filter_v = qrw.Filter()
+ self.filter_v.initialize(params)
+
device = DummyDevice(params.h_ref)
device.joints.positions = q_init
self.compute(device)
@@ -127,9 +141,16 @@ class Controller:
t_measures = time.time()
self.t_measures = t_measures - t_start
- self.target_footstep = self.target.compute(
- self.k + self.params.T * self.params.mpc_wbc_ratio
- )
+ if self.params.movement == "base_circle":
+ self.target_base = self.target.compute(
+ self.k + self.params.T * self.params.mpc_wbc_ratio
+ )
+ self.target_footstep = np.zeros((3, 4))
+ else:
+ self.target_base = np.zeros(3)
+ self.target_footstep = self.target.compute(
+ self.k + self.params.T * self.params.mpc_wbc_ratio
+ )
if self.k % self.params.mpc_wbc_ratio == 0:
if self.mpc_solved:
@@ -142,6 +163,7 @@ class Controller:
self.k,
self.x,
self.target_footstep.copy(),
+ self.target_base.copy(),
self.gait,
self.xs_init,
self.us_init,
@@ -155,6 +177,7 @@ class Controller:
self.k,
self.mpc_result.xs[1],
self.target_footstep.copy(),
+ self.target_base.copy(),
self.gait,
self.xs_init,
self.us_init,
@@ -341,7 +364,7 @@ class Controller:
self.estimator.run(
self.gait,
- footstep.reshape((3, 4), order='F'),
+ footstep.reshape((3, 4), order="F"),
device.imu.linear_acceleration,
device.imu.gyroscope,
device.imu.attitude_euler,
@@ -361,6 +384,7 @@ class Controller:
self.v_ref = self.estimator.get_base_vel_ref()
self.h_v = self.estimator.get_h_v()
self.h_v_windowed = self.estimator.get_h_v_filtered()
+ self.v_windowed = self.estimator.get_v_filtered()
self.q_estimate = self.estimator.get_q_estimate()
self.v_estimate = self.estimator.get_v_estimate()
@@ -373,7 +397,19 @@ class Controller:
self.v = self.estimator.get_v_reference()
- self.x = np.concatenate([self.q_estimate, self.v_estimate])
+ self.base_position_filtered = self.filter_q.filter(self.q[:6], True)
+
+ self.q_filtered = self.q_estimate.copy()
+ self.q_filtered[:3] = self.base_position_filtered[:3]
+ self.q_filtered[3:7] = pin.Quaternion(
+ pin.rpy.rpyToMatrix(self.base_position_filtered[3:])
+ ).coeffs()
+ self.v_filtered = self.v_estimate.copy()
+ # self.v_filtered[:6] = np.zeros(6)
+ # self.v_filtered[:6] = self.filter_v.filter(self.v_windowed, False)
+ self.v_filtered[:6] = self.filter_v.filter(self.v_estimate[:6], False)
+
+ self.x = np.concatenate([self.q_filtered, self.v_filtered])
return oRh, hRb, oTh
def compute_torque(self):
diff --git a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
index 9543ec26..0a41fdc7 100644
--- a/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
+++ b/python/quadruped_reactive_walking/WB_MPC/CrocoddylOCP.py
@@ -10,7 +10,7 @@ from time import time
class OCP:
- def __init__(self, pd: ProblemData, params, footsteps, gait):
+ def __init__(self, pd: ProblemData, params, footsteps, base_refs, gait):
self.pd = pd
self.params = params
self.max_iter = 1000 if params.save_guess else 1
@@ -21,17 +21,17 @@ class OCP:
self.t_update_last_model = 0.0
self.t_shift = 0.0
- rm, tm = self.initialize_models(gait, footsteps)
+ rm, tm = self.initialize_models(gait, footsteps, base_refs)
self.x0 = self.pd.x0
self.problem = crocoddyl.ShootingProblem(self.x0, rm, tm)
self.ddp = crocoddyl.SolverFDDP(self.problem)
- def initialize_models(self, gait, footsteps):
+ def initialize_models(self, gait, footsteps, base_refs):
models = []
for t, step in enumerate(gait[:-1]):
- tasks = self.make_task(footsteps[t])
+ tasks = self.make_task(footsteps[t], base_refs[t])
support_feet = [self.pd.feet_ids[i] for i in np.nonzero(step == 1)[0]]
models.append(self.create_model(support_feet, tasks))
@@ -40,10 +40,10 @@ class OCP:
return models, terminal_model
- def solve(self, x0, footstep, gait, xs_init=None, us_init=None):
+ def solve(self, x0, footstep, base_ref, gait, xs_init=None, us_init=None):
t_start = time()
self.x0 = x0
- self.make_ocp(footstep, gait)
+ self.make_ocp(footstep, base_ref, gait)
t_update = time()
self.t_update = t_update - t_start
@@ -66,7 +66,7 @@ class OCP:
self.t_solve = time() - t_start
- def make_ocp(self, footstep, gait):
+ def make_ocp(self, footstep, base_ref, gait):
"""
Create a shooting problem for a simple walking gait.
@@ -76,7 +76,7 @@ class OCP:
pin.updateFramePlacements(self.pd.model, self.pd.rdata)
if self.initialized:
- tasks = self.make_task(footstep)
+ tasks = self.make_task(footstep, base_ref)
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)
@@ -89,8 +89,11 @@ class OCP:
self.initialized = True
- def make_task(self, footstep):
+ def make_task(self, footstep, base_ref):
task = [[], []]
+ if base_ref is not None:
+ task[0].append(self.pd.base_id)
+ task[1].append(base_ref)
for foot in range(4):
if footstep[:, foot].any():
task[0].append(self.pd.feet_ids[foot])
@@ -219,7 +222,7 @@ class OCP:
nu = model.differential.actuation.nu
costs = model.differential.costs
for i in self.pd.feet_ids:
- cone = crocoddyl.FrictionCone(self.pd.Rsurf, self.pd.mu, 4, False, 3)
+ cone = crocoddyl.FrictionCone(self.pd.Rsurf, self.pd.mu, 4, False, min_nforce=3.)
residual = crocoddyl.ResidualModelContactFrictionCone(
self.state, i, cone, nu
)
@@ -241,6 +244,10 @@ class OCP:
costs.addCost(name, foot_tracking, self.pd.foot_tracking_w)
costs.changeCostStatus(name, False)
+
+ residual = crocoddyl.ResidualModelFrameTranslation(self.state, self.pd.base_id, np.zeros(3), nu)
+ base_tracking = crocoddyl.CostModelResidual(self.state, residual)
+ costs.addCost("base_tracking", base_tracking, self.pd.base_tracking_w)
control_residual = crocoddyl.ResidualModelControl(self.state, self.pd.uref)
control_reg = crocoddyl.CostModelResidual(self.state, control_residual)
@@ -259,11 +266,13 @@ class OCP:
def update_tracking_costs(self, costs, tasks, support_feet):
index = 0
+ if tasks[0][0] == self.pd.base_id:
+ costs.costs["base_tracking"].cost.residual.reference = tasks[1][index]
+ index += 1
+
for i in self.pd.feet_ids:
name = self.pd.model.frames[i].name + "_foot_tracking"
if i in tasks[0]:
costs.costs[name].cost.residual.reference = tasks[1][index]
index += 1
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 48e91ad3..b5048ac7 100644
--- a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
+++ b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py
@@ -32,7 +32,8 @@ class problemDataAbstract:
self.v0 = np.zeros(18)
self.x0 = np.concatenate([self.q0, self.v0])
- self.u0 = np.zeros(self.nu)
+
+ self.u0 = np.array([0.0, 0.0, 9.81 * params.mass / 4.0] * 4)
self.baumgarte_gains = np.array([0, 100])
@@ -59,6 +60,7 @@ class ProblemData(problemDataAbstract):
super().__init__(params)
self.useFixedBase = 0
+ self.base_id = self.model.getFrameId("base_link")
# Cost function weights
self.mu = 0.7
@@ -66,23 +68,23 @@ class ProblemData(problemDataAbstract):
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.foot_tracking_w = 0.
+ self.base_tracking_w = 1e5
+ self.friction_cone_w = 0.0 # 1e4
+ self.control_bound_w = 0.
self.control_reg_w = 1e0
self.state_reg_w = np.array(
[0] * 3
+ [1e1] * 3
+ [1e1] * 3
- + [1e-3] * 3
+ + [1e1] * 3
+ [1e1] * 6
+ [0] * 6
- + [1e1] * 3
+ [1e-1] * 3
- + [1e1] * 6
+ + [1e-1] * 3
+ + [1e-1] * 6
)
- self.terminal_velocity_w = np.array([0] * self.nv + [1e4] * self.nv)
- self.force_reg_w = 1e0
+ self.terminal_velocity_w = np.array([0] * self.nv + [0.] * self.nv)
self.xref = self.x0
self.uref = self.u0
diff --git a/python/quadruped_reactive_walking/WB_MPC/Target.py b/python/quadruped_reactive_walking/WB_MPC/Target.py
index e28de2df..10d383b3 100644
--- a/python/quadruped_reactive_walking/WB_MPC/Target.py
+++ b/python/quadruped_reactive_walking/WB_MPC/Target.py
@@ -11,7 +11,16 @@ class Target:
self.dt_wbc = params.dt_wbc
self.k_per_step = 160
- if params.movement == "circle":
+ if params.movement == "base_circle":
+ self.initial_base = np.array([0.0, 0.0, params.h_ref])
+ self.A = np.array([0.02, 0.0, 0.0])
+ self.offset = np.array([0.0, 0.0, 0.0])
+ self.freq = np.array([0.5, 0.5, 0.0])
+ self.phase = np.array([0.0, 0.0, 0.0])
+ elif params.movement == "circle":
+ self.position = np.array(params.footsteps_init.tolist()).reshape(
+ (3, 4), order="F"
+ )
self.A = np.array([0.05, 0.0, 0.04])
self.offset = np.array([0.05, 0, 0.05])
self.freq = np.array([0.5, 0.0, 0.5])
@@ -31,26 +40,42 @@ class Target:
self.params.footsteps_init.tolist()
).reshape((3, 4), order="F")
self.ramp_length = 100
- self.target_ramp = np.linspace(0.0, 0.1, self.ramp_length)
+ self.target_ramp_x = np.linspace(0.0, -0.0, self.ramp_length)
+ self.target_ramp_y = np.linspace(0.0, 0.0, self.ramp_length)
+ self.target_ramp_z = np.linspace(0.0, 0.05, self.ramp_length)
def compute(self, k):
footstep = np.zeros((3, 4))
- if self.params.movement == "circle":
- footstep[:, 1] = self.evaluate_circle(k)
+ if self.params.movement == "base_circle":
+ target = self.evaluate_circle(k, self.initial_base)
+ elif self.params.movement == "circle":
+ target[:, 1] = self.evaluate_circle(k, self.position[:, 1])
elif self.params.movement == "step":
- footstep[:, 1] = self.evaluate_step(1, k)
- footstep[2, 1] += 0.015
+ target[:, 1] = self.evaluate_step(1, k)
+ target[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]
+ target = self.target_footstep.copy()
+ target[0, 1] = (
+ self.target_ramp_x[k]
+ if k < self.ramp_length
+ else self.target_ramp_x[-1]
+ )
+ target[1, 1] = (
+ self.target_ramp_y[k]
+ if k < self.ramp_length
+ else self.target_ramp_y[-1]
+ )
+ target[2, 1] = (
+ self.target_ramp_z[k]
+ if k < self.ramp_length
+ else self.target_ramp_z[-1]
)
- return footstep
+ return target
- def evaluate_circle(self, k):
+ def evaluate_circle(self, k, initial_position):
return (
- self.position[:, 1]
+ initial_position
+ self.offset
+ self.A * np.sin(2 * np.pi * self.freq * k * self.dt_wbc + self.phase)
)
diff --git a/python/quadruped_reactive_walking/WB_MPC_Wrapper.py b/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
index 2c7e092e..5fd83da5 100644
--- a/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
+++ b/python/quadruped_reactive_walking/WB_MPC_Wrapper.py
@@ -26,7 +26,7 @@ class MPC_Wrapper:
a parallel process
"""
- def __init__(self, pd, params, footsteps, gait):
+ def __init__(self, pd, params, footsteps, base_refs, gait):
self.params = params
self.pd = pd
self.T = params.T
@@ -34,12 +34,14 @@ class MPC_Wrapper:
self.nx = pd.nx
self.ndx = pd.ndx
- self.footsteps_plan = footsteps
self.initial_gait = gait
self.multiprocessing = params.enable_multiprocessing
if self.multiprocessing:
+ self.footsteps_plan = footsteps
+ self.base_refs = base_refs
+
self.new_data = Value("b", False)
self.running = Value("b", True)
self.in_k = Value("i", 0)
@@ -48,18 +50,19 @@ class MPC_Wrapper:
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_base_ref = Array("d", [0] * 3)
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, params, footsteps, gait)
+ self.ocp = OCP(pd, params, footsteps, base_refs, gait)
self.last_available_result = Result(pd, params)
self.new_result = Value("b", False)
- def solve(self, k, x0, footstep, gait, xs=None, us=None):
+ def solve(self, k, x0, footstep, base_ref, gait, xs=None, us=None):
"""
Call either the asynchronous MPC or the synchronous MPC depending on the value
of multiprocessing during the creation of the wrapper
@@ -68,9 +71,9 @@ class MPC_Wrapper:
k (int): Number of inv dynamics iterations since the start of the simulation
"""
if self.multiprocessing:
- self.run_MPC_asynchronous(k, x0, footstep, gait, xs, us)
+ self.run_MPC_asynchronous(k, x0, footstep, base_ref, gait, xs, us)
else:
- self.run_MPC_synchronous(x0, footstep, gait, xs, us)
+ self.run_MPC_synchronous(x0, footstep, base_ref, gait, xs, us)
def get_latest_result(self):
"""
@@ -95,11 +98,11 @@ class MPC_Wrapper:
return self.last_available_result
- def run_MPC_synchronous(self, x0, footstep, gait, xs, us):
+ def run_MPC_synchronous(self, x0, footstep, base_ref, gait, xs, us):
"""
Run the MPC (synchronous version)
"""
- self.ocp.solve(x0, footstep, gait, xs, us)
+ self.ocp.solve(x0, footstep, base_ref, gait, xs, us)
(
self.last_available_result.xs,
self.last_available_result.us,
@@ -108,7 +111,7 @@ class MPC_Wrapper:
) = self.ocp.get_results()
self.new_result.value = True
- def run_MPC_asynchronous(self, k, x0, footstep, gait, xs, us):
+ def run_MPC_asynchronous(self, k, x0, footstep, base_ref, gait, xs, us):
"""
Run the MPC (asynchronous version)
"""
@@ -117,7 +120,7 @@ class MPC_Wrapper:
p = Process(target=self.MPC_asynchronous)
p.start()
- self.add_new_data(k, x0, footstep, gait, xs, us)
+ self.add_new_data(k, x0, footstep, base_ref, gait, xs, us)
def MPC_asynchronous(self):
"""
@@ -129,29 +132,29 @@ class MPC_Wrapper:
self.new_data.value = False
- k, x0, footstep, gait, xs, us = self.decompress_dataIn()
+ k, x0, footstep, base_ref, gait, xs, us = self.decompress_dataIn()
if k == 0:
loop_ocp = OCP(
- self.pd, self.params, self.footsteps_plan, self.initial_gait
+ self.pd, self.params, self.footsteps_plan, self.base_refs, self.initial_gait
)
- loop_ocp.solve(x0, footstep, gait, xs, us)
+ loop_ocp.solve(x0, footstep, base_ref, gait, xs, us)
xs, us, K, solving_time = loop_ocp.get_results()
self.compress_dataOut(xs, us, K, solving_time)
self.new_result.value = True
- def add_new_data(self, k, x0, footstep, gait, xs, us):
+ def add_new_data(self, k, x0, footstep, base_ref, gait, xs, us):
"""
Compress data in a C-type structure that belongs to the shared memory to send
data from the main control loop to the asynchronous MPC and notify the process
that there is a new data
"""
- self.compress_dataIn(k, x0, footstep, gait, xs, us)
+ self.compress_dataIn(k, x0, footstep, base_ref, gait, xs, us)
self.new_data.value = True
- def compress_dataIn(self, k, x0, footstep, gait, xs, us):
+ def compress_dataIn(self, k, x0, footstep, base_ref, gait, xs, us):
"""
Decompress data from a C-type structure that belongs to the shared memory to
retrieve data from the main control loop in the asynchronous MPC
@@ -163,6 +166,8 @@ class MPC_Wrapper:
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_base_ref.get_lock():
+ np.frombuffer(self.in_base_ref.get_obj()).reshape(3)[:] = base_ref
with self.in_gait.get_lock():
np.frombuffer(self.in_gait.get_obj()).reshape((self.T + 1, 4))[:, :] = gait
@@ -191,11 +196,13 @@ class MPC_Wrapper:
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_base_ref.get_lock():
+ base_ref = np.frombuffer(self.in_base_ref.get_obj()).reshape(3)
with self.in_gait.get_lock():
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
+ return k, x0, footstep, base_ref, gait, None, None
with self.in_xs.get_lock():
xs = list(
@@ -204,7 +211,7 @@ class MPC_Wrapper:
with self.in_us.get_lock():
us = list(np.frombuffer(self.in_us.get_obj()).reshape((self.T, self.nu)))
- return k, x0, footstep, gait, xs, us
+ return k, x0, footstep, base_ref, gait, xs, us
def compress_dataOut(self, xs, us, K, solving_time):
"""
diff --git a/python/quadruped_reactive_walking/tools/LoggerControl.py b/python/quadruped_reactive_walking/tools/LoggerControl.py
index 1f3e02e1..4d0d92ad 100644
--- a/python/quadruped_reactive_walking/tools/LoggerControl.py
+++ b/python/quadruped_reactive_walking/tools/LoggerControl.py
@@ -55,8 +55,11 @@ class LoggerControl:
self.t_ocp_solve = np.zeros(size)
# MPC
- self.q = np.zeros([size, pd.nq])
- self.v = np.zeros([size, pd.nv])
+ self.q_estimate_rpy = np.zeros([size, pd.nq - 1])
+ self.q_estimate = np.zeros([size, pd.nq])
+ self.v_estimate = np.zeros([size, pd.nv])
+ self.q_filtered = np.zeros([size, pd.nq])
+ self.v_filtered = np.zeros([size, pd.nv])
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])
@@ -64,6 +67,7 @@ class LoggerControl:
self.MPC_equivalent_Kd = np.zeros([size, self.pd.nu])
self.target = np.zeros([size, 3])
+ self.target_base = np.zeros([size, 3])
# Whole body control
self.wbc_P = np.zeros([size, 12]) # proportionnal gains of the PD+
@@ -108,8 +112,11 @@ class LoggerControl:
self.t_measures[self.i] = controller.t_measures
# Logging from model predictive control
- self.q[self.i] = np.array(controller.q_estimate)
- self.v[self.i] = np.array(controller.v_estimate)
+ self.q_estimate_rpy[self.i] = np.array(controller.q)
+ self.q_estimate[self.i] = np.array(controller.q_estimate)
+ self.v_estimate[self.i] = np.array(controller.v_estimate)
+ self.q_filtered[self.i] = np.array(controller.q_filtered)
+ self.v_filtered[self.i] = np.array(controller.v_filtered)
self.ocp_xs[self.i] = np.array(controller.mpc_result.xs)
self.ocp_us[self.i] = np.array(controller.mpc_result.us)
self.ocp_K[self.i] = controller.mpc_result.K[0]
@@ -128,10 +135,16 @@ class LoggerControl:
self.target[i] = controller.footsteps[i // self.params.mpc_wbc_ratio][
:, 1
]
+ self.target_base[i] = controller.base_refs[
+ i // self.params.mpc_wbc_ratio
+ ]
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]
+ self.target_base[
+ self.i + self.params.T * self.params.mpc_wbc_ratio
+ ] = controller.target_base[:]
if not self.params.enable_multiprocessing:
self.t_ocp_update[self.i] = controller.mpc.ocp.t_update
@@ -223,20 +236,38 @@ class LoggerControl:
def plot_target(self, save=False, fileName="/tmp"):
import matplotlib.pyplot as plt
- x_mes = np.concatenate([self.q, self.v], axis=1)
+ x = np.concatenate([self.q_filtered, self.v_filtered], axis=1)
m_feet_p_log = {
- idx: get_translation_array(self.pd, x_mes, idx)[0]
- for idx in self.pd.feet_ids
+ idx: get_translation_array(self.pd, x, idx)[0] for idx in self.pd.feet_ids
}
# Target plot
- _, axs = plt.subplots(3, sharex=True)
+ _, axs = plt.subplots(3, 2, 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[p, 0].set_title("Base position on " + legend[p])
+ axs[p, 0].plot(self.target_base[:, p])
+ axs[p, 0].plot(self.q_estimate[:, p])
+ axs[p, 0].plot(self.q_filtered[:, p])
+ axs[p, 0].legend(["Target", "Estimated", "Filtered"])
+
+ axs[p, 1].set_title("Base rotation on " + legend[p])
+ axs[p, 1].plot(self.q_estimate_rpy[:, 3 + p])
+ axs[p, 1].legend(["Estimated"])
+
+ _, axs = plt.subplots(3, 2, sharex=True)
+ legend = ["x", "y", "z"]
+ for p in range(3):
+ axs[p, 0].set_title("Base velocity on " + legend[p])
+ axs[p, 0].plot([self.pd.xref[19 + p]] * self.log_size)
+ axs[p, 0].plot(self.v_estimate[:, p])
+ axs[p, 0].plot(self.v_filtered[:, p])
+ axs[p, 0].legend(["Target", "Estimated", "Filtered"])
+
+ axs[p, 1].set_title("Base angular velocity on " + legend[p])
+ axs[p, 1].plot(self.v_estimate[:, 3 + p])
+ axs[p, 1].plot(self.v_filtered[:, 3 + p])
+ axs[p, 1].legend(["Estimated", "Filtered"])
_, axs = plt.subplots(3, sharex=True)
legend = ["x", "y", "z"]
@@ -317,8 +348,13 @@ class LoggerControl:
np.savez_compressed(
name,
- q=self.q,
- v=self.v,
+ target=self.target,
+ target_base=self.target_base,
+ q_estimate_rpy=self.q_estimate_rpy,
+ q_estimate=self.q_estimate,
+ v_estimate=self.v_estimate,
+ q_filtered=self.q_filtered,
+ v_filtered=self.v_filtered,
ocp_xs=self.ocp_xs,
ocp_us=self.ocp_us,
ocp_K=self.ocp_K,
@@ -364,6 +400,8 @@ class LoggerControl:
return
# Load sensors arrays
+ self.target = self.data["target"]
+ self.target_base = self.data["target_base"]
self.q_mes = self.data["q_mes"]
self.v_mes = self.data["v_mes"]
self.baseOrientation = self.data["baseOrientation"]
@@ -389,8 +427,11 @@ class LoggerControl:
self.t_loop = self.data["t_loop"]
self.t_measures = self.data["t_meausres"]
- self.q = self.data["q"]
- self.v = self.data["v"]
+ self.q_estimate_rpy = self.data["q_estimate_rpy"]
+ self.q_estimate = self.data["q_estimate"]
+ self.v_estimate = self.data["v_estimate"]
+ self.q_filtered = self.data["q_filtered"]
+ self.v_filtered = self.data["v_filtered"]
self.ocp_xs = self.data["ocp_xs"]
self.ocp_us = self.data["ocp_us"]
self.ocp_K = self.data["ocp_K"]
--
GitLab