Merge pull request #217 from ADVRHumanoids/local_api_fix

Local api fix

bindings/python/constraints/velocity.hpp

@@ -24,12 +24,14 @@ 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);
 
 }

bindings/python/tasks/velocity.hpp

@@ -50,7 +50,7 @@ void pyVelocityCartesian(py::module& m) {
           .def("getActualPose", py::overload_cast<>(&Cartesian::getActualPose, py::const_))
           .def("getError", &Cartesian::getError)
           .def("reset", &Cartesian::reset)
-          .def("setIsBodyJacobian", &Cartesian::setIsBodyJacobian)
+          .def("setVelocityLocalReference", &Cartesian::setVelocityLocalReference)
           .def_property("orientationErrorGain", &Cartesian::getOrientationErrorGain, &Cartesian::setOrientationErrorGain)
           .def_property_readonly("distalLink", &Cartesian::getDistalLink)
           .def_property_readonly("baseLink", &Cartesian::getBaseLink)

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

include/OpenSoT/tasks/velocity/Cartesian.h

@@ -84,11 +84,14 @@
                 Eigen::Vector3d positionError;
                 Eigen::Vector3d orientationError;
 
-                bool _is_body_jacobian;
+                bool _rotate_to_local;
+                bool _velocity_refs_are_local;
 
                 Eigen::MatrixXd _tmp_A;
                 Eigen::VectorXd _tmp_b;
 
+                Eigen::Vector6d _tmp_twist;
+
             public:
                 /*********** TASK PARAMETERS ************/
 
@@ -131,7 +134,7 @@
                  * since THE _update() RESETS THE FEED-FORWARD VELOCITY TERM for safety reasons.
                  * @param desiredPose the \f$R^{4x4}\f$ homogeneous transform matrix describing the desired pose
                  * for the distal_link in the base_link frame of reference.
-                 * @param desireVelocity is a \f$R^{6}\f$ twist describing the desired trajectory velocity, and it represents
+                 * @param desireTwist is a \f$R^{6}\f$ twist describing the desired trajectory velocity, and it represents
                  * a feed-forward term in the cartesian task computation. NOTICE how the velocities are in units/sample,
                  * instead of units/s. This means that if you have a twist expressed in SI units, you have to call the function as
                  * setReference(desiredPose, desiredTwist*dt)
@@ -143,6 +146,15 @@
                 void setReference(const KDL::Frame& desiredPose,
                                   const KDL::Twist& desiredTwist);
 
+                /**
+                 * @brief setVelocityLocalReference permits to set velocity expressed in local (ee) distal frame
+                 * @param desireTwist is a \f$R^{6}\f$ twist describing the desired trajectory velocity, and it represents
+                 * a feed-forward term in the cartesian task computation. NOTICE how the velocities are in units/sample,
+                 * instead of units/s. This means that if you have a twist expressed in SI units, you have to call the function as
+                 * setVelocityLocalReference(desiredTwist*dt)
+                 */
+                void setVelocityLocalReference(const Eigen::Vector6d& desiredTwist);
+
                 /**
                  * @brief getReference returns the Cartesian task reference
                  * @return the Cartesian task reference \f$R^{4x4}\f$ homogeneous transform matrix describing the desired pose
@@ -219,10 +231,10 @@
                 virtual bool reset();
 
                 /**
-                 * @brief setIsBodyJacobian
-                 * @param is_body_jacobian if true jacobians are in body (ee reference)
+                 * @brief rotateToLocal rotates both Jacobian and references to local (ee) distal frame, this is mostly used for local subtasks
+                 * @param rotate_to_local default is false
                  */
-                void setIsBodyJacobian(const bool is_body_jacobian);
+                void rotateToLocal(const bool rotate_to_local);
 
                 static bool isCartesian(OpenSoT::Task<Eigen::MatrixXd, Eigen::VectorXd>::TaskPtr task);
 

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,7 +69,10 @@ OmniWheels4X::OmniWheels4X(const double l1, const double l2, const double r,
 
 void OmniWheels4X::update()
 {
-    _robot.getPose(_base_link, _w_T_b);
+    _w_T_b.setIdentity();
+
+    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);
 }
 

src/tasks/velocity/Cartesian.cpp

@@ -30,7 +30,7 @@ Cartesian::Cartesian(std::string task_id,
     Task(task_id, robot.getNv()), _robot(robot),
     _distal_link(distal_link), _base_link(base_link),
     _orientationErrorGain(1.0), _is_initialized(false),
-    _error(6), _is_body_jacobian(false)
+    _error(6), _rotate_to_local(false), _velocity_refs_are_local(false)
 {
     _error.setZero(6);
 
@@ -87,10 +87,16 @@ void Cartesian::_update() {
         _is_initialized = true;
     }
 
+    if(_velocity_refs_are_local)
+    {
+        _tmp_twist = _desiredTwist;
+        _desiredTwist = XBot::Utils::adjointFromRotation(_actualPose.linear()) * _tmp_twist;
+    }
+
     this->update_b();
 
     //Here we rotate A and b
-    if(_is_body_jacobian)
+    if(_rotate_to_local)
     {
         _tmp_A = _A;
         _A = XBot::Utils::adjointFromRotation(_actualPose.linear().transpose())*_tmp_A;
@@ -100,10 +106,17 @@ void Cartesian::_update() {
     }
 
     this->_desiredTwist.setZero(6);
-
+    _velocity_refs_are_local = false;
     /**********************************************************************/
 }
 
+void Cartesian::setVelocityLocalReference(const Eigen::Vector6d& desiredTwist)
+{
+    _velocity_refs_are_local = true;
+    _desiredTwist = desiredTwist;
+    _desiredTwistRef = _desiredTwist;
+}
+
 void Cartesian::setReference(const Eigen::Affine3d& desiredPose)
 {
     _desiredPose = desiredPose;
@@ -136,6 +149,7 @@ void Cartesian::setReference(const KDL::Frame& desiredPose)
 void Cartesian::setReference(const Eigen::Affine3d& desiredPose,
                   const Eigen::Vector6d& desiredTwist)
 {
+    _velocity_refs_are_local = false;
     _desiredPose = desiredPose;
     _desiredTwist = desiredTwist;
     _desiredTwistRef = _desiredTwist;
@@ -145,6 +159,7 @@ void Cartesian::setReference(const Eigen::Affine3d& desiredPose,
 void Cartesian::setReference(const Eigen::Matrix4d &desiredPose,
                              const Eigen::Vector6d &desiredTwist)
 {
+    _velocity_refs_are_local = false;
     _desiredPose.matrix() = desiredPose;
     _desiredTwist = desiredTwist;
     _desiredTwistRef = _desiredTwist;
@@ -154,6 +169,7 @@ void Cartesian::setReference(const Eigen::Matrix4d &desiredPose,
 void Cartesian::setReference(const KDL::Frame& desiredPose,
                   const KDL::Twist& desiredTwist)
 {
+    _velocity_refs_are_local = false;
     _desiredPose(0,3) = desiredPose.p.x();
     _desiredPose(1,3) = desiredPose.p.y();
     _desiredPose(2,3) = desiredPose.p.z();
@@ -360,7 +376,7 @@ bool Cartesian::reset()
     return true;
 }
 
-void Cartesian::setIsBodyJacobian(const bool is_body_jacobian)
+void Cartesian::rotateToLocal(const bool rotate_to_local)
 {
-    _is_body_jacobian = is_body_jacobian;
+    _rotate_to_local = rotate_to_local;
 }