From 85fdd08b0866ece4e7857bc9046f70fa2b907977 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Wed, 8 Jul 2015 20:15:36 -0500 Subject: [PATCH 1/9] Added Joint::isPositionLimited function. --- dart/dynamics/Joint.h | 6 ++++++ dart/dynamics/MultiDofJoint.h | 3 +++ dart/dynamics/SingleDofJoint.cpp | 13 +++++++++++++ dart/dynamics/SingleDofJoint.h | 3 +++ dart/dynamics/ZeroDofJoint.cpp | 6 ++++++ dart/dynamics/ZeroDofJoint.h | 3 +++ dart/dynamics/detail/MultiDofJoint.h | 14 ++++++++++++++ 7 files changed, 48 insertions(+) diff --git a/dart/dynamics/Joint.h b/dart/dynamics/Joint.h index f911f4e662f66..9fee20be01b22 100644 --- a/dart/dynamics/Joint.h +++ b/dart/dynamics/Joint.h @@ -347,6 +347,12 @@ class Joint : public virtual common::Subject /// Get upper limit for position virtual double getPositionUpperLimit(size_t _index) const = 0; + /// Gets whether a generalized coordinate is cyclic; i.e. has SO(2) topology. + virtual bool isCyclic(size_t _index) const = 0; + + /// Gets whether the position of a generalized coordinate is limited. + virtual bool isPositionLimited(size_t _index) const = 0; + /// \} //---------------------------------------------------------------------------- diff --git a/dart/dynamics/MultiDofJoint.h b/dart/dynamics/MultiDofJoint.h index 6a9051b1ac769..8e55e89b0afe1 100644 --- a/dart/dynamics/MultiDofJoint.h +++ b/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 isPositionLimited(size_t _index) const override; + //---------------------------------------------------------------------------- // Velocity //---------------------------------------------------------------------------- diff --git a/dart/dynamics/SingleDofJoint.cpp b/dart/dynamics/SingleDofJoint.cpp index 971e1452724ee..766296ae64e80 100644 --- a/dart/dynamics/SingleDofJoint.cpp +++ b/dart/dynamics/SingleDofJoint.cpp @@ -473,6 +473,19 @@ double SingleDofJoint::getPositionUpperLimit(size_t _index) const return mSingleDofP.mPositionUpperLimit; } +//============================================================================== +bool SingleDofJoint::isPositionLimited(size_t _index) const +{ + if (_index != 0) + { + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( isPositionLimited, _index ); + return true; + } + + return !(std::isinf(mSingleDofP.mPositionLowerLimit) + && std::isinf(mSingleDofP.mPositionUpperLimit)); +} + //============================================================================== void SingleDofJoint::setVelocity(size_t _index, double _velocity) { diff --git a/dart/dynamics/SingleDofJoint.h b/dart/dynamics/SingleDofJoint.h index 4ed8ee99d2b6f..bf8565910073a 100644 --- a/dart/dynamics/SingleDofJoint.h +++ b/dart/dynamics/SingleDofJoint.h @@ -227,6 +227,9 @@ class SingleDofJoint : public Joint // Documentation inherited double getPositionUpperLimit(size_t _index) const override; + // Documentation inherited + bool isPositionLimited(size_t _index) const override; + //---------------------------------------------------------------------------- // Velocity //---------------------------------------------------------------------------- diff --git a/dart/dynamics/ZeroDofJoint.cpp b/dart/dynamics/ZeroDofJoint.cpp index c52913d4ce3e2..21eec0cb1c8bd 100644 --- a/dart/dynamics/ZeroDofJoint.cpp +++ b/dart/dynamics/ZeroDofJoint.cpp @@ -221,6 +221,12 @@ double ZeroDofJoint::getPositionUpperLimit(size_t _index) const return 0.0; } +//============================================================================== +bool ZeroDofJoint::isPositionLimited(size_t _index) const +{ + return true; +} + //============================================================================== void ZeroDofJoint::setVelocity(size_t _index, double _velocity) diff --git a/dart/dynamics/ZeroDofJoint.h b/dart/dynamics/ZeroDofJoint.h index 14d9763dee5f6..6d82b393cc048 100644 --- a/dart/dynamics/ZeroDofJoint.h +++ b/dart/dynamics/ZeroDofJoint.h @@ -144,6 +144,9 @@ class ZeroDofJoint : public Joint // Documentation inherited virtual double getPositionUpperLimit(size_t _index) const override; + // Documentation inherited + virtual bool isPositionLimited(size_t _index) const override; + //---------------------------------------------------------------------------- // Velocity //---------------------------------------------------------------------------- diff --git a/dart/dynamics/detail/MultiDofJoint.h b/dart/dynamics/detail/MultiDofJoint.h index 8813485e2a6a9..3e1a06214c674 100644 --- a/dart/dynamics/detail/MultiDofJoint.h +++ b/dart/dynamics/detail/MultiDofJoint.h @@ -586,6 +586,20 @@ double MultiDofJoint::getPositionUpperLimit(size_t _index) const return mMultiDofP.mPositionUpperLimits[_index]; } +//============================================================================== +template +bool MultiDofJoint::isPositionLimited(size_t _index) const +{ + if (_index >= getNumDofs()) + { + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(isPositionLimited, _index); + return true; + } + + return !(std::isinf(mMultiDofP.mPositionUpperLimits[_index]) + && std::isinf(mMultiDofP.mPositionLowerLimits[_index])); +} + //============================================================================== template void MultiDofJoint::setVelocity(size_t _index, double _velocity) From bf258b88cc5c4ceb6ab484a72702e9523489fad5 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Tue, 7 Jul 2015 21:17:40 -0500 Subject: [PATCH 2/9] Added Joint::isCyclic to mark SO(2) topology. --- dart/dynamics/BallJoint.cpp | 6 ++++++ dart/dynamics/BallJoint.h | 3 +++ dart/dynamics/EulerJoint.cpp | 6 ++++++ dart/dynamics/EulerJoint.h | 3 +++ dart/dynamics/FreeJoint.cpp | 6 ++++++ dart/dynamics/FreeJoint.h | 3 +++ dart/dynamics/PlanarJoint.cpp | 6 ++++++ dart/dynamics/PlanarJoint.h | 3 +++ dart/dynamics/PrismaticJoint.cpp | 6 ++++++ dart/dynamics/PrismaticJoint.h | 3 +++ dart/dynamics/RevoluteJoint.cpp | 6 ++++++ dart/dynamics/RevoluteJoint.h | 3 +++ dart/dynamics/ScrewJoint.cpp | 6 ++++++ dart/dynamics/ScrewJoint.h | 3 +++ dart/dynamics/TranslationalJoint.cpp | 6 ++++++ dart/dynamics/TranslationalJoint.h | 3 +++ dart/dynamics/UniversalJoint.cpp | 6 ++++++ dart/dynamics/UniversalJoint.h | 3 +++ dart/dynamics/WeldJoint.cpp | 6 ++++++ dart/dynamics/WeldJoint.h | 3 +++ 20 files changed, 90 insertions(+) diff --git a/dart/dynamics/BallJoint.cpp b/dart/dynamics/BallJoint.cpp index f345ff2b10cfa..6b8b44907100c 100644 --- a/dart/dynamics/BallJoint.cpp +++ b/dart/dynamics/BallJoint.cpp @@ -70,6 +70,12 @@ const std::string& BallJoint::getStaticType() return name; } +//============================================================================== +bool BallJoint::isCyclic(size_t _index) const +{ + return !isPositionLimited(_index); +} + //============================================================================== BallJoint::Properties BallJoint::getBallJointProperties() const { diff --git a/dart/dynamics/BallJoint.h b/dart/dynamics/BallJoint.h index 50c8609a25545..ea44fbf2f16c2 100644 --- a/dart/dynamics/BallJoint.h +++ b/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; diff --git a/dart/dynamics/EulerJoint.cpp b/dart/dynamics/EulerJoint.cpp index 83099f708b62c..f1ca9b2beb7f2 100644 --- a/dart/dynamics/EulerJoint.cpp +++ b/dart/dynamics/EulerJoint.cpp @@ -126,6 +126,12 @@ const std::string& EulerJoint::getStaticType() return name; } +//============================================================================== +bool EulerJoint::isCyclic(size_t _index) const +{ + return !isPositionLimited(_index); +} + //============================================================================== void EulerJoint::setAxisOrder(EulerJoint::AxisOrder _order, bool _renameDofs) { diff --git a/dart/dynamics/EulerJoint.h b/dart/dynamics/EulerJoint.h index 05855ffbfcc65..f2e3fae098523 100644 --- a/dart/dynamics/EulerJoint.h +++ b/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 diff --git a/dart/dynamics/FreeJoint.cpp b/dart/dynamics/FreeJoint.cpp index 64e9e8702014b..c53112637f868 100644 --- a/dart/dynamics/FreeJoint.cpp +++ b/dart/dynamics/FreeJoint.cpp @@ -151,6 +151,12 @@ const std::string& FreeJoint::getStaticType() return name; } +//============================================================================== +bool FreeJoint::isCyclic(size_t _index) const +{ + return _index < 3 && !isPositionLimited(_index); +} + //============================================================================== void FreeJoint::integratePositions(double _dt) { diff --git a/dart/dynamics/FreeJoint.h b/dart/dynamics/FreeJoint.h index 8f61557e5e0db..c699fa70a7597 100644 --- a/dart/dynamics/FreeJoint.h +++ b/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 diff --git a/dart/dynamics/PlanarJoint.cpp b/dart/dynamics/PlanarJoint.cpp index f820ea5d37d96..2e7c279a1ce3e 100644 --- a/dart/dynamics/PlanarJoint.cpp +++ b/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 && !isPositionLimited(_index); +} + //============================================================================== void PlanarJoint::setXYPlane(bool _renameDofs) { diff --git a/dart/dynamics/PlanarJoint.h b/dart/dynamics/PlanarJoint.h index e54597ae838a0..56f0427fca1e6 100644 --- a/dart/dynamics/PlanarJoint.h +++ b/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. diff --git a/dart/dynamics/PrismaticJoint.cpp b/dart/dynamics/PrismaticJoint.cpp index 9391987faaf90..1977627690211 100644 --- a/dart/dynamics/PrismaticJoint.cpp +++ b/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) { diff --git a/dart/dynamics/PrismaticJoint.h b/dart/dynamics/PrismaticJoint.h index 0fbf41a2d7615..3ce7a38723724 100644 --- a/dart/dynamics/PrismaticJoint.h +++ b/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); diff --git a/dart/dynamics/RevoluteJoint.cpp b/dart/dynamics/RevoluteJoint.cpp index 789923fa3cc05..206b15f8ec8d1 100644 --- a/dart/dynamics/RevoluteJoint.cpp +++ b/dart/dynamics/RevoluteJoint.cpp @@ -120,6 +120,12 @@ const std::string& RevoluteJoint::getType() const return getStaticType(); } +//============================================================================== +bool RevoluteJoint::isCyclic(size_t _index) const +{ + return !isPositionLimited(_index); +} + //============================================================================== const std::string& RevoluteJoint::getStaticType() { diff --git a/dart/dynamics/RevoluteJoint.h b/dart/dynamics/RevoluteJoint.h index c6e43448993d1..4831b07303b2f 100644 --- a/dart/dynamics/RevoluteJoint.h +++ b/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(); diff --git a/dart/dynamics/ScrewJoint.cpp b/dart/dynamics/ScrewJoint.cpp index 90e2a56dcb413..4f6771df39f2c 100644 --- a/dart/dynamics/ScrewJoint.cpp +++ b/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) { diff --git a/dart/dynamics/ScrewJoint.h b/dart/dynamics/ScrewJoint.h index d4164f51e5dc0..be80b97248c55 100644 --- a/dart/dynamics/ScrewJoint.h +++ b/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); diff --git a/dart/dynamics/TranslationalJoint.cpp b/dart/dynamics/TranslationalJoint.cpp index 7ec30da72371d..0ae6490190466 100644 --- a/dart/dynamics/TranslationalJoint.cpp +++ b/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() { diff --git a/dart/dynamics/TranslationalJoint.h b/dart/dynamics/TranslationalJoint.h index e51e87eee6747..e6e1ed8c832b0 100644 --- a/dart/dynamics/TranslationalJoint.h +++ b/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 getLocalJacobianStatic( const Eigen::Vector3d& _positions) const override; diff --git a/dart/dynamics/UniversalJoint.cpp b/dart/dynamics/UniversalJoint.cpp index 341bf81d0c4ef..eefb177107d99 100644 --- a/dart/dynamics/UniversalJoint.cpp +++ b/dart/dynamics/UniversalJoint.cpp @@ -127,6 +127,12 @@ const std::string& UniversalJoint::getStaticType() return name; } +//============================================================================== +bool UniversalJoint::isCyclic(size_t _index) const +{ + return !isPositionLimited(_index); +} + //============================================================================== void UniversalJoint::setAxis1(const Eigen::Vector3d& _axis) { diff --git a/dart/dynamics/UniversalJoint.h b/dart/dynamics/UniversalJoint.h index 8d767fd726577..faf0cbe4afb5e 100644 --- a/dart/dynamics/UniversalJoint.h +++ b/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); diff --git a/dart/dynamics/WeldJoint.cpp b/dart/dynamics/WeldJoint.cpp index f8a2af60d60ca..e8c054ef4b277 100644 --- a/dart/dynamics/WeldJoint.cpp +++ b/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 { diff --git a/dart/dynamics/WeldJoint.h b/dart/dynamics/WeldJoint.h index 291cae1cd3ccb..a24dc8419d10e 100644 --- a/dart/dynamics/WeldJoint.h +++ b/dart/dynamics/WeldJoint.h @@ -71,6 +71,9 @@ class WeldJoint : public ZeroDofJoint /// Get joint type for this class static const std::string& getStaticType(); + // Documentation inherited + virtual bool isCyclic(size_t _index) const override; + // Documentation inherited virtual void setTransformFromParentBodyNode(const Eigen::Isometry3d& _T) override; From 57d93f3c7db8495b88c5093c8ffb3dbf1073abcc Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Wed, 8 Jul 2015 21:01:22 -0500 Subject: [PATCH 3/9] Switched isPositionLimited to hasPositionLimit. --- dart/dynamics/BallJoint.cpp | 2 +- dart/dynamics/EulerJoint.cpp | 2 +- dart/dynamics/FreeJoint.cpp | 2 +- dart/dynamics/Joint.h | 2 +- dart/dynamics/MultiDofJoint.h | 2 +- dart/dynamics/PlanarJoint.cpp | 2 +- dart/dynamics/RevoluteJoint.cpp | 2 +- dart/dynamics/SingleDofJoint.cpp | 4 ++-- dart/dynamics/SingleDofJoint.h | 2 +- dart/dynamics/UniversalJoint.cpp | 2 +- dart/dynamics/ZeroDofJoint.cpp | 2 +- dart/dynamics/ZeroDofJoint.h | 2 +- dart/dynamics/detail/MultiDofJoint.h | 4 ++-- 13 files changed, 15 insertions(+), 15 deletions(-) diff --git a/dart/dynamics/BallJoint.cpp b/dart/dynamics/BallJoint.cpp index 6b8b44907100c..15ea54e61552a 100644 --- a/dart/dynamics/BallJoint.cpp +++ b/dart/dynamics/BallJoint.cpp @@ -73,7 +73,7 @@ const std::string& BallJoint::getStaticType() //============================================================================== bool BallJoint::isCyclic(size_t _index) const { - return !isPositionLimited(_index); + return !hasPositionLimit(_index); } //============================================================================== diff --git a/dart/dynamics/EulerJoint.cpp b/dart/dynamics/EulerJoint.cpp index f1ca9b2beb7f2..0458c8cc1014b 100644 --- a/dart/dynamics/EulerJoint.cpp +++ b/dart/dynamics/EulerJoint.cpp @@ -129,7 +129,7 @@ const std::string& EulerJoint::getStaticType() //============================================================================== bool EulerJoint::isCyclic(size_t _index) const { - return !isPositionLimited(_index); + return !hasPositionLimit(_index); } //============================================================================== diff --git a/dart/dynamics/FreeJoint.cpp b/dart/dynamics/FreeJoint.cpp index c53112637f868..39e0a43485d2b 100644 --- a/dart/dynamics/FreeJoint.cpp +++ b/dart/dynamics/FreeJoint.cpp @@ -154,7 +154,7 @@ const std::string& FreeJoint::getStaticType() //============================================================================== bool FreeJoint::isCyclic(size_t _index) const { - return _index < 3 && !isPositionLimited(_index); + return _index < 3 && !hasPositionLimit(_index); } //============================================================================== diff --git a/dart/dynamics/Joint.h b/dart/dynamics/Joint.h index 9fee20be01b22..50c09357589f4 100644 --- a/dart/dynamics/Joint.h +++ b/dart/dynamics/Joint.h @@ -351,7 +351,7 @@ class Joint : public virtual common::Subject virtual bool isCyclic(size_t _index) const = 0; /// Gets whether the position of a generalized coordinate is limited. - virtual bool isPositionLimited(size_t _index) const = 0; + virtual bool hasPositionLimit(size_t _index) const = 0; /// \} diff --git a/dart/dynamics/MultiDofJoint.h b/dart/dynamics/MultiDofJoint.h index 8e55e89b0afe1..61a54481a1225 100644 --- a/dart/dynamics/MultiDofJoint.h +++ b/dart/dynamics/MultiDofJoint.h @@ -253,7 +253,7 @@ class MultiDofJoint : public Joint double getPositionUpperLimit(size_t _index) const override; // Documentation inherited - virtual bool isPositionLimited(size_t _index) const override; + virtual bool hasPositionLimit(size_t _index) const override; //---------------------------------------------------------------------------- // Velocity diff --git a/dart/dynamics/PlanarJoint.cpp b/dart/dynamics/PlanarJoint.cpp index 2e7c279a1ce3e..6192f0d2d31be 100644 --- a/dart/dynamics/PlanarJoint.cpp +++ b/dart/dynamics/PlanarJoint.cpp @@ -215,7 +215,7 @@ const std::string& PlanarJoint::getStaticType() //============================================================================== bool PlanarJoint::isCyclic(size_t _index) const { - return _index == 2 && !isPositionLimited(_index); + return _index == 2 && !hasPositionLimit(_index); } //============================================================================== diff --git a/dart/dynamics/RevoluteJoint.cpp b/dart/dynamics/RevoluteJoint.cpp index 206b15f8ec8d1..c288d09cd2c22 100644 --- a/dart/dynamics/RevoluteJoint.cpp +++ b/dart/dynamics/RevoluteJoint.cpp @@ -123,7 +123,7 @@ const std::string& RevoluteJoint::getType() const //============================================================================== bool RevoluteJoint::isCyclic(size_t _index) const { - return !isPositionLimited(_index); + return !hasPositionLimit(_index); } //============================================================================== diff --git a/dart/dynamics/SingleDofJoint.cpp b/dart/dynamics/SingleDofJoint.cpp index 766296ae64e80..7f437be9d611e 100644 --- a/dart/dynamics/SingleDofJoint.cpp +++ b/dart/dynamics/SingleDofJoint.cpp @@ -474,11 +474,11 @@ double SingleDofJoint::getPositionUpperLimit(size_t _index) const } //============================================================================== -bool SingleDofJoint::isPositionLimited(size_t _index) const +bool SingleDofJoint::hasPositionLimit(size_t _index) const { if (_index != 0) { - SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( isPositionLimited, _index ); + SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( hasPositionLimit, _index ); return true; } diff --git a/dart/dynamics/SingleDofJoint.h b/dart/dynamics/SingleDofJoint.h index bf8565910073a..08b7d68e76910 100644 --- a/dart/dynamics/SingleDofJoint.h +++ b/dart/dynamics/SingleDofJoint.h @@ -228,7 +228,7 @@ class SingleDofJoint : public Joint double getPositionUpperLimit(size_t _index) const override; // Documentation inherited - bool isPositionLimited(size_t _index) const override; + bool hasPositionLimit(size_t _index) const override; //---------------------------------------------------------------------------- // Velocity diff --git a/dart/dynamics/UniversalJoint.cpp b/dart/dynamics/UniversalJoint.cpp index eefb177107d99..925f7916793e3 100644 --- a/dart/dynamics/UniversalJoint.cpp +++ b/dart/dynamics/UniversalJoint.cpp @@ -130,7 +130,7 @@ const std::string& UniversalJoint::getStaticType() //============================================================================== bool UniversalJoint::isCyclic(size_t _index) const { - return !isPositionLimited(_index); + return !hasPositionLimit(_index); } //============================================================================== diff --git a/dart/dynamics/ZeroDofJoint.cpp b/dart/dynamics/ZeroDofJoint.cpp index 21eec0cb1c8bd..4651ba157a0a3 100644 --- a/dart/dynamics/ZeroDofJoint.cpp +++ b/dart/dynamics/ZeroDofJoint.cpp @@ -222,7 +222,7 @@ double ZeroDofJoint::getPositionUpperLimit(size_t _index) const } //============================================================================== -bool ZeroDofJoint::isPositionLimited(size_t _index) const +bool ZeroDofJoint::hasPositionLimit(size_t _index) const { return true; } diff --git a/dart/dynamics/ZeroDofJoint.h b/dart/dynamics/ZeroDofJoint.h index 6d82b393cc048..c33290a44ce7c 100644 --- a/dart/dynamics/ZeroDofJoint.h +++ b/dart/dynamics/ZeroDofJoint.h @@ -145,7 +145,7 @@ class ZeroDofJoint : public Joint virtual double getPositionUpperLimit(size_t _index) const override; // Documentation inherited - virtual bool isPositionLimited(size_t _index) const override; + virtual bool hasPositionLimit(size_t _index) const override; //---------------------------------------------------------------------------- // Velocity diff --git a/dart/dynamics/detail/MultiDofJoint.h b/dart/dynamics/detail/MultiDofJoint.h index 3e1a06214c674..71563ad388672 100644 --- a/dart/dynamics/detail/MultiDofJoint.h +++ b/dart/dynamics/detail/MultiDofJoint.h @@ -588,11 +588,11 @@ double MultiDofJoint::getPositionUpperLimit(size_t _index) const //============================================================================== template -bool MultiDofJoint::isPositionLimited(size_t _index) const +bool MultiDofJoint::hasPositionLimit(size_t _index) const { if (_index >= getNumDofs()) { - MULTIDOFJOINT_REPORT_OUT_OF_RANGE(isPositionLimited, _index); + MULTIDOFJOINT_REPORT_OUT_OF_RANGE(hasPositionLimit, _index); return true; } From 78bbdf1458d61f093f1dbfdda26564d02edf5066 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Wed, 8 Jul 2015 21:08:42 -0500 Subject: [PATCH 4/9] Added hasPositionLimit and isCyclic on DOF. --- dart/dynamics/DegreeOfFreedom.cpp | 12 ++++++++++++ dart/dynamics/DegreeOfFreedom.h | 6 ++++++ dart/dynamics/Joint.h | 4 ++-- 3 files changed, 20 insertions(+), 2 deletions(-) diff --git a/dart/dynamics/DegreeOfFreedom.cpp b/dart/dynamics/DegreeOfFreedom.cpp index 3b88ecc367d23..4d1346e75175e 100644 --- a/dart/dynamics/DegreeOfFreedom.cpp +++ b/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) { diff --git a/dart/dynamics/DegreeOfFreedom.h b/dart/dynamics/DegreeOfFreedom.h index dcbde81572bfb..2d7f907d85938 100644 --- a/dart/dynamics/DegreeOfFreedom.h +++ b/dart/dynamics/DegreeOfFreedom.h @@ -159,6 +159,12 @@ class DegreeOfFreedom : public virtual common::Subject /// Get the upper position limit of this DegreeOfFreedom double getPositionUpperLimit() const; + /// Gets whether a DegreeOfFreedom has SO(2) topology. + bool isCyclic() const; + + /// Gets whether the position of this DegreeOfFreedom has limits. + bool hasPositionLimit() const; + /// \} //---------------------------------------------------------------------------- diff --git a/dart/dynamics/Joint.h b/dart/dynamics/Joint.h index 50c09357589f4..6af1776270610 100644 --- a/dart/dynamics/Joint.h +++ b/dart/dynamics/Joint.h @@ -347,10 +347,10 @@ class Joint : public virtual common::Subject /// Get upper limit for position virtual double getPositionUpperLimit(size_t _index) const = 0; - /// Gets whether a generalized coordinate is cyclic; i.e. has SO(2) topology. + /// Gets whether a generalized coordinate has SO(2) topology. virtual bool isCyclic(size_t _index) const = 0; - /// Gets whether the position of a generalized coordinate is limited. + /// Gets whether the position of a generalized coordinate has limits. virtual bool hasPositionLimit(size_t _index) const = 0; /// \} From e5fc7c12ca1efc5ba9b4f6373c3194caeb5e7591 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Wed, 8 Jul 2015 21:48:23 -0500 Subject: [PATCH 5/9] Added isPositionLimitEnforced. - Added isPositionLimitEnforced and setPositionLimitEnforced. - Deprecated isPositionLimited and setPositionLimited. - Updated references throughout DART. --- dart/constraint/ConstraintSolver.cpp | 2 +- dart/dynamics/Joint.cpp | 18 +++++++++++++++--- dart/dynamics/Joint.h | 11 +++++++++-- 3 files changed, 25 insertions(+), 6 deletions(-) diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index 5113966758fd1..fc13796744e3d 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/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)); } } diff --git a/dart/dynamics/Joint.cpp b/dart/dynamics/Joint.cpp index 1a731095ccae6..09eabfaee5a75 100644 --- a/dart/dynamics/Joint.cpp +++ b/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 { diff --git a/dart/dynamics/Joint.h b/dart/dynamics/Joint.h index 6af1776270610..96562554322a6 100644 --- a/dart/dynamics/Joint.h +++ b/dart/dynamics/Joint.h @@ -42,6 +42,7 @@ #include #include +#include "dart/common/Deprecated.h" #include "dart/common/Subject.h" #include "dart/math/MathTypes.h" #include "dart/dynamics/SmartPointer.h" @@ -244,7 +245,10 @@ 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 /// @@ -252,7 +256,10 @@ class Joint : public virtual common::Subject /// 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; From ede1cbc4ef85d248d380184bfb28c0f8cfee7e60 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Sun, 12 Jul 2015 18:32:20 -0500 Subject: [PATCH 6/9] Fixed isCyclic on BallJoint and FreeJoint. --- dart/dynamics/BallJoint.cpp | 3 ++- dart/dynamics/FreeJoint.cpp | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/dart/dynamics/BallJoint.cpp b/dart/dynamics/BallJoint.cpp index 15ea54e61552a..c04559f304110 100644 --- a/dart/dynamics/BallJoint.cpp +++ b/dart/dynamics/BallJoint.cpp @@ -73,7 +73,8 @@ const std::string& BallJoint::getStaticType() //============================================================================== bool BallJoint::isCyclic(size_t _index) const { - return !hasPositionLimit(_index); + return _index < 3 + && !hasPositionLimit(0) && !hasPositionLimit(1) && !hasPositionLimit(2); } //============================================================================== diff --git a/dart/dynamics/FreeJoint.cpp b/dart/dynamics/FreeJoint.cpp index 39e0a43485d2b..8125bef074a0f 100644 --- a/dart/dynamics/FreeJoint.cpp +++ b/dart/dynamics/FreeJoint.cpp @@ -154,7 +154,8 @@ const std::string& FreeJoint::getStaticType() //============================================================================== bool FreeJoint::isCyclic(size_t _index) const { - return _index < 3 && !hasPositionLimit(_index); + return _index < 3 + && !hasPositionLimit(0) && !hasPositionLimit(1) && !hasPositionLimit(2); } //============================================================================== From b11707519bfa4b6d85dd26451eb26fb95b8a9e4c Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Sun, 12 Jul 2015 18:49:48 -0500 Subject: [PATCH 7/9] Corrected isCyclic comments. --- dart/dynamics/DegreeOfFreedom.h | 5 ++++- dart/dynamics/Joint.h | 6 +++++- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/dart/dynamics/DegreeOfFreedom.h b/dart/dynamics/DegreeOfFreedom.h index 2d7f907d85938..71140b5a3ac3f 100644 --- a/dart/dynamics/DegreeOfFreedom.h +++ b/dart/dynamics/DegreeOfFreedom.h @@ -159,7 +159,10 @@ class DegreeOfFreedom : public virtual common::Subject /// Get the upper position limit of this DegreeOfFreedom double getPositionUpperLimit() const; - /// Gets whether a DegreeOfFreedom has SO(2) topology. + /// Gets whether this DOF is cyclic. Returns 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; /// Gets whether the position of this DegreeOfFreedom has limits. diff --git a/dart/dynamics/Joint.h b/dart/dynamics/Joint.h index 96562554322a6..444c9451b4d3b 100644 --- a/dart/dynamics/Joint.h +++ b/dart/dynamics/Joint.h @@ -354,7 +354,11 @@ class Joint : public virtual common::Subject /// Get upper limit for position virtual double getPositionUpperLimit(size_t _index) const = 0; - /// Gets whether a generalized coordinate has SO(2) topology. + /// Gets whether a generalized coordinate is cyclic. Returns 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; /// Gets whether the position of a generalized coordinate has limits. From 29f016749b44e7b742cf0c1cca4da802dd267871 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Tue, 14 Jul 2015 00:30:16 -0500 Subject: [PATCH 8/9] Switched from std::isinf to std::isfinite. --- dart/dynamics/SingleDofJoint.cpp | 4 ++-- dart/dynamics/detail/MultiDofJoint.h | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/dart/dynamics/SingleDofJoint.cpp b/dart/dynamics/SingleDofJoint.cpp index 7f437be9d611e..67a313c860d96 100644 --- a/dart/dynamics/SingleDofJoint.cpp +++ b/dart/dynamics/SingleDofJoint.cpp @@ -482,8 +482,8 @@ bool SingleDofJoint::hasPositionLimit(size_t _index) const return true; } - return !(std::isinf(mSingleDofP.mPositionLowerLimit) - && std::isinf(mSingleDofP.mPositionUpperLimit)); + return std::isfinite(mSingleDofP.mPositionLowerLimit) + || std::isfinite(mSingleDofP.mPositionUpperLimit); } //============================================================================== diff --git a/dart/dynamics/detail/MultiDofJoint.h b/dart/dynamics/detail/MultiDofJoint.h index 71563ad388672..1eb8f35de1fad 100644 --- a/dart/dynamics/detail/MultiDofJoint.h +++ b/dart/dynamics/detail/MultiDofJoint.h @@ -596,8 +596,8 @@ bool MultiDofJoint::hasPositionLimit(size_t _index) const return true; } - return !(std::isinf(mMultiDofP.mPositionUpperLimits[_index]) - && std::isinf(mMultiDofP.mPositionLowerLimits[_index])); + return std::isfinite(mMultiDofP.mPositionUpperLimits[_index]) + || std::isfinite(mMultiDofP.mPositionLowerLimits[_index]); } //============================================================================== From db05bd3a79e1a5e193d6de1c8b3da01270462bb5 Mon Sep 17 00:00:00 2001 From: Michael Koval Date: Tue, 14 Jul 2015 00:31:15 -0500 Subject: [PATCH 9/9] Fixed comment style. --- dart/dynamics/DegreeOfFreedom.h | 4 ++-- dart/dynamics/Joint.h | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/dart/dynamics/DegreeOfFreedom.h b/dart/dynamics/DegreeOfFreedom.h index 71140b5a3ac3f..6cfde71d749ee 100644 --- a/dart/dynamics/DegreeOfFreedom.h +++ b/dart/dynamics/DegreeOfFreedom.h @@ -159,13 +159,13 @@ class DegreeOfFreedom : public virtual common::Subject /// Get the upper position limit of this DegreeOfFreedom double getPositionUpperLimit() const; - /// Gets whether this DOF is cyclic. Returns true if and only if an infinite + /// 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; - /// Gets whether the position of this DegreeOfFreedom has limits. + /// Get whether the position of this DegreeOfFreedom has limits. bool hasPositionLimit() const; /// \} diff --git a/dart/dynamics/Joint.h b/dart/dynamics/Joint.h index 444c9451b4d3b..11725bead1b92 100644 --- a/dart/dynamics/Joint.h +++ b/dart/dynamics/Joint.h @@ -354,14 +354,14 @@ class Joint : public virtual common::Subject /// Get upper limit for position virtual double getPositionUpperLimit(size_t _index) const = 0; - /// Gets whether a generalized coordinate is cyclic. Returns true if and only + /// 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; - /// Gets whether the position of a generalized coordinate has limits. + /// Get whether the position of a generalized coordinate has limits. virtual bool hasPositionLimit(size_t _index) const = 0; /// \}