Added setter/getter of flag for specify if velocity is in local or gl…

…obal for the Moniwheel4X constraint
ADVRHumanoids · Oct 10, 2024 · e409979 · e409979
1 parent 7c92b15
commit e409979
Show file tree

Hide file tree

Showing 3 changed files with 22 additions and 7 deletions.
diff --git a/bindings/python/constraints/velocity.hpp b/bindings/python/constraints/velocity.hpp
@@ -26,13 +26,15 @@ void pyVelocityLimits(py::module& m) {
         .def("setVelocityLimits", py::overload_cast<const double>(&VelocityLimits::setVelocityLimits))
         .def("setVelocityLimits", py::overload_cast<const Eigen::VectorXd&>(&VelocityLimits::setVelocityLimits))
         .def("getDT", &VelocityLimits::getDT)
-        .def("update", &VelocityLimits::update);;
+        .def("update", &VelocityLimits::update);
 }
 
 void pyVelocityOmniWheels4X(py::module& m) {
     py::class_<OmniWheels4X, std::shared_ptr<OmniWheels4X>, OpenSoT::Constraint<Eigen::MatrixXd, Eigen::VectorXd>>(m, "OmniWheels4X")
         .def(py::init<const double, const double, const double, const std::vector<std::string>, const std::string, XBot::ModelInterface&>())
-        .def("update", &OmniWheels4X::update);
+        .def("update", &OmniWheels4X::update)
+        .def("setIsGlobalVelocity", &OmniWheels4X::setIsGlobalVelocity)
+        .def("getIsGlobalVelocity", &OmniWheels4X::getIsGlobalVelocity);
 
 }
 

diff --git a/include/OpenSoT/constraints/velocity/OmniWheels4X.h b/include/OpenSoT/constraints/velocity/OmniWheels4X.h
@@ -75,12 +75,25 @@ namespace OpenSoT {
             */
            virtual void update();
 
+           /**
+            * @brief setIsGlobalVelocity set the flag to consider the velocity in the global frame
+            * @param is_global_velocity default is false
+            */
+           void setIsGlobalVelocity(bool is_global_velocity) { _is_global_velocity = is_global_velocity; }
+
+           /**
+            * @brief getIsGlobalVelocity get the flag to consider the velocity in the global frame
+            * @return true or false
+            */
+           bool getIsGlobalVelocity() const { return _is_global_velocity; }
+
 
        private:
            XBot::ModelInterface& _robot;
            Eigen::MatrixXd _J;
            Eigen::Affine3d _w_T_b;
            const std::string _base_link;
+           bool _is_global_velocity;
        };
 
        }

diff --git a/src/constraints/velocity/OmniWheels4X.cpp b/src/constraints/velocity/OmniWheels4X.cpp
@@ -23,7 +23,7 @@ OmniWheels4X::OmniWheels4X(const double l1, const double l2, const double r,
                          const std::vector<std::string> joint_wheels_name,
                          const std::string base_link,
                          XBot::ModelInterface &robot):
-    Constraint("OmniWheels4X", robot.getNv()), _robot(robot), _base_link(base_link)
+    Constraint("OmniWheels4X", robot.getNv()), _robot(robot), _base_link(base_link), _is_global_velocity(false)
 {
     _J.resize(3, _x_size);
     _J.setZero();
@@ -69,11 +69,11 @@ OmniWheels4X::OmniWheels4X(const double l1, const double l2, const double r,
 
 void OmniWheels4X::update()
 {
-    // _robot.getPose(_base_link, _w_T_b);
-    // _Aineq.rightCols(_x_size-6).noalias() = _w_T_b.linear() * _J.rightCols(_x_size-6);
+    _w_T_b.setIdentity();
 
-    //todo: here we assumes velocity of the floating base are in local frame!
-    _Aineq.rightCols(_x_size-6).noalias() = _J.rightCols(_x_size-6);
+    if(_is_global_velocity)
+        _robot.getPose(_base_link, _w_T_b);
+    _Aineq.rightCols(_x_size-6).noalias() = _w_T_b.linear() * _J.rightCols(_x_size-6);
 }