diff --git a/include/qrw/Gait.hpp b/include/qrw/Gait.hpp index 4fc5a488ae4b63e8a90fa61edabc95e57f617c70..d17bfc9f8e0894bc99a5785afbe1ca54a96b5f94 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 c6e4d76d9aa6c3c7495fbed4b577dbf25aa00154..d6a765be8840705b573bedc601b5b0d761b7edf3 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 d1c624e6c91c7ef5535c72eccaf8a17c261f1294..31976196ecc6d781a36d818a2865e19e9743f51c 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; +}