Added Joint::isCyclic to mark SO(2) topology

dart/constraint/ConstraintSolver.cpp

@@ -410,7 +410,7 @@ void ConstraintSolver::updateConstraints()
     {
       dynamics::Joint* joint = skel->getBodyNode(i)->getParentJoint();
 
-      if (joint->isDynamic() && joint->isPositionLimited())
+      if (joint->isDynamic() && joint->isPositionLimitEnforced())
         mJointLimitConstraints.push_back(new JointLimitConstraint(joint));
     }
   }

dart/dynamics/BallJoint.cpp

@@ -70,6 +70,13 @@ const std::string& BallJoint::getStaticType()
   return name;
 }
 
+//==============================================================================
+bool BallJoint::isCyclic(size_t _index) const
+{
+  return _index < 3
+      && !hasPositionLimit(0) && !hasPositionLimit(1) && !hasPositionLimit(2);
+}
+
 //==============================================================================
 BallJoint::Properties BallJoint::getBallJointProperties() const
 {

dart/dynamics/BallJoint.h

@@ -67,6 +67,9 @@ class BallJoint : public MultiDofJoint<3>
   /// Get joint type for this class
   static const std::string& getStaticType();
 
+  // Documentation inherited
+  virtual bool isCyclic(size_t _index) const override;
+
   /// Get the Properties of this BallJoint
   Properties getBallJointProperties() const;
 

dart/dynamics/DegreeOfFreedom.cpp

@@ -171,6 +171,18 @@ double DegreeOfFreedom::getPositionUpperLimit() const
   return mJoint->getPositionUpperLimit(mIndexInJoint);
 }
 
+//==============================================================================
+bool DegreeOfFreedom::hasPositionLimit() const
+{
+  return mJoint->hasPositionLimit(mIndexInJoint);
+}
+
+//==============================================================================
+bool DegreeOfFreedom::isCyclic() const
+{
+  return mJoint->isCyclic(mIndexInJoint);
+}
+
 //==============================================================================
 void DegreeOfFreedom::setVelocity(double _velocity)
 {

dart/dynamics/DegreeOfFreedom.h

@@ -159,6 +159,15 @@ class DegreeOfFreedom : public virtual common::Subject
   /// Get the upper position limit of this DegreeOfFreedom
   double getPositionUpperLimit() const;
 
+  /// Get whether this DOF is cyclic. Return true if and only if an infinite
+  /// number of DOF positions produce the same local transform. If this DOF is
+  /// part of a multi-DOF joint, then producing a cycle may require altering
+  /// the position of the Joint's other DOFs.
+  bool isCyclic() const;
+
+  /// Get whether the position of this DegreeOfFreedom has limits.
+  bool hasPositionLimit() const;
+
   /// \}
 
   //----------------------------------------------------------------------------

dart/dynamics/EulerJoint.cpp

@@ -126,6 +126,12 @@ const std::string& EulerJoint::getStaticType()
   return name;
 }
 
+//==============================================================================
+bool EulerJoint::isCyclic(size_t _index) const
+{
+  return !hasPositionLimit(_index);
+}
+
 //==============================================================================
 void EulerJoint::setAxisOrder(EulerJoint::AxisOrder _order, bool _renameDofs)
 {

dart/dynamics/EulerJoint.h

@@ -108,6 +108,9 @@ class EulerJoint : public MultiDofJoint<3>
   /// Get joint type for this class
   static const std::string& getStaticType();
 
+  // Documentation inherited
+  virtual bool isCyclic(size_t _index) const override;
+
   /// Set the axis order
   /// \param[in] _order Axis order
   /// \param[in] _renameDofs If true, the names of dofs in this joint will be

dart/dynamics/FreeJoint.cpp

@@ -151,6 +151,13 @@ const std::string& FreeJoint::getStaticType()
   return name;
 }
 
+//==============================================================================
+bool FreeJoint::isCyclic(size_t _index) const
+{
+  return _index < 3
+      && !hasPositionLimit(0) && !hasPositionLimit(1) && !hasPositionLimit(2);
+}
+
 //==============================================================================
 void FreeJoint::integratePositions(double _dt)
 {

dart/dynamics/FreeJoint.h

@@ -73,6 +73,9 @@ class FreeJoint : public MultiDofJoint<6>
   /// Get joint type for this class
   static const std::string& getStaticType();
 
+  // Documentation inherited
+  virtual bool isCyclic(size_t _index) const override;
+
   /// Convert a transform into a 6D vector that can be used to set the positions
   /// of a FreeJoint. The positions returned by this function will result in a
   /// relative transform of

dart/dynamics/Joint.cpp

@@ -79,7 +79,7 @@ void Joint::setProperties(const Properties& _properties)
   setName(_properties.mName);
   setTransformFromParentBodyNode(_properties.mT_ParentBodyToJoint);
   setTransformFromChildBodyNode(_properties.mT_ChildBodyToJoint);
-  setPositionLimited(_properties.mIsPositionLimited);
+  setPositionLimitEnforced(_properties.mIsPositionLimited);
   setActuatorType(_properties.mActuatorType);
 }
 
@@ -277,17 +277,29 @@ const Eigen::Vector6d& Joint::getLocalPrimaryAcceleration() const
 }
 
 //==============================================================================
-void Joint::setPositionLimited(bool _isPositionLimited)
+void Joint::setPositionLimitEnforced(bool _isPositionLimited)
 {
   mJointP.mIsPositionLimited = _isPositionLimited;
 }
 
 //==============================================================================
-bool Joint::isPositionLimited() const
+void Joint::setPositionLimited(bool _isPositionLimited)
+{
+  setPositionLimitEnforced(_isPositionLimited);
+}
+
+//==============================================================================
+bool Joint::isPositionLimitEnforced() const
 {
   return mJointP.mIsPositionLimited;
 }
 
+//==============================================================================
+bool Joint::isPositionLimited() const
+{
+  return isPositionLimitEnforced();
+}
+
 //==============================================================================
 size_t Joint::getJointIndexInSkeleton() const
 {

dart/dynamics/Joint.h

@@ -42,6 +42,7 @@
 #include <vector>
 #include <memory>
 
+#include "dart/common/Deprecated.h"
 #include "dart/common/Subject.h"
 #include "dart/math/MathTypes.h"
 #include "dart/dynamics/SmartPointer.h"
@@ -244,15 +245,21 @@ class Joint : public virtual common::Subject
   /// PASSIVE/FORCE.
   ///
   /// \sa ActuatorType
-  void setPositionLimited(bool _isPositionLimited);
+  void setPositionLimitEnforced(bool _isPositionLimited);
+
+  /// Deprecated. Replaced by setPositionLimitEnforced.
+  DEPRECATED(5.0) void setPositionLimited(bool _isPositionLimited);
 
   /// Get whether enforcing joint position limit
   ///
   /// The joint position limit is valid when the actutor type is one of
   /// PASSIVE/FORCE.
   ///
   /// \sa ActuatorType
-  bool isPositionLimited() const;
+  bool isPositionLimitEnforced() const;
+
+  /// Deprecated. Replaced by isPositionLimitEnforced.
+  DEPRECATED(5.0) bool isPositionLimited() const;
 
   /// Get a unique index in skeleton of a generalized coordinate in this Joint
   virtual size_t getIndexInSkeleton(size_t _index) const = 0;
@@ -347,6 +354,16 @@ class Joint : public virtual common::Subject
   /// Get upper limit for position
   virtual double getPositionUpperLimit(size_t _index) const = 0;
 
+  /// Get whether a generalized coordinate is cyclic. Return true if and only
+  /// if this generalized coordinate has an infinite number of positions that
+  /// produce the same local transform. Note that, for a multi-DOF joint,
+  /// producing a cycle may require altering the position of this Joint's other
+  /// generalized coordinates.
+  virtual bool isCyclic(size_t _index) const = 0;
+
+  /// Get whether the position of a generalized coordinate has limits.
+  virtual bool hasPositionLimit(size_t _index) const = 0;
+
   /// \}
 
   //----------------------------------------------------------------------------

dart/dynamics/MultiDofJoint.h

@@ -252,6 +252,9 @@ class MultiDofJoint : public Joint
   // Documentation inherited
   double getPositionUpperLimit(size_t _index) const override;
 
+  // Documentation inherited
+  virtual bool hasPositionLimit(size_t _index) const override;
+
   //----------------------------------------------------------------------------
   // Velocity
   //----------------------------------------------------------------------------

dart/dynamics/PlanarJoint.cpp

@@ -212,6 +212,12 @@ const std::string& PlanarJoint::getStaticType()
   return name;
 }
 
+//==============================================================================
+bool PlanarJoint::isCyclic(size_t _index) const
+{
+  return _index == 2 && !hasPositionLimit(_index);
+}
+
 //==============================================================================
 void PlanarJoint::setXYPlane(bool _renameDofs)
 {

dart/dynamics/PlanarJoint.h

@@ -148,6 +148,9 @@ class PlanarJoint : public MultiDofJoint<3>
   /// Get joint type for this class
   static const std::string& getStaticType();
 
+  // Documentation inherited
+  virtual bool isCyclic(size_t _index) const override;
+
   /// \brief Set plane type as XY-plane
   /// \param[in] _renameDofs If true, the names of dofs in this joint will be
   /// renmaed according to the plane type.

dart/dynamics/PrismaticJoint.cpp

@@ -126,6 +126,12 @@ const std::string& PrismaticJoint::getStaticType()
   return name;
 }
 
+//==============================================================================
+bool PrismaticJoint::isCyclic(size_t _index) const
+{
+  return false;
+}
+
 //==============================================================================
 void PrismaticJoint::setAxis(const Eigen::Vector3d& _axis)
 {

dart/dynamics/PrismaticJoint.h

@@ -99,6 +99,9 @@ class PrismaticJoint : public SingleDofJoint
   /// Get joint type for this class
   static const std::string& getStaticType();
 
+  // Documentation inherited
+  virtual bool isCyclic(size_t _index) const override;
+
   ///
   void setAxis(const Eigen::Vector3d& _axis);
 

dart/dynamics/RevoluteJoint.cpp

@@ -120,6 +120,12 @@ const std::string& RevoluteJoint::getType() const
   return getStaticType();
 }
 
+//==============================================================================
+bool RevoluteJoint::isCyclic(size_t _index) const
+{
+  return !hasPositionLimit(_index);
+}
+
 //==============================================================================
 const std::string& RevoluteJoint::getStaticType()
 {

dart/dynamics/RevoluteJoint.h

@@ -98,6 +98,9 @@ class RevoluteJoint : public SingleDofJoint
   // Documentation inherited
   virtual const std::string& getType() const override;
 
+  // Documentation inherited
+  virtual bool isCyclic(size_t _index) const override;
+
   /// Get joint type for this class
   static const std::string& getStaticType();
 

dart/dynamics/ScrewJoint.cpp

@@ -129,6 +129,12 @@ const std::string& ScrewJoint::getStaticType()
   return name;
 }
 
+//==============================================================================
+bool ScrewJoint::isCyclic(size_t _index) const
+{
+  return false;
+}
+
 //==============================================================================
 void ScrewJoint::setAxis(const Eigen::Vector3d& _axis)
 {

dart/dynamics/ScrewJoint.h

@@ -105,6 +105,9 @@ class ScrewJoint : public SingleDofJoint
   /// Get joint type for this class
   static const std::string& getStaticType();
 
+  // Documentation inherited
+  virtual bool isCyclic(size_t _index) const override;
+
   ///
   void setAxis(const Eigen::Vector3d& _axis);
 

dart/dynamics/SingleDofJoint.cpp

@@ -473,6 +473,19 @@ double SingleDofJoint::getPositionUpperLimit(size_t _index) const
   return mSingleDofP.mPositionUpperLimit;
 }
 
+//==============================================================================
+bool SingleDofJoint::hasPositionLimit(size_t _index) const
+{
+  if (_index != 0)
+  {
+    SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( hasPositionLimit, _index );
+    return true;
+  }
+
+  return std::isfinite(mSingleDofP.mPositionLowerLimit)
+      || std::isfinite(mSingleDofP.mPositionUpperLimit);
+}
+
 //==============================================================================
 void SingleDofJoint::setVelocity(size_t _index, double _velocity)
 {

dart/dynamics/SingleDofJoint.h

@@ -227,6 +227,9 @@ class SingleDofJoint : public Joint
   // Documentation inherited
   double getPositionUpperLimit(size_t _index) const override;
 
+  // Documentation inherited
+  bool hasPositionLimit(size_t _index) const override;
+
   //----------------------------------------------------------------------------
   // Velocity
   //----------------------------------------------------------------------------

dart/dynamics/TranslationalJoint.cpp

@@ -99,6 +99,12 @@ const std::string& TranslationalJoint::getStaticType()
   return name;
 }
 
+//==============================================================================
+bool TranslationalJoint::isCyclic(size_t _index) const
+{
+  return false;
+}
+
 //==============================================================================
 void TranslationalJoint::updateDegreeOfFreedomNames()
 {

dart/dynamics/TranslationalJoint.h

@@ -71,6 +71,9 @@ class TranslationalJoint : public MultiDofJoint<3>
   /// Get joint type for this class
   static const std::string& getStaticType();
 
+  // Documentation inherited
+  virtual bool isCyclic(size_t _index) const override;
+
   Eigen::Matrix<double, 6, 3> getLocalJacobianStatic(
       const Eigen::Vector3d& _positions) const override;
 

dart/dynamics/UniversalJoint.cpp

@@ -127,6 +127,12 @@ const std::string& UniversalJoint::getStaticType()
   return name;
 }
 
+//==============================================================================
+bool UniversalJoint::isCyclic(size_t _index) const
+{
+  return !hasPositionLimit(_index);
+}
+
 //==============================================================================
 void UniversalJoint::setAxis1(const Eigen::Vector3d& _axis)
 {

dart/dynamics/UniversalJoint.h

@@ -100,6 +100,9 @@ class UniversalJoint : public MultiDofJoint<2>
   /// Get joint type for this class
   static const std::string& getStaticType();
 
+  // Documentation inherited
+  virtual bool isCyclic(size_t _index) const override;
+
   ///
   void setAxis1(const Eigen::Vector3d& _axis);
 

dart/dynamics/WeldJoint.cpp

@@ -70,6 +70,12 @@ const std::string& WeldJoint::getStaticType()
   return name;
 }
 
+//==============================================================================
+bool WeldJoint::isCyclic(size_t _index) const
+{
+  return false;
+}
+
 //==============================================================================
 WeldJoint::Properties WeldJoint::getWeldJointProperties() const
 {