Skip to content
Snippets Groups Projects
Commit 5f74e3b5 authored by Fanny Risbourg's avatar Fanny Risbourg
Browse files

log q, v and plot measured foot position

parent e8ab9ae5
No related branches found
No related tags found
No related merge requests found
Pipeline #21364 failed
Stage: test
Hide whitespace changes
Inline Side-by-side
config/walk_parameters.yaml
+ 1
− 1
View file @ 5f74e3b5
......@@ -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
......
python/quadruped_reactive_walking/Controller.py
+ 30
− 30
View file @ 5f74e3b5
......@@ -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()
python/quadruped_reactive_walking/tools/LoggerControl.py
+ 21
− 43
View file @ 5f74e3b5
......@@ -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"]
......
python/quadruped_reactive_walking/tools/kinematics_utils.py
+ 3
− 2
View file @ 5f74e3b5
......@@ -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()]
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment