Fix getAxis implementation in Revolute and Prismatic Joint and docume…

…ntation cleanup The direction of the axis in a joint depends on which body you consider parent and which body you consider child . See "Minimal formulation of joint motion for biomechanisms", Section 2.4 and Traversaro's PhD Thesis, Chapter 3. This is a small change, but it is extremly important. I hope that the additional documentation will help document it.
robotology · Aug 2, 2017 · 6d45e05 · 6d45e05
1 parent d2b5dd4
commit 6d45e05
Show file tree

Hide file tree

Showing 9 changed files with 156 additions and 18 deletions.
diff --git a/src/core/include/iDynTree/Core/Axis.h b/src/core/include/iDynTree/Core/Axis.h
@@ -119,6 +119,19 @@ namespace iDynTree
 
         SpatialAcc getTranslationSpatialAcc(const double d2dist) const;
 
+        /**
+         * Check if two axes are parallel (i.e. their direction are parallel).
+         *
+         * @param otherAxis the axes to check for parallelism.
+         * @param tolerance tolerance to use in the parallelism check.
+         */
+        bool isParallel(const Axis & otherAxis, const double tolerance) const;
+
+        /**
+         * Return the axis with the same origin, but reversed direction.
+         */
+        Axis reverse() const;
+
         /**
          * @name Output helpers.
          *  Output helpers.

diff --git a/src/core/include/iDynTree/Core/Direction.h b/src/core/include/iDynTree/Core/Direction.h
@@ -66,6 +66,28 @@ namespace iDynTree
          */
         void Normalize(double tol=DEFAULT_TOL);
 
+        /**
+         * Check if two directions are parallel.
+         *
+         * @param otherDirection the direction to check for parallelism.
+         * @param tolerance tolerance to use in the parallelism check.
+         */
+        bool isParallel(const Direction & otherDirection, double tolerance) const;
+
+        /**
+         * Check if two directions are perpendicular.
+         *
+         * @param otherDirection the direction to check for the perpendicular check.
+         * @param tolerance tolerance to use in the perpendicular check.
+         */
+        bool isPerpendicular(const Direction & otherDirection, double tolerance) const;
+
+        /**
+         * Return the direction, i.e. return its opposite.
+         *
+         */
+        Direction reverse() const;
+
         /**
          * @name Output helpers.
          *  Output helpers.

diff --git a/src/core/src/Axis.cpp b/src/core/src/Axis.cpp
@@ -245,6 +245,17 @@ namespace iDynTree
         return ret;
     }
 
+    bool Axis::isParallel(const Axis& otherAxis, const double tolerance) const
+    {
+        return this->direction.isParallel(otherAxis.direction,tolerance);
+    }
+
+    Axis Axis::reverse() const
+    {
+        return Axis(this->getDirection().reverse(),
+                    this->getOrigin());
+    }
+
     std::string Axis::toString() const
     {
         std::stringstream ss;

diff --git a/src/core/src/Direction.cpp b/src/core/src/Direction.cpp
@@ -7,6 +7,7 @@
 
 #include <iDynTree/Core/Direction.h>
 
+#include <iDynTree/Core/EigenHelpers.h>
 #include <Eigen/Dense>
 
 #include <cstdio>
@@ -60,6 +61,52 @@ namespace iDynTree
         return;
     }
 
+    bool Direction::isParallel(const Direction& otherDirection, double tolerance) const
+    {
+        // The tolerance should be positive
+        assert(tolerance > 0);
+
+        // Compute the difference of the norm
+        Eigen::Vector3d diff = toEigen(*this)-toEigen(otherDirection);
+        double diffNorm = diff.norm();
+
+        // There are two possibilities for two directions to be parallel
+        // either they point in the same direction, or in opposite directions
+        if( ( diffNorm < tolerance ) ||
+            ( fabs(diffNorm-2) < tolerance ) )
+        {
+            return true;
+        }
+        else
+        {
+            return false;
+        }
+    }
+
+    bool Direction::isPerpendicular(const Direction& otherDirection, double tolerance) const
+    {
+        assert(tolerance > 0);
+
+        double fabsDotProduct = fabs(toEigen(*this).dot(toEigen(otherDirection)));
+
+        if( fabsDotProduct > tolerance )
+        {
+            return false;
+        }
+        else
+        {
+            return true;
+        }
+    }
+
+    Direction Direction::reverse() const
+    {
+        return Direction(-this->m_data[0],
+                         -this->m_data[1],
+                         -this->m_data[2]);
+    }
+
+
     std::string Direction::toString() const
     {
         std::stringstream ss;

diff --git a/src/model/include/iDynTree/Model/IJoint.h b/src/model/include/iDynTree/Model/IJoint.h
@@ -168,7 +168,14 @@ namespace iDynTree
          * vector  such that
          * v_child = S_{child,parent}*dq_i + child_X_parent*v_parent
          * if the velocities associated to all other DOFs of the joint
-         * are considered zero.
+         * are considered zero, where v_child and v_parent are the left-trivialized
+         * (body) velocities of the link child and parent.
+         *
+         * See
+         * "Modelling, Estimation and Identification of Humanoid Robots Dynamics"
+         * Silvio Traversaro - Section 3.2
+         * https://traversaro.github.io/preprints/traversaro-phd-thesis.pdf
+         * for more details.
          *
          * @return the motion subspace vector.
          *

diff --git a/src/model/include/iDynTree/Model/PrismaticJoint.h b/src/model/include/iDynTree/Model/PrismaticJoint.h
@@ -12,7 +12,7 @@
 #include <iDynTree/Core/SpatialMotionVector.h>
 
 #include <iDynTree/Core/Axis.h>
-#include <iDynTree/Model/Indeces.h>
+#include <iDynTree/Model/Indices.h>
 #include <iDynTree/Model/MovableJointImpl.h>
 
 namespace iDynTree
@@ -85,13 +85,31 @@ namespace iDynTree
         virtual LinkIndex getSecondAttachedLink() const;
 
         /**
-         * Get the prismatic axis of the robot, expressed in linkA frame.
+         * Get the revolute axis of the robot, expressed in linkA frame.
          *
-         * @param linkA the link frame (one of the two at which the link is attached)
-         *              in which the returned axis is expressed.
+         * @param child the link frame (one of the two at which the link is attached)
+         *              in which the returned axis is expressed. Furthermore, the
+         *              axis direction depends on the assumption that this frame is
+         *              considered the "child" in the relationship.
          *
+         * See
+         *
+         * Seth, A., Sherman, M., Eastman, P., & Delp, S. (2010).
+         * Minimal formulation of joint motion for biomechanisms.
+         * Nonlinear Dynamics, 62(1), 291-303.
+         * https://nmbl.stanford.edu/publications/pdf/Seth2010.pdf
+         * Section 2.4
+         *
+         * and
+         *
+         * "Modelling, Estimation and Identification of Humanoid Robots Dynamics"
+         * Traversaro - Section 3.2
+         * https://traversaro.github.io/preprints/traversaro-phd-thesis.pdf
+         *
+         * for more details.
          */
-        virtual Axis getAxis(const LinkIndex linkA) const;
+        virtual Axis getAxis(const LinkIndex child,
+                             const LinkIndex parent=LINK_INVALID_INDEX) const;
 
         // Documentation inherited
         virtual Transform getRestTransform(const LinkIndex child,

diff --git a/src/model/include/iDynTree/Model/RevoluteJoint.h b/src/model/include/iDynTree/Model/RevoluteJoint.h
@@ -87,11 +87,29 @@ namespace iDynTree
         /**
          * Get the revolute axis of the robot, expressed in linkA frame.
          *
-         * @param linkA the link frame (one of the two at which the link is attached)
-         *              in which the returned axis is expressed.
+         * @param child the link frame (one of the two at which the link is attached)
+         *              in which the returned axis is expressed. Furthermore, the
+         *              axis direction depends on the assumption that this frame is
+         *              considered the "child" in the relationship.
          *
+         * See
+         *
+         * Seth, A., Sherman, M., Eastman, P., & Delp, S. (2010).
+         * Minimal formulation of joint motion for biomechanisms.
+         * Nonlinear Dynamics, 62(1), 291-303.
+         * https://nmbl.stanford.edu/publications/pdf/Seth2010.pdf
+         * Section 2.4
+         *
+         * and
+         *
+         * "Modelling, Estimation and Identification of Humanoid Robots Dynamics"
+         * Traversaro - Section 3.2
+         * https://traversaro.github.io/preprints/traversaro-phd-thesis.pdf
+         *
+         * for more details.
          */
-        virtual Axis getAxis(const LinkIndex linkA) const;
+        virtual Axis getAxis(const LinkIndex child,
+                             const LinkIndex parent=LINK_INVALID_INDEX) const;
 
         // Documentation inherited
         virtual Transform getRestTransform(const LinkIndex child,

diff --git a/src/model/src/PrismaticJoint.cpp b/src/model/src/PrismaticJoint.cpp
@@ -110,7 +110,7 @@ void PrismaticJoint::resetBuffers(const double new_q) const
 
 void PrismaticJoint::resetAxisBuffers() const
 {
-    this->S_link1_link2 = translation_axis_wrt_link1.getTranslationTwist(1.0);
+    this->S_link1_link2 = -translation_axis_wrt_link1.getTranslationTwist(1.0);
     this->S_link2_link1 = (link1_X_link2_at_rest.inverse()*translation_axis_wrt_link1).getTranslationTwist(1.0);
 }
 
@@ -170,15 +170,16 @@ SpatialMotionVector PrismaticJoint::getMotionSubspaceVector(int dof_i,
 }
 
 
-Axis PrismaticJoint::getAxis(const LinkIndex linkA) const
+Axis PrismaticJoint::getAxis(const LinkIndex child,
+                             const LinkIndex /*parent*/) const
 {
-    if( linkA == link1 )
+    if( child == link1 )
     {
-        return translation_axis_wrt_link1;
+        return translation_axis_wrt_link1.reverse();
     }
     else
     {
-        assert(linkA == link2);
+        assert(child == link2);
         return link1_X_link2_at_rest.inverse()*translation_axis_wrt_link1;
     }
 }

diff --git a/src/model/src/RevoluteJoint.cpp b/src/model/src/RevoluteJoint.cpp
@@ -170,15 +170,16 @@ SpatialMotionVector RevoluteJoint::getMotionSubspaceVector(int dof_i,
 }
 
 
-Axis RevoluteJoint::getAxis(const LinkIndex linkA) const
+Axis RevoluteJoint::getAxis(const LinkIndex child,
+                            const LinkIndex /*parent*/) const
 {
-    if( linkA == link1 )
+    if( child == link1 )
     {
-        return rotation_axis_wrt_link1;
+        return rotation_axis_wrt_link1.reverse();
     }
     else
     {
-        assert(linkA == link2);
+        assert(child == link2);
         return link1_X_link2_at_rest.inverse()*rotation_axis_wrt_link1;
     }
 }