Add set functions for past and desired gait

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); }

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>()));
   }

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;
+}