Third draft of iJointCoupling

src/libYARP_dev/src/yarp/dev/IJointCoupling.h

@@ -16,7 +16,45 @@ class IJointCoupling;
 /**
  * @ingroup dev_iface_motor
  *
- * Interface for a generic control board device implementing position control.
+ * Interface for joint coupling. It contains the methods to convert from Physical Joint to Actuated Axes and viceversa.
+ *
+ * $$
+ * \theta \in \mathbb{R}^{m} : \text{Actuated Axes positions}
+ * $$
+ *
+ * $$
+ * q \in \mathbb{R}^{n} : \text{Physical Joints positions}
+ * $$
+ *
+ * In general, we have $m \leq n$.
+ *
+ * There is unique value of Actuated Axes corresponding to a given Physical Joint position, while in general there may be multiple Physical Joint position corresponding to a given Actuated Axes.
+ *
+ * So, the mapping from Physical Joint $q$ to Actuated Axes $\theta$ is defined as:
+ *
+ * $$
+ * \theta = f(q)
+ * $$
+ *
+ * while the mapping from Actuated Axes $\theta$  to Physical Joint $q$  is defined as:
+ *
+ * $$
+ * q = g(\theta)
+ * $$
+ *
+ * We have the following property:
+ *
+ * $$
+ * f(g(\theta)) = \theta
+ * $$
+ *
+ * while in general it is not always true that the inverse holds, i.e.
+ *
+ * $$
+ * g(f(q)) \neq q
+ * $$
+ *
+ * In the rest of the section, we assume that $\theta(t) = f(q(t))$ and $\dot{\theta}(t)^T \tau_{\theta}(t) = \dot{q}(t)^T \tau_{q}(t)$
  */
 class YARP_dev_API yarp::dev::IJointCoupling
 {
@@ -26,19 +64,150 @@ class YARP_dev_API yarp::dev::IJointCoupling
      */
     virtual ~IJointCoupling() {}
 
+    /**
+     * @brief Convert from Physical Joint to Actuated Axes position.
+     * This method implements $f(q)$.
+     *
+     * @param[in] physJointPos Physical Joint position
+     * @param[out] actAxisPos Actuated Axes position
+     * @return, true/false on success/failure
+     */
     virtual bool convertFromPhysicalJointPosToActuatedAxisPos(const yarp::sig::Vector& physJointPos, yarp::sig::Vector& actAxisPos) = 0;
+
+    /**
+     * @brief Convert from Physical Joint to Actuated Axes velocity.
+     * This method implements $f'$ that can be used to compute $\dot{\theta}(t)$, defined such that:
+     *
+     * $$
+     * \dot{\theta}(t) = \frac{\partial}{\partial q} f(q(t)) \dot{q}(t) = f'( q(t), \dot{q}(t))
+     * $$
+     *
+     * @param[in] physJointPos Physical Joint position
+     * @param[in] physJointVel Physical Joint velocity
+     * @param[out] actAxisVel  Actuated Axes velocity
+     * @return, true/false on success/failure
+     */
     virtual bool convertFromPhysicalJointVelToActuatedAxisVel(const yarp::sig::Vector& physJointPos, const yarp::sig::Vector& physJointVel, yarp::sig::Vector& actAxisVel) = 0;
+
+    /**
+     * @brief Convert from Physical Joint to Actuated Axes acceleration.
+     * This method implements $f''$ that can be used to compute $\ddot{\theta}(t)$,  defined such that:
+     *
+     * $$
+     * \ddot{\theta}(t) = \frac{\partial}{\partial q} f'(q(t), \dot{q}(t)) \dot{q}(t) + \frac{\partial}{\partial \dot{q}} f'(q(t), \dot{q}(t)) \ddot{q}(t) = f''( q(t), \dot{q}(t), \ddot{q}(t))
+     * $$
+     *
+     * @param[in] physJointPos Physical Joint position
+     * @param[in] physJointVel Physical Joint velocity
+     * @param[in] physJointAcc Physical Joint acceleration
+     * @param[out] actAxisAcc  Actuated Axes acceleration
+     * @return, true/false on success/failure
+     */
     virtual bool convertFromPhysicalJointPosToActuatedAxisAcc(const yarp::sig::Vector& physJointPos, const yarp::sig::Vector& physJointVel, const yarp::sig::Vector& physJointAcc, yarp::sig::Vector& actAxisAcc) = 0;
+
+    /**
+     * @brief Convert from Physical Joint to Actuated Axes torque
+     *
+     * @param[in] physJointPos Physical Joint position
+     * @param[in] physJointTrq Physical Joint torque
+     * @param[out] actAxisTrq  Actuated Axes torque
+     * @return true/false on success/failure
+     */
     virtual bool convertFromPhysicalJointTrqToActuatedAxisTrq(const yarp::sig::Vector& physJointPos, const yarp::sig::Vector& physJointTrq, yarp::sig::Vector& actAxisTrq) = 0;
+
+    /**
+     * @brief Convert from Actuated Axes to Physical Joint position
+     * This method implements $g(\theta)$.
+     * @param[in] actAxisPos Actuated Axes position
+     * @param[out] physJointPos Physical Joint position
+     * @return true/false on success/failure
+     */
     virtual bool convertFromActuatedAxisToPhysicalJointPos(const yarp::sig::Vector& actAxisPos, yarp::sig::Vector& physJointPos) = 0;
+
+    /**
+     * @brief Convert from Actuated Axes to Physical Joint velocity
+     * This method implements $g'$ that can be used to compute $\dot{q}(t)$, defined such that:
+     *
+     * $$
+     * \dot{q}(t) = \frac{\partial}{\partial \theta} g(\theta(t)) \dot{\theta}(t) = g'( \theta(t), \dot{\theta}(t))
+     * $$
+     *
+     * @param[in] actAxisPos Actuated Axes position
+     * @param[in] actAxisVel Actuated Axes velocity
+     * @param[out] physJointVel Physical Joint velocity
+     * @return true/false on success/failure
+     */
     virtual bool convertFromActuatedAxisToPhysicalJointVel(const yarp::sig::Vector& actAxisPos, const yarp::sig::Vector& actAxisVel, yarp::sig::Vector& physJointVel) = 0;
+
+    /**
+     * @brief Convert from Actuated Axes to Physical Joint acceleration
+     *
+     * This method implements $f''$ that can be used to compute $\ddot{\theta}(t)$,  defined such that:
+     *
+     * $$
+     * \ddot{q}(t) = \frac{\partial}{\partial \theta} g'(\theta(t), \dot{\theta}(t)) \dot{\theta}(t) + \frac{\partial}{\partial \dot{\theta}} g'(\theta(t), \dot{\theta}(t)) \ddot{\theta}(t) = g''( \theta(t), \dot{\theta}(t), \ddot{\theta}(t))
+     * $$
+     *
+     * @param[in] actAxisPos Actuated Axes position
+     * @param[in] actAxisVel Actuated Axes velocity
+     * @param[in] actAxisAcc Actuated Axes acceleration
+     * @param[out] physJointAcc Physical Joint acceleration
+     * @return true/false on success/failure
+     */
     virtual bool convertFromActuatedAxisToPhysicalJointAcc(const yarp::sig::Vector& actAxisPos, const yarp::sig::Vector& actAxisVel, const yarp::sig::Vector& actAxisAcc, yarp::sig::Vector& physJointAcc) = 0;
+
+    /**
+     * @brief Convert from Actuated Axes to Physical Joint acceleration
+     *
+     * @param[in] actAxisPos Actuated Axes position
+     * @param[in] actAxisTrq Actuated Axes torque
+     * @param[out] physJointTrq Physical Joint torque
+     * @return true/false on success/failure
+     */
     virtual bool convertFromActuatedAxisToPhysicalJointTrq(const yarp::sig::Vector& actAxisPos, const yarp::sig::Vector& actAxisTrq, yarp::sig::Vector& physJointTrq) = 0;
-    virtual yarp::sig::VectorOf<int> getCoupledJoints()=0;
-    virtual std::string getCoupledJointName(int joint)=0;
-    virtual bool checkJointIsCoupled(int joint)=0;
-    virtual void setCoupledJointLimit(int joint, const double& min, const double& max)=0;
-    virtual void getCoupledJointLimit(int joint, double& min, double& max)=0;
+
+    /**
+     * @brief Get the physical joints object
+     *
+     * @return yarp::sig::VectorOf<int>
+     */
+    virtual yarp::sig::VectorOf<size_t> getPhysicalJoints()=0;
+
+    /**
+     * @brief Get the name of a physical joint
+     *
+     * @param joint index of the joint
+     * @return name of the joint
+     */
+    virtual std::string getPhysicalJointName(size_t joint)=0;
+
+    /**
+     * @brief Check if a physical joint is coupled
+     *
+     * @param joint index of the joint
+     * @return true/false on coupled/not coupled
+     */
+    virtual bool checkPhysicalJointIsCoupled(size_t joint)=0;
+
+    /**
+     * @brief Set limts for a physical joint
+     *
+     * @param[in] joint index of the joint
+     * @param[in] min minimum value
+     * @param[in] max maximum value
+     * @return true/false on success/failure
+     */
+    virtual bool setPhysicalJointLimits(size_t joint, const double& min, const double& max)=0;
+
+    /**
+     * @brief Get the Physical Joint Limit object
+     *
+     * @param[in] joint index of the joint
+     * @param[out] min minimum value
+     * @param[out] max maximum value
+     * @return true/false on success/failure
+     */
+    virtual bool getPhysicalJointLimits(size_t joint, double& min, double& max)=0;
 };
 
 #endif // YARP_DEV_IJOINTCOUPLING_H

src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.cpp

@@ -10,18 +10,18 @@
 using namespace yarp::dev;
 
 
-void ImplementJointCoupling::initialise(yarp::sig::VectorOf<int> coupled_joints, std::vector<std::string> coupled_joint_names, std::vector<std::pair<double, double>> coupled_joint_limits) {
-    m_coupledJoints=coupled_joints;
-    m_coupledJointNames=coupled_joint_names;
+void ImplementJointCoupling::initialise(yarp::sig::VectorOf<size_t> physical_joints, std::vector<std::string> physical_joint_names, std::vector<std::pair<double, double>> physical_joint_limits) {
+    m_physicalJoints=physical_joints;
+    m_physicalJointNames=physical_joint_names;
 
-    // Configure a map between coupled joints and limits
-    for (std::size_t i = 0, j = 0; i < coupled_joints.size(); i++)
+    // Configure a map between physical joints and limits
+    for (std::size_t i = 0, j = 0; i < physical_joints.size(); i++)
     {
-        const int coupled_joint_index = coupled_joints(i);
-        const std::string coupled_joint_name = getCoupledJointName(coupled_joint_index);
-        if (coupled_joint_name != "invalid" && coupled_joint_name != "reserved")
+        const int physical_joint_index = physical_joints(i);
+        const std::string physical_joint_name = getPhysicalJointName(physical_joint_index);
+        if (physical_joint_name != "invalid" && physical_joint_name != "reserved")
         {
-            m_coupledJointLimits[coupled_joints[i]] = coupled_joint_limits[j];
+            m_physicalJointLimits[physical_joints[i]] = physical_joint_limits[j];
             j++;
         }
     }
@@ -30,49 +30,53 @@ void ImplementJointCoupling::initialise(yarp::sig::VectorOf<int> coupled_joints,
 }
 
 
-yarp::sig::VectorOf<int> ImplementJointCoupling::getCoupledJoints() {
-    return m_coupledJoints;
+yarp::sig::VectorOf<size_t> ImplementJointCoupling::getPhysicalJoints() {
+    return m_physicalJoints;
 }
 
-std::string ImplementJointCoupling::getCoupledJointName(int joint){
+std::string ImplementJointCoupling::getPhysicalJointName(size_t joint){
     int c_joint = -1;
-    for (size_t i = 0; i < m_coupledJoints.size(); ++i)
+    for (size_t i = 0; i < m_physicalJoints.size(); ++i)
     {
-        if (m_coupledJoints[i]==joint) c_joint = i;
+        if (m_physicalJoints[i]==joint) c_joint = i;
     }
 
-    if (c_joint >= 0 && static_cast<size_t>(c_joint) < m_coupledJoints.size())
+    if (c_joint >= 0 && static_cast<size_t>(c_joint) < m_physicalJoints.size())
     {
-        return m_coupledJointNames[c_joint];
+        return m_physicalJointNames[c_joint];
     }
     else
     {
         return std::string("invalid");
     }
 }
 
-bool ImplementJointCoupling::checkJointIsCoupled(int joint){
-    for (size_t i = 0; i < m_coupledJoints.size(); ++i)
+bool ImplementJointCoupling::checkPhysicalJointIsCoupled(size_t joint){
+    for (size_t i = 0; i < m_physicalJoints.size(); ++i)
     {
-        if (m_coupledJoints[i]==joint) return true;
+        if (m_physicalJoints[i]==joint) return true;
     }
     return false;
 }
-void ImplementJointCoupling::setCoupledJointLimit(int joint, const double& min, const double& max){
-    const std::string coupled_joint_name = getCoupledJointName(joint);
+bool ImplementJointCoupling::setPhysicalJointLimits(size_t joint, const double& min, const double& max){
+    const std::string physical_joint_name = getPhysicalJointName(joint);
 
-    if (coupled_joint_name != "reserved" && coupled_joint_name != "invalid")
+    if (physical_joint_name != "reserved" && physical_joint_name != "invalid")
     {
-        m_coupledJointLimits.at(joint).first = min;
-        m_coupledJointLimits.at(joint).second = max;
+        m_physicalJointLimits.at(joint).first = min;
+        m_physicalJointLimits.at(joint).second = max;
+        return true;
     }
+    return false;
 }
-void ImplementJointCoupling::getCoupledJointLimit(int joint, double& min, double& max){
-    const std::string coupled_joint_name = getCoupledJointName(joint);
+bool ImplementJointCoupling::getPhysicalJointLimits(size_t joint, double& min, double& max){
+    const std::string physical_joint_name = getPhysicalJointName(joint);
 
-    if (coupled_joint_name != "reserved" && coupled_joint_name != "gyp_invalid")
+    if (physical_joint_name != "reserved" && physical_joint_name != "gyp_invalid")
     {
-        min = m_coupledJointLimits.at(joint).first;
-        max = m_coupledJointLimits.at(joint).second;
+        min = m_physicalJointLimits.at(joint).first;
+        max = m_physicalJointLimits.at(joint).second;
+        return true;
     }
+    return false;
 }

src/libYARP_dev/src/yarp/dev/ImplementJointCoupling.h

@@ -29,17 +29,17 @@ class YARP_dev_API yarp::dev::ImplementJointCoupling: public IJointCoupling
      */
     virtual ~ImplementJointCoupling() = default;
 
-    void initialise(yarp::sig::VectorOf<int> coupled_joints, std::vector<std::string> coupled_joint_names, std::vector<std::pair<double, double>> coupled_joint_limits);
+    void initialise(yarp::sig::VectorOf<size_t> physical_joints, std::vector<std::string> physical_joint_names, std::vector<std::pair<double, double>> physical_joint_limits);
 
-    yarp::sig::VectorOf<int> getCoupledJoints() override final;
-    std::string getCoupledJointName(int joint) override final;
-    bool checkJointIsCoupled(int joint) override final;
-    void setCoupledJointLimit(int joint, const double& min, const double& max) override final;
-    void getCoupledJointLimit(int joint, double& min, double& max) override final;
+    yarp::sig::VectorOf<size_t> getPhysicalJoints() override final;
+    std::string getPhysicalJointName(size_t joint) override final;
+    bool checkPhysicalJointIsCoupled(size_t joint) override final;
+    bool setPhysicalJointLimits(size_t joint, const double& min, const double& max) override final;
+    bool getPhysicalJointLimits(size_t joint, double& min, double& max) override final;
 protected:
-    yarp::sig::VectorOf<int> m_coupledJoints;
-    std::vector<std::string> m_coupledJointNames;
-    std::unordered_map<int, std::pair<double, double>> m_coupledJointLimits;
+    yarp::sig::VectorOf<size_t> m_physicalJoints;
+    std::vector<std::string> m_physicalJointNames;
+    std::unordered_map<size_t, std::pair<double, double>> m_physicalJointLimits;
     unsigned int m_controllerPeriod;
     unsigned int m_couplingSize;