diff --git a/bindings/python/constraints/velocity.hpp b/bindings/python/constraints/velocity.hpp index e44be79d..6ac25b71 100644 --- a/bindings/python/constraints/velocity.hpp +++ b/bindings/python/constraints/velocity.hpp @@ -24,12 +24,14 @@ void pyVelocityLimits(py::module& m) { .def("setVelocityLimits", py::overload_cast(&VelocityLimits::setVelocityLimits)) .def("setVelocityLimits", py::overload_cast(&VelocityLimits::setVelocityLimits)) .def("getDT", &VelocityLimits::getDT) - .def("update", &VelocityLimits::update);; + .def("update", &VelocityLimits::update); } void pyVelocityOmniWheels4X(py::module& m) { py::class_, OpenSoT::Constraint>(m, "OmniWheels4X") .def(py::init, const std::string, XBot::ModelInterface&>()) - .def("update", &OmniWheels4X::update); + .def("update", &OmniWheels4X::update) + .def("setIsGlobalVelocity", &OmniWheels4X::setIsGlobalVelocity) + .def("getIsGlobalVelocity", &OmniWheels4X::getIsGlobalVelocity); } diff --git a/bindings/python/tasks/velocity.hpp b/bindings/python/tasks/velocity.hpp index 895542fb..dcec7d23 100644 --- a/bindings/python/tasks/velocity.hpp +++ b/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) diff --git a/include/OpenSoT/constraints/velocity/OmniWheels4X.h b/include/OpenSoT/constraints/velocity/OmniWheels4X.h index e313a9c5..9d41eb4b 100644 --- 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/include/OpenSoT/tasks/velocity/Cartesian.h b/include/OpenSoT/tasks/velocity/Cartesian.h index dfef2614..11337a57 100644 --- a/include/OpenSoT/tasks/velocity/Cartesian.h +++ b/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::TaskPtr task); diff --git a/src/constraints/velocity/OmniWheels4X.cpp b/src/constraints/velocity/OmniWheels4X.cpp index 286cf12c..9bf34dcb 100644 --- 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 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); } diff --git a/src/tasks/velocity/Cartesian.cpp b/src/tasks/velocity/Cartesian.cpp index 8685024e..cfdcbbdc 100644 --- a/src/tasks/velocity/Cartesian.cpp +++ b/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; }