diff --git a/src/libYARP_dev/src/yarp/dev/IJointCoupling.h b/src/libYARP_dev/src/yarp/dev/IJointCoupling.h index d11a93a0d46..ea966595e44 100644 --- a/src/libYARP_dev/src/yarp/dev/IJointCoupling.h +++ b/src/libYARP_dev/src/yarp/dev/IJointCoupling.h @@ -9,6 +9,7 @@ #include #include #include +#include namespace yarp::dev { class IJointCoupling; @@ -236,6 +237,38 @@ class YARP_dev_API yarp::dev::IJointCoupling * @return true/false on success/failure */ virtual bool getPhysicalJointLimits(size_t physicalJointIndex, double& min, double& max)=0; + + /** + * @brief Get the Jacobian mapping the Actuated Axes to Physical Joints velocity + * + * This method implements $\frac{\partial}{\partial \theta} g(\theta(t))$, defined such that: + * + * $$ + * \dot{q}(t) = \frac{\partial}{\partial \theta} g(\theta(t)) \dot{\theta}(t) + * $$ + * + * @return true/false on success/failure + * @param[in] actAxesPos Actuated Axes position + * @param[out] actAxesToPhysJointsVelJacobian Jacobian mapping the Actuated + * Axes to Physical Joints velocity + */ + virtual bool evaluateJacobianFromActuatedAxesToPhysicalJointsVel(const yarp::sig::Vector& actAxesPos, yarp::sig::Matrix& actAxesToPhysJointsVelJacobian)=0; + + /** + * @brief Get the Jacobian mapping the Physical Joints to Actuated Axes velocity + * + * This method implements $\frac{\partial}{\partial \q} f(\q(t))$, defined such that: + * + * $$ + * \dot{theta}(t) = \frac{\partial}{\partial \q} f(\q(t)) \dot{\q}(t) + * $$ + * + * @return true/false on success/failure + * @param[in] physJointsPos Physical Joints position + * @param[out] physJointsToActAxesVelJacobian Jacobian mapping the Physical Joints + * to Actuated Axes velocity + */ + virtual bool evaluateJacobianFromPhysicalJointsToActuatedAxeseVel(const yarp::sig::Vector& physJointsPos, yarp::sig::Matrix& physJointsToActAxesVelJacobian)=0; }; #endif // YARP_DEV_IJOINTCOUPLING_H diff --git a/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.cpp b/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.cpp index 0dd469f1d86..e88218f13f6 100644 --- a/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.cpp +++ b/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.cpp @@ -79,3 +79,13 @@ bool ImplementJointCoupling::getPhysicalJointLimits(size_t physicalJointIndex, d } return false; } + +bool ImplementJointCoupling::evaluateJacobianFromActuatedAxesToPhysicalJointsVel(const yarp::sig::Vector& actAxesPos, yarp::sig::Matrix& actAxesToPhysJointsVelJacobian){ + /* Override this function */ + return false; +} + +bool ImplementJointCoupling::evaluateJacobianFromPhysicalJointsToActuatedAxeseVel(const yarp::sig::Vector& physJointsPos, yarp::sig::Matrix& physJointsToActAxesVelJacobian){ + /* Override this function */ + return false; +} diff --git a/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.h b/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.h index 45279a18543..9c2afb8c4fa 100644 --- a/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.h +++ b/src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.h @@ -40,6 +40,8 @@ class YARP_dev_API yarp::dev::ImplementJointCoupling: public IJointCoupling bool getPhysicalJointName(size_t physicalJointIndex, std::string& physicalJointName) override final; bool getActuatedAxisName(size_t actuatedAxisIndex, std::string& actuatedAxisName) override final; bool getPhysicalJointLimits(size_t physicalJointIndex, double& min, double& max) override final; + bool evaluateJacobianFromActuatedAxesToPhysicalJointsVel(const yarp::sig::Vector& actAxesPos, yarp::sig::Matrix& actAxesToPhysJointsVelJacobian) override; + bool evaluateJacobianFromPhysicalJointsToActuatedAxeseVel(const yarp::sig::Vector& physJointsPos, yarp::sig::Matrix& physJointsToActAxesVelJacobian) override; protected: bool checkPhysicalJointIsCoupled(size_t physicalJointIndex);