From 3078cc48ef2fa83eb3365a544a701145e24daf76 Mon Sep 17 00:00:00 2001
From: Fanny Risbourg <frisbourg@laas.fr>
Date: Wed, 6 Apr 2022 15:46:54 +0200
Subject: [PATCH] debug
---
include/qrw/FootTrajectoryGenerator.hpp | 3 +-
python/Params.cpp | 2 +
.../quadruped_reactive_walking/Controller.py | 42 ++++++++++---------
.../timeMPC/TimeMPC.py | 8 ++--
src/Controller.cpp | 2 +-
src/FootTrajectoryGenerator.cpp | 34 +++++++++++----
6 files changed, 57 insertions(+), 34 deletions(-)
diff --git a/include/qrw/FootTrajectoryGenerator.hpp b/include/qrw/FootTrajectoryGenerator.hpp
index 41fdcd88..14d535de 100644
--- a/include/qrw/FootTrajectoryGenerator.hpp
+++ b/include/qrw/FootTrajectoryGenerator.hpp
@@ -61,8 +61,7 @@ class FootTrajectoryGenerator {
/// \param[in] footstep desired footstep location
///
////////////////////////////////////////////////////////////////////////////////////////////////
- void update(int const newPhase, Vector4 const stepDurations, Vector4 const elapsedTimes, MatrixN const &gait,
- MatrixN const &footstep);
+ void update(int const newStep, double const stepDuration, MatrixN const &gait, MatrixN const &footstep);
MatrixN getFootPosition() { return position_; } // Get the next foot position
MatrixN getFootVelocity() { return velocity_; } // Get the next foot velocity
diff --git a/python/Params.cpp b/python/Params.cpp
index ac6ca014..b465fbd7 100644
--- a/python/Params.cpp
+++ b/python/Params.cpp
@@ -12,6 +12,8 @@ struct ParamsVisitor : public bp::def_visitor<ParamsVisitor<Params>> {
// Read Params from Python
.def_readwrite("config_file", &Params::config_file)
+ .def_readwrite("pickle_dir", &Params::pickle_dir)
+ .def_readwrite("networks_dir", &Params::networks_dir)
.def_readwrite("interface", &Params::interface)
.def_readwrite("DEMONSTRATION", &Params::DEMONSTRATION)
.def_readwrite("SIMULATION", &Params::SIMULATION)
diff --git a/python/quadruped_reactive_walking/Controller.py b/python/quadruped_reactive_walking/Controller.py
index fdf59af4..1a2849f8 100644
--- a/python/quadruped_reactive_walking/Controller.py
+++ b/python/quadruped_reactive_walking/Controller.py
@@ -92,18 +92,21 @@ class Controller:
if self.type_MPC == 4:
self.mpc = TimeMPC.TimeMPC(params, self.q_init)
- self.mpc_result = np.zeros((28, 1))
+ self.mpc_result = np.zeros((28, params.gait.shape[0]))
self.mpc_result[20, 0] = self.dt_mpc
+ self.mpc_forces = np.zeros(12)
self.n_nodes_first_step = params.N_nodes_step
self.n_nodes_step = params.N_nodes_step
# self.combine_pickle_dirs(params.pickle_dir)
self.h_mpc_footstep = np.array([0.1946, 0.14695, 0.1946, -0.14695, -0.1946, 0.14695, -0.1946, -0.14695])
self.o_mpc_footstep = self.h_mpc_footstep.copy().reshape((2, 4), order='F')
+
+ # self.networks = self.get_networks(params.networks_dir)
else:
self.mpc = MPC_Wrapper.MPC_Wrapper(params, self.q_init)
- self.next_footstep = np.reshape(
- params.footsteps_under_shoulders, (3, 4), order="F"
- )
+ self.next_footstep = np.reshape(
+ params.footsteps_under_shoulders, (3, 4), order="F"
+ )
self.enable_multiprocessing_mip = params.enable_multiprocessing_mip
if params.solo3D:
@@ -253,10 +256,11 @@ class Controller:
)
self.footTrajectoryGenerator.update(
new_phase,
- step_durations,
- elapsed_durations,
+ self.gait.n_nodes_first_step * self.dt_mpc,
+ # step_durations,
+ # elapsed_durations,
gait_matrix,
- self.next_footstep,
+ self.o_mpc_footstep
)
if not self.error and not self.joystick.get_stop():
@@ -574,14 +578,15 @@ class Controller:
if (self.k % self.k_mpc) == 0:
try:
if self.type_MPC == 4:
- reduced_input = self.get_reduced_input(
- gait.copy(),
- reference_state[:, 0].copy(),
- self.vref_filtered.copy(),
- self.h_mpc_footstep,
- self.n_nodes_first_step,
- )
- print(reduced_input)
+ # reduced_input = self.get_reduced_input(
+ # gait.copy(),
+ # reference_state[:, 0].copy(),
+ # self.vref_filtered.copy(),
+ # self.h_mpc_footstep,
+ # self.n_nodes_first_step,
+ # )
+ xs_in, us_in = None, None
+ # print(reduced_input)
# if not new_phase:
# xs_in, us_in = None, None
# elif self.networks[int(reduced_input[0])][int(reduced_input[1]) - 1] is not None:
@@ -590,7 +595,6 @@ class Controller:
# else:
# print("Error with NN " + str(reduced_input[0]) + " " + str(reduced_input[1] - 1))
# raise AssertionError
- xs_in, us_in = None, None
# xs_out, us_out = self.mpc.solve(
self.mpc.solve(
self.k,
@@ -632,7 +636,6 @@ class Controller:
self.mpc_cost = 0
if self.type_MPC == 4:
- self.mpc_forces = np.zeros(12)
i = 0
for foot in range(4):
if gait[0, foot] == 1:
@@ -659,7 +662,8 @@ class Controller:
self.h_mpc_footstep[2 * foot : 2 * foot + 2] = footpos[:2]
self.o_mpc_footstep[:, foot] = (oRh @ footpos + oTh.ravel())[:2]
- self.next_footstep[:, foot] = (oRh @ footpos + oTh.ravel())
+ print("----------------------------")
+ print(self.h_mpc_footstep)
if self.DEMONSTRATION and self.gait.is_static:
self.mpc_result[12:24, 0] = [0.0, 0.0, 9.81 * 2.5 / 4.0] * 4
@@ -703,7 +707,7 @@ class Controller:
R = pin.rpy.rpyToMatrix(0.0, 0.0, self.q_filtered[5]).transpose()
else:
T = -oTh - np.array([0.0, 0.0, self.h_ref]).reshape((3, 1))
- R = hRb @ oRh.transpose()
+ R = oRh.transpose()
self.feet_a_cmd = R @ self.footTrajectoryGenerator.get_foot_acceleration()
self.feet_v_cmd = R @ self.footTrajectoryGenerator.get_foot_velocity()
diff --git a/python/quadruped_reactive_walking/timeMPC/TimeMPC.py b/python/quadruped_reactive_walking/timeMPC/TimeMPC.py
index bc595efe..a9887f32 100644
--- a/python/quadruped_reactive_walking/timeMPC/TimeMPC.py
+++ b/python/quadruped_reactive_walking/timeMPC/TimeMPC.py
@@ -87,12 +87,14 @@ class TimeMPC:
if self.multiprocessing:
self.run_MPC_asynchronous(k, gait.copy(), self.x0.copy(), x_ref[:, 1:].copy(), n_nodes_first_step)
else:
- # import matplotlib.pyplot as plt
+ import matplotlib.pyplot as plt
+
+ print(np.repeat(gait[0, :], 2) * footsteps)
self.result, xs_out, us_out = self.casadi_mpc.solve_problem(gait.copy(), self.x0.copy(), x_ref[:, 1:].copy(), n_nodes_first_step, xs=xs_in, us=us_in, use_constraint=True)
# self.result, xs_out, us_out = self.casadi_mpc.solve_problem(gait.copy(), self.x0.copy(), x_ref[:, 1:].copy(), n_nodes_first_step, xs=xs_out, us=us_out, use_constraint=False)
# self.plot_state(xs_out, us_out, n_nodes, x_ref[:, 1:].copy())
- # self.plot_feet_time(xs_out, us_out, self.x0, n_nodes)
+ self.plot_feet_time(xs_out, us_out, self.x0, n_nodes)
# self.result, xs_out, us_out = self.mpc.solve_problem(gait.copy(), self.x0.copy(), x_ref[:, 1:].copy(), n_nodes_first_step, xs=xs_in, us=us_in)
# self.plot_state(self.mpc.x_init, self.mpc.u_init, n_nodes, x_ref[:, 1:].copy())
# self.plot_feet_time(self.mpc.x_init, self.mpc.u_init, self.x0, n_nodes)
@@ -100,7 +102,7 @@ class TimeMPC:
# self.plot_state(xs_out, us_out, n_nodes, x_ref[:, 1:].copy())
# self.plot_feet_time(xs_out, us_out, self.x0, n_nodes)
- # plt.show()
+ plt.show()
return xs_out, us_out
diff --git a/src/Controller.cpp b/src/Controller.cpp
index bc44a3c8..1b544c2a 100644
--- a/src/Controller.cpp
+++ b/src/Controller.cpp
@@ -100,7 +100,7 @@ void Controller::compute(FakeRobot* robot) {
stepDurations(i) = gait.getPhaseDuration(0, i);
elapsedTimes(i) = gait.getElapsedTime(0, i);
}
- footTrajectoryGenerator.update(gait.getNewPhase(), stepDurations, elapsedTimes, gait.getCurrentGait(), o_targetFootstep);
+ footTrajectoryGenerator.update(gait.getNewPhase(), stepDurations[0], gait.getCurrentGait(), o_targetFootstep);
// Whole Body Control
// If nothing wrong happened yet in the WBC controller
diff --git a/src/FootTrajectoryGenerator.cpp b/src/FootTrajectoryGenerator.cpp
index e9031bc4..e5e05fb8 100644
--- a/src/FootTrajectoryGenerator.cpp
+++ b/src/FootTrajectoryGenerator.cpp
@@ -163,15 +163,13 @@ void FootTrajectoryGenerator::updateFootPosition(int const j, Vector3 const &foo
Az(0, j) * std::pow(evz, 6);
}
-void FootTrajectoryGenerator::update(int const newPhase, Vector4 const stepDurations, Vector4 const elapsedTimes,
- MatrixN const &gait, MatrixN const &footstep) {
- if (gait.row(0).sum() == 4) return;
-
- if (newPhase) {
- stepDuration_ = stepDurations;
- tStep_ = elapsedTimes;
+void FootTrajectoryGenerator::update(int const newStep, double const stepDuration, MatrixN const &gait,
+ MatrixN const &footstep) {
+ if (newStep) {
+ stepDuration_ = stepDuration * Vector4::Ones(); // 0.0 for swing phase
+ tStep_ = Vector4::Zero();
} else {
- tStep_ += dt_wbc * Vector4::Ones();
+ tStep_ += dt_wbc * Vector4::Ones();
}
for (int i = 0; i < 4; i++) {
@@ -179,4 +177,22 @@ void FootTrajectoryGenerator::update(int const newPhase, Vector4 const stepDurat
updateFootPosition(i, footstep.col(i));
}
}
-}
\ No newline at end of file
+}
+
+// void FootTrajectoryGenerator::update(int const newPhase, Vector4 const stepDurations, Vector4 const elapsedTimes,
+// MatrixN const &gait, MatrixN const &footstep) {
+// if (gait.row(0).sum() == 4) return;
+
+// if (newPhase) {
+// stepDuration_ = stepDurations;
+// tStep_ = elapsedTimes;
+// } else {
+// tStep_ += dt_wbc * Vector4::Ones();
+// }
+
+// for (int i = 0; i < 4; i++) {
+// if ((int)gait(0, i) == 0) {
+// updateFootPosition(i, footstep.col(i));
+// }
+// }
+// }
\ No newline at end of file
--
GitLab