From 5f74e3b5d00a8844dc35856a661be95e7fc01cd7 Mon Sep 17 00:00:00 2001
From: Fanny Risbourg <frisbourg@laas.fr>
Date: Mon, 29 Aug 2022 16:24:17 +0200
Subject: [PATCH] log q, v and plot measured foot position
---
config/walk_parameters.yaml | 2 +-
.../quadruped_reactive_walking/Controller.py | 60 ++++++++---------
.../tools/LoggerControl.py | 64 ++++++-------------
.../tools/kinematics_utils.py | 5 +-
4 files changed, 55 insertions(+), 76 deletions(-)
diff --git a/config/walk_parameters.yaml b/config/walk_parameters.yaml
index 2ebaf357..ca782763 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: true # 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
diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index ce5d1a5d..155b8a62 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -265,7 +265,7 @@ class Controller:
if self.mpc_result.new_result:
self.mpc_solved = True
self.k_new = self.k
- # print(f"MPC solved in {self.k - self.k_solve} iterations")
+ print(f"MPC solved in {self.k - self.k_solve} iterations")
# self.plot_mpc()
if not self.initialized and self.params.save_guess:
@@ -293,8 +293,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()
@@ -462,6 +462,8 @@ class Controller:
vj_m = device.joints.velocities
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])
+ self.q = np.concatenate([bp_m, qj_m])
+ self.v = np.concatenate([bv_m, vj_m])
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}
@@ -511,32 +513,30 @@ class Controller:
axs[1].legend(legend)
axs[1].set_title("Base velocity")
- plt.show()
-
- legend = ["Hip", "Shoulder", "Knee"]
- _, 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, 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, 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])
+ # legend = ["Hip", "Shoulder", "Knee"]
+ # _, 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, 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, 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])
plt.show()
diff --git a/python/quadruped_reactive_walking/tools/LoggerControl.py b/python/quadruped_reactive_walking/tools/LoggerControl.py
index dd514f28..201c6ea2 100644
--- a/python/quadruped_reactive_walking/tools/LoggerControl.py
+++ b/python/quadruped_reactive_walking/tools/LoggerControl.py
@@ -55,6 +55,8 @@ 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.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])
@@ -106,6 +108,8 @@ class LoggerControl:
self.t_measures[self.i] = controller.t_measures
# Logging from model predictive control
+ self.q[self.i] = np.array(controller.q)
+ self.v[self.i] = np.array(controller.v)
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]
@@ -219,53 +223,23 @@ 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.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
- # }
+ x_mes = np.concatenate([self.q, self.v], axis=1)
+ m_feet_p_log = {
+ idx: get_translation_array(self.pd, x_mes, idx)[0]
+ for idx in self.pd.feet_ids
+ }
# Target plot
legend = ["x", "y", "z"]
fig, axs = plt.subplots(3, sharex=True)
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].legend()
-
+ plt.subplot(3, 1, p + 1)
+ plt.title("Free foot on " + legend[p])
+ plt.plot(self.target[:, p])
+ plt.plot(m_feet_p_log[self.pd.feet_ids[1]][:, p])
+ plt.legend(["Target", "Measured"])
+ # "Predicted"])
if save:
plt.savefig(fileName + "/target")
@@ -298,7 +272,7 @@ class LoggerControl:
def plot_controller_times(self):
import matplotlib.pyplot as plt
- t_range = np.array([k * self.pd.dt for k in range(self.tstamps.shape[0])])
+ t_range = np.array([k * self.params.dt_mpc for k in range(self.tstamps.shape[0])])
plt.figure()
plt.plot(t_range, self.t_measures, "r+")
@@ -315,7 +289,7 @@ class LoggerControl:
def plot_OCP_times(self):
import matplotlib.pyplot as plt
- t_range = np.array([k * self.pd.dt for k in range(self.tstamps.shape[0])])
+ t_range = np.array([k * self.params.dt_mpc for k in range(self.tstamps.shape[0])])
plt.figure()
plt.plot(t_range, self.t_ocp_update, "r+")
@@ -333,6 +307,8 @@ class LoggerControl:
np.savez_compressed(
name,
+ q=self.q,
+ v=self.v,
ocp_xs=self.ocp_xs,
ocp_us=self.ocp_us,
ocp_K=self.ocp_K,
@@ -403,6 +379,8 @@ 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.ocp_xs = self.data["ocp_xs"]
self.ocp_us = self.data["ocp_us"]
self.ocp_K = self.data["ocp_K"]
diff --git a/python/quadruped_reactive_walking/tools/kinematics_utils.py b/python/quadruped_reactive_walking/tools/kinematics_utils.py
index 5dfcf907..ac960091 100644
--- a/python/quadruped_reactive_walking/tools/kinematics_utils.py
+++ b/python/quadruped_reactive_walking/tools/kinematics_utils.py
@@ -20,8 +20,9 @@ def get_translation_array(pd:ProblemData, x, idx, ref_frame=pin.WORLD, x0=None):
xiter = x
for xs in xiter:
- q = xs[: pd.nq]
- v = xs[pd.nq :]
+ q = xs[:pd.nq]
+ v = xs[pd.nq:]
+ print(v)
pin.forwardKinematics(pd.model, pd.rdata, q, v)
pin.updateFramePlacements(pd.model, pd.rdata)
frame_p += [pd.rdata.oMf[idx].translation.copy()]
--
GitLab