Add joint effort and velocity limits handling

bindings/pybind11/idyntree_model.cpp

@@ -110,7 +110,15 @@ void jointClassDefinition(py::class_<IJoint>& joint)
         .def("get_static_friction", &IJoint::getStaticFriction)
         .def("set_joint_dynamics_type", &IJoint::setJointDynamicsType)
         .def("set_damping", &IJoint::setDamping)
-        .def("set_static_friction", &IJoint::setStaticFriction);
+        .def("set_static_friction", &IJoint::setStaticFriction)
+        .def("has_effort_limits", &IJoint::hasEffortLimits)
+        .def("enable_effort_limits", &IJoint::enableEffortLimits)
+        .def("get_effort_limit", &IJoint::getEffortLimit)
+        .def("set_effort_limit", &IJoint::setEffortLimit)
+        .def("has_velocity_limits", &IJoint::hasVelocityLimits)
+        .def("enable_velocity_limits", &IJoint::enableVelocityLimits)
+        .def("get_velocity_limit", &IJoint::getVelocityLimit)
+        .def("set_velocity_limit", &IJoint::setVelocityLimit);
 }
 
 void traversalClassDefinition(py::class_<Traversal>& traversal)

src/model/include/iDynTree/FixedJoint.h

@@ -191,6 +191,18 @@ class FixedJoint : public IJoint
     virtual double getStaticFriction(const size_t _index) const;
     virtual bool setDamping(const size_t _index, double damping);
     virtual bool setStaticFriction(const size_t _index, double staticFriction);
+
+    // EFFORT LIMIT METHODS
+    virtual bool hasEffortLimits() const;
+    virtual bool enableEffortLimits(const bool enable);
+    virtual double getEffortLimit(const size_t _index) const;
+    virtual bool setEffortLimit(const size_t _index, double effortLimit);
+
+    // VELOCITY LIMIT METHODS
+    virtual bool hasVelocityLimits() const;
+    virtual bool enableVelocityLimits(const bool enable);
+    virtual double getVelocityLimit(const size_t _index) const;
+    virtual bool setVelocityLimit(const size_t _index, double velocityLimit);
 };
 } // namespace iDynTree
 

src/model/include/iDynTree/IJoint.h

@@ -492,6 +492,98 @@ class IJoint
 
     ///@}
 
+    /**
+     * @name Joint effort limit methods.
+     *  Methods for handling effort limits of joints.
+     *
+     *  The effort limits are read from the URDF specification and represent
+     *  the maximum absolute torque (for revolute joints) or force (for prismatic joints)
+     *  that can be applied at the joint.
+     */
+    ///@{
+
+    /**
+     * Check if the joint has effort limits.
+     *
+     * @return true if the joint has effort limits, false otherwise
+     */
+    virtual bool hasEffortLimits() const = 0;
+
+    /**
+     * Enable or disable effort limits for the joint.
+     *
+     * @param[in] enable true to enable effort limits, false to disable
+     * @return true if everything went correctly, false otherwise
+     */
+    virtual bool enableEffortLimits(const bool enable) = 0;
+
+    /**
+     * Get the effort limit of the joint for the _index dof.
+     *
+     * @param[in] _index index of the dof for which the limit is obtained.
+     * @return the effort limit, or 0.0 if no limit is set.
+     */
+    virtual double getEffortLimit(const size_t _index) const = 0;
+
+    /**
+     * Set the effort limit for a dof of the joint.
+     *
+     * @param[in] _index index of the dof for which the limit is set.
+     * @param[in] effortLimit the effort limit value
+     * @return true if everything is correct, false otherwise.
+     * @note This just sets the internal effort limit of the joint.
+     *       To set them as enabled, you need to call the enableEffortLimits(true) method.
+     */
+    virtual bool setEffortLimit(const size_t _index, double effortLimit) = 0;
+
+    ///@}
+
+    /**
+     * @name Joint velocity limit methods.
+     *  Methods for handling velocity limits of joints.
+     *
+     *  The velocity limits are read from the URDF specification and represent
+     *  the maximum absolute velocity (angular velocity for revolute joints,
+     *  linear velocity for prismatic joints) that can be applied at the joint.
+     */
+    ///@{
+
+    /**
+     * Check if the joint has velocity limits.
+     *
+     * @return true if the joint has velocity limits, false otherwise
+     */
+    virtual bool hasVelocityLimits() const = 0;
+
+    /**
+     * Enable or disable velocity limits for the joint.
+     *
+     * @param[in] enable true to enable velocity limits, false to disable
+     * @return true if everything went correctly, false otherwise
+     */
+    virtual bool enableVelocityLimits(const bool enable) = 0;
+
+    /**
+     * Get the velocity limit of the joint for the _index dof.
+     *
+     * @param[in] _index index of the dof for which the limit is obtained.
+     * @return the velocity limit, or 0.0 if no limit is set.
+     */
+    virtual double getVelocityLimit(const size_t _index) const = 0;
+
+    /**
+     * Set the velocity limit for a dof of the joint.
+     *
+     * @param[in] _index index of the dof for which the limit is set.
+     * @param[in] velocityLimit the velocity limit value
+     * @return true if everything is correct, false otherwise.
+     * @note This just sets the internal velocity limit of the joint.
+     *       To set them as enabled, you need to call the enableVelocityLimits(true) method.
+     */
+    virtual bool setVelocityLimit(const size_t _index, double velocityLimit) = 0;
+
+    ///@}
+
     /**
      * @name Methods to manipulate joint positions
      * Methods to manipulate joint positions

src/model/include/iDynTree/PrismaticJoint.h

@@ -34,6 +34,14 @@ class PrismaticJoint : public MovableJointImpl1
     double m_minPos;
     double m_maxPos;
 
+    // Effort limits
+    bool m_hasEffortLimits;
+    double m_effortLimit;
+
+    // Velocity limits
+    bool m_hasVelocityLimits;
+    double m_velocityLimit;
+
     // Dynamic parameters
     void resetJointDynamics();
     JointDynamicsType m_joint_dynamics_type;
@@ -228,6 +236,18 @@ class PrismaticJoint : public MovableJointImpl1
     virtual double getStaticFriction(const size_t _index) const;
     virtual bool setDamping(const size_t _index, double damping);
     virtual bool setStaticFriction(const size_t _index, double staticFriction);
+
+    // EFFORT LIMIT METHODS
+    virtual bool hasEffortLimits() const;
+    virtual bool enableEffortLimits(const bool enable);
+    virtual double getEffortLimit(const size_t _index) const;
+    virtual bool setEffortLimit(const size_t _index, double effortLimit);
+
+    // VELOCITY LIMIT METHODS
+    virtual bool hasVelocityLimits() const;
+    virtual bool enableVelocityLimits(const bool enable);
+    virtual double getVelocityLimit(const size_t _index) const;
+    virtual bool setVelocityLimit(const size_t _index, double velocityLimit);
 };
 } // namespace iDynTree
 

src/model/include/iDynTree/RevoluteJoint.h

@@ -34,6 +34,14 @@ class RevoluteJoint : public MovableJointImpl1
     double m_minPos;
     double m_maxPos;
 
+    // Effort limits
+    bool m_hasEffortLimits;
+    double m_effortLimit;
+
+    // Velocity limits
+    bool m_hasVelocityLimits;
+    double m_velocityLimit;
+
     // Dynamic parameters
     void resetJointDynamics();
     JointDynamicsType m_joint_dynamics_type;
@@ -236,6 +244,18 @@ class RevoluteJoint : public MovableJointImpl1
     virtual double getStaticFriction(const size_t _index) const;
     virtual bool setDamping(const size_t _index, double damping);
     virtual bool setStaticFriction(const size_t _index, double staticFriction);
+
+    // EFFORT LIMIT METHODS
+    virtual bool hasEffortLimits() const;
+    virtual bool enableEffortLimits(const bool enable);
+    virtual double getEffortLimit(const size_t _index) const;
+    virtual bool setEffortLimit(const size_t _index, double effortLimit);
+
+    // VELOCITY LIMIT METHODS
+    virtual bool hasVelocityLimits() const;
+    virtual bool enableVelocityLimits(const bool enable);
+    virtual double getVelocityLimit(const size_t _index) const;
+    virtual bool setVelocityLimit(const size_t _index, double velocityLimit);
 };
 } // namespace iDynTree
 

src/model/include/iDynTree/RevoluteSO2Joint.h

@@ -45,6 +45,14 @@ class RevoluteSO2Joint : public MovableJointImpl21
     double m_minPos;
     double m_maxPos;
 
+    // Effort limits
+    bool m_hasEffortLimits;
+    double m_effortLimit;
+
+    // Velocity limits
+    bool m_hasVelocityLimits;
+    double m_velocityLimit;
+
     // Dynamic parameters
     void resetJointDynamics();
     JointDynamicsType m_joint_dynamics_type;
@@ -248,6 +256,18 @@ class RevoluteSO2Joint : public MovableJointImpl21
     virtual bool setDamping(const size_t _index, double damping);
     virtual bool setStaticFriction(const size_t _index, double staticFriction);
 
+    // EFFORT LIMIT METHODS
+    virtual bool hasEffortLimits() const;
+    virtual bool enableEffortLimits(const bool enable);
+    virtual double getEffortLimit(const size_t _index) const;
+    virtual bool setEffortLimit(const size_t _index, double effortLimit);
+
+    // VELOCITY LIMIT METHODS
+    virtual bool hasVelocityLimits() const;
+    virtual bool enableVelocityLimits(const bool enable);
+    virtual double getVelocityLimit(const size_t _index) const;
+    virtual bool setVelocityLimit(const size_t _index, double velocityLimit);
+
     /**
      * Get the current angle from the complex representation.
      * @param jntPos the joint positions vector

src/model/include/iDynTree/SphericalJoint.h

@@ -200,6 +200,18 @@ class SphericalJoint : public MovableJointImpl<4, 3>
     // Documentation inherited
     virtual double getStaticFriction(const size_t _index) const;
 
+    // EFFORT LIMIT METHODS
+    virtual bool hasEffortLimits() const;
+    virtual bool enableEffortLimits(const bool enable);
+    virtual double getEffortLimit(const size_t _index) const;
+    virtual bool setEffortLimit(const size_t _index, double effortLimit);
+
+    // VELOCITY LIMIT METHODS
+    virtual bool hasVelocityLimits() const;
+    virtual bool enableVelocityLimits(const bool enable);
+    virtual double getVelocityLimit(const size_t _index) const;
+    virtual bool setVelocityLimit(const size_t _index, double velocityLimit);
+
     // Documentation inherited
     virtual bool getPositionDerivativeVelocityJacobian(const iDynTree::Span<const double> jntPos,
                                                        iDynTree::MatrixView<double>& jac) const;

src/model/src/FixedJoint.cpp

@@ -329,4 +329,48 @@ bool FixedJoint::normalizeJointPosCoords(iDynTree::Span<double> jntPos) const
     return true;
 }
 
+// EFFORT LIMIT METHODS
+// Fixed joints don't support effort limits
+bool FixedJoint::hasEffortLimits() const
+{
+    return false;
+}
+
+bool FixedJoint::enableEffortLimits(const bool /*enable*/)
+{
+    return false;
+}
+
+double FixedJoint::getEffortLimit(const size_t /*_index*/) const
+{
+    return 0.0;
+}
+
+bool FixedJoint::setEffortLimit(const size_t /*_index*/, double /*effortLimit*/)
+{
+    return false;
+}
+
+// VELOCITY LIMIT METHODS
+// Fixed joints don't support velocity limits
+bool FixedJoint::hasVelocityLimits() const
+{
+    return false;
+}
+
+bool FixedJoint::enableVelocityLimits(const bool /*enable*/)
+{
+    return false;
+}
+
+double FixedJoint::getVelocityLimit(const size_t /*_index*/) const
+{
+    return 0.0;
+}
+
+bool FixedJoint::setVelocityLimit(const size_t /*_index*/, double /*velocityLimit*/)
+{
+    return false;
+}
+
 } // namespace iDynTree

src/model/src/PrismaticJoint.cpp

@@ -30,6 +30,10 @@ PrismaticJoint::PrismaticJoint()
     this->resetBuffers(0);
     this->disablePosLimits();
     this->resetJointDynamics();
+    m_hasEffortLimits = false;
+    m_effortLimit = 0.0;
+    m_hasVelocityLimits = false;
+    m_velocityLimit = 0.0;
 }
 
 PrismaticJoint::PrismaticJoint(const LinkIndex _link1,
@@ -48,6 +52,10 @@ PrismaticJoint::PrismaticJoint(const LinkIndex _link1,
     this->resetBuffers(0);
     this->disablePosLimits();
     this->resetJointDynamics();
+    m_hasEffortLimits = false;
+    m_effortLimit = 0.0;
+    m_hasVelocityLimits = false;
+    m_velocityLimit = 0.0;
 }
 
 PrismaticJoint::PrismaticJoint(const PrismaticJoint& other)
@@ -58,6 +66,10 @@ PrismaticJoint::PrismaticJoint(const PrismaticJoint& other)
     , m_hasPosLimits(other.m_hasPosLimits)
     , m_minPos(other.m_minPos)
     , m_maxPos(other.m_maxPos)
+    , m_hasEffortLimits(other.m_hasEffortLimits)
+    , m_effortLimit(other.m_effortLimit)
+    , m_hasVelocityLimits(other.m_hasVelocityLimits)
+    , m_velocityLimit(other.m_velocityLimit)
     , m_joint_dynamics_type(other.m_joint_dynamics_type)
     , m_damping(other.m_damping)
     , m_static_friction(other.m_static_friction)
@@ -470,4 +482,50 @@ bool PrismaticJoint::normalizeJointPosCoords(iDynTree::Span<double> jntPos) cons
     return true;
 }
 
+// EFFORT LIMIT METHODS
+bool PrismaticJoint::hasEffortLimits() const
+{
+    return m_hasEffortLimits;
+}
+
+bool PrismaticJoint::enableEffortLimits(const bool enable)
+{
+    m_hasEffortLimits = enable;
+    return true;
+}
+
+double PrismaticJoint::getEffortLimit(const size_t /*_index*/) const
+{
+    return m_effortLimit;
+}
+
+bool PrismaticJoint::setEffortLimit(const size_t /*_index*/, double effortLimit)
+{
+    m_effortLimit = effortLimit;
+    return true;
+}
+
+// VELOCITY LIMIT METHODS
+bool PrismaticJoint::hasVelocityLimits() const
+{
+    return m_hasVelocityLimits;
+}
+
+bool PrismaticJoint::enableVelocityLimits(const bool enable)
+{
+    m_hasVelocityLimits = enable;
+    return true;
+}
+
+double PrismaticJoint::getVelocityLimit(const size_t /*_index*/) const
+{
+    return m_velocityLimit;
+}
+
+bool PrismaticJoint::setVelocityLimit(const size_t /*_index*/, double velocityLimit)
+{
+    m_velocityLimit = velocityLimit;
+    return true;
+}
+
 } // namespace iDynTree