Introduce IJointCoupling::evaluateJacobianFromActuatedAxesToPhysicalJ…

…ointsVel() Introduce IJointCoupling::evaluateJacobianFromPhysicalJointsToActuatedAxeseVel()
robotology · Feb 17, 2025 · 08c183e · 08c183e
1 parent aff0b4b
commit 08c183e
Show file tree

Hide file tree

Showing 3 changed files with 45 additions and 0 deletions.
diff --git a/src/libYARP_dev/src/yarp/dev/IJointCoupling.h b/src/libYARP_dev/src/yarp/dev/IJointCoupling.h
@@ -9,6 +9,7 @@
 #include <yarp/dev/api.h>
 #include <yarp/os/Vocab.h>
 #include <yarp/sig/Vector.h>
+#include <yarp/sig/Matrix.h>
 
 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
@@ -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
@@ -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);