From f3497f6e2cc46ea3a147f169b3517e30a7ecd490 Mon Sep 17 00:00:00 2001
From: paleziart <paleziart@laas.fr>
Date: Tue, 23 Aug 2022 17:12:08 +0200
Subject: [PATCH] Add set functions for past and desired gait
---
include/qrw/Gait.hpp | 18 ++++++++++++++++++
python/Gait.cpp | 5 +++++
src/Gait.cpp | 24 +++++++++++++++++++++++-
3 files changed, 46 insertions(+), 1 deletion(-)
diff --git a/include/qrw/Gait.hpp b/include/qrw/Gait.hpp
index 4fc5a488..d17bfc9f 100644
--- a/include/qrw/Gait.hpp
+++ b/include/qrw/Gait.hpp
@@ -136,6 +136,24 @@ class Gait {
////////////////////////////////////////////////////////////////////////////////////////////////
void setCurrentGait(MatrixN4 const& gaitMatrix);
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Update the past gait matrix externally (directly set the gait matrix)
+ ///
+ /// \param[in] gaitMatrix Gait matrix that should be used for the past gait matrix
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void setPastGait(MatrixN4 const& gaitMatrix);
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ ///
+ /// \brief Update the desired gait matrix externally (directly set the gait matrix)
+ ///
+ /// \param[in] gaitMatrix Gait matrix that should be used for the desired gait matrix
+ ///
+ ////////////////////////////////////////////////////////////////////////////////////////////////
+ void setDesiredGait(MatrixN4 const& gaitMatrix);
+
MatrixN4 getPastGait() { return pastGait_; }
MatrixN4 getCurrentGait() { return currentGait_; }
double getCurrentGaitCoeff(int i, int j) { return currentGait_(i, j); }
diff --git a/python/Gait.cpp b/python/Gait.cpp
index c6e4d76d..d6a765be 100644
--- a/python/Gait.cpp
+++ b/python/Gait.cpp
@@ -29,6 +29,11 @@ struct GaitVisitor : public bp::def_visitor<GaitVisitor<Gait>> {
.def("set_current_gait", &Gait::setCurrentGait, bp::args("gaitMatrix"),
"Set current gait matrix from Python.\n")
+ .def("set_past_gait", &Gait::setPastGait, bp::args("gaitMatrix"), "Set past gait matrix from Python.\n")
+
+ .def("set_desired_gait", &Gait::setDesiredGait, bp::args("gaitMatrix"),
+ "Set desired gait matrix from Python.\n")
+
.add_property("matrix",
bp::make_function(&Gait::getCurrentGait, bp::return_value_policy<bp::return_by_value>()));
}
diff --git a/src/Gait.cpp b/src/Gait.cpp
index d1c624e6..31976196 100644
--- a/src/Gait.cpp
+++ b/src/Gait.cpp
@@ -231,4 +231,26 @@ void Gait::setCurrentGait(MatrixN4 const& gaitMatrix) {
}
currentGait_ = gaitMatrix;
-}
\ No newline at end of file
+}
+
+void Gait::setPastGait(MatrixN4 const& gaitMatrix) {
+ if (gaitMatrix.rows() != pastGait_.rows()) {
+ throw std::runtime_error("Input matrix should have the same number of rows than the past gait matrix.");
+ }
+ if (gaitMatrix.cols() != pastGait_.cols()) {
+ throw std::runtime_error("Input matrix should have the same number of columns than the past gait matrix.");
+ }
+
+ pastGait_ = gaitMatrix;
+}
+
+void Gait::setDesiredGait(MatrixN4 const& gaitMatrix) {
+ if (gaitMatrix.rows() != desiredGait_.rows()) {
+ throw std::runtime_error("Input matrix should have the same number of rows than the desired gait matrix.");
+ }
+ if (gaitMatrix.cols() != desiredGait_.cols()) {
+ throw std::runtime_error("Input matrix should have the same number of columns than the desired gait matrix.");
+ }
+
+ desiredGait_ = gaitMatrix;
+}
--
GitLab