diff --git a/include/qrw/FootstepPlanner.hpp b/include/qrw/FootstepPlanner.hpp
index 09d9288f4655ed33ee6805ff4d275b59927d670d..c29de3d8a38b6738ffcf2b312bfaf6cc1f1799f4 100644
--- a/include/qrw/FootstepPlanner.hpp
+++ b/include/qrw/FootstepPlanner.hpp
@@ -65,7 +65,6 @@ class FootstepPlanner {
MatrixN getFootsteps();
MatrixN getTargetFootsteps();
- MatrixN getRz();
private:
////////////////////////////////////////////////////////////////////////////////////////////////
diff --git a/include/qrw/InvKin.hpp b/include/qrw/InvKin.hpp
index b2259718b533b919bc299dc25e18861580d1ab8f..114607529da416e92080142c8bd9032cfba62d6a 100644
--- a/include/qrw/InvKin.hpp
+++ b/include/qrw/InvKin.hpp
@@ -66,7 +66,7 @@ class InvKin {
/// \param[in] x_cmd Desired position, orientation and velocity of the base
///
////////////////////////////////////////////////////////////////////////////////////////////////
- void run_InvKin(VectorN const& q, VectorN const& dq, MatrixN const& contacts, MatrixN const& pgoals,
+ void run(VectorN const& q, VectorN const& dq, MatrixN const& contacts, MatrixN const& pgoals,
MatrixN const& vgoals, MatrixN const& agoals, MatrixN const& x_cmd);
VectorN get_q_step() { return q_step_; }
diff --git a/include/qrw/Solo3D/FootstepPlannerQP.hpp b/include/qrw/Solo3D/FootstepPlannerQP.hpp
index 9a482b2fa8b531e0b7ba37d0d80a923f5a7b1e54..dc9c2df5d74b5ffc77814c9a0ba627067ac002d7 100644
--- a/include/qrw/Solo3D/FootstepPlannerQP.hpp
+++ b/include/qrw/Solo3D/FootstepPlannerQP.hpp
@@ -95,7 +95,6 @@ class FootstepPlannerQP {
MatrixN getFootsteps();
MatrixN getTargetFootsteps();
- MatrixN getRz();
private:
////////////////////////////////////////////////////////////////////////////////////////////////
diff --git a/include/qrw/Solo3D/StatePlanner3D.hpp b/include/qrw/Solo3D/StatePlanner3D.hpp
index 66945ea3bd8d59a4b249d64ed570a895d5d197f6..b4dee41f80010c4f35a230dc55ef94ce0f5ba7b9 100644
--- a/include/qrw/Solo3D/StatePlanner3D.hpp
+++ b/include/qrw/Solo3D/StatePlanner3D.hpp
@@ -74,7 +74,6 @@ class StatePlanner3D {
void computeConfigurations(VectorN const& q, Vector6 const& vref);
MatrixN getReferenceStates() { return referenceStates_; }
- int getNumberStates() { return nStates_; }
MatrixN getConfigurations() { return configs_; }
VectorN getFit() { return fit_; }
diff --git a/include/qrw/StatePlanner.hpp b/include/qrw/StatePlanner.hpp
index 9055eac0099e95a7d64e8113c6bcc22494123ff6..fd7e0ca0e197f47f340e56eb6f94430b88a7288f 100644
--- a/include/qrw/StatePlanner.hpp
+++ b/include/qrw/StatePlanner.hpp
@@ -53,7 +53,6 @@ class StatePlanner {
void computeReferenceStates(VectorN const& q, Vector6 const& v, Vector6 const& vref);
MatrixN getReferenceStates() { return referenceStates_; }
- int getNumberStates() { return n_steps_; }
private:
double dt_; // Time step of the contact sequence (time step of the MPC)
diff --git a/python/Filter.cpp b/python/Filter.cpp
index 80968fc99080de8ea5d60e63ad5f48dc45521282..00fc7e812e422882c8e7588734e535c27c573637 100644
--- a/python/Filter.cpp
+++ b/python/Filter.cpp
@@ -7,8 +7,7 @@ struct FilterVisitor : public bp::def_visitor<FilterVisitor<Filter>> {
void visit(PyClassFilter& cl) const {
cl.def(bp::init<>(bp::arg(""), "Default constructor."))
.def("initialize", &Filter::initialize, bp::args("params"), "Initialize Filter from Python.\n")
- .def("filter", &Filter::filter, bp::args("x", "check_modulo"), "Run Filter from Python.\n")
- .def("getFilt", &Filter::getFilt, "Get filtered quantity.\n");
+ .def("filter", &Filter::filter, bp::args("x", "check_modulo"), "Run Filter from Python.\n");
}
static void expose() { bp::class_<Filter>("Filter", bp::no_init).def(FilterVisitor<Filter>()); }
diff --git a/python/FootTrajectoryGenerator.cpp b/python/FootTrajectoryGenerator.cpp
index 607439fab9caa37b1a03a1afedfb1d613dfa7488..6b3271b74e1f2b8b0c50551ad8804d82ed7cc7aa 100644
--- a/python/FootTrajectoryGenerator.cpp
+++ b/python/FootTrajectoryGenerator.cpp
@@ -9,18 +9,18 @@ struct FootTrajectoryGeneratorVisitor
void visit(PyClassFootTrajectoryGenerator& cl) const {
cl.def(bp::init<>(bp::arg(""), "Default constructor."))
- .def("getFootPositionBaseFrame", &FootTrajectoryGenerator::getFootPositionBaseFrame, bp::args("R", "T"),
+ .def("get_foot_position_base_frame", &FootTrajectoryGenerator::getFootPositionBaseFrame, bp::args("R", "T"),
"Get position_ matrix in base frame.\n")
- .def("getFootVelocityBaseFrame", &FootTrajectoryGenerator::getFootVelocityBaseFrame,
+ .def("get_foot_velocity_base_frame", &FootTrajectoryGenerator::getFootVelocityBaseFrame,
bp::args("R", "v_ref", "w_ref"), "Get velocity_ matrix in base frame.\n")
- .def("getFootAccelerationBaseFrame", &FootTrajectoryGenerator::getFootAccelerationBaseFrame,
+ .def("get_foot_acceleration_base_frame", &FootTrajectoryGenerator::getFootAccelerationBaseFrame,
bp::args("R", "w_ref", "a_ref"), "Get acceleration_ matrix in base frame.\n")
- .def("getTargetPosition", &FootTrajectoryGenerator::getTargetPosition, "Get targetFootstep_ matrix.\n")
- .def("getFootPosition", &FootTrajectoryGenerator::getFootPosition, "Get position_ matrix.\n")
- .def("getFootVelocity", &FootTrajectoryGenerator::getFootVelocity, "Get velocity_ matrix.\n")
- .def("getFootAcceleration", &FootTrajectoryGenerator::getFootAcceleration, "Get acceleration_ matrix.\n")
- .def("getFootJerk", &FootTrajectoryGenerator::getFootJerk, "Get jerk_ matrix.\n")
+ .def("get_target_position", &FootTrajectoryGenerator::getTargetPosition, "Get targetFootstep_ matrix.\n")
+ .def("get_foot_position", &FootTrajectoryGenerator::getFootPosition, "Get position_ matrix.\n")
+ .def("get_foot_velocity", &FootTrajectoryGenerator::getFootVelocity, "Get velocity_ matrix.\n")
+ .def("get_foot_acceleration", &FootTrajectoryGenerator::getFootAcceleration, "Get acceleration_ matrix.\n")
+ .def("get_foot_jerk", &FootTrajectoryGenerator::getFootJerk, "Get jerk_ matrix.\n")
.def("initialize", &FootTrajectoryGenerator::initialize, bp::args("params", "gaitIn"),
"Initialize FootTrajectoryGenerator from Python.\n")
@@ -28,8 +28,8 @@ struct FootTrajectoryGeneratorVisitor
.def("update", &FootTrajectoryGenerator::update, bp::args("k", "targetFootstep"),
"Compute target location of footsteps from Python.\n")
- .def("getT0s", &FootTrajectoryGenerator::getT0s, "Get the current timings of the flying feet.\n")
- .def("getTswing", &FootTrajectoryGenerator::getTswing, "Get the flying period of the feet.\n");
+ .def("get_elapsed_durations", &FootTrajectoryGenerator::getT0s, "Get the current timings of the flying feet.\n")
+ .def("get_phase_durations", &FootTrajectoryGenerator::getTswing, "Get the flying period of the feet.\n");
}
static void expose() {
diff --git a/python/FootstepPlanner.cpp b/python/FootstepPlanner.cpp
index 6eee66ebee6bb1a90084d17e082b60eade7fed6d..fba7046d58be293441b54243cadc11bcab9cb8c8 100644
--- a/python/FootstepPlanner.cpp
+++ b/python/FootstepPlanner.cpp
@@ -8,15 +8,14 @@ struct FootstepPlannerVisitor : public bp::def_visitor<FootstepPlannerVisitor<Fo
void visit(PyClassFootstepPlanner& cl) const {
cl.def(bp::init<>(bp::arg(""), "Default constructor."))
- .def("getFootsteps", &FootstepPlanner::getFootsteps, "Get footsteps_ matrix.\n")
- .def("getTargetFootsteps", &FootstepPlanner::getTargetFootsteps, "Get footsteps_ matrix.\n")
- .def("getRz", &FootstepPlanner::getRz, "Get rotation along z matrix.\n")
+ .def("get_footsteps", &FootstepPlanner::getFootsteps, "Get footsteps_ matrix.\n")
+ .def("get_target_footsteps", &FootstepPlanner::getTargetFootsteps, "Get footsteps_ matrix.\n")
.def("initialize", &FootstepPlanner::initialize, bp::args("params", "gaitIn"),
"Initialize FootstepPlanner from Python.\n")
// Compute target location of footsteps from Python
- .def("updateFootsteps", &FootstepPlanner::updateFootsteps, bp::args("refresh", "k", "q", "b_v", "b_vref"),
+ .def("update_footsteps", &FootstepPlanner::updateFootsteps, bp::args("refresh", "k", "q", "b_v", "b_vref"),
"Update and compute location of footsteps from Python.\n");
}
diff --git a/python/InvKin.cpp b/python/InvKin.cpp
index 941878a1d1068bc3cbf123c0a4939f2b70b00e7e..75a9de3e1ce2dabd8da508825ffd3c802d06d47d 100644
--- a/python/InvKin.cpp
+++ b/python/InvKin.cpp
@@ -17,13 +17,11 @@ struct InvKinVisitor : public bp::def_visitor<InvKinVisitor<InvKin>> {
.def("get_foot_id", &InvKin::get_foot_id, bp::args("i"), "Get food frame id.\n")
// Run InvKin from Python
- .def("run_InvKin", &InvKin::run_InvKin, bp::args("contacts", "pgoals", "vgoals", "agoals", "x_cmd"),
+ .def("run", &InvKin::run, bp::args("contacts", "pgoals", "vgoals", "agoals", "x_cmd"),
"Run InvKin from Python.\n");
}
- static void expose() {
- bp::class_<InvKin>("InvKin", bp::no_init).def(InvKinVisitor<InvKin>());
- }
+ static void expose() { bp::class_<InvKin>("InvKin", bp::no_init).def(InvKinVisitor<InvKin>()); }
};
void exposeInvKin() { InvKinVisitor<InvKin>::expose(); }
diff --git a/python/Joystick.cpp b/python/Joystick.cpp
index b187c39b6a82fcf10ee6641291c69f4b96703bce..a4fb280adaee3caf79907c5fa9735ccba79ef0d0 100644
--- a/python/Joystick.cpp
+++ b/python/Joystick.cpp
@@ -12,17 +12,18 @@ struct JoystickVisitor : public bp::def_visitor<JoystickVisitor<Joystick>> {
.def("update_v_ref", &Joystick::update_v_ref, bp::args("k", "velID", "gait_is_static", "h_v"),
"Update joystick values.")
- .def("getPRef", &Joystick::getPRef, "Get Reference Position")
- .def("getVRef", &Joystick::getVRef, "Get Reference Velocity")
- .def("getJoystickCode", &Joystick::getJoystickCode, "Get Joystick Code")
- .def("getStart", &Joystick::getStart, "Get Joystick Start")
- .def("getStop", &Joystick::getStop, "Get Joystick Stop")
- .def("getCross", &Joystick::getCross, "Get Joystick Cross status")
- .def("getCircle", &Joystick::getCircle, "Get Joystick Circle status")
- .def("getTriangle", &Joystick::getTriangle, "Get Joystick Triangle status")
- .def("getSquare", &Joystick::getSquare, "Get Joystick Square status")
- .def("getL1", &Joystick::getL1, "Get Joystick L1 status")
- .def("getR1", &Joystick::getR1, "Get Joystick R1 status");
+
+ .def("get_p_ref", &Joystick::getPRef, "Get Reference Position")
+ .def("get_v_ref", &Joystick::getVRef, "Get Reference Velocity")
+ .def("get_joystick_code", &Joystick::getJoystickCode, "Get Joystick Code")
+ .def("get_start", &Joystick::getStart, "Get Joystick Start")
+ .def("get_stop", &Joystick::getStop, "Get Joystick Stop")
+ .def("get_cross", &Joystick::getCross, "Get Joystick Cross status")
+ .def("get_circle", &Joystick::getCircle, "Get Joystick Circle status")
+ .def("get_triangle", &Joystick::getTriangle, "Get Joystick Triangle status")
+ .def("get_square", &Joystick::getSquare, "Get Joystick Square status")
+ .def("get_l1", &Joystick::getL1, "Get Joystick L1 status")
+ .def("get_r1", &Joystick::getR1, "Get Joystick R1 status");
}
static void expose() { bp::class_<Joystick>("Joystick", bp::no_init).def(JoystickVisitor<Joystick>()); }
diff --git a/python/Solo3D/FootTrajectoryGeneratorBezier.cpp b/python/Solo3D/FootTrajectoryGeneratorBezier.cpp
index 11ce47ed6bda45a7ab7d8efe1c365aa3d3119a83..922910506c015c905bed9c58da36f69e74e7f984 100644
--- a/python/Solo3D/FootTrajectoryGeneratorBezier.cpp
+++ b/python/Solo3D/FootTrajectoryGeneratorBezier.cpp
@@ -9,17 +9,18 @@ struct FootTrajectoryGeneratorBezierVisitor
void visit(PyClassFootTrajectoryGeneratorBezier& cl) const {
cl.def(bp::init<>(bp::arg(""), "Default constructor."))
- .def("getFootPosition", &FootTrajectoryGeneratorBezier::getFootPosition, "Get position_ matrix.\n")
- .def("getFootVelocity", &FootTrajectoryGeneratorBezier::getFootVelocity, "Get velocity_ matrix.\n")
- .def("getFootAcceleration", &FootTrajectoryGeneratorBezier::getFootAcceleration, "Get acceleration_ matrix.\n")
- .def("getFootJerk", &FootTrajectoryGeneratorBezier::getFootJerk, "Get jerk_ matrix.\n")
- .def("evaluateBezier", &FootTrajectoryGeneratorBezier::evaluateBezier, "Evaluate Bezier curve by foot.\n")
- .def("evaluatePoly", &FootTrajectoryGeneratorBezier::evaluatePoly, "Evaluate Bezier curve by foot.\n")
- .def("getFootPositionBaseFrame", &FootTrajectoryGeneratorBezier::getFootPositionBaseFrame, bp::args("R", "T"),
- "Get position_ matrix in base frame.\n")
- .def("getFootVelocityBaseFrame", &FootTrajectoryGeneratorBezier::getFootVelocityBaseFrame,
+ .def("get_foot_position", &FootTrajectoryGeneratorBezier::getFootPosition, "Get position_ matrix.\n")
+ .def("get_foot_velocity", &FootTrajectoryGeneratorBezier::getFootVelocity, "Get velocity_ matrix.\n")
+ .def("get_foot_acceleration", &FootTrajectoryGeneratorBezier::getFootAcceleration,
+ "Get acceleration_ matrix.\n")
+ .def("get_foot_jerk", &FootTrajectoryGeneratorBezier::getFootJerk, "Get jerk_ matrix.\n")
+ .def("evaluate_bezier", &FootTrajectoryGeneratorBezier::evaluateBezier, "Evaluate Bezier curve by foot.\n")
+ .def("evaluate_polynom", &FootTrajectoryGeneratorBezier::evaluatePoly, "Evaluate Bezier curve by foot.\n")
+ .def("get_foot_position_base_frame", &FootTrajectoryGeneratorBezier::getFootPositionBaseFrame,
+ bp::args("R", "T"), "Get position_ matrix in base frame.\n")
+ .def("get_foot_velocity_base_frame", &FootTrajectoryGeneratorBezier::getFootVelocityBaseFrame,
bp::args("R", "v_ref", "w_ref"), "Get velocity_ matrix in base frame.\n")
- .def("getFootAccelerationBaseFrame", &FootTrajectoryGeneratorBezier::getFootAccelerationBaseFrame,
+ .def("get_foot_acceleration_base_frame", &FootTrajectoryGeneratorBezier::getFootAccelerationBaseFrame,
bp::args("R", "w_ref", "a_ref"), "Get acceleration_ matrix in base frame.\n")
.def("initialize", &FootTrajectoryGeneratorBezier::initialize, bp::args("params", "gaitIn"),
diff --git a/python/Solo3D/FootstepPlannerQP.cpp b/python/Solo3D/FootstepPlannerQP.cpp
index 3fb91cc0628635e3b1996768b910d7534183f955..73490ffcafb7094f6064496823098c308baf7ae3 100644
--- a/python/Solo3D/FootstepPlannerQP.cpp
+++ b/python/Solo3D/FootstepPlannerQP.cpp
@@ -9,17 +9,16 @@ struct FootstepPlannerQPVisitor : public bp::def_visitor<FootstepPlannerQPVisito
void visit(PyClassFootstepPlannerQP& cl) const {
cl.def(bp::init<>(bp::arg(""), "Default constructor."))
- .def("getFootsteps", &FootstepPlannerQP::getFootsteps, "Get footsteps_ matrix.\n")
- .def("getTargetFootsteps", &FootstepPlannerQP::getTargetFootsteps, "Get footsteps_ matrix.\n")
- .def("getRz", &FootstepPlannerQP::getRz, "Get rotation along z matrix.\n")
+ .def("get_footsteps", &FootstepPlannerQP::getFootsteps, "Get footsteps_ matrix.\n")
+ .def("get_target_footsteps", &FootstepPlannerQP::getTargetFootsteps, "Get footsteps_ matrix.\n")
.def("initialize", &FootstepPlannerQP::initialize, bp::args("params", "gaitIn"),
"Initialize FootstepPlanner from Python.\n")
// Compute target location of footsteps from Python
- .def("updateFootsteps", &FootstepPlannerQP::updateFootsteps, bp::args("refresh", "k", "q", "b_v", "b_vref"),
+ .def("update_footsteps", &FootstepPlannerQP::updateFootsteps, bp::args("refresh", "k", "q", "b_v", "b_vref"),
"Update and compute location of footsteps from Python.\n")
- .def("updateSurfaces", &FootstepPlannerQP::updateSurfaces,
+ .def("update_surfaces", &FootstepPlannerQP::updateSurfaces,
bp::args("potential_surfaces", "selected_surfaces", "status", "iterations"),
"Update the surfaces from surface planner.\n");
}
diff --git a/python/Solo3D/StatePlanner3D.cpp b/python/Solo3D/StatePlanner3D.cpp
index 1ef2cc79504cbe97fca786f0886a4f9d79e46305..f93158c7d1d45ba7a433097d52ef1577648e9ba6 100644
--- a/python/Solo3D/StatePlanner3D.cpp
+++ b/python/Solo3D/StatePlanner3D.cpp
@@ -8,17 +8,16 @@ struct StatePlanner3DVisitor : public bp::def_visitor<StatePlanner3DVisitor<Stat
void visit(PyClassStatePlanner3D& cl) const {
cl.def(bp::init<>(bp::arg(""), "Default constructor."))
- .def("getReferenceStates", &StatePlanner3D::getReferenceStates, "Get xref matrix.\n")
- .def("getNumberStates", &StatePlanner3D::getNumberStates, "Get number of steps in prediction horizon.\n")
- .def("getConfigurations", &StatePlanner3D::getConfigurations, "Get conf vector.\n")
+ .def("get_reference_states", &StatePlanner3D::getReferenceStates, "Get xref matrix.\n")
+ .def("get_configurations", &StatePlanner3D::getConfigurations, "Get conf vector.\n")
.def("initialize", &StatePlanner3D::initialize, bp::args("params"), "Initialize StatePlanner3D from Python.\n")
// Run StatePlanner3D from Python
- .def("computeReferenceStates", &StatePlanner3D::computeReferenceStates, bp::args("q", "v", "b_vref"),
+ .def("compute_reference_states", &StatePlanner3D::computeReferenceStates, bp::args("q", "v", "b_vref"),
"Run StatePlanner from Python.\n")
- .def("getFit", &StatePlanner3D::getFit, "Get the fitted surface.\n")
- .def("updateSurface", &StatePlanner3D::updateSurface, bp::args("q", "b_vref"),
+ .def("get_fit", &StatePlanner3D::getFit, "Get the fitted surface.\n")
+ .def("update_surface", &StatePlanner3D::updateSurface, bp::args("q", "b_vref"),
"Update the average surface from heightmap and positions.\n");
}
diff --git a/python/Solo3D/Surface.cpp b/python/Solo3D/Surface.cpp
index c15373ef8a465e8bbe46aa20b12c3c8e98bf9983..ff901b70ad910d04e2dbd77173acc190a80185eb 100644
--- a/python/Solo3D/Surface.cpp
+++ b/python/Solo3D/Surface.cpp
@@ -18,7 +18,7 @@ struct SurfaceVisitor : public bp::def_visitor<SurfaceVisitor<Surface>> {
.add_property("vertices",
bp::make_function(&Surface::getVertices, bp::return_value_policy<bp::return_by_value>()))
- .def("getHeight", &Surface::getHeight, bp::args("point"), "get the height of a point of the surface.\n")
+ .def("get_height", &Surface::getHeight, bp::args("point"), "get the height of a point of the surface.\n")
.def("has_point", &Surface::hasPoint, bp::args("point"), "return true if the point is in the surface.\n");
}
diff --git a/python/StatePlanner.cpp b/python/StatePlanner.cpp
index 2e06269060688928556affd91a13089c578a7f10..40a04612c94cd6be42ce8c33fccc1fec34aa6a05 100644
--- a/python/StatePlanner.cpp
+++ b/python/StatePlanner.cpp
@@ -8,13 +8,9 @@ struct StatePlannerVisitor : public bp::def_visitor<StatePlannerVisitor<StatePla
void visit(PyClassStatePlanner& cl) const {
cl.def(bp::init<>(bp::arg(""), "Default constructor."))
- .def("getReferenceStates", &StatePlanner::getReferenceStates, "Get xref matrix.\n")
- .def("getNumberStates", &StatePlanner::getNumberStates, "Get number of steps in prediction horizon.\n")
-
+ .def("get_reference_states", &StatePlanner::getReferenceStates, "Get xref matrix.\n")
.def("initialize", &StatePlanner::initialize, bp::args("params"), "Initialize StatePlanner from Python.\n")
-
- // Run StatePlanner from Python
- .def("computeReferenceStates", &StatePlanner::computeReferenceStates, bp::args("q", "v", "b_vref"),
+ .def("compute_reference_states", &StatePlanner::computeReferenceStates, bp::args("q", "v", "b_vref"),
"Run StatePlanner from Python.\n");
}
diff --git a/python/WbcWrapper.cpp b/python/WbcWrapper.cpp
index ce54cd2e8750267e73096afb659b7a6ff8a1b8c8..7a47b75ff174c55bf627bfdd4c7e3e12afd32632 100644
--- a/python/WbcWrapper.cpp
+++ b/python/WbcWrapper.cpp
@@ -38,7 +38,6 @@ struct WbcWrapperVisitor : public bp::def_visitor<WbcWrapperVisitor<WbcWrapper>>
.def_readonly("Mddq_out", &WbcWrapper::get_Mddq_out)
.def_readonly("JcTf_out", &WbcWrapper::get_JcTf_out)
- // Run WbcWrapper from Python
.def("compute", &WbcWrapper::compute,
bp::args("q", "dq", "f_cmd", "contacts", "pgoals", "vgoals", "agoals", "xgoals"),
"Run WbcWrapper from Python.\n");
diff --git a/scripts/Controller.py b/scripts/Controller.py
index 8e1d9af8d688120c6ed7170030934d781279fae8..af012dc435afeb53601956c74f98baf7fd825f1f 100644
--- a/scripts/Controller.py
+++ b/scripts/Controller.py
@@ -291,7 +291,7 @@ class Controller:
self.n_nan = 0
self.estimator.run(self.gait.matrix,
- self.footTrajectoryGenerator.getFootPosition(),
+ self.footTrajectoryGenerator.get_foot_position(),
device.imu.linear_acceleration,
device.imu.gyroscope,
device.imu.attitude_euler,
@@ -300,7 +300,7 @@ class Controller:
q_perfect, b_baseVel_perfect)
# Update state vectors of the robot (q and v) + transformation matrices between world and horizontal frames
- self.estimator.update_reference_state(self.joystick.getVRef())
+ self.estimator.update_reference_state(self.joystick.get_v_ref())
oRh = self.estimator.get_oRh()
hRb = self.estimator.get_hRb()
oTh = self.estimator.get_oTh().reshape((3, 1))
@@ -331,31 +331,31 @@ class Controller:
t_filter = time.time()
self.t_filter = t_filter - t_start
- self.gait.update(self.k, self.k_mpc, self.joystick.getJoystickCode())
+ self.gait.update(self.k, self.k_mpc, self.joystick.get_joystick_code())
self.update_mip = self.k % self.k_mpc == 0 and self.gait.is_new_step()
if self.solo3D:
if self.update_mip:
- self.statePlanner.updateSurface(self.q_filter[:6, :1], self.vref_filt_mpc[:6, :1])
+ self.statePlanner.update_surface(self.q_filter[:6, :1], self.vref_filt_mpc[:6, :1])
if self.surfacePlanner.initialized:
self.error = self.surfacePlanner.get_latest_results()
- self.footstepPlanner.updateSurfaces(self.surfacePlanner.potential_surfaces, self.surfacePlanner.selected_surfaces,
+ self.footstepPlanner.update_surfaces(self.surfacePlanner.potential_surfaces, self.surfacePlanner.selected_surfaces,
self.surfacePlanner.mip_success, self.surfacePlanner.mip_iteration)
- self.o_targetFootstep = self.footstepPlanner.updateFootsteps(self.k % self.k_mpc == 0 and self.k != 0,
+ self.o_targetFootstep = self.footstepPlanner.update_footsteps(self.k % self.k_mpc == 0 and self.k != 0,
int(self.k_mpc - self.k % self.k_mpc),
self.q_filter[:, 0], self.h_v_windowed[:6, :1].copy(),
self.v_ref[:6, :1])
- self.statePlanner.computeReferenceStates(self.q_filter[:6, :1], self.h_v_filt_mpc[:6, :1].copy(), self.vref_filt_mpc[:6, :1])
+ self.statePlanner.compute_reference_states(self.q_filter[:6, :1], self.h_v_filt_mpc[:6, :1].copy(), self.vref_filt_mpc[:6, :1])
- xref = self.statePlanner.getReferenceStates()
- fsteps = self.footstepPlanner.getFootsteps()
+ xref = self.statePlanner.get_reference_states()
+ fsteps = self.footstepPlanner.get_footsteps()
gait_matrix = self.gait.matrix
if self.update_mip and self.solo3D:
- configs = self.statePlanner.getConfigurations().transpose()
+ configs = self.statePlanner.get_configurations().transpose()
self.surfacePlanner.run(configs, gait_matrix, self.o_targetFootstep, self.vref_filt_mpc[:3, 0].copy())
self.surfacePlanner.initialized = True
if not self.enable_multiprocessing_mip and self.SIMULATION:
@@ -371,11 +371,11 @@ class Controller:
# Compute the target foostep in local frame, to stop the optimisation around it when t_lock overpass
l_targetFootstep = oRh.transpose() @ (self.o_targetFootstep - oTh)
self.mpc_wrapper.solve(self.k, xref, fsteps, gait_matrix, l_targetFootstep, oRh, oTh,
- self.footTrajectoryGenerator.getFootPosition(),
- self.footTrajectoryGenerator.getFootVelocity(),
- self.footTrajectoryGenerator.getFootAcceleration(),
- self.footTrajectoryGenerator.getFootJerk(),
- self.footTrajectoryGenerator.getTswing() - self.footTrajectoryGenerator.getT0s())
+ self.footTrajectoryGenerator.get_foot_position(),
+ self.footTrajectoryGenerator.get_foot_velocity(),
+ self.footTrajectoryGenerator.get_foot_acceleration(),
+ self.footTrajectoryGenerator.get_foot_jerk(),
+ self.footTrajectoryGenerator.get_phase_durations() - self.footTrajectoryGenerator.get_elapsed_durations())
else:
self.mpc_wrapper.solve(self.k, xref, fsteps, gait_matrix, np.zeros((3, 4)))
except ValueError:
@@ -400,14 +400,14 @@ class Controller:
else:
self.footTrajectoryGenerator.update(self.k, self.o_targetFootstep)
- if not self.error and not self.joystick.getStop():
+ if not self.error and not self.joystick.get_stop():
if self.DEMONSTRATION and self.gait.is_static():
hRb = np.eye(3)
# Desired position, orientation and velocities of the base
self.xgoals[:6, 0] = np.zeros((6,))
- if self.DEMONSTRATION and self.joystick.getL1() and self.gait.is_static():
- self.p_ref[:, 0] = self.joystick.getPRef()
+ if self.DEMONSTRATION and self.joystick.get_l1() and self.gait.is_static():
+ self.p_ref[:, 0] = self.joystick.get_p_ref()
self.xgoals[[3, 4], 0] = self.p_ref[[3, 4], 0]
self.h_ref = self.p_ref[2, 0]
hRb = pin.rpy.rpyToMatrix(0.0, 0.0, self.p_ref[5, 0])
@@ -429,18 +429,18 @@ class Controller:
# Feet command position, velocity and acceleration in base frame
if self.solo3D: # Use estimated base frame
- self.feet_a_cmd = self.footTrajectoryGenerator.getFootAccelerationBaseFrame(
+ self.feet_a_cmd = self.footTrajectoryGenerator.get_foot_acceleration_base_frame(
oRh_3d.transpose(), np.zeros((3, 1)), np.zeros((3, 1)))
- self.feet_v_cmd = self.footTrajectoryGenerator.getFootVelocityBaseFrame(
+ self.feet_v_cmd = self.footTrajectoryGenerator.get_foot_velocity_base_frame(
oRh_3d.transpose(), np.zeros((3, 1)), np.zeros((3, 1)))
- self.feet_p_cmd = self.footTrajectoryGenerator.getFootPositionBaseFrame(
+ self.feet_p_cmd = self.footTrajectoryGenerator.get_foot_position_base_frame(
oRh_3d.transpose(), oTh_3d + np.array([[0.0], [0.0], [xref[2, 1]]]))
else: # Use ideal base frame
- self.feet_a_cmd = self.footTrajectoryGenerator.getFootAccelerationBaseFrame(
+ self.feet_a_cmd = self.footTrajectoryGenerator.get_foot_acceleration_base_frame(
hRb @ oRh.transpose(), np.zeros((3, 1)), np.zeros((3, 1)))
- self.feet_v_cmd = self.footTrajectoryGenerator.getFootVelocityBaseFrame(
+ self.feet_v_cmd = self.footTrajectoryGenerator.get_foot_velocity_base_frame(
hRb @ oRh.transpose(), np.zeros((3, 1)), np.zeros((3, 1)))
- self.feet_p_cmd = self.footTrajectoryGenerator.getFootPositionBaseFrame(
+ self.feet_p_cmd = self.footTrajectoryGenerator.get_foot_position_base_frame(
hRb @ oRh.transpose(), oTh + np.array([[0.0], [0.0], [self.h_ref]]))
self.xgoals[6:, 0] = self.vref_filt_mpc[:, 0] # Velocities (in horizontal frame!)
@@ -474,7 +474,7 @@ class Controller:
self.t_wbc = time.time() - t_mpc
self.security_check()
- if self.error or self.joystick.getStop():
+ if self.error or self.joystick.get_stop():
self.set_null_control()
# Update PyBullet camera
@@ -570,7 +570,7 @@ class Controller:
Check if the command is fine and set the command to zero in case of error
"""
- if not (self.error or self.joystick.getStop()):
+ if not (self.error or self.joystick.get_stop()):
if (np.abs(self.estimator.get_q_estimate()[7:]) > self.q_security).any():
print("-- POSITION LIMIT ERROR --")
print(self.estimator.get_q_estimate()[7:])
diff --git a/scripts/solo3D/pyb_environment_3D.py b/scripts/solo3D/pyb_environment_3D.py
index de7c5e9f0732f30355f8dcf49aa7fc07d064e29f..4997cf92ef34c59e5053fcd5242828da25bad33e 100644
--- a/scripts/solo3D/pyb_environment_3D.py
+++ b/scripts/solo3D/pyb_environment_3D.py
@@ -85,7 +85,7 @@ class PybEnvironment3D():
'''
gait = self.gait.get_gait_matrix()
- fsteps = self.footStepPlanner.getFootsteps()
+ fsteps = self.footStepPlanner.get_footsteps()
for j in range(4):
# Count the position of the plotted trajectory in the temporal horizon
@@ -106,9 +106,9 @@ class PybEnvironment3D():
for id_t, t in enumerate(t_vector):
# Bezier trajectory
- pos = self.footTrajectoryGenerator.evaluateBezier(j, 0, t)
+ pos = self.footTrajectoryGenerator.evaluate_bezier(j, 0, t)
# Polynomial Curve 5th order
- # pos = self.footTrajectoryGenerator.evaluatePoly(j, 0, t)
+ # pos = self.footTrajectoryGenerator.evaluate_polynom(j, 0, t)
pyb.resetBasePositionAndOrientation(int(self.trajectory_Ids[c, j, id_t]),
posObj=pos,
ornObj=np.array([0.0, 0.0, 0.0, 1.0]))
diff --git a/scripts/tools/LoggerControl.py b/scripts/tools/LoggerControl.py
index 984193fbeada324cd55e56a7d9b546671adb1225..b92d0b3ad24a545e23a9d8f50e2317d3e3a7b6b6 100644
--- a/scripts/tools/LoggerControl.py
+++ b/scripts/tools/LoggerControl.py
@@ -136,7 +136,7 @@ class LoggerControl():
return
# Logging from joystick
- self.joy_v_ref[self.i] = joystick.getVRef()
+ self.joy_v_ref[self.i] = joystick.get_v_ref()
# Logging from estimator
self.esti_feet_status[self.i] = estimator.get_feet_status()
@@ -181,14 +181,14 @@ class LoggerControl():
self.loop_vref_filt_mpc[self.i] = controller.vref_filt_mpc[:, 0]
# Logging from the planner
- self.planner_xref[self.i] = statePlanner.getReferenceStates()
- self.planner_fsteps[self.i] = footstepPlanner.getFootsteps()
- self.planner_target_fsteps[self.i] = footstepPlanner.getTargetFootsteps()
+ self.planner_xref[self.i] = statePlanner.get_reference_states()
+ self.planner_fsteps[self.i] = footstepPlanner.get_footsteps()
+ self.planner_target_fsteps[self.i] = footstepPlanner.get_target_footsteps()
self.planner_gait[self.i] = gait.get_gait_matrix()
- self.planner_goals[self.i] = footTrajectoryGenerator.getFootPosition()
- self.planner_vgoals[self.i] = footTrajectoryGenerator.getFootVelocity()
- self.planner_agoals[self.i] = footTrajectoryGenerator.getFootAcceleration()
- self.planner_jgoals[self.i] = footTrajectoryGenerator.getFootJerk()
+ self.planner_goals[self.i] = footTrajectoryGenerator.get_foot_position()
+ self.planner_vgoals[self.i] = footTrajectoryGenerator.get_foot_velocity()
+ self.planner_agoals[self.i] = footTrajectoryGenerator.get_foot_acceleration()
+ self.planner_jgoals[self.i] = footTrajectoryGenerator.get_foot_jerk()
self.planner_is_static[self.i] = gait.is_static()
self.planner_h_ref[self.i] = controller.h_ref
@@ -223,7 +223,7 @@ class LoggerControl():
# solo3d
if self.solo3d:
self.update_mip[self.i] = controller.update_mip
- self.configs[self.i] = statePlanner.getConfigurations()
+ self.configs[self.i] = statePlanner.get_configurations()
self.initial_contacts[self.i] = controller.o_targetFootstep
self.t_mip[self.i] = controller.surfacePlanner.t_mip
self.i += 1
diff --git a/src/FootstepPlanner.cpp b/src/FootstepPlanner.cpp
index ad6227f036ce45740de19554ded19886501a8eac..fa0c5a6ab594dcec6d1483de91f39d29093832eb 100644
--- a/src/FootstepPlanner.cpp
+++ b/src/FootstepPlanner.cpp
@@ -255,7 +255,6 @@ void FootstepPlanner::updateNewContact(Vector18 const& q) {
MatrixN FootstepPlanner::getFootsteps() { return vectorToMatrix(footsteps_); }
MatrixN FootstepPlanner::getTargetFootsteps() { return targetFootstep_; }
-MatrixN FootstepPlanner::getRz() { return Rz; }
MatrixN FootstepPlanner::vectorToMatrix(std::vector<Matrix34> const& array) {
MatrixN M = MatrixN::Zero(array.size(), 12);
diff --git a/src/InvKin.cpp b/src/InvKin.cpp
index 12f55a2a6600517764b799acddf0f5010e9d3178..49e7475a0d5779369b8a46de2e398c850d6596d4 100644
--- a/src/InvKin.cpp
+++ b/src/InvKin.cpp
@@ -316,7 +316,7 @@ void InvKin::refreshAndCompute(RowVector4 const& contacts, Matrix43 const& pgoal
*/
}
-void InvKin::run_InvKin(VectorN const& q, VectorN const& dq, MatrixN const& contacts, MatrixN const& pgoals,
+void InvKin::run(VectorN const& q, VectorN const& dq, MatrixN const& contacts, MatrixN const& pgoals,
MatrixN const& vgoals, MatrixN const& agoals, MatrixN const& x_cmd) {
// std::cout << "run invkin q: " << q << std::endl;
diff --git a/src/Solo3D/FootstepPlannerQP.cpp b/src/Solo3D/FootstepPlannerQP.cpp
index 5b3fb8b0f483d42c9099a3408e63c80537937dc3..905848d26057cb13f18ca7948a70498edda6f377 100644
--- a/src/Solo3D/FootstepPlannerQP.cpp
+++ b/src/Solo3D/FootstepPlannerQP.cpp
@@ -490,7 +490,6 @@ int FootstepPlannerQP::surfaceInequalities(int i_start, Surface const& surface,
MatrixN FootstepPlannerQP::getFootsteps() { return vectorToMatrix(b_footsteps_); }
MatrixN FootstepPlannerQP::getTargetFootsteps() { return targetFootstep_; }
-MatrixN FootstepPlannerQP::getRz() { return Rz; }
MatrixN FootstepPlannerQP::vectorToMatrix(std::vector<Matrix34> const& array) {
MatrixN M = MatrixN::Zero(array.size(), 12);
diff --git a/src/WbcWrapper.cpp b/src/WbcWrapper.cpp
index b92627969a06ae33b030c1977e6c1dc4917b510e..7802958304e1dca4c375985d270a7cf5cd8408bc 100644
--- a/src/WbcWrapper.cpp
+++ b/src/WbcWrapper.cpp
@@ -97,7 +97,7 @@ void WbcWrapper::compute(VectorN const &q, VectorN const &dq, VectorN const &f_c
data_.M.triangularView<Eigen::StrictlyLower>() = data_.M.transpose().triangularView<Eigen::StrictlyLower>();
// Compute Inverse Kinematics
- invkin_->run_InvKin(q_wbc_, dq_wbc_, contacts, pgoals.transpose(), vgoals.transpose(), agoals.transpose(), xgoals);
+ invkin_->run(q_wbc_, dq_wbc_, contacts, pgoals.transpose(), vgoals.transpose(), agoals.transpose(), xgoals);
ddq_cmd_ = invkin_->get_ddq_cmd();
// TODO: Check if we can save time by switching MatrixXd to defined sized vector since they are