From a93ba3b8de8ada5d74cfd49e3453d72ec2db1f18 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Fri, 14 Nov 2014 16:05:19 -0500 Subject: [PATCH 01/30] getting read to add DoF class --- dart/dynamics/DegreeOfFreedom.h | 119 ++++++++++++++++++++++++++++++++ dart/dynamics/MultiDofJoint.h | 37 +++++----- 2 files changed, 139 insertions(+), 17 deletions(-) create mode 100644 dart/dynamics/DegreeOfFreedom.h diff --git a/dart/dynamics/DegreeOfFreedom.h b/dart/dynamics/DegreeOfFreedom.h new file mode 100644 index 0000000000000..c4b34bd66b383 --- /dev/null +++ b/dart/dynamics/DegreeOfFreedom.h @@ -0,0 +1,119 @@ +/* + * Copyright (c) 2011-2014, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_DYNAMICS_DEGREEOFFREEDOM_H_ +#define DART_DYNAMICS_DEGREEOFFREEDOM_H_ + +#include +#include + +namespace dart { +namespace dynamics { + +class Skeleton; +class Joint; +class BodyNode; + +/// DegreeOfFreedom class represents a single degree of freedom (or generalized coordinate) +/// of the Skeleton. +/// +/// DegreeOfFreedom stores properties like position, velocity, and acceleration values and +/// limits for each DegreeOfFreedom in the Skeleton, and the Skeleton's DegreesOfFreedom +/// will be accessible by name or by index + +class DegreeOfFreedomProperties +{ +public: + + DegreeOfFreedomProperties(); + + double mPosition; + double mPositionLowerLimit; + double mPositionUpperLimit; + + double mVelocity; + double mVelocityLowerLimit; + double mVelocityUpperLimit; + + double mAcceleration; + double mAccelerationLowerLimit; + double mAccelerationUpperLimit; + + double mEffort; + double mEffortLowerLimit; + double mEffortUpperLimit; + + double mVelocityChange; + + double mConstraintImpulse; + + double mSpringStiffness; + double mRestPosition; + double mDampingCoefficient; + + Eigen::Matrix mJacobian; + Eigen::Matrix mJacobianDeriv; + + + +}; + +class DegreeOfFreedom : protected DegreeOfFreedomProperties +{ +public: + + DegreeOfFreedom(); + + bool setPosition(); + double getPosition() const; + + void setPositionLowerLimit(); + double getPositionLowerLimit() const; + void setPositionUpperLimit(); + double getPositionUpperLimit() const; + + // TODO: the rest + +}; + + + + + +} // namespace dynamics +} // namespace dart + +#endif // DART_DYNAMICS_DEGREEOFFREEDOM_H_ diff --git a/dart/dynamics/MultiDofJoint.h b/dart/dynamics/MultiDofJoint.h index 885e025cd6670..6c14642d491c2 100644 --- a/dart/dynamics/MultiDofJoint.h +++ b/dart/dynamics/MultiDofJoint.h @@ -402,90 +402,93 @@ class MultiDofJoint : public Joint /// Derivatives w.r.t. an arbitrary scalr variable Eigen::Matrix mPositionDeriv; + // TODO: Should this^ be deprecated? It never seems to get used and is not accessible. //---------------------------------------------------------------------------- // Velocity //---------------------------------------------------------------------------- /// Generalized velocity - Eigen::Matrix mVelocities; + Eigen::Matrix mVelocities; // move to DoF /// Min value allowed. - Eigen::Matrix mVelocityLowerLimits; + Eigen::Matrix mVelocityLowerLimits; // move to DoF /// Max value allowed. - Eigen::Matrix mVelocityUpperLimits; + Eigen::Matrix mVelocityUpperLimits; // move to DoF /// Derivatives w.r.t. an arbitrary scalr variable Eigen::Matrix mVelocitiesDeriv; + // TODO: Should this^ be deprecated? It never seems to get used and is not accessible. //---------------------------------------------------------------------------- // Acceleration //---------------------------------------------------------------------------- /// Generalized acceleration - Eigen::Matrix mAccelerations; + Eigen::Matrix mAccelerations; // move to DoF /// Min value allowed. - Eigen::Matrix mAccelerationLowerLimits; + Eigen::Matrix mAccelerationLowerLimits; // move to DoF /// upper limit of generalized acceleration - Eigen::Matrix mAccelerationUpperLimits; + Eigen::Matrix mAccelerationUpperLimits; // move to DoF /// Derivatives w.r.t. an arbitrary scalr variable Eigen::Matrix mAccelerationsDeriv; + // TODO: Should this^ be deprecated? It never seems to get used and is not accessible. //---------------------------------------------------------------------------- // Force //---------------------------------------------------------------------------- /// Generalized force - Eigen::Matrix mForces; + Eigen::Matrix mForces; // move to DoF /// Min value allowed. - Eigen::Matrix mForceLowerLimits; + Eigen::Matrix mForceLowerLimits; // move to DoF /// Max value allowed. - Eigen::Matrix mForceUpperLimits; + Eigen::Matrix mForceUpperLimits; // move to DoF /// Derivatives w.r.t. an arbitrary scalr variable - Eigen::Matrix mForcesDeriv; + Eigen::Matrix mForcesDeriv; // move to DoF //---------------------------------------------------------------------------- // Impulse //---------------------------------------------------------------------------- /// Change of generalized velocity - Eigen::Matrix mVelocityChanges; + Eigen::Matrix mVelocityChanges; // move to DoF // /// Generalized impulse // Eigen::Matrix mImpulse; /// Generalized constraint impulse - Eigen::Matrix mConstraintImpulses; + Eigen::Matrix mConstraintImpulses; // move to DoF //---------------------------------------------------------------------------- // Spring and damper //---------------------------------------------------------------------------- /// Joint spring stiffness - Eigen::Matrix mSpringStiffness; + Eigen::Matrix mSpringStiffness; // move to DoF /// Rest joint position for joint spring - Eigen::Matrix mRestPosition; + Eigen::Matrix mRestPosition; // move to DoF /// Joint damping coefficient - Eigen::Matrix mDampingCoefficient; + Eigen::Matrix mDampingCoefficient; // move to DoF //---------------------------------------------------------------------------- // For recursive dynamics algorithms //---------------------------------------------------------------------------- /// Spatial Jacobian - Eigen::Matrix mJacobian; + Eigen::Matrix mJacobian; // move to DoF /// Time derivative of spatial Jacobian - Eigen::Matrix mJacobianDeriv; + Eigen::Matrix mJacobianDeriv; // move to DoF /// Inverse of projected articulated inertia Eigen::Matrix mInvProjArtInertia; From 21ee5f502f2e853922c1e0949dba6a40daf2794e Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Mon, 1 Dec 2014 17:45:53 -0500 Subject: [PATCH 02/30] adding DegreeOfFreedom class --- dart/dynamics/DegreeOfFreedom.cpp | 369 ++++++++++++++++++++++++++++++ dart/dynamics/DegreeOfFreedom.h | 213 +++++++++++++---- dart/dynamics/Joint.cpp | 17 ++ dart/dynamics/MultiDofJoint.h | 37 ++- dart/dynamics/Skeleton.cpp | 10 + dart/dynamics/Skeleton.h | 8 + 6 files changed, 593 insertions(+), 61 deletions(-) create mode 100644 dart/dynamics/DegreeOfFreedom.cpp diff --git a/dart/dynamics/DegreeOfFreedom.cpp b/dart/dynamics/DegreeOfFreedom.cpp new file mode 100644 index 0000000000000..dcf5b64d9fb1e --- /dev/null +++ b/dart/dynamics/DegreeOfFreedom.cpp @@ -0,0 +1,369 @@ +/* + * Copyright (c) 2014, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "DegreeOfFreedom.h" +#include "Joint.h" +#include "Skeleton.h" + +namespace dart { +namespace dynamics { + +//============================================================================== +const std::string& DegreeOfFreedom::setName(const std::string& _name) +{ + if(mName == _name) + return mName; + + Skeleton* skel = mJoint->getSkeleton(); + if(skel) + { + skel->mNameMgrForDofs.removeName(mName); + mName = _name; + skel->addEntryToDofNameMgr(this); + } + else + mName = _name; + + return mName; +} + +//============================================================================== +const std::string& DegreeOfFreedom::getName() const +{ + return mName; +} + +//============================================================================== +size_t DegreeOfFreedom::getIndexInSkeleton() const +{ + return mIndexInSkeleton; +} + +//============================================================================== +size_t DegreeOfFreedom::getIndexInJoint() const +{ + return mIndexInJoint; +} + +//============================================================================== +void DegreeOfFreedom::setPosition(double _position) +{ + mJoint->setPosition(mIndexInJoint, _position); +} + +//============================================================================== +double DegreeOfFreedom::getPosition() const +{ + return mJoint->getPosition(mIndexInJoint); +} + +//============================================================================== +void DegreeOfFreedom::setPositionLimits(double _lowerLimit, double _upperLimit) +{ + setPositionLowerLimit(_lowerLimit); + setPositionUpperLimit(_upperLimit); +} + +//============================================================================== +void DegreeOfFreedom::setPositionLimits(const std::pair& _limits) +{ + setPositionLimits(_limits.first, _limits.second); +} + +//============================================================================== +std::pair DegreeOfFreedom::getPositionLimits() const +{ + return std::pair(getPositionLowerLimit(), + getPositionUpperLimit()); +} + +//============================================================================== + +void DegreeOfFreedom::setPositionLowerLimit(double _limit) +{ + mJoint->setPositionLowerLimit(mIndexInJoint, _limit); +} + +//============================================================================== +double DegreeOfFreedom::getPositionLowerLimit() const +{ + return mJoint->getPositionLowerLimit(mIndexInJoint); +} + +//============================================================================== +void DegreeOfFreedom::setPositionUpperLimit(double _limit) +{ + mJoint->setPositionUpperLimit(mIndexInJoint, _limit); +} + +//============================================================================== +double DegreeOfFreedom::getPositionUpperLimit() const +{ + return mJoint->getPositionUpperLimit(mIndexInJoint); +} + +//============================================================================== +void DegreeOfFreedom::setVelocity(double _velocity) +{ + mJoint->setVelocity(mIndexInJoint, _velocity); +} + +//============================================================================== +double DegreeOfFreedom::getVelocity() const +{ + return mJoint->getVelocity(mIndexInJoint); +} + +//============================================================================== +void DegreeOfFreedom::setVelocityLimits(double _lowerLimit, double _upperLimit) +{ + setVelocityLowerLimit(_lowerLimit); + setVelocityUpperLimit(_upperLimit); +} + +//============================================================================== +void DegreeOfFreedom::setVelocityLimits(const std::pair& _limits) +{ + setVelocityLimits(_limits.first, _limits.second); +} + +//============================================================================== +std::pair DegreeOfFreedom::getVelocityLimits() const +{ + return std::pair(getVelocityLowerLimit(), + getVelocityUpperLimit()); +} + +//============================================================================== +void DegreeOfFreedom::setVelocityLowerLimit(double _limit) +{ + mJoint->setVelocityLowerLimit(mIndexInJoint, _limit); +} + +//============================================================================== +double DegreeOfFreedom::getVelocityLowerLimit() const +{ + return mJoint->getVelocityLowerLimit(mIndexInJoint); +} + +//============================================================================== +void DegreeOfFreedom::setVelocityUpperLimit(double _limit) +{ + mJoint->setVelocityUpperLimit(mIndexInJoint, _limit); +} + +//============================================================================== +double DegreeOfFreedom::getVelocityUpperLimit() const +{ + return mJoint->getVelocityLowerLimit(mIndexInJoint); +} + +//============================================================================== +void DegreeOfFreedom::setAcceleration(double _acceleration) +{ + mJoint->setAcceleration(mIndexInJoint, _acceleration); +} + +//============================================================================== +double DegreeOfFreedom::getAcceleration() const +{ + return mJoint->getAcceleration(mIndexInJoint); +} + +//============================================================================== +void DegreeOfFreedom::setAccelerationLimits(double _lowerLimit, + double _upperLimit) +{ + setAccelerationLowerLimit(_lowerLimit); + setAccelerationUpperLimit(_upperLimit); +} + +//============================================================================== +void DegreeOfFreedom::setAccelerationLimits( + const std::pair& _limits) +{ + setAccelerationLimits(_limits.first, _limits.second); +} + +//============================================================================== +std::pair DegreeOfFreedom::getAccelerationLimits() const +{ + return std::pair(getAccelerationLowerLimit(), + getAccelerationUpperLimit()); +} + +//============================================================================== +void DegreeOfFreedom::setAccelerationLowerLimit(double _limit) +{ + mJoint->setAccelerationLowerLimit(mIndexInJoint, _limit); +} + +//============================================================================== +double DegreeOfFreedom::getAccelerationLowerLimit() const +{ + return mJoint->getAccelerationLowerLimit(mIndexInJoint); +} + +//============================================================================== +void DegreeOfFreedom::setAccelerationUpperLimit(double _limit) +{ + mJoint->setAccelerationUpperLimit(mIndexInJoint, _limit); +} + +//============================================================================== +double DegreeOfFreedom::getAccelerationUpperLimit() const +{ + return mJoint->getAccelerationUpperLimit(mIndexInJoint); +} + +//============================================================================== +void DegreeOfFreedom::setEffort(double _effort) +{ + mJoint->setForce(mIndexInJoint, _effort); +} + +//============================================================================== +double DegreeOfFreedom::getEffort() const +{ + return mJoint->getForce(mIndexInJoint); +} + +//============================================================================== +void DegreeOfFreedom::setEffortLimits(double _lowerLimit, double _upperLimit) +{ + setEffortLowerLimit(_lowerLimit); + setEffortUpperLimit(_upperLimit); +} + +//============================================================================== +void DegreeOfFreedom::setEffortLimits(const std::pair &_limits) +{ + setEffortLimits(_limits.first, _limits.second); +} + +//============================================================================== +std::pair DegreeOfFreedom::getEffortLimits() const +{ + return std::pair(getEffortLowerLimit(), getEffortUpperLimit()); +} + +//============================================================================== +void DegreeOfFreedom::setEffortLowerLimit(double _limit) +{ + mJoint->setForceLowerLimit(mIndexInJoint, _limit); +} + +//============================================================================== +double DegreeOfFreedom::getEffortLowerLimit() const +{ + return mJoint->getForce(mIndexInJoint); +} + +//============================================================================== +void DegreeOfFreedom::setEffortUpperLimit(double _limit) +{ + mJoint->setForceUpperLimit(mIndexInJoint, _limit); +} + +//============================================================================== +double DegreeOfFreedom::getEffortUpperLimit() const +{ + return mJoint->getForceUpperLimit(mIndexInJoint); +} + +//============================================================================== +Joint* DegreeOfFreedom::getJoint() +{ + return mJoint; +} + +//============================================================================== +const Joint* DegreeOfFreedom::getJoint() const +{ + return mJoint; +} + +//============================================================================== +Skeleton* DegreeOfFreedom::getSkeleton() +{ + return mJoint->getSkeleton(); +} + +//============================================================================== +const Skeleton* DegreeOfFreedom::getSkeleton() const +{ + return mJoint->getSkeleton(); +} + +//============================================================================== +BodyNode* DegreeOfFreedom::getChildBodyNode() +{ + return mJoint->getChildBodyNode(); +} + +//============================================================================== +const BodyNode* DegreeOfFreedom::getChildBodyNode() const +{ + return mJoint->getChildBodyNode(); +} + +//============================================================================== +BodyNode* DegreeOfFreedom::getParentBodyNode() +{ + return mJoint->getParentBodyNode(); +} + +//============================================================================== +const BodyNode* DegreeOfFreedom::getParentBodyNode() const +{ + return mJoint->getParentBodyNode(); +} + +//============================================================================== +DegreeOfFreedom::DegreeOfFreedom(Joint* _joint, + const std::string &_name, + size_t _indexInJoint, + size_t _indexInSkeleton) : + mName(_name), + mIndexInJoint(_indexInJoint), + mIndexInSkeleton(_indexInSkeleton), + mJoint(_joint) +{ + +} + +} // namespace dynamics +} // namespace dart diff --git a/dart/dynamics/DegreeOfFreedom.h b/dart/dynamics/DegreeOfFreedom.h index c4b34bd66b383..d236767386e5b 100644 --- a/dart/dynamics/DegreeOfFreedom.h +++ b/dart/dynamics/DegreeOfFreedom.h @@ -1,5 +1,5 @@ /* - * Copyright (c) 2011-2014, Georgia Tech Research Corporation + * Copyright (c) 2014, Georgia Tech Research Corporation * All rights reserved. * * Author(s): Michael X. Grey @@ -47,71 +47,202 @@ class Skeleton; class Joint; class BodyNode; -/// DegreeOfFreedom class represents a single degree of freedom (or generalized coordinate) -/// of the Skeleton. -/// -/// DegreeOfFreedom stores properties like position, velocity, and acceleration values and -/// limits for each DegreeOfFreedom in the Skeleton, and the Skeleton's DegreesOfFreedom -/// will be accessible by name or by index +/// DegreeOfFreedom class is a proxy class for accessing single degrees of +/// freedom (or generalized coordinate) of the Skeleton. -class DegreeOfFreedomProperties +class DegreeOfFreedom { public: - DegreeOfFreedomProperties(); + friend class dart::dynamics::Joint; + friend class dart::dynamics::Skeleton; - double mPosition; - double mPositionLowerLimit; - double mPositionUpperLimit; + /// Change the name of this DegreeOfFreedom + const std::string& setName(const std::string& _name); - double mVelocity; - double mVelocityLowerLimit; - double mVelocityUpperLimit; + /// Get the name of this DegreeOfFreedom + const std::string& getName() const; - double mAcceleration; - double mAccelerationLowerLimit; - double mAccelerationUpperLimit; + /// Get this DegreeOfFreedom's index within its Skeleton + size_t getIndexInSkeleton() const; - double mEffort; - double mEffortLowerLimit; - double mEffortUpperLimit; + /// Get this DegreeOfFreedom's index within its Joint + size_t getIndexInJoint() const; - double mVelocityChange; + // -- Position functions ----------------------------------------------------- - double mConstraintImpulse; + /// Set the position of this DegreeOfFreedom + void setPosition(double _position); - double mSpringStiffness; - double mRestPosition; - double mDampingCoefficient; + /// Get the position of this DegreeOfFreedom + double getPosition() const; - Eigen::Matrix mJacobian; - Eigen::Matrix mJacobianDeriv; + /// Set the position limits of this DegreeOfFreedom + void setPositionLimits(double _lowerLimit, double _upperLimit); + /// Set the position limits of this DegreeOfFreedom + void setPositionLimits(const std::pair& _limits); + /// Get the position limits of this DegreeOfFreedom + std::pair getPositionLimits() const; -}; + /// Set the lower position limit of this DegreeOfFreedom + void setPositionLowerLimit(double _limit); -class DegreeOfFreedom : protected DegreeOfFreedomProperties -{ -public: - - DegreeOfFreedom(); + /// Get the lower position limit of this DegreeOfFreedom + double getPositionLowerLimit() const; - bool setPosition(); - double getPosition() const; + /// Set the upper position limit of this DegreeOfFreedom + void setPositionUpperLimit(double _limit); - void setPositionLowerLimit(); - double getPositionLowerLimit() const; - void setPositionUpperLimit(); + /// Get the upper position limit of this DegreeOfFreedom double getPositionUpperLimit() const; - // TODO: the rest + // -- Velocity functions ----------------------------------------------------- -}; + /// Set the velocity of this DegreeOfFreedom + void setVelocity(double _velocity); + + /// Get the velocity of this DegreeOfFreedom + double getVelocity() const; + + /// Set the velocity limits of this DegreeOfFreedom + void setVelocityLimits(double _lowerLimit, double _upperLimit); + + /// Set the velocity limtis of this DegreeOfFreedom + void setVelocityLimits(const std::pair& _limits); + + /// Get the velocity limits of this DegreeOfFreedom + std::pair getVelocityLimits() const; + + /// Set the lower velocity limit of this DegreeOfFreedom + void setVelocityLowerLimit(double _limit); + + /// Get the lower velocity limit of this DegreeOfFreedom + double getVelocityLowerLimit() const; + + /// Set the upper velocity limit of this DegreeOfFreedom + void setVelocityUpperLimit(double _limit); + + /// Get the upper Velocity limit of this DegreeOfFreedom + double getVelocityUpperLimit() const; + + // -- Acceleration functions ------------------------------------------------- + + /// Set the acceleration of this DegreeOfFreedom + void setAcceleration(double _acceleration); + + /// Get the acceleration of this DegreeOfFreedom + double getAcceleration() const; + + /// Set the acceleration limits of this DegreeOfFreedom + void setAccelerationLimits(double _lowerLimit, double _upperLimit); + + /// Set the acceleartion limits of this DegreeOfFreedom + void setAccelerationLimits(const std::pair& _limits); + + /// Get the acceleration limits of this DegreeOfFreedom + std::pair getAccelerationLimits() const; + + /// Set the lower acceleration limit of this DegreeOfFreedom + void setAccelerationLowerLimit(double _limit); + + /// Get the lower acceleration limit of this DegreeOfFreedom + double getAccelerationLowerLimit() const; + /// Set the upper acceleration limit of this DegreeOfFreedom + void setAccelerationUpperLimit(double _limit); + /// Get the upper acceleration limit of this DegreeOfFreedom + double getAccelerationUpperLimit() const; + // -- Effort/Force functions ------------------------------------------------- + // Note: In these functions, the word "Effort" is being used instead of + // "Force". Currently "Force" is being used throughout DART to refer to the + // Generalized Force of a Generalized Coordinate, but I propose we use the + // word "Effort" instead. We can change the names of these functions later if + // "Effort" is deemed inappropriate or undesirable. + /// Set the generalized force of this DegreeOfFreedom + void setEffort(double _effort); + + /// Get the generalized force of this DegreeOfFreedom + double getEffort() const; + + /// Set the generalized force limits of this DegreeOfFreedom + void setEffortLimits(double _lowerLimit, double _upperLimit); + + /// Set the generalized force limits of this DegreeOfFreedom + void setEffortLimits(const std::pair& _limits); + + /// Get the generalized force limits of this DegreeOfFreedom + std::pair getEffortLimits() const; + + /// Set the lower generalized force limit of this DegreeOfFreedom + void setEffortLowerLimit(double _limit); + + /// Get the lower generalized force limit of this DegreeOfFreedom + double getEffortLowerLimit() const; + + /// Set the upper generalized force limit of this DegreeOfFreedom + void setEffortUpperLimit(double _limit); + + /// Get the upper generalized force limit of this DegreeOfFreedom + double getEffortUpperLimit() const; + + // -- Relationships ---------------------------------------------------------- + + /// Get the Joint that this DegreeOfFreedom belongs to + Joint* getJoint(); + + /// Get the Joint that this DegreeOfFreedom belongs to + const Joint* getJoint() const; + + /// Get the Skeleton that this DegreeOfFreedom is inside of + Skeleton* getSkeleton(); + + /// Get the Skeleton that this DegreeOfFreedom is inside of + const Skeleton* getSkeleton() const; + + /// Get the BodyNode downstream of this DegreeOfFreedom + BodyNode* getChildBodyNode(); + + /// Get the BodyNode downstream of this DegreeOfFreedom + const BodyNode* getChildBodyNode() const; + + /// Get the BodyNode upstream of this DegreeOfFreedom + BodyNode* getParentBodyNode(); + + /// Get the BodyNode upstream of this DegreeOfFreedom + const BodyNode* getParentBodyNode() const; + +protected: + + /// The constructor is protected so that only Joints can create + /// DegreeOfFreedom classes + DegreeOfFreedom(Joint* _joint, + const std::string& _name, + size_t _indexInJoint, + size_t _indexInSkeleton); + + /// Name of this DegreeOfFreedom + std::string mName; + + /// Index of this DegreeOfFreedom within its Joint + size_t mIndexInJoint; + + /// Index of this DegreeOfFreedom within its Skeleton + size_t mIndexInSkeleton; + + /// The joint that this DegreeOfFreedom belongs to + Joint* mJoint; + // Note that we do not need to store BodyNode or Skeleton, because we can + // access them through this joint pointer. Moreover, we never need to check + // whether mJoint is nullptr, because only Joints are allowed to create a + // DegreeOfFreedom and DegreesOfFreedom are deleted when their Joint is + // destructed. + +}; } // namespace dynamics } // namespace dart diff --git a/dart/dynamics/Joint.cpp b/dart/dynamics/Joint.cpp index 848bd4366fedb..ccc937a3ba9a1 100644 --- a/dart/dynamics/Joint.cpp +++ b/dart/dynamics/Joint.cpp @@ -62,9 +62,11 @@ Joint::Joint(const std::string& _name) { } +//============================================================================== Joint::~Joint() { } +//============================================================================== const std::string& Joint::setName(const std::string& _name) { if(mName == _name) @@ -82,20 +84,24 @@ const std::string& Joint::setName(const std::string& _name) { return mName; } +//============================================================================== const std::string& Joint::getName() const { return mName; } +//============================================================================== BodyNode* Joint::getChildBodyNode() { return mChildBodyNode; } +//============================================================================== const BodyNode* Joint::getChildBodyNode() const { return mChildBodyNode; } +//============================================================================== BodyNode* Joint::getParentBodyNode() { if(mChildBodyNode) @@ -104,21 +110,25 @@ BodyNode* Joint::getParentBodyNode() return NULL; } +//============================================================================== const BodyNode* Joint::getParentBodyNode() const { return const_cast(this)->getParentBodyNode(); } +//============================================================================== Skeleton* Joint::getSkeleton() { return mSkeleton; } +//============================================================================== const Skeleton* Joint::getSkeleton() const { return mSkeleton; } +//============================================================================== const Eigen::Isometry3d& Joint::getLocalTransform() const { return mT; } @@ -142,32 +152,39 @@ void Joint::setPositionLimited(bool _isPositionLimited) { mIsPositionLimited = _isPositionLimited; } +//============================================================================== bool Joint::isPositionLimited() const { return mIsPositionLimited; } +//============================================================================== void Joint::setTransformFromParentBodyNode(const Eigen::Isometry3d& _T) { assert(math::verifyTransform(_T)); mT_ParentBodyToJoint = _T; } +//============================================================================== void Joint::setTransformFromChildBodyNode(const Eigen::Isometry3d& _T) { assert(math::verifyTransform(_T)); mT_ChildBodyToJoint = _T; } +//============================================================================== const Eigen::Isometry3d&Joint::getTransformFromParentBodyNode() const { return mT_ParentBodyToJoint; } +//============================================================================== const Eigen::Isometry3d&Joint::getTransformFromChildBodyNode() const { return mT_ChildBodyToJoint; } +//============================================================================== void Joint::applyGLTransform(renderer::RenderInterface* _ri) { _ri->transform(mT); } +//============================================================================== void Joint::init(Skeleton* _skel) { mSkeleton = _skel; diff --git a/dart/dynamics/MultiDofJoint.h b/dart/dynamics/MultiDofJoint.h index 6c14642d491c2..885e025cd6670 100644 --- a/dart/dynamics/MultiDofJoint.h +++ b/dart/dynamics/MultiDofJoint.h @@ -402,93 +402,90 @@ class MultiDofJoint : public Joint /// Derivatives w.r.t. an arbitrary scalr variable Eigen::Matrix mPositionDeriv; - // TODO: Should this^ be deprecated? It never seems to get used and is not accessible. //---------------------------------------------------------------------------- // Velocity //---------------------------------------------------------------------------- /// Generalized velocity - Eigen::Matrix mVelocities; // move to DoF + Eigen::Matrix mVelocities; /// Min value allowed. - Eigen::Matrix mVelocityLowerLimits; // move to DoF + Eigen::Matrix mVelocityLowerLimits; /// Max value allowed. - Eigen::Matrix mVelocityUpperLimits; // move to DoF + Eigen::Matrix mVelocityUpperLimits; /// Derivatives w.r.t. an arbitrary scalr variable Eigen::Matrix mVelocitiesDeriv; - // TODO: Should this^ be deprecated? It never seems to get used and is not accessible. //---------------------------------------------------------------------------- // Acceleration //---------------------------------------------------------------------------- /// Generalized acceleration - Eigen::Matrix mAccelerations; // move to DoF + Eigen::Matrix mAccelerations; /// Min value allowed. - Eigen::Matrix mAccelerationLowerLimits; // move to DoF + Eigen::Matrix mAccelerationLowerLimits; /// upper limit of generalized acceleration - Eigen::Matrix mAccelerationUpperLimits; // move to DoF + Eigen::Matrix mAccelerationUpperLimits; /// Derivatives w.r.t. an arbitrary scalr variable Eigen::Matrix mAccelerationsDeriv; - // TODO: Should this^ be deprecated? It never seems to get used and is not accessible. //---------------------------------------------------------------------------- // Force //---------------------------------------------------------------------------- /// Generalized force - Eigen::Matrix mForces; // move to DoF + Eigen::Matrix mForces; /// Min value allowed. - Eigen::Matrix mForceLowerLimits; // move to DoF + Eigen::Matrix mForceLowerLimits; /// Max value allowed. - Eigen::Matrix mForceUpperLimits; // move to DoF + Eigen::Matrix mForceUpperLimits; /// Derivatives w.r.t. an arbitrary scalr variable - Eigen::Matrix mForcesDeriv; // move to DoF + Eigen::Matrix mForcesDeriv; //---------------------------------------------------------------------------- // Impulse //---------------------------------------------------------------------------- /// Change of generalized velocity - Eigen::Matrix mVelocityChanges; // move to DoF + Eigen::Matrix mVelocityChanges; // /// Generalized impulse // Eigen::Matrix mImpulse; /// Generalized constraint impulse - Eigen::Matrix mConstraintImpulses; // move to DoF + Eigen::Matrix mConstraintImpulses; //---------------------------------------------------------------------------- // Spring and damper //---------------------------------------------------------------------------- /// Joint spring stiffness - Eigen::Matrix mSpringStiffness; // move to DoF + Eigen::Matrix mSpringStiffness; /// Rest joint position for joint spring - Eigen::Matrix mRestPosition; // move to DoF + Eigen::Matrix mRestPosition; /// Joint damping coefficient - Eigen::Matrix mDampingCoefficient; // move to DoF + Eigen::Matrix mDampingCoefficient; //---------------------------------------------------------------------------- // For recursive dynamics algorithms //---------------------------------------------------------------------------- /// Spatial Jacobian - Eigen::Matrix mJacobian; // move to DoF + Eigen::Matrix mJacobian; /// Time derivative of spatial Jacobian - Eigen::Matrix mJacobianDeriv; // move to DoF + Eigen::Matrix mJacobianDeriv; /// Inverse of projected articulated inertia Eigen::Matrix mInvProjArtInertia; diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index 7446922067266..e6fe9536da41a 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -50,6 +50,7 @@ #include "dart/dynamics/PointMass.h" #include "dart/dynamics/Joint.h" #include "dart/dynamics/Marker.h" +#include "dart/dynamics/DegreeOfFreedom.h" namespace dart { namespace dynamics { @@ -121,6 +122,15 @@ const std::string& Skeleton::addEntryToJointNameMgr(Joint *_newJoint) return _newJoint->mName; } +//============================================================================== +const std::string& Skeleton::addEntryToDofNameMgr(DegreeOfFreedom *_newDof) +{ + _newDof->mName = mNameMgrForDofs.issueNewNameAndAdd(_newDof->getName(), + _newDof); + + return _newDof->mName; +} + //============================================================================== void Skeleton::addEntryToSoftBodyNodeNameMgr(SoftBodyNode *_newNode) { diff --git a/dart/dynamics/Skeleton.h b/dart/dynamics/Skeleton.h index fc59435ae42a7..7d4e27b1045e1 100644 --- a/dart/dynamics/Skeleton.h +++ b/dart/dynamics/Skeleton.h @@ -61,6 +61,7 @@ class SoftBodyNode; class PointMass; class Joint; class Marker; +class DegreeOfFreedom; /// struct GenCoordInfo struct GenCoordInfo @@ -591,6 +592,7 @@ class Skeleton //---------------------------------------------------------------------------- friend class BodyNode; friend class Joint; + friend class DegreeOfFreedom; protected: /// Update mass matrix of the skeleton. @@ -643,6 +645,9 @@ class Skeleton /// Add a Joint to to the Joint NameManager const std::string& addEntryToJointNameMgr(Joint* _newJoint); + /// Add a DegreeOfFreedom to the Dof NameManager + const std::string& addEntryToDofNameMgr(DegreeOfFreedom* _newDof); + /// Add a SoftBodyNode to the SoftBodyNode NameManager void addEntryToSoftBodyNodeNameMgr(SoftBodyNode* _newNode); @@ -683,6 +688,9 @@ class Skeleton /// NameManager for tracking Joints dart::common::NameManager mNameMgrForJoints; + /// NameManager for tracking DegreesOfFreedom + dart::common::NameManager mNameMgrForDofs; + /// NameManager for tracking SoftBodyNodes dart::common::NameManager mNameMgrForSoftBodyNodes; From 1efb377c92d716aa03a93fe8da18a60f7b69435a Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Tue, 2 Dec 2014 16:27:35 -0500 Subject: [PATCH 03/30] integrating DegreeOfFreedom class int othe rest of the code --- dart/dynamics/DegreeOfFreedom.cpp | 5 ++--- dart/dynamics/DegreeOfFreedom.h | 3 +-- dart/dynamics/Joint.cpp | 8 ++++++++ dart/dynamics/Joint.h | 15 +++++++++++++++ dart/dynamics/MultiDofJoint.h | 31 +++++++++++++++++++++++++++++++ dart/dynamics/SingleDofJoint.cpp | 19 +++++++++++++++++++ dart/dynamics/SingleDofJoint.h | 9 +++++++++ dart/dynamics/ZeroDofJoint.cpp | 12 ++++++++++++ dart/dynamics/ZeroDofJoint.h | 6 ++++++ 9 files changed, 103 insertions(+), 5 deletions(-) diff --git a/dart/dynamics/DegreeOfFreedom.cpp b/dart/dynamics/DegreeOfFreedom.cpp index dcf5b64d9fb1e..97b0afe1910f0 100644 --- a/dart/dynamics/DegreeOfFreedom.cpp +++ b/dart/dynamics/DegreeOfFreedom.cpp @@ -355,11 +355,10 @@ const BodyNode* DegreeOfFreedom::getParentBodyNode() const //============================================================================== DegreeOfFreedom::DegreeOfFreedom(Joint* _joint, const std::string &_name, - size_t _indexInJoint, - size_t _indexInSkeleton) : + size_t _indexInJoint) : mName(_name), mIndexInJoint(_indexInJoint), - mIndexInSkeleton(_indexInSkeleton), + mIndexInSkeleton(0), mJoint(_joint) { diff --git a/dart/dynamics/DegreeOfFreedom.h b/dart/dynamics/DegreeOfFreedom.h index d236767386e5b..ce2030abf4d05 100644 --- a/dart/dynamics/DegreeOfFreedom.h +++ b/dart/dynamics/DegreeOfFreedom.h @@ -222,8 +222,7 @@ class DegreeOfFreedom /// DegreeOfFreedom classes DegreeOfFreedom(Joint* _joint, const std::string& _name, - size_t _indexInJoint, - size_t _indexInSkeleton); + size_t _indexInJoint); /// Name of this DegreeOfFreedom std::string mName; diff --git a/dart/dynamics/Joint.cpp b/dart/dynamics/Joint.cpp index ccc937a3ba9a1..c720123fe8916 100644 --- a/dart/dynamics/Joint.cpp +++ b/dart/dynamics/Joint.cpp @@ -44,6 +44,7 @@ #include "dart/renderer/RenderInterface.h" #include "dart/dynamics/BodyNode.h" #include "dart/dynamics/Skeleton.h" +#include "dart/dynamics/DegreeOfFreedom.h" namespace dart { namespace dynamics { @@ -190,6 +191,13 @@ void Joint::init(Skeleton* _skel) mSkeleton = _skel; } +//============================================================================== +DegreeOfFreedom* Joint::createDofPointer(const std::string &_name, + size_t _indexInJoint) +{ + return new DegreeOfFreedom(this, _name, _indexInJoint); +} + //============================================================================== //Eigen::VectorXd Joint::getDampingForces() const //{ diff --git a/dart/dynamics/Joint.h b/dart/dynamics/Joint.h index b988327093918..2beece5217740 100644 --- a/dart/dynamics/Joint.h +++ b/dart/dynamics/Joint.h @@ -55,6 +55,7 @@ namespace dynamics { class BodyNode; class Skeleton; +class DegreeOfFreedom; /// class Joint class Joint @@ -116,6 +117,14 @@ class Joint /// Get number of generalized coordinates DEPRECATED(4.1) virtual size_t getDof() const = 0; + /// Get an object to access the _index-th degree of freedom (generalized + /// coordinate) of this Joint + virtual DegreeOfFreedom* getDof(size_t _index) = 0; + + /// Get an object to access the _index-th degree of freedom (generalized + /// coordinate) of this Joint + virtual const DegreeOfFreedom* getDof(size_t _index) const = 0; + /// Get number of generalized coordinates virtual size_t getNumDofs() const = 0; @@ -387,6 +396,12 @@ class Joint /// Initialize this joint. This function is called by BodyNode::init() virtual void init(Skeleton* _skel); + /// Create a DegreeOfFreedom pointer (because the DegreeOfFreedom class has a + /// protected constructor). You are responsible for memory management of the + /// pointer which is returned. + DegreeOfFreedom* createDofPointer(const std::string& _name, + size_t _indexInJoint); + //---------------------------------------------------------------------------- // Recursive algorithms //---------------------------------------------------------------------------- diff --git a/dart/dynamics/MultiDofJoint.h b/dart/dynamics/MultiDofJoint.h index 885e025cd6670..e73709b3410ca 100644 --- a/dart/dynamics/MultiDofJoint.h +++ b/dart/dynamics/MultiDofJoint.h @@ -69,6 +69,12 @@ class MultiDofJoint : public Joint // Documentation inherited DEPRECATED(4.1) virtual size_t getDof() const; + // Documentation inherited + virtual DegreeOfFreedom* getDof(size_t _index); + + // Documentation inherited + virtual const DegreeOfFreedom* getDof(size_t _index) const; + // Documentation inherited virtual size_t getNumDofs() const; @@ -387,6 +393,9 @@ class MultiDofJoint : public Joint /// Eigen::Matrix mIndexInSkeleton; + // TODO: Replace with std::array when we migrate to C++11 + DegreeOfFreedom* mDofs[DOF]; + //---------------------------------------------------------------------------- // Configuration //---------------------------------------------------------------------------- @@ -546,6 +555,10 @@ MultiDofJoint::MultiDofJoint(const std::string& _name) mTotalForce(Eigen::Matrix::Zero()), mTotalImpulse(Eigen::Matrix::Zero()) { + // Note: These should be created by the various MultiDofJoint implementations, + // because of differences in naming conventions for the degrees of freedom + for(size_t i=0; i::getDof() const return getNumDofs(); } +//============================================================================== +template +DegreeOfFreedom* MultiDofJoint::getDof(size_t _index) +{ + if(_index < DOF) + return mDofs[_index]; + return NULL; +} + +//============================================================================== +template +const DegreeOfFreedom* MultiDofJoint::getDof(size_t _index) const +{ + if(_index < DOF) + return mDofs[_index]; + return NULL; +} + //============================================================================== template size_t MultiDofJoint::getNumDofs() const diff --git a/dart/dynamics/SingleDofJoint.cpp b/dart/dynamics/SingleDofJoint.cpp index 0278174dd1247..7da851baca7a6 100644 --- a/dart/dynamics/SingleDofJoint.cpp +++ b/dart/dynamics/SingleDofJoint.cpp @@ -40,6 +40,7 @@ #include "dart/math/Helpers.h" #include "dart/dynamics/BodyNode.h" #include "dart/dynamics/Skeleton.h" +#include "dart/dynamics/DegreeOfFreedom.h" namespace dart { namespace dynamics { @@ -77,11 +78,13 @@ SingleDofJoint::SingleDofJoint(const std::string& _name) mTotalForce(0.0), mTotalImpulse(0.0) { + mDof = createDofPointer(_name, 0); } //============================================================================== SingleDofJoint::~SingleDofJoint() { + delete mDof; } //============================================================================== @@ -122,6 +125,22 @@ size_t SingleDofJoint::getIndexInSkeleton(size_t _index) const return mIndexInSkeleton; } +//============================================================================== +DegreeOfFreedom* SingleDofJoint::getDof(size_t _index) +{ + if(0 == _index) + return mDof; + return NULL; +} + +//============================================================================== +const DegreeOfFreedom* SingleDofJoint::getDof(size_t _index) const +{ + if(0 == _index) + return mDof; + return NULL; +} + //============================================================================== void SingleDofJoint::setPosition(size_t _index, double _position) { diff --git a/dart/dynamics/SingleDofJoint.h b/dart/dynamics/SingleDofJoint.h index 40a2091ca906f..337cc7784364d 100644 --- a/dart/dynamics/SingleDofJoint.h +++ b/dart/dynamics/SingleDofJoint.h @@ -46,6 +46,7 @@ namespace dynamics { class BodyNode; class Skeleton; +class DegreeOfFreedom; /// class SingleDofJoint class SingleDofJoint : public Joint @@ -75,6 +76,12 @@ class SingleDofJoint : public Joint // TODO(JS): Not to use Eigen::VectorXd + // Documentation inherited + virtual DegreeOfFreedom* getDof(size_t _index); + + // Documentation inherited + virtual const DegreeOfFreedom* getDof(size_t _index) const; + // Documentation inherited virtual void setPosition(size_t _index, double _position); @@ -381,6 +388,8 @@ class SingleDofJoint : public Joint /// size_t mIndexInSkeleton; + DegreeOfFreedom* mDof; + //---------------------------------------------------------------------------- // Configuration //---------------------------------------------------------------------------- diff --git a/dart/dynamics/ZeroDofJoint.cpp b/dart/dynamics/ZeroDofJoint.cpp index c6f0105bb85a9..300df908cfef8 100644 --- a/dart/dynamics/ZeroDofJoint.cpp +++ b/dart/dynamics/ZeroDofJoint.cpp @@ -61,6 +61,18 @@ size_t ZeroDofJoint::getDof() const return getNumDofs(); } +//============================================================================== +DegreeOfFreedom* ZeroDofJoint::getDof(size_t) +{ + return NULL; +} + +//============================================================================== +const DegreeOfFreedom* ZeroDofJoint::getDof(size_t) const +{ + return NULL; +} + //============================================================================== size_t ZeroDofJoint::getNumDofs() const { diff --git a/dart/dynamics/ZeroDofJoint.h b/dart/dynamics/ZeroDofJoint.h index 19c8aa5cbd346..8017e4d658940 100644 --- a/dart/dynamics/ZeroDofJoint.h +++ b/dart/dynamics/ZeroDofJoint.h @@ -64,6 +64,12 @@ class ZeroDofJoint : public Joint // Documentation inherited DEPRECATED(4.1) virtual size_t getDof() const; + // Documentation inherited + virtual DegreeOfFreedom* getDof(size_t); + + // Documentation inherited + virtual const DegreeOfFreedom* getDof(size_t) const; + // Documentation inherited virtual size_t getNumDofs() const; From 477c04861798b2a31fd8b0890ca5cdf6b009c336 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Mon, 15 Dec 2014 16:19:14 -0500 Subject: [PATCH 04/30] Finished DOF class, needs testing --- dart/dynamics/BallJoint.cpp | 10 +++++ dart/dynamics/BallJoint.h | 3 ++ dart/dynamics/BodyNode.cpp | 6 +-- dart/dynamics/DegreeOfFreedom.cpp | 2 +- dart/dynamics/DegreeOfFreedom.h | 6 +-- dart/dynamics/EulerJoint.cpp | 44 ++++++++++++++++++++- dart/dynamics/EulerJoint.h | 6 ++- dart/dynamics/FreeJoint.cpp | 13 +++++++ dart/dynamics/FreeJoint.h | 3 ++ dart/dynamics/Joint.cpp | 9 ++++- dart/dynamics/Joint.h | 6 ++- dart/dynamics/MultiDofJoint.h | 8 ++-- dart/dynamics/PlanarJoint.cpp | 57 ++++++++++++++++++++++++++-- dart/dynamics/PlanarJoint.h | 13 +++++-- dart/dynamics/SingleDofJoint.cpp | 6 +++ dart/dynamics/SingleDofJoint.h | 4 ++ dart/dynamics/Skeleton.cpp | 38 ++++++++++++++++++- dart/dynamics/Skeleton.h | 6 +++ dart/dynamics/TranslationalJoint.cpp | 9 +++++ dart/dynamics/TranslationalJoint.h | 3 ++ dart/dynamics/UniversalJoint.cpp | 9 +++++ dart/dynamics/UniversalJoint.h | 6 +++ dart/dynamics/ZeroDofJoint.cpp | 6 +++ dart/dynamics/ZeroDofJoint.h | 4 ++ 24 files changed, 253 insertions(+), 24 deletions(-) diff --git a/dart/dynamics/BallJoint.cpp b/dart/dynamics/BallJoint.cpp index 672ce503c3191..c7003bac97c49 100644 --- a/dart/dynamics/BallJoint.cpp +++ b/dart/dynamics/BallJoint.cpp @@ -58,6 +58,8 @@ BallJoint::BallJoint(const std::string& _name) assert(!math::isNan(mJacobian)); // Time derivative of Jacobian is always zero + + updateDegreeOfFreedomNames(); } //============================================================================== @@ -92,6 +94,14 @@ void BallJoint::integratePositions(double _dt) mPositions = math::logMap(mR.linear()); } +//============================================================================== +void BallJoint::updateDegreeOfFreedomNames() +{ + mDofs[0]->setName(mName+"_x"); + mDofs[1]->setName(mName+"_y"); + mDofs[2]->setName(mName+"_z"); +} + //============================================================================== void BallJoint::updateLocalTransform() { diff --git a/dart/dynamics/BallJoint.h b/dart/dynamics/BallJoint.h index 9f514b38cca6b..124a6b8884ebb 100644 --- a/dart/dynamics/BallJoint.h +++ b/dart/dynamics/BallJoint.h @@ -67,6 +67,9 @@ class BallJoint : public MultiDofJoint<3> // Documentation inherited virtual void integratePositions(double _dt); + // Documentation inherited + virtual void updateDegreeOfFreedomNames(); + // Documentation inherited virtual void updateLocalTransform(); diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index 1e0db0143b9fd..740b00093f38d 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -391,8 +391,8 @@ void BodyNode::setParentJoint(Joint* _joint) if(mSkeleton) { - mSkeleton->mNameMgrForJoints.removeName(mParentJoint->getName()); - mSkeleton->addEntryToJointNameMgr(_joint); + mSkeleton->unregisterJoint(mParentJoint); + mSkeleton->registerJoint(_joint); } if(mParentJoint) @@ -400,7 +400,7 @@ void BodyNode::setParentJoint(Joint* _joint) mParentJoint = _joint; mParentJoint->mChildBodyNode = this; - // TODO: Shouldn't we delete the original mParentJoint? Seems like the BodyNode + // TODO: Should we delete the original mParentJoint? Seems like the BodyNode // should be responsible for its parent joint } diff --git a/dart/dynamics/DegreeOfFreedom.cpp b/dart/dynamics/DegreeOfFreedom.cpp index 97b0afe1910f0..b794979cd5ba0 100644 --- a/dart/dynamics/DegreeOfFreedom.cpp +++ b/dart/dynamics/DegreeOfFreedom.cpp @@ -354,7 +354,7 @@ const BodyNode* DegreeOfFreedom::getParentBodyNode() const //============================================================================== DegreeOfFreedom::DegreeOfFreedom(Joint* _joint, - const std::string &_name, + const std::string& _name, size_t _indexInJoint) : mName(_name), mIndexInJoint(_indexInJoint), diff --git a/dart/dynamics/DegreeOfFreedom.h b/dart/dynamics/DegreeOfFreedom.h index ce2030abf4d05..891a4e96cfb32 100644 --- a/dart/dynamics/DegreeOfFreedom.h +++ b/dart/dynamics/DegreeOfFreedom.h @@ -48,7 +48,7 @@ class Joint; class BodyNode; /// DegreeOfFreedom class is a proxy class for accessing single degrees of -/// freedom (or generalized coordinate) of the Skeleton. +/// freedom (aka generalized coordinates) of the Skeleton. class DegreeOfFreedom { @@ -161,7 +161,7 @@ class DegreeOfFreedom // "Force". Currently "Force" is being used throughout DART to refer to the // Generalized Force of a Generalized Coordinate, but I propose we use the // word "Effort" instead. We can change the names of these functions later if - // "Effort" is deemed inappropriate or undesirable. + // it's decided that "Effort" is not a good word. /// Set the generalized force of this DegreeOfFreedom void setEffort(double _effort); @@ -238,7 +238,7 @@ class DegreeOfFreedom // Note that we do not need to store BodyNode or Skeleton, because we can // access them through this joint pointer. Moreover, we never need to check // whether mJoint is nullptr, because only Joints are allowed to create a - // DegreeOfFreedom and DegreesOfFreedom are deleted when their Joint is + // DegreeOfFreedom and every DegreeOfFreedom is deleted when its Joint is // destructed. }; diff --git a/dart/dynamics/EulerJoint.cpp b/dart/dynamics/EulerJoint.cpp index c80066b35fc37..20895e8aae8f3 100644 --- a/dart/dynamics/EulerJoint.cpp +++ b/dart/dynamics/EulerJoint.cpp @@ -41,6 +41,7 @@ #include "dart/common/Console.h" #include "dart/math/Helpers.h" #include "dart/math/Geometry.h" +#include "dart/dynamics/DegreeOfFreedom.h" namespace dart { namespace dynamics { @@ -50,6 +51,7 @@ EulerJoint::EulerJoint(const std::string& _name) : MultiDofJoint(_name), mAxisOrder(AO_XYZ) { + updateDegreeOfFreedomNames(); } //============================================================================== @@ -58,9 +60,11 @@ EulerJoint::~EulerJoint() } //============================================================================== -void EulerJoint::setAxisOrder(EulerJoint::AxisOrder _order) +void EulerJoint::setAxisOrder(EulerJoint::AxisOrder _order, bool _rename_dofs) { mAxisOrder = _order; + if(_rename_dofs) + updateDegreeOfFreedomNames(); } //============================================================================== @@ -69,6 +73,44 @@ EulerJoint::AxisOrder EulerJoint::getAxisOrder() const return mAxisOrder; } +//============================================================================== +void EulerJoint::updateDegreeOfFreedomNames() +{ + std::vector affixes; + switch(mAxisOrder) + { + case AO_ZYX: + affixes.push_back("_z"); + affixes.push_back("_y"); + affixes.push_back("_x"); + break; + case AO_ZYZ: + affixes.push_back("_z"); + affixes.push_back("_y"); + affixes.push_back("_z"); + break; + case AO_XYZ: + affixes.push_back("_x"); + affixes.push_back("_y"); + affixes.push_back("_z"); + break; + case AO_ZXY: + affixes.push_back("_z"); + affixes.push_back("_x"); + affixes.push_back("_y"); + break; + default: + dterr << "Unsupported axis order in EulerJoint named '" << mName + << "' (" << mAxisOrder << ")\n"; + } + + if(affixes.size() == 3) + { + for(size_t i=0; i<3; ++i) + mDofs[i]->setName(mName+affixes[i]); + } +} + //============================================================================== void EulerJoint::updateLocalTransform() { diff --git a/dart/dynamics/EulerJoint.h b/dart/dynamics/EulerJoint.h index 8b0b2e4fbbd80..a999d0025fd43 100644 --- a/dart/dynamics/EulerJoint.h +++ b/dart/dynamics/EulerJoint.h @@ -64,7 +64,7 @@ class EulerJoint : public MultiDofJoint<3> virtual ~EulerJoint(); /// - void setAxisOrder(AxisOrder _order); + void setAxisOrder(AxisOrder _order, bool _rename_dofs=true); /// AxisOrder getAxisOrder() const; @@ -76,6 +76,10 @@ class EulerJoint : public MultiDofJoint<3> } protected: + /// Set the names of this joint's DegreesOfFreedom. Used during construction + /// and when axis order is changed. + virtual void updateDegreeOfFreedomNames(); + // Documentation inherited virtual void updateLocalTransform(); diff --git a/dart/dynamics/FreeJoint.cpp b/dart/dynamics/FreeJoint.cpp index 97ba02220700a..39bdee2140b9c 100644 --- a/dart/dynamics/FreeJoint.cpp +++ b/dart/dynamics/FreeJoint.cpp @@ -60,6 +60,8 @@ FreeJoint::FreeJoint(const std::string& _name) assert(!math::isNan(mJacobian)); // Time derivative of Jacobian is always zero + + updateDegreeOfFreedomNames(); } //============================================================================== @@ -92,6 +94,17 @@ void FreeJoint::integratePositions(double _dt) mPositions = math::logMap(mQ); } +//============================================================================== +void FreeJoint::updateDegreeOfFreedomNames() +{ + mDofs[0]->setName(mName+"_rot_x"); + mDofs[1]->setName(mName+"_rot_y"); + mDofs[2]->setName(mName+"_rot_z"); + mDofs[3]->setName(mName+"_pos_x"); + mDofs[4]->setName(mName+"_pos_y"); + mDofs[5]->setName(mName+"_pos_z"); +} + //============================================================================== void FreeJoint::updateLocalTransform() { diff --git a/dart/dynamics/FreeJoint.h b/dart/dynamics/FreeJoint.h index 75525cc6e6a81..f87ed15f01d6c 100644 --- a/dart/dynamics/FreeJoint.h +++ b/dart/dynamics/FreeJoint.h @@ -69,6 +69,9 @@ class FreeJoint : public MultiDofJoint<6> // Documentation inherited virtual void integratePositions(double _dt); + // Documentation inherited + virtual void updateDegreeOfFreedomNames(); + // Documentation inherited virtual void updateLocalTransform(); diff --git a/dart/dynamics/Joint.cpp b/dart/dynamics/Joint.cpp index c720123fe8916..746a68714187b 100644 --- a/dart/dynamics/Joint.cpp +++ b/dart/dynamics/Joint.cpp @@ -68,10 +68,14 @@ Joint::~Joint() { } //============================================================================== -const std::string& Joint::setName(const std::string& _name) { +const std::string& Joint::setName(const std::string& _name, bool _rename_dofs) { if(mName == _name) + { + if(_rename_dofs) + updateDegreeOfFreedomNames(); return mName; + } if(mSkeleton) { @@ -82,6 +86,9 @@ const std::string& Joint::setName(const std::string& _name) { else mName = _name; + if(_rename_dofs) + updateDegreeOfFreedomNames(); + return mName; } diff --git a/dart/dynamics/Joint.h b/dart/dynamics/Joint.h index 2beece5217740..90a943d35d034 100644 --- a/dart/dynamics/Joint.h +++ b/dart/dynamics/Joint.h @@ -68,7 +68,7 @@ class Joint virtual ~Joint(); /// Set joint name - const std::string& setName(const std::string& _name); + const std::string& setName(const std::string& _name, bool _rename_dofs=true); /// Get joint name const std::string& getName() const; @@ -402,6 +402,10 @@ class Joint DegreeOfFreedom* createDofPointer(const std::string& _name, size_t _indexInJoint); + /// Update the names of the joint's degrees of freedom. Used when setName() is + /// called with _rename_dofs set to true. + virtual void updateDegreeOfFreedomNames() = 0; + //---------------------------------------------------------------------------- // Recursive algorithms //---------------------------------------------------------------------------- diff --git a/dart/dynamics/MultiDofJoint.h b/dart/dynamics/MultiDofJoint.h index e73709b3410ca..08e164d558ca2 100644 --- a/dart/dynamics/MultiDofJoint.h +++ b/dart/dynamics/MultiDofJoint.h @@ -44,6 +44,7 @@ #include "dart/math/Helpers.h" #include "dart/dynamics/Joint.h" #include "dart/dynamics/Skeleton.h" +#include "dart/dynamics/DegreeOfFreedom.h" namespace dart { namespace dynamics { @@ -394,6 +395,7 @@ class MultiDofJoint : public Joint Eigen::Matrix mIndexInSkeleton; // TODO: Replace with std::array when we migrate to C++11 + /// Array of DegreeOfFreedom objects DegreeOfFreedom* mDofs[DOF]; //---------------------------------------------------------------------------- @@ -555,16 +557,16 @@ MultiDofJoint::MultiDofJoint(const std::string& _name) mTotalForce(Eigen::Matrix::Zero()), mTotalImpulse(Eigen::Matrix::Zero()) { - // Note: These should be created by the various MultiDofJoint implementations, - // because of differences in naming conventions for the degrees of freedom for(size_t i=0; i MultiDofJoint::~MultiDofJoint() { + for(size_t i=0; i affixes; + switch(mPlaneType) + { + case PT_XY: + affixes.push_back("_x"); + affixes.push_back("_y"); + break; + case PT_YZ: + affixes.push_back("_y"); + affixes.push_back("_z"); + break; + case PT_ZX: + affixes.push_back("_z"); + affixes.push_back("_x"); + break; + case PT_ARBITRARY: + affixes.push_back("_1"); + affixes.push_back("_2"); + break; + default: + dterr << "Unsupported plane type in PlanarJoint named '" << mName + << "' (" << mPlaneType << ")\n"; + } + + if(affixes.size() == 2) + { + for(size_t i=0; i<2; ++i) + mDofs[i]->setName(mName+affixes[i]); + } +} + //============================================================================== void PlanarJoint::updateLocalTransform() { diff --git a/dart/dynamics/PlanarJoint.h b/dart/dynamics/PlanarJoint.h index f174acdd52299..1321b6502f242 100644 --- a/dart/dynamics/PlanarJoint.h +++ b/dart/dynamics/PlanarJoint.h @@ -69,17 +69,18 @@ class PlanarJoint : public MultiDofJoint<3> virtual ~PlanarJoint(); /// Set plane type as XY-plane - void setXYPlane(); + void setXYPlane(bool _rename_dofs=true); /// Set plane type as YZ-plane - void setYZPlane(); + void setYZPlane(bool _rename_dofs=true); /// Set plane type as ZX-plane - void setZXPlane(); + void setZXPlane(bool _rename_dofs=true); /// Set plane type as arbitrary plane with two orthogonal translational axes void setArbitraryPlane(const Eigen::Vector3d& _transAxis1, - const Eigen::Vector3d& _transAxis2); + const Eigen::Vector3d& _transAxis2, + bool _rename_dofs=true); /// Return plane type PlaneType getPlaneType() const; @@ -100,6 +101,10 @@ class PlanarJoint : public MultiDofJoint<3> } protected: + /// Set the names of this joint's DegreesOfFreedom. Used during construction + /// and when the Plane type is changed. + virtual void updateDegreeOfFreedomNames(); + // Documentation inherited virtual void updateLocalTransform(); diff --git a/dart/dynamics/SingleDofJoint.cpp b/dart/dynamics/SingleDofJoint.cpp index 7da851baca7a6..b31ef8a2cf8ba 100644 --- a/dart/dynamics/SingleDofJoint.cpp +++ b/dart/dynamics/SingleDofJoint.cpp @@ -721,6 +721,12 @@ double SingleDofJoint::getPotentialEnergy() const return pe; } +//============================================================================== +void SingleDofJoint::updateDegreeOfFreedomNames() +{ + mDof->setName(mName); +} + //============================================================================== const math::Jacobian SingleDofJoint::getLocalJacobian() const { diff --git a/dart/dynamics/SingleDofJoint.h b/dart/dynamics/SingleDofJoint.h index 337cc7784364d..d497f999ef640 100644 --- a/dart/dynamics/SingleDofJoint.h +++ b/dart/dynamics/SingleDofJoint.h @@ -266,6 +266,10 @@ class SingleDofJoint : public Joint virtual double getPotentialEnergy() const; protected: + + // Documentation inherited + virtual void updateDegreeOfFreedomNames(); + //---------------------------------------------------------------------------- // Recursive dynamics algorithms //---------------------------------------------------------------------------- diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index e6fe9536da41a..83855ae1f05a5 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -235,10 +235,9 @@ void Skeleton::addBodyNode(BodyNode* _body) mBodyNodes.push_back(_body); addEntryToBodyNodeNameMgr(_body); - addEntryToJointNameMgr(_body->getParentJoint()); addMarkersOfBodyNode(_body); _body->mSkeleton = this; - _body->mParentJoint->mSkeleton = this; + registerJoint(_body->getParentJoint()); SoftBodyNode* softBodyNode = dynamic_cast(_body); if (softBodyNode) @@ -1223,6 +1222,41 @@ void Skeleton::drawMarkers(renderer::RenderInterface* _ri, getRootBodyNode()->drawMarkers(_ri, _color, _useDefaultColor); } +//============================================================================== +void Skeleton::registerJoint(Joint *_newJoint) +{ + if(NULL == _newJoint) + { + dterr << "[Skeleton::registerJoint] Error: Attempting to add a NULL joint " + "to the Skeleton named '" << mName << "'!\n"; + return; + } + + addEntryToJointNameMgr(_newJoint); + _newJoint->mSkeleton = this; + + for(size_t i=0; i<_newJoint->getNumDofs(); ++i) + { + DegreeOfFreedom* dof = _newJoint->getDof(i); + dof->mName = mNameMgrForDofs.issueNewNameAndAdd(dof->getName(), dof); + } +} + +//============================================================================== +void Skeleton::unregisterJoint(Joint *_oldJoint) +{ + if(NULL == _oldJoint) + return; + + mNameMgrForJoints.removeName(_oldJoint->getName()); + + for(size_t i=0; i<_oldJoint->getNumDofs(); ++i) + { + DegreeOfFreedom* dof = _oldJoint->getDof(i); + mNameMgrForDofs.removeName(dof->getName()); + } +} + //============================================================================== void Skeleton::updateMassMatrix() { diff --git a/dart/dynamics/Skeleton.h b/dart/dynamics/Skeleton.h index 7d4e27b1045e1..1860265609f41 100644 --- a/dart/dynamics/Skeleton.h +++ b/dart/dynamics/Skeleton.h @@ -595,6 +595,12 @@ class Skeleton friend class DegreeOfFreedom; protected: + /// Register a joint with the Skeleton. Internal use only. + void registerJoint(dart::dynamics::Joint* _newJoint); + + /// Remove a joint from the Skeleton. Internal use only. + void unregisterJoint(dart::dynamics::Joint* _oldJoint); + /// Update mass matrix of the skeleton. void updateMassMatrix(); diff --git a/dart/dynamics/TranslationalJoint.cpp b/dart/dynamics/TranslationalJoint.cpp index 401b6cdbd5d72..5c6e0b55f1ed9 100644 --- a/dart/dynamics/TranslationalJoint.cpp +++ b/dart/dynamics/TranslationalJoint.cpp @@ -48,6 +48,7 @@ namespace dynamics { TranslationalJoint::TranslationalJoint(const std::string& _name) : MultiDofJoint(_name) { + updateDegreeOfFreedomNames(); } //============================================================================== @@ -55,6 +56,14 @@ TranslationalJoint::~TranslationalJoint() { } +//============================================================================== +void TranslationalJoint::updateDegreeOfFreedomNames() +{ + mDofs[0]->setName(mName+"_x"); + mDofs[1]->setName(mName+"_y"); + mDofs[2]->setName(mName+"_z"); +} + //============================================================================== void TranslationalJoint::updateLocalTransform() { diff --git a/dart/dynamics/TranslationalJoint.h b/dart/dynamics/TranslationalJoint.h index e69fffb338c35..83401e29b4795 100644 --- a/dart/dynamics/TranslationalJoint.h +++ b/dart/dynamics/TranslationalJoint.h @@ -61,6 +61,9 @@ class TranslationalJoint : public MultiDofJoint<3> } protected: + // Documentation inherited + virtual void updateDegreeOfFreedomNames(); + // Documentation inherited virtual void updateLocalTransform(); diff --git a/dart/dynamics/UniversalJoint.cpp b/dart/dynamics/UniversalJoint.cpp index f25d24e35b3ef..3a38e683e2d4d 100644 --- a/dart/dynamics/UniversalJoint.cpp +++ b/dart/dynamics/UniversalJoint.cpp @@ -52,6 +52,8 @@ UniversalJoint::UniversalJoint(const Eigen::Vector3d& _axis0, { mAxis[0] = _axis0.normalized(); mAxis[1] = _axis1.normalized(); + + updateDegreeOfFreedomNames(); } //============================================================================== @@ -83,6 +85,13 @@ const Eigen::Vector3d& UniversalJoint::getAxis2() const return mAxis[1]; } +//============================================================================== +void UniversalJoint::updateDegreeOfFreedomNames() +{ + mDofs[0]->setName(mName+"_1"); + mDofs[1]->setName(mName+"_2"); +} + //============================================================================== void UniversalJoint::updateLocalTransform() { diff --git a/dart/dynamics/UniversalJoint.h b/dart/dynamics/UniversalJoint.h index a83e0b1d444ec..8a7ee2389ae59 100644 --- a/dart/dynamics/UniversalJoint.h +++ b/dart/dynamics/UniversalJoint.h @@ -54,6 +54,9 @@ class UniversalJoint : public MultiDofJoint<2> UniversalJoint(const Eigen::Vector3d& _axis0 = Eigen::Vector3d::UnitX(), const Eigen::Vector3d& _axis1 = Eigen::Vector3d::UnitY(), const std::string& _name = "Universal joint"); + // TODO(MXG): We should be more consistent about indexing starting from 0 or 1 + // In the constructor arguments, we start with axis0, but in the member + // functions we start with axis1. /// Destructor virtual ~UniversalJoint(); @@ -77,6 +80,9 @@ class UniversalJoint : public MultiDofJoint<2> } protected: + // Documentation inherited + virtual void updateDegreeOfFreedomNames(); + // Documentation inherited virtual void updateLocalTransform(); diff --git a/dart/dynamics/ZeroDofJoint.cpp b/dart/dynamics/ZeroDofJoint.cpp index 300df908cfef8..e12ff0ff6c449 100644 --- a/dart/dynamics/ZeroDofJoint.cpp +++ b/dart/dynamics/ZeroDofJoint.cpp @@ -407,6 +407,12 @@ double ZeroDofJoint::getPotentialEnergy() const return 0.0; } +//============================================================================== +void ZeroDofJoint::updateDegreeOfFreedomNames() +{ + // Do nothing +} + //============================================================================== const math::Jacobian ZeroDofJoint::getLocalJacobian() const { diff --git a/dart/dynamics/ZeroDofJoint.h b/dart/dynamics/ZeroDofJoint.h index 8017e4d658940..9592dbe15dcd4 100644 --- a/dart/dynamics/ZeroDofJoint.h +++ b/dart/dynamics/ZeroDofJoint.h @@ -267,6 +267,10 @@ class ZeroDofJoint : public Joint virtual double getPotentialEnergy() const; protected: + + // Documentation inherited + virtual void updateDegreeOfFreedomNames(); + //---------------------------------------------------------------------------- // Recursive dynamics algorithms //---------------------------------------------------------------------------- From a9a311a7e1d188d929c714f419286610978b253f Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Mon, 15 Dec 2014 16:20:15 -0500 Subject: [PATCH 05/30] more appropriate name for the name management unit test --- unittests/{testNameChanges.cpp => testNameManagement.cpp} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename unittests/{testNameChanges.cpp => testNameManagement.cpp} (100%) diff --git a/unittests/testNameChanges.cpp b/unittests/testNameManagement.cpp similarity index 100% rename from unittests/testNameChanges.cpp rename to unittests/testNameManagement.cpp From f098b82bd53b8b21f81665c69ab438aff4cdb326 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Mon, 15 Dec 2014 17:21:05 -0500 Subject: [PATCH 06/30] Finished integrating DOF class into Skeleton --- dart/dynamics/DegreeOfFreedom.h | 5 ++++- dart/dynamics/MultiDofJoint.h | 6 ++++-- dart/dynamics/SingleDofJoint.cpp | 6 ++++-- dart/dynamics/Skeleton.cpp | 35 ++++++++++++++++++++++++++++++++ dart/dynamics/Skeleton.h | 15 ++++++++++++++ dart/dynamics/ZeroDofJoint.cpp | 6 +++--- dart/dynamics/ZeroDofJoint.h | 2 +- 7 files changed, 66 insertions(+), 9 deletions(-) diff --git a/dart/dynamics/DegreeOfFreedom.h b/dart/dynamics/DegreeOfFreedom.h index 891a4e96cfb32..b1cf0919fcd3a 100644 --- a/dart/dynamics/DegreeOfFreedom.h +++ b/dart/dynamics/DegreeOfFreedom.h @@ -46,6 +46,8 @@ namespace dynamics { class Skeleton; class Joint; class BodyNode; +class SingleDofJoint; +template class MultiDofJoint; /// DegreeOfFreedom class is a proxy class for accessing single degrees of /// freedom (aka generalized coordinates) of the Skeleton. @@ -55,6 +57,8 @@ class DegreeOfFreedom public: friend class dart::dynamics::Joint; + friend class dart::dynamics::SingleDofJoint; + template friend class dart::dynamics::MultiDofJoint; friend class dart::dynamics::Skeleton; /// Change the name of this DegreeOfFreedom @@ -217,7 +221,6 @@ class DegreeOfFreedom const BodyNode* getParentBodyNode() const; protected: - /// The constructor is protected so that only Joints can create /// DegreeOfFreedom classes DegreeOfFreedom(Joint* _joint, diff --git a/dart/dynamics/MultiDofJoint.h b/dart/dynamics/MultiDofJoint.h index 08e164d558ca2..af15568f5c061 100644 --- a/dart/dynamics/MultiDofJoint.h +++ b/dart/dynamics/MultiDofJoint.h @@ -608,12 +608,14 @@ void MultiDofJoint::setIndexInSkeleton(size_t _index, { if (_index >= getNumDofs()) { - dterr << "setIndexInSkeleton index[" << _index << "] out of range" - << std::endl; + dterr << "[MultiDofJoint::setIndexInSkeleton] index[" << _index + << "] out of range" << std::endl; return; } mIndexInSkeleton[_index] = _indexInSkeleton; + // TODO(MXG): I think ^this array is redundant now that we have the mDofs array + mDofs[_index]->mIndexInSkeleton = _indexInSkeleton; } //============================================================================== diff --git a/dart/dynamics/SingleDofJoint.cpp b/dart/dynamics/SingleDofJoint.cpp index b31ef8a2cf8ba..a6dcbac8b7a7a 100644 --- a/dart/dynamics/SingleDofJoint.cpp +++ b/dart/dynamics/SingleDofJoint.cpp @@ -104,12 +104,14 @@ void SingleDofJoint::setIndexInSkeleton(size_t _index, size_t _indexInSkeleton) { if (_index != 0) { - dterr << "setIndexInSkeleton index[" << _index << "] out of range" - << std::endl; + dterr << "[SingleDofJoint::setIndexInSkeleton] index[" << _index + << "] out of range" << std::endl; return; } mIndexInSkeleton = _indexInSkeleton; + // TODO(MXG): I think ^this member variable is redundant now that we have mDof + mDof->mIndexInSkeleton = _indexInSkeleton; } //============================================================================== diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index 83855ae1f05a5..f95c778b8f67d 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -119,6 +119,7 @@ const std::string& Skeleton::addEntryToJointNameMgr(Joint *_newJoint) { _newJoint->mName = mNameMgrForJoints.issueNewNameAndAdd(_newJoint->getName(), _newJoint); + _newJoint->updateDegreeOfFreedomNames(); return _newJoint->mName; } @@ -367,6 +368,40 @@ const Joint* Skeleton::getJoint(const std::string& _name) const return mNameMgrForJoints.getObject(_name); } +//============================================================================== +DegreeOfFreedom* Skeleton::getDof(size_t _idx) +{ + assert(_idx < getNumDofs()); + + if(_idx >= getNumDofs()) + return NULL; + + return mGenCoordInfos[_idx].joint->getDof(mGenCoordInfos[_idx].localIndex); +} + +//============================================================================== +const DegreeOfFreedom* Skeleton::getDof(size_t _idx) const +{ + assert(_idx < getNumDofs()); + + if(_idx >= getNumDofs()) + return NULL; + + return mGenCoordInfos[_idx].joint->getDof(mGenCoordInfos[_idx].localIndex); +} + +//============================================================================== +DegreeOfFreedom* Skeleton::getDof(const std::string &name) +{ + return mNameMgrForDofs.getObject(name); +} + +//============================================================================== +const DegreeOfFreedom* Skeleton::getDof(const std::string &name) const +{ + return mNameMgrForDofs.getObject(name); +} + //============================================================================== Marker* Skeleton::getMarker(const std::string& _name) { diff --git a/dart/dynamics/Skeleton.h b/dart/dynamics/Skeleton.h index 1860265609f41..c9753c2a1a4b4 100644 --- a/dart/dynamics/Skeleton.h +++ b/dart/dynamics/Skeleton.h @@ -188,6 +188,18 @@ class Skeleton /// Get joint whose name is _name Joint* getJoint(const std::string &_name); + /// Get degree of freedom (aka generalized coordinate) whose index is _idx + DegreeOfFreedom* getDof(size_t _idx); + + /// Get degree of freedom (aka generalized coordinate) whose index is _idx + const DegreeOfFreedom* getDof(size_t _idx) const; + + /// Get degree of freedom (aka generalized coordinate) whose name is _name + DegreeOfFreedom* getDof(const std::string& name); + + /// Get degree of freedom (aka generalized coordinate) whose name is _name + const DegreeOfFreedom* getDof(const std::string& name) const; + /// Get const joint whose name is _name const Joint* getJoint(const std::string& _name) const; @@ -675,6 +687,9 @@ class Skeleton /// std::vector mGenCoordInfos; + // TODO(MXG): Now that we have the DegreeOfFreedom class, this vector is + // probably outdated. We can instead have a std::vector, + // because each DegreeOfFreedom has all the information that GenCoordInfo has /// True if self collision check is enabled bool mEnabledSelfCollisionCheck; diff --git a/dart/dynamics/ZeroDofJoint.cpp b/dart/dynamics/ZeroDofJoint.cpp index e12ff0ff6c449..0ce02bd5e8efd 100644 --- a/dart/dynamics/ZeroDofJoint.cpp +++ b/dart/dynamics/ZeroDofJoint.cpp @@ -80,10 +80,10 @@ size_t ZeroDofJoint::getNumDofs() const } //============================================================================== -void ZeroDofJoint::setIndexInSkeleton(size_t _index, size_t _indexInSkeleton) +void ZeroDofJoint::setIndexInSkeleton(size_t _index, size_t) { - dterr << "setIndexInSkeleton index[" << _index << "] out of range" - << std::endl; + dterr << "[ZeroDofJoint::setIndexInSkeleton] index[" << _index + << "] out of range" << std::endl; } //============================================================================== diff --git a/dart/dynamics/ZeroDofJoint.h b/dart/dynamics/ZeroDofJoint.h index 9592dbe15dcd4..290f0de68a1d4 100644 --- a/dart/dynamics/ZeroDofJoint.h +++ b/dart/dynamics/ZeroDofJoint.h @@ -74,7 +74,7 @@ class ZeroDofJoint : public Joint virtual size_t getNumDofs() const; // Documentation inherited - virtual void setIndexInSkeleton(size_t _index, size_t _indexInSkeleton); + virtual void setIndexInSkeleton(size_t _index, size_t); // Documentation inherited virtual size_t getIndexInSkeleton(size_t _index) const; From ecd227ffe1a414af1622c277c8264bb1f37ff166 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Mon, 15 Dec 2014 17:21:30 -0500 Subject: [PATCH 07/30] created test for DOF class related features -- passing --- unittests/testNameManagement.cpp | 86 +++++++++++++++++++++++++++++--- 1 file changed, 80 insertions(+), 6 deletions(-) diff --git a/unittests/testNameManagement.cpp b/unittests/testNameManagement.cpp index b87497419f92a..47803f8da3326 100644 --- a/unittests/testNameManagement.cpp +++ b/unittests/testNameManagement.cpp @@ -9,7 +9,7 @@ using namespace math; using namespace dynamics; -TEST(NAMECHANGES, BASIC) +TEST(NAME_MANAGEMENT, BASIC) { //-------------------------------------------------------------------------- // @@ -24,9 +24,23 @@ TEST(NAMECHANGES, BASIC) // so he is leaving that for someone else for the time being. // Joints - RevoluteJoint* joint1 = new RevoluteJoint; - RevoluteJoint* joint2 = new RevoluteJoint; - RevoluteJoint* joint3 = new RevoluteJoint; + Joint* joint1 = new RevoluteJoint(Eigen::Vector3d::UnitX(), "joint"); + Joint* joint2 = new TranslationalJoint("joint"); + Joint* joint3 = new FreeJoint("joint"); + + // Testing whether DegreesOfFreedom get named correctly + EXPECT_TRUE(joint1->getDof(0)->getName() == "joint"); + + EXPECT_TRUE(joint2->getDof(0)->getName() == "joint_x"); + EXPECT_TRUE(joint2->getDof(1)->getName() == "joint_y"); + EXPECT_TRUE(joint2->getDof(2)->getName() == "joint_z"); + + EXPECT_TRUE(joint3->getDof(0)->getName() == "joint_rot_x"); + EXPECT_TRUE(joint3->getDof(1)->getName() == "joint_rot_y"); + EXPECT_TRUE(joint3->getDof(2)->getName() == "joint_rot_z"); + EXPECT_TRUE(joint3->getDof(3)->getName() == "joint_pos_x"); + EXPECT_TRUE(joint3->getDof(4)->getName() == "joint_pos_y"); + EXPECT_TRUE(joint3->getDof(5)->getName() == "joint_pos_z"); // Skeleton Skeleton* skel = new Skeleton; @@ -44,15 +58,31 @@ TEST(NAMECHANGES, BASIC) skel->init(); + // Testing whether the repeated names of BodyNodes and Joints get resolved + // correctly as BodyNodes get added to the Skeleton EXPECT_FALSE(body1->getName() == body2->getName()); EXPECT_FALSE(body2->getName() == body3->getName()); EXPECT_FALSE(body3->getName() == body1->getName()); - EXPECT_FALSE(joint1->getName() == joint2->getName()); EXPECT_FALSE(joint2->getName() == joint3->getName()); EXPECT_FALSE(joint3->getName() == joint1->getName()); + EXPECT_TRUE(joint1->getDof(0)->getName() == "joint"); + + EXPECT_TRUE(joint2->getDof(0)->getName() == "joint(1)_x"); + EXPECT_TRUE(joint2->getDof(1)->getName() == "joint(1)_y"); + EXPECT_TRUE(joint2->getDof(2)->getName() == "joint(1)_z"); + + EXPECT_TRUE(joint3->getDof(0)->getName() == "joint(2)_rot_x"); + EXPECT_TRUE(joint3->getDof(1)->getName() == "joint(2)_rot_y"); + EXPECT_TRUE(joint3->getDof(2)->getName() == "joint(2)_rot_z"); + EXPECT_TRUE(joint3->getDof(3)->getName() == "joint(2)_pos_x"); + EXPECT_TRUE(joint3->getDof(4)->getName() == "joint(2)_pos_y"); + EXPECT_TRUE(joint3->getDof(5)->getName() == "joint(2)_pos_z"); + + // Testing whether the repeated names of BodyNodes get resolved correctly + // as they are changed with BodyNode::setName(~) std::string newname1 = body1->setName("same_name"); std::string newname2 = body2->setName("same_name"); std::string newname3 = body3->setName("same_name"); @@ -69,6 +99,8 @@ TEST(NAMECHANGES, BASIC) EXPECT_TRUE(skel->getBodyNode(newname2) == body2); EXPECT_TRUE(skel->getBodyNode(newname3) == body3); + // Testing whether the repeated names of Joints get resolved correctly + // as they are changed with Joint::setName(~) newname1 = joint1->setName("another_name"); newname2 = joint2->setName("another_name"); newname3 = joint3->setName("another_name"); @@ -85,6 +117,7 @@ TEST(NAMECHANGES, BASIC) EXPECT_TRUE(skel->getJoint(newname2) == joint2); EXPECT_TRUE(skel->getJoint(newname3) == joint3); + // Testing whether unique names get accidentally changed by the NameManager std::string unique_name = body2->setName("a_unique_name"); EXPECT_TRUE(body2->getName() == "a_unique_name"); EXPECT_TRUE(skel->getBodyNode("a_unique_name") == body2); @@ -97,10 +130,47 @@ TEST(NAMECHANGES, BASIC) EXPECT_TRUE(joint3->getName() == "a_unique_name"); EXPECT_TRUE(skel->getJoint("a_unique_name") == joint3); + // Testing whether the DegreeOfFreedom names get updated correctly upon their + // joint's name change + EXPECT_TRUE(joint3->getDof(0)->getName() == "a_unique_name_rot_x"); + EXPECT_TRUE(joint3->getDof(1)->getName() == "a_unique_name_rot_y"); + EXPECT_TRUE(joint3->getDof(2)->getName() == "a_unique_name_rot_z"); + EXPECT_TRUE(joint3->getDof(3)->getName() == "a_unique_name_pos_x"); + EXPECT_TRUE(joint3->getDof(4)->getName() == "a_unique_name_pos_y"); + EXPECT_TRUE(joint3->getDof(5)->getName() == "a_unique_name_pos_z"); + + EXPECT_TRUE(joint3->getDof(0) == skel->getDof("a_unique_name_rot_x")); + EXPECT_TRUE(joint3->getDof(3) == skel->getDof("a_unique_name_pos_x")); + + // Note: The following assumes the joint order in the Skeleton is: + // RevoluteJoint -> TranslationalJoint -> FreeJoint + EXPECT_TRUE(joint1->getDof(0) == skel->getDof(0)); + + EXPECT_TRUE(joint2->getDof(0) == skel->getDof(1)); + EXPECT_TRUE(joint2->getDof(1) == skel->getDof(2)); + EXPECT_TRUE(joint2->getDof(2) == skel->getDof(3)); + + EXPECT_TRUE(joint3->getDof(0) == skel->getDof(4)); + EXPECT_TRUE(joint3->getDof(1) == skel->getDof(5)); + EXPECT_TRUE(joint3->getDof(2) == skel->getDof(6)); + EXPECT_TRUE(joint3->getDof(3) == skel->getDof(7)); + EXPECT_TRUE(joint3->getDof(4) == skel->getDof(8)); + EXPECT_TRUE(joint3->getDof(5) == skel->getDof(9)); + + // Test that GenCoordInfos agree with the DegreeOfFreedom classes + for(size_t i=0; igetNumDofs(); ++i) + { + EXPECT_TRUE(skel->getDof(i)->getIndexInSkeleton() == i); + EXPECT_TRUE(skel->getDof(i)->getIndexInJoint() == + skel->getGenCoordInfo(i).localIndex); + } + + // Test whether all the joint names are still unique EXPECT_FALSE(joint1->getName() == joint2->getName()); EXPECT_FALSE(joint2->getName() == joint3->getName()); EXPECT_FALSE(joint3->getName() == joint1->getName()); + // Make sure that the Skeleton gives back NULL for non existent names EXPECT_TRUE(skel->getBodyNode("nonexistent_name") == NULL); EXPECT_TRUE(skel->getJoint("nonexistent_name") == NULL); EXPECT_TRUE(skel->getSoftBodyNode("nonexistent_name") == NULL); @@ -110,10 +180,14 @@ TEST(NAMECHANGES, BASIC) Joint* newJoint = new RevoluteJoint(Eigen::Vector3d(1,0,0), "a_new_joint"); body3->setParentJoint(newJoint); EXPECT_TRUE(skel->getJoint("a_new_joint") == newJoint); + + // Make sure that the Skeleton returns NULL on any Joint names that have been + // taken away from it EXPECT_FALSE(skel->getJoint(oldJointName) == oldJoint); + EXPECT_TRUE(skel->getJoint(oldJointName) == NULL); } -TEST(NAMECHANGES, SETPATTERN) +TEST(NAME_MANAGEMENT, SETPATTERN) { dart::common::NameManager test_mgr; From c11170b4f4cc38389f4c73a281a92614c0cfc3c3 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 31 Dec 2014 18:42:15 -0500 Subject: [PATCH 08/30] Add some comment and fix style --- dart/dynamics/DegreeOfFreedom.cpp | 14 ++++----- dart/dynamics/DegreeOfFreedom.h | 17 ++++++++-- dart/dynamics/EulerJoint.cpp | 12 +++---- dart/dynamics/EulerJoint.h | 6 ++-- dart/dynamics/FreeJoint.cpp | 12 +++---- dart/dynamics/Joint.cpp | 47 +++++++++++++++++----------- dart/dynamics/Joint.h | 21 ++++++++++--- dart/dynamics/MultiDofJoint.h | 6 ++-- dart/dynamics/PlanarJoint.cpp | 22 ++++++------- dart/dynamics/PlanarJoint.h | 31 +++++++++++------- dart/dynamics/SingleDofJoint.cpp | 7 +++-- dart/dynamics/SingleDofJoint.h | 1 + dart/dynamics/Skeleton.cpp | 43 +++++++++++++------------ dart/dynamics/Skeleton.h | 4 +-- dart/dynamics/TranslationalJoint.cpp | 6 ++-- dart/dynamics/UniversalJoint.cpp | 4 +-- unittests/testNameManagement.cpp | 12 +++---- 17 files changed, 156 insertions(+), 109 deletions(-) diff --git a/dart/dynamics/DegreeOfFreedom.cpp b/dart/dynamics/DegreeOfFreedom.cpp index b794979cd5ba0..38875e53c51db 100644 --- a/dart/dynamics/DegreeOfFreedom.cpp +++ b/dart/dynamics/DegreeOfFreedom.cpp @@ -44,11 +44,11 @@ namespace dynamics { //============================================================================== const std::string& DegreeOfFreedom::setName(const std::string& _name) { - if(mName == _name) + if (mName == _name) return mName; Skeleton* skel = mJoint->getSkeleton(); - if(skel) + if (skel) { skel->mNameMgrForDofs.removeName(mName); mName = _name; @@ -355,11 +355,11 @@ const BodyNode* DegreeOfFreedom::getParentBodyNode() const //============================================================================== DegreeOfFreedom::DegreeOfFreedom(Joint* _joint, const std::string& _name, - size_t _indexInJoint) : - mName(_name), - mIndexInJoint(_indexInJoint), - mIndexInSkeleton(0), - mJoint(_joint) + size_t _indexInJoint) + : mName(_name), + mIndexInJoint(_indexInJoint), + mIndexInSkeleton(0), + mJoint(_joint) { } diff --git a/dart/dynamics/DegreeOfFreedom.h b/dart/dynamics/DegreeOfFreedom.h index b1cf0919fcd3a..d071e548f93e5 100644 --- a/dart/dynamics/DegreeOfFreedom.h +++ b/dart/dynamics/DegreeOfFreedom.h @@ -51,7 +51,6 @@ template class MultiDofJoint; /// DegreeOfFreedom class is a proxy class for accessing single degrees of /// freedom (aka generalized coordinates) of the Skeleton. - class DegreeOfFreedom { public: @@ -227,7 +226,21 @@ class DegreeOfFreedom const std::string& _name, size_t _indexInJoint); - /// Name of this DegreeOfFreedom + /// \brief Name of this DegreeOfFreedom + /// + /// DegreeOfFreedom's name will be automatically given by the joint it belongs + /// to. Below is the naming policy: + /// - SingleDofJoint \n + /// Same name as the joint it belongs to. + /// - MultiDofJoint \n + /// "[Joint_name]+[affix]" is used. The affix is determined according + /// to the role they play in the joint. For example, suppose there's a + /// TranslationalJoint named "trans_joint". Then the each dof to be + /// named "trans_joint_x", "trans_joint_y", and "trans_joint_z". + /// - ZeroDofJoint \n + /// ZeroDofJoint doesn't have dof. + /// + /// The default name can be renamed by setName() as well. std::string mName; /// Index of this DegreeOfFreedom within its Joint diff --git a/dart/dynamics/EulerJoint.cpp b/dart/dynamics/EulerJoint.cpp index 20895e8aae8f3..ff2d94123e222 100644 --- a/dart/dynamics/EulerJoint.cpp +++ b/dart/dynamics/EulerJoint.cpp @@ -60,10 +60,10 @@ EulerJoint::~EulerJoint() } //============================================================================== -void EulerJoint::setAxisOrder(EulerJoint::AxisOrder _order, bool _rename_dofs) +void EulerJoint::setAxisOrder(EulerJoint::AxisOrder _order, bool _renameDofs) { mAxisOrder = _order; - if(_rename_dofs) + if (_renameDofs) updateDegreeOfFreedomNames(); } @@ -77,7 +77,7 @@ EulerJoint::AxisOrder EulerJoint::getAxisOrder() const void EulerJoint::updateDegreeOfFreedomNames() { std::vector affixes; - switch(mAxisOrder) + switch (mAxisOrder) { case AO_ZYX: affixes.push_back("_z"); @@ -104,10 +104,10 @@ void EulerJoint::updateDegreeOfFreedomNames() << "' (" << mAxisOrder << ")\n"; } - if(affixes.size() == 3) + if (affixes.size() == 3) { - for(size_t i=0; i<3; ++i) - mDofs[i]->setName(mName+affixes[i]); + for (size_t i = 0; i < 3; ++i) + mDofs[i]->setName(mName + affixes[i]); } } diff --git a/dart/dynamics/EulerJoint.h b/dart/dynamics/EulerJoint.h index a999d0025fd43..3fc9e7188a381 100644 --- a/dart/dynamics/EulerJoint.h +++ b/dart/dynamics/EulerJoint.h @@ -63,8 +63,10 @@ class EulerJoint : public MultiDofJoint<3> /// Destructor virtual ~EulerJoint(); - /// - void setAxisOrder(AxisOrder _order, bool _rename_dofs=true); + /// \brief Set the axis order + /// \param[in] _renameDofs If true, the names of dofs in this joint will be + /// renmaed according to the axis order. + void setAxisOrder(AxisOrder _order, bool _renameDofs = true); /// AxisOrder getAxisOrder() const; diff --git a/dart/dynamics/FreeJoint.cpp b/dart/dynamics/FreeJoint.cpp index 5672fbe7183c8..bc547db625e25 100644 --- a/dart/dynamics/FreeJoint.cpp +++ b/dart/dynamics/FreeJoint.cpp @@ -71,12 +71,12 @@ void FreeJoint::integratePositions(double _dt) //============================================================================== void FreeJoint::updateDegreeOfFreedomNames() { - mDofs[0]->setName(mName+"_rot_x"); - mDofs[1]->setName(mName+"_rot_y"); - mDofs[2]->setName(mName+"_rot_z"); - mDofs[3]->setName(mName+"_pos_x"); - mDofs[4]->setName(mName+"_pos_y"); - mDofs[5]->setName(mName+"_pos_z"); + mDofs[0]->setName(mName + "_rot_x"); + mDofs[1]->setName(mName + "_rot_y"); + mDofs[2]->setName(mName + "_rot_z"); + mDofs[3]->setName(mName + "_pos_x"); + mDofs[4]->setName(mName + "_pos_y"); + mDofs[5]->setName(mName + "_pos_z"); } //============================================================================== diff --git a/dart/dynamics/Joint.cpp b/dart/dynamics/Joint.cpp index 4aa378c9c36b0..281bf14d27bd3 100644 --- a/dart/dynamics/Joint.cpp +++ b/dart/dynamics/Joint.cpp @@ -43,8 +43,8 @@ #include "dart/math/Helpers.h" #include "dart/renderer/RenderInterface.h" #include "dart/dynamics/BodyNode.h" -#include "dart/dynamics/Skeleton.h" #include "dart/dynamics/DegreeOfFreedom.h" +#include "dart/dynamics/Skeleton.h" namespace dart { namespace dynamics { @@ -64,20 +64,21 @@ Joint::Joint(const std::string& _name) } //============================================================================== -Joint::~Joint() { +Joint::~Joint() +{ } //============================================================================== -const std::string& Joint::setName(const std::string& _name, bool _rename_dofs) { - - if(mName == _name) +const std::string& Joint::setName(const std::string& _name, bool _renameDofs) +{ + if (mName == _name) { - if(_rename_dofs) + if (_renameDofs) updateDegreeOfFreedomNames(); return mName; } - if(mSkeleton) + if (mSkeleton) { mSkeleton->mNameMgrForJoints.removeName(mName); mName = _name; @@ -88,14 +89,15 @@ const std::string& Joint::setName(const std::string& _name, bool _rename_dofs) { mName = _name; } - if(_rename_dofs) + if (_renameDofs) updateDegreeOfFreedomNames(); return mName; } //============================================================================== -const std::string& Joint::getName() const { +const std::string& Joint::getName() const +{ return mName; } @@ -114,7 +116,7 @@ const BodyNode* Joint::getChildBodyNode() const //============================================================================== BodyNode* Joint::getParentBodyNode() { - if(mChildBodyNode) + if (mChildBodyNode) return mChildBodyNode->getParentBodyNode(); return NULL; @@ -139,7 +141,8 @@ const Skeleton* Joint::getSkeleton() const } //============================================================================== -const Eigen::Isometry3d& Joint::getLocalTransform() const { +const Eigen::Isometry3d& Joint::getLocalTransform() const +{ return mT; } @@ -158,39 +161,47 @@ const Eigen::Isometry3d& Joint::getLocalTransform() const { // return -1; //} -void Joint::setPositionLimited(bool _isPositionLimited) { +//============================================================================== +void Joint::setPositionLimited(bool _isPositionLimited) +{ mIsPositionLimited = _isPositionLimited; } //============================================================================== -bool Joint::isPositionLimited() const { +bool Joint::isPositionLimited() const +{ return mIsPositionLimited; } //============================================================================== -void Joint::setTransformFromParentBodyNode(const Eigen::Isometry3d& _T) { +void Joint::setTransformFromParentBodyNode(const Eigen::Isometry3d& _T) +{ assert(math::verifyTransform(_T)); mT_ParentBodyToJoint = _T; } //============================================================================== -void Joint::setTransformFromChildBodyNode(const Eigen::Isometry3d& _T) { +void Joint::setTransformFromChildBodyNode(const Eigen::Isometry3d& _T) +{ assert(math::verifyTransform(_T)); mT_ChildBodyToJoint = _T; } //============================================================================== -const Eigen::Isometry3d&Joint::getTransformFromParentBodyNode() const { +const Eigen::Isometry3d&Joint::getTransformFromParentBodyNode() const +{ return mT_ParentBodyToJoint; } //============================================================================== -const Eigen::Isometry3d&Joint::getTransformFromChildBodyNode() const { +const Eigen::Isometry3d&Joint::getTransformFromChildBodyNode() const +{ return mT_ChildBodyToJoint; } //============================================================================== -void Joint::applyGLTransform(renderer::RenderInterface* _ri) { +void Joint::applyGLTransform(renderer::RenderInterface* _ri) +{ _ri->transform(mT); } diff --git a/dart/dynamics/Joint.h b/dart/dynamics/Joint.h index 25a63d3ef7380..24bf77c20592d 100644 --- a/dart/dynamics/Joint.h +++ b/dart/dynamics/Joint.h @@ -67,8 +67,14 @@ class Joint /// Destructor virtual ~Joint(); - /// Set joint name - const std::string& setName(const std::string& _name, bool _rename_dofs=true); + /// \brief Set joint name and return the name. + /// \param[in] _renameDofs If true, the names of the joint's degrees of + /// freedom will be updated by calling updateDegreeOfFreedomNames(). + /// + /// If the name is already taken, this will return an altered version which + /// will be used by the Skeleton. Otherwise, return _name. + const std::string& setName(const std::string& _name, + bool _renameDofs = true); /// Get joint name const std::string& getName() const; @@ -399,9 +405,14 @@ class Joint /// Initialize this joint. This function is called by BodyNode::init() virtual void init(Skeleton* _skel); - /// Create a DegreeOfFreedom pointer (because the DegreeOfFreedom class has a - /// protected constructor). You are responsible for memory management of the - /// pointer which is returned. + /// \brief Create a DegreeOfFreedom pointer. + /// \param[in] _name DegreeOfFreedom's name. + /// \param[in] _indexInJoint DegreeOfFreedom's index within the joint. Note + /// that the index should be unique within the joint. + /// + /// DegreeOfFreedom should be created by the Joint because the DegreeOfFreedom + /// class has a protected constructor, and the Joint is responsible for memory + /// management of the pointer which is returned. DegreeOfFreedom* createDofPointer(const std::string& _name, size_t _indexInJoint); diff --git a/dart/dynamics/MultiDofJoint.h b/dart/dynamics/MultiDofJoint.h index 5cb382de805d0..343efca982037 100644 --- a/dart/dynamics/MultiDofJoint.h +++ b/dart/dynamics/MultiDofJoint.h @@ -557,7 +557,7 @@ MultiDofJoint::MultiDofJoint(const std::string& _name) mTotalForce(Eigen::Matrix::Zero()), mTotalImpulse(Eigen::Matrix::Zero()) { - for(size_t i=0; i::getDof() const template DegreeOfFreedom* MultiDofJoint::getDof(size_t _index) { - if(_index < DOF) + if (_index < DOF) return mDofs[_index]; return NULL; } @@ -589,7 +589,7 @@ DegreeOfFreedom* MultiDofJoint::getDof(size_t _index) template const DegreeOfFreedom* MultiDofJoint::getDof(size_t _index) const { - if(_index < DOF) + if (_index < DOF) return mDofs[_index]; return NULL; } diff --git a/dart/dynamics/PlanarJoint.cpp b/dart/dynamics/PlanarJoint.cpp index 9ee553ce3bd6c..76a6dd69443c8 100644 --- a/dart/dynamics/PlanarJoint.cpp +++ b/dart/dynamics/PlanarJoint.cpp @@ -60,45 +60,45 @@ PlanarJoint::~PlanarJoint() } //============================================================================== -void PlanarJoint::setXYPlane(bool _rename_dofs) +void PlanarJoint::setXYPlane(bool _renameDofs) { mPlaneType = PT_XY; mRotAxis = Eigen::Vector3d::UnitZ(); mTransAxis1 = Eigen::Vector3d::UnitX(); mTransAxis2 = Eigen::Vector3d::UnitY(); - if(_rename_dofs) + if (_renameDofs) updateDegreeOfFreedomNames(); } //============================================================================== -void PlanarJoint::setYZPlane(bool _rename_dofs) +void PlanarJoint::setYZPlane(bool _renameDofs) { mPlaneType = PT_YZ; mRotAxis = Eigen::Vector3d::UnitX(); mTransAxis1 = Eigen::Vector3d::UnitY(); mTransAxis2 = Eigen::Vector3d::UnitZ(); - if(_rename_dofs) + if (_renameDofs) updateDegreeOfFreedomNames(); } //============================================================================== -void PlanarJoint::setZXPlane(bool _rename_dofs) +void PlanarJoint::setZXPlane(bool _renameDofs) { mPlaneType = PT_ZX; mRotAxis = Eigen::Vector3d::UnitY(); mTransAxis1 = Eigen::Vector3d::UnitZ(); mTransAxis2 = Eigen::Vector3d::UnitX(); - if(_rename_dofs) + if (_renameDofs) updateDegreeOfFreedomNames(); } //============================================================================== void PlanarJoint::setArbitraryPlane(const Eigen::Vector3d& _transAxis1, const Eigen::Vector3d& _transAxis2, - bool _rename_dofs) + bool _renameDofs) { // Set plane type as arbitrary plane mPlaneType = PT_ARBITRARY; @@ -118,7 +118,7 @@ void PlanarJoint::setArbitraryPlane(const Eigen::Vector3d& _transAxis1, // Rotational axis mRotAxis = (mTransAxis1.cross(mTransAxis2)).normalized(); - if(_rename_dofs) + if (_renameDofs) updateDegreeOfFreedomNames(); } @@ -150,7 +150,7 @@ const Eigen::Vector3d& PlanarJoint::getTranslationalAxis2() const void PlanarJoint::updateDegreeOfFreedomNames() { std::vector affixes; - switch(mPlaneType) + switch (mPlaneType) { case PT_XY: affixes.push_back("_x"); @@ -173,9 +173,9 @@ void PlanarJoint::updateDegreeOfFreedomNames() << "' (" << mPlaneType << ")\n"; } - if(affixes.size() == 2) + if (affixes.size() == 2) { - for(size_t i=0; i<2; ++i) + for (size_t i = 0; i < 2; ++i) mDofs[i]->setName(mName+affixes[i]); } } diff --git a/dart/dynamics/PlanarJoint.h b/dart/dynamics/PlanarJoint.h index 1321b6502f242..ffd77ea11466f 100644 --- a/dart/dynamics/PlanarJoint.h +++ b/dart/dynamics/PlanarJoint.h @@ -68,19 +68,28 @@ class PlanarJoint : public MultiDofJoint<3> /// Destructor virtual ~PlanarJoint(); - /// Set plane type as XY-plane - void setXYPlane(bool _rename_dofs=true); - - /// Set plane type as YZ-plane - void setYZPlane(bool _rename_dofs=true); - - /// Set plane type as ZX-plane - void setZXPlane(bool _rename_dofs=true); - - /// Set plane type as arbitrary plane with two orthogonal translational axes + /// \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. + void setXYPlane(bool _renameDofs = true); + + /// \brief Set plane type as YZ-plane + /// \param[in] _renameDofs If true, the names of dofs in this joint will be + /// renmaed according to the plane type. + void setYZPlane(bool _renameDofs = true); + + /// \brief Set plane type as ZX-plane + /// \param[in] _renameDofs If true, the names of dofs in this joint will be + /// renmaed according to the plane type. + void setZXPlane(bool _renameDofs = true); + + /// \brief Set plane type as arbitrary plane with two orthogonal translational + /// axes + /// \param[in] _renameDofs If true, the names of dofs in this joint will be + /// renmaed according to the plane type. void setArbitraryPlane(const Eigen::Vector3d& _transAxis1, const Eigen::Vector3d& _transAxis2, - bool _rename_dofs=true); + bool _renameDofs = true); /// Return plane type PlaneType getPlaneType() const; diff --git a/dart/dynamics/SingleDofJoint.cpp b/dart/dynamics/SingleDofJoint.cpp index a6dcbac8b7a7a..e971b0499fc6c 100644 --- a/dart/dynamics/SingleDofJoint.cpp +++ b/dart/dynamics/SingleDofJoint.cpp @@ -49,6 +49,7 @@ namespace dynamics { SingleDofJoint::SingleDofJoint(const std::string& _name) : Joint(_name), mIndexInSkeleton(0u), + mDof(createDofPointer(_name, 0)), mPosition(0.0), mPositionLowerLimit(-DART_DBL_INF), mPositionUpperLimit(DART_DBL_INF), @@ -78,7 +79,6 @@ SingleDofJoint::SingleDofJoint(const std::string& _name) mTotalForce(0.0), mTotalImpulse(0.0) { - mDof = createDofPointer(_name, 0); } //============================================================================== @@ -130,7 +130,7 @@ size_t SingleDofJoint::getIndexInSkeleton(size_t _index) const //============================================================================== DegreeOfFreedom* SingleDofJoint::getDof(size_t _index) { - if(0 == _index) + if (0 == _index) return mDof; return NULL; } @@ -138,7 +138,7 @@ DegreeOfFreedom* SingleDofJoint::getDof(size_t _index) //============================================================================== const DegreeOfFreedom* SingleDofJoint::getDof(size_t _index) const { - if(0 == _index) + if (0 == _index) return mDof; return NULL; } @@ -726,6 +726,7 @@ double SingleDofJoint::getPotentialEnergy() const //============================================================================== void SingleDofJoint::updateDegreeOfFreedomNames() { + // Same name as the joint it belongs to. mDof->setName(mName); } diff --git a/dart/dynamics/SingleDofJoint.h b/dart/dynamics/SingleDofJoint.h index 3c229f68f911c..8217ae0c3a858 100644 --- a/dart/dynamics/SingleDofJoint.h +++ b/dart/dynamics/SingleDofJoint.h @@ -392,6 +392,7 @@ class SingleDofJoint : public Joint /// size_t mIndexInSkeleton; + /// \brief DegreeOfFreedom pointer DegreeOfFreedom* mDof; //---------------------------------------------------------------------------- diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index 587601e12c061..3181d16f86298 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -46,11 +46,11 @@ #include "dart/math/Geometry.h" #include "dart/math/Helpers.h" #include "dart/dynamics/BodyNode.h" -#include "dart/dynamics/SoftBodyNode.h" -#include "dart/dynamics/PointMass.h" +#include "dart/dynamics/DegreeOfFreedom.h" #include "dart/dynamics/Joint.h" #include "dart/dynamics/Marker.h" -#include "dart/dynamics/DegreeOfFreedom.h" +#include "dart/dynamics/PointMass.h" +#include "dart/dynamics/SoftBodyNode.h" namespace dart { namespace dynamics { @@ -124,11 +124,10 @@ const std::string& Skeleton::addEntryToJointNameMgr(Joint* _newJoint) } //============================================================================== -const std::string& Skeleton::addEntryToDofNameMgr(DegreeOfFreedom *_newDof) +const std::string& Skeleton::addEntryToDofNameMgr(DegreeOfFreedom* _newDof) { _newDof->mName = mNameMgrForDofs.issueNewNameAndAdd(_newDof->getName(), _newDof); - return _newDof->mName; } @@ -144,14 +143,14 @@ void Skeleton::addEntryToSoftBodyNodeNameMgr(SoftBodyNode* _newNode) //============================================================================== void Skeleton::addMarkersOfBodyNode(BodyNode* _node) { - for(size_t i=0; i<_node->getNumMarkers(); ++i) + for (size_t i=0; i<_node->getNumMarkers(); ++i) addEntryToMarkerNameMgr(_node->getMarker(i)); } //============================================================================== void Skeleton::removeMarkersOfBodyNode(BodyNode* _node) { - for(size_t i=0; i<_node->getNumMarkers(); ++i) + for (size_t i=0; i<_node->getNumMarkers(); ++i) mNameMgrForMarkers.removeName(_node->getMarker(i)->getName()); } @@ -272,7 +271,7 @@ size_t Skeleton::getNumSoftBodyNodes() const //============================================================================== BodyNode* Skeleton::getRootBodyNode() { - if(mBodyNodes.size()==0) + if (mBodyNodes.size()==0) return NULL; // We assume that the first element of body nodes is root. @@ -290,7 +289,7 @@ template static T getVectorObjectIfAvailable(size_t _idx, const std::vector& _vec) { // TODO: Should we have an out-of-bounds assertion or throw here? - if(_idx < _vec.size()) + if (_idx < _vec.size()) return _vec[_idx]; return NULL; @@ -347,7 +346,7 @@ const SoftBodyNode* Skeleton::getSoftBodyNode(const std::string& _name) const Joint* Skeleton::getJoint(size_t _idx) { BodyNode* bn = getVectorObjectIfAvailable(_idx, mBodyNodes); - if(bn) + if (bn) return bn->getParentJoint(); return NULL; @@ -376,7 +375,7 @@ DegreeOfFreedom* Skeleton::getDof(size_t _idx) { assert(_idx < getNumDofs()); - if(_idx >= getNumDofs()) + if (_idx >= getNumDofs()) return NULL; return mGenCoordInfos[_idx].joint->getDof(mGenCoordInfos[_idx].localIndex); @@ -387,22 +386,22 @@ const DegreeOfFreedom* Skeleton::getDof(size_t _idx) const { assert(_idx < getNumDofs()); - if(_idx >= getNumDofs()) + if (_idx >= getNumDofs()) return NULL; return mGenCoordInfos[_idx].joint->getDof(mGenCoordInfos[_idx].localIndex); } //============================================================================== -DegreeOfFreedom* Skeleton::getDof(const std::string &name) +DegreeOfFreedom* Skeleton::getDof(const std::string& _name) { - return mNameMgrForDofs.getObject(name); + return mNameMgrForDofs.getObject(_name); } //============================================================================== -const DegreeOfFreedom* Skeleton::getDof(const std::string &name) const +const DegreeOfFreedom* Skeleton::getDof(const std::string& _name) const { - return mNameMgrForDofs.getObject(name); + return mNameMgrForDofs.getObject(_name); } //============================================================================== @@ -1261,9 +1260,9 @@ void Skeleton::drawMarkers(renderer::RenderInterface* _ri, } //============================================================================== -void Skeleton::registerJoint(Joint *_newJoint) +void Skeleton::registerJoint(Joint* _newJoint) { - if(NULL == _newJoint) + if (NULL == _newJoint) { dterr << "[Skeleton::registerJoint] Error: Attempting to add a NULL joint " "to the Skeleton named '" << mName << "'!\n"; @@ -1273,7 +1272,7 @@ void Skeleton::registerJoint(Joint *_newJoint) addEntryToJointNameMgr(_newJoint); _newJoint->mSkeleton = this; - for(size_t i=0; i<_newJoint->getNumDofs(); ++i) + for (size_t i = 0; i < _newJoint->getNumDofs(); ++i) { DegreeOfFreedom* dof = _newJoint->getDof(i); dof->mName = mNameMgrForDofs.issueNewNameAndAdd(dof->getName(), dof); @@ -1281,14 +1280,14 @@ void Skeleton::registerJoint(Joint *_newJoint) } //============================================================================== -void Skeleton::unregisterJoint(Joint *_oldJoint) +void Skeleton::unregisterJoint(Joint* _oldJoint) { - if(NULL == _oldJoint) + if (NULL == _oldJoint) return; mNameMgrForJoints.removeName(_oldJoint->getName()); - for(size_t i=0; i<_oldJoint->getNumDofs(); ++i) + for (size_t i = 0; i < _oldJoint->getNumDofs(); ++i) { DegreeOfFreedom* dof = _oldJoint->getDof(i); mNameMgrForDofs.removeName(dof->getName()); diff --git a/dart/dynamics/Skeleton.h b/dart/dynamics/Skeleton.h index 3bd8b4c770651..13f71f417c77e 100644 --- a/dart/dynamics/Skeleton.h +++ b/dart/dynamics/Skeleton.h @@ -195,10 +195,10 @@ class Skeleton const DegreeOfFreedom* getDof(size_t _idx) const; /// Get degree of freedom (aka generalized coordinate) whose name is _name - DegreeOfFreedom* getDof(const std::string& name); + DegreeOfFreedom* getDof(const std::string& _name); /// Get degree of freedom (aka generalized coordinate) whose name is _name - const DegreeOfFreedom* getDof(const std::string& name) const; + const DegreeOfFreedom* getDof(const std::string& _name) const; /// Get const joint whose name is _name const Joint* getJoint(const std::string& _name) const; diff --git a/dart/dynamics/TranslationalJoint.cpp b/dart/dynamics/TranslationalJoint.cpp index 5c6e0b55f1ed9..e820909b25473 100644 --- a/dart/dynamics/TranslationalJoint.cpp +++ b/dart/dynamics/TranslationalJoint.cpp @@ -59,9 +59,9 @@ TranslationalJoint::~TranslationalJoint() //============================================================================== void TranslationalJoint::updateDegreeOfFreedomNames() { - mDofs[0]->setName(mName+"_x"); - mDofs[1]->setName(mName+"_y"); - mDofs[2]->setName(mName+"_z"); + mDofs[0]->setName(mName + "_x"); + mDofs[1]->setName(mName + "_y"); + mDofs[2]->setName(mName + "_z"); } //============================================================================== diff --git a/dart/dynamics/UniversalJoint.cpp b/dart/dynamics/UniversalJoint.cpp index 3a38e683e2d4d..33a4ecdf6e3e0 100644 --- a/dart/dynamics/UniversalJoint.cpp +++ b/dart/dynamics/UniversalJoint.cpp @@ -88,8 +88,8 @@ const Eigen::Vector3d& UniversalJoint::getAxis2() const //============================================================================== void UniversalJoint::updateDegreeOfFreedomNames() { - mDofs[0]->setName(mName+"_1"); - mDofs[1]->setName(mName+"_2"); + mDofs[0]->setName(mName + "_1"); + mDofs[1]->setName(mName + "_2"); } //============================================================================== diff --git a/unittests/testNameManagement.cpp b/unittests/testNameManagement.cpp index 47803f8da3326..bf3e52b7d2b20 100644 --- a/unittests/testNameManagement.cpp +++ b/unittests/testNameManagement.cpp @@ -8,8 +8,8 @@ using namespace dart; using namespace math; using namespace dynamics; - -TEST(NAME_MANAGEMENT, BASIC) +//============================================================================== +TEST(NameManagement, Basic) { //-------------------------------------------------------------------------- // @@ -158,7 +158,7 @@ TEST(NAME_MANAGEMENT, BASIC) EXPECT_TRUE(joint3->getDof(5) == skel->getDof(9)); // Test that GenCoordInfos agree with the DegreeOfFreedom classes - for(size_t i=0; igetNumDofs(); ++i) + for (size_t i = 0; i < skel->getNumDofs(); ++i) { EXPECT_TRUE(skel->getDof(i)->getIndexInSkeleton() == i); EXPECT_TRUE(skel->getDof(i)->getIndexInJoint() == @@ -187,7 +187,8 @@ TEST(NAME_MANAGEMENT, BASIC) EXPECT_TRUE(skel->getJoint(oldJointName) == NULL); } -TEST(NAME_MANAGEMENT, SETPATTERN) +//============================================================================== +TEST(NameManagement, SetPattern) { dart::common::NameManager test_mgr; @@ -223,10 +224,9 @@ TEST(NAME_MANAGEMENT, SETPATTERN) delete bn0; delete bn1; delete bn2; - } - +//============================================================================== int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv); From 0bfcdbedbca3523508ebc0acd7a87ce0df777b6d Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 31 Dec 2014 18:57:56 -0500 Subject: [PATCH 09/30] Remove Joint::mIndexInSkeleton which is replaced by DegreeOfFreedom::mIndexInSkeleton --- dart/dynamics/DegreeOfFreedom.h | 5 ++++- dart/dynamics/MultiDofJoint.h | 15 ++++----------- dart/dynamics/SingleDofJoint.cpp | 9 +++------ dart/dynamics/SingleDofJoint.h | 4 ---- 4 files changed, 11 insertions(+), 22 deletions(-) diff --git a/dart/dynamics/DegreeOfFreedom.h b/dart/dynamics/DegreeOfFreedom.h index d071e548f93e5..9837e4b2b869b 100644 --- a/dart/dynamics/DegreeOfFreedom.h +++ b/dart/dynamics/DegreeOfFreedom.h @@ -243,7 +243,10 @@ class DegreeOfFreedom /// The default name can be renamed by setName() as well. std::string mName; - /// Index of this DegreeOfFreedom within its Joint + /// \brief Index of this DegreeOfFreedom within its Joint + /// + /// The index is determined when this DegreeOfFreedom is created by the Joint + /// it belongs to. Note that the index should be unique within the Joint. size_t mIndexInJoint; /// Index of this DegreeOfFreedom within its Skeleton diff --git a/dart/dynamics/MultiDofJoint.h b/dart/dynamics/MultiDofJoint.h index 343efca982037..51b620bf47e97 100644 --- a/dart/dynamics/MultiDofJoint.h +++ b/dart/dynamics/MultiDofJoint.h @@ -390,10 +390,6 @@ class MultiDofJoint : public Joint const Eigen::Vector6d& _spatial); protected: - // TODO(JS): Need? - /// - Eigen::Matrix mIndexInSkeleton; - // TODO: Replace with std::array when we migrate to C++11 /// Array of DegreeOfFreedom objects DegreeOfFreedom* mDofs[DOF]; @@ -527,7 +523,6 @@ class MultiDofJoint : public Joint template MultiDofJoint::MultiDofJoint(const std::string& _name) : Joint(_name), - mIndexInSkeleton(Eigen::Matrix::Constant(0u)), mPositions(Eigen::Matrix::Constant(0.0)), mPositionLowerLimits(Eigen::Matrix::Constant(-DART_DBL_INF)), mPositionUpperLimits(Eigen::Matrix::Constant(DART_DBL_INF)), @@ -565,7 +560,7 @@ MultiDofJoint::MultiDofJoint(const std::string& _name) template MultiDofJoint::~MultiDofJoint() { - for(size_t i=0; i::setIndexInSkeleton(size_t _index, return; } - mIndexInSkeleton[_index] = _indexInSkeleton; - // TODO(MXG): I think ^this array is redundant now that we have the mDofs array mDofs[_index]->mIndexInSkeleton = _indexInSkeleton; } @@ -629,7 +622,7 @@ size_t MultiDofJoint::getIndexInSkeleton(size_t _index) const return 0; } - return mIndexInSkeleton[_index]; + return mDofs[_index]->mIndexInSkeleton; } //============================================================================== @@ -1611,7 +1604,7 @@ void MultiDofJoint::getInvMassMatrixSegment( assert(!math::isNan(mInvMassMatrixSegment)); // Index - size_t iStart = mIndexInSkeleton[0]; + size_t iStart = mDofs[0]->mIndexInSkeleton; // Assign _invMassMat.block(iStart, _col) = mInvMassMatrixSegment; @@ -1635,7 +1628,7 @@ void MultiDofJoint::getInvAugMassMatrixSegment( assert(!math::isNan(mInvMassMatrixSegment)); // Index - size_t iStart = mIndexInSkeleton[0]; + size_t iStart = mDofs[0]->mIndexInSkeleton; // Assign _invMassMat.block(iStart, _col) = mInvMassMatrixSegment; diff --git a/dart/dynamics/SingleDofJoint.cpp b/dart/dynamics/SingleDofJoint.cpp index e971b0499fc6c..7f4b83b8121cb 100644 --- a/dart/dynamics/SingleDofJoint.cpp +++ b/dart/dynamics/SingleDofJoint.cpp @@ -48,7 +48,6 @@ namespace dynamics { //============================================================================== SingleDofJoint::SingleDofJoint(const std::string& _name) : Joint(_name), - mIndexInSkeleton(0u), mDof(createDofPointer(_name, 0)), mPosition(0.0), mPositionLowerLimit(-DART_DBL_INF), @@ -109,8 +108,6 @@ void SingleDofJoint::setIndexInSkeleton(size_t _index, size_t _indexInSkeleton) return; } - mIndexInSkeleton = _indexInSkeleton; - // TODO(MXG): I think ^this member variable is redundant now that we have mDof mDof->mIndexInSkeleton = _indexInSkeleton; } @@ -124,7 +121,7 @@ size_t SingleDofJoint::getIndexInSkeleton(size_t _index) const return 0; } - return mIndexInSkeleton; + return mDof->mIndexInSkeleton; } //============================================================================== @@ -1034,7 +1031,7 @@ void SingleDofJoint::getInvMassMatrixSegment(Eigen::MatrixXd& _invMassMat, assert(!math::isNan(mInvMassMatrixSegment)); // Index - size_t iStart = mIndexInSkeleton; + size_t iStart = mDof->mIndexInSkeleton; // Assign _invMassMat(iStart, _col) = mInvMassMatrixSegment; @@ -1057,7 +1054,7 @@ void SingleDofJoint::getInvAugMassMatrixSegment( assert(!math::isNan(mInvMassMatrixSegment)); // Index - size_t iStart = mIndexInSkeleton; + size_t iStart = mDof->mIndexInSkeleton; // Assign _invMassMat(iStart, _col) = mInvMassMatrixSegment; diff --git a/dart/dynamics/SingleDofJoint.h b/dart/dynamics/SingleDofJoint.h index 8217ae0c3a858..68b49a3bb364a 100644 --- a/dart/dynamics/SingleDofJoint.h +++ b/dart/dynamics/SingleDofJoint.h @@ -388,10 +388,6 @@ class SingleDofJoint : public Joint const Eigen::Vector6d& _spatial); protected: - // TODO(JS): Need? - /// - size_t mIndexInSkeleton; - /// \brief DegreeOfFreedom pointer DegreeOfFreedom* mDof; From d247b0d9aa44c2f5e8021933c532d177501ad503 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 1 Jan 2015 00:29:37 -0500 Subject: [PATCH 10/30] Deprecate GenCoordInfo struct whos functionality is replaced by DegreeOfFreedom --- dart/dynamics/Skeleton.cpp | 295 +++++++++++++------------------ dart/dynamics/Skeleton.h | 33 ++-- dart/utils/FileInfoDof.cpp | 8 +- unittests/testNameManagement.cpp | 7 +- 4 files changed, 150 insertions(+), 193 deletions(-) diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index 3181d16f86298..2651872c29c63 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -378,7 +378,7 @@ DegreeOfFreedom* Skeleton::getDof(size_t _idx) if (_idx >= getNumDofs()) return NULL; - return mGenCoordInfos[_idx].joint->getDof(mGenCoordInfos[_idx].localIndex); + return mDofs[_idx]; } //============================================================================== @@ -389,7 +389,7 @@ const DegreeOfFreedom* Skeleton::getDof(size_t _idx) const if (_idx >= getNumDofs()) return NULL; - return mGenCoordInfos[_idx].joint->getDof(mGenCoordInfos[_idx].localIndex); + return mDofs[_idx]; } //============================================================================== @@ -449,25 +449,23 @@ void Skeleton::init(double _timeStep, const Eigen::Vector3d& _gravity) } // Initialize body nodes and generalized coordinates - mGenCoordInfos.clear(); + mDofs.clear(); mDof = 0; - for (size_t i = 0; i < getNumBodyNodes(); ++i) + const size_t numBodyNodes = getNumBodyNodes(); + for (size_t i = 0; i < numBodyNodes; ++i) { - Joint* joint = mBodyNodes[i]->getParentJoint(); + BodyNode* bodyNode = mBodyNodes[i]; + Joint* joint = bodyNode->getParentJoint(); - for (size_t j = 0; j < joint->getNumDofs(); ++j) + const size_t numDofsOfJoint = joint->getNumDofs(); + for (size_t j = 0; j < numDofsOfJoint; ++j) { - GenCoordInfo genCoord; - genCoord.joint = joint; - genCoord.localIndex = j; - - mGenCoordInfos.push_back(genCoord); - - joint->setIndexInSkeleton(j, mGenCoordInfos.size() - 1); + mDofs.push_back(joint->getDof(j)); + joint->setIndexInSkeleton(j, mDof + j); } - mBodyNodes[i]->init(this); - mDof += mBodyNodes[i]->getParentJoint()->getNumDofs(); + bodyNode->init(this); + mDof += joint->getNumDofs(); } // Compute transformations, velocities, and partial accelerations @@ -521,8 +519,7 @@ void Skeleton::setPosition(size_t _index, double _position) { assert(_index < getNumDofs()); - mGenCoordInfos[_index].joint->setPosition(mGenCoordInfos[_index].localIndex, - _position); + mDofs[_index]->setPosition(_position); } //============================================================================== @@ -530,28 +527,21 @@ double Skeleton::getPosition(size_t _index) const { assert(_index getPosition( - mGenCoordInfos[_index].localIndex); + return mDofs[_index]->getPosition(); } //============================================================================== -void Skeleton::setPositions(const Eigen::VectorXd& _configs) +void Skeleton::setPositions(const Eigen::VectorXd& _positions) { - size_t index = 0; - size_t dof = 0; - Joint* joint; - - for (size_t i = 0; i < mBodyNodes.size(); ++i) + for (const auto& bodyNode : mBodyNodes) { - joint = mBodyNodes[i]->getParentJoint(); - - dof = joint->getNumDofs(); + Joint* joint = bodyNode->getParentJoint(); + const size_t dof = joint->getNumDofs(); if (dof) { - joint->setPositions(_configs.segment(index, dof)); - - index += dof; + size_t index = joint->getDof(0)->getIndexInSkeleton(); + joint->setPositions(_positions.segment(index, dof)); } } } @@ -559,19 +549,21 @@ void Skeleton::setPositions(const Eigen::VectorXd& _configs) //============================================================================== Eigen::VectorXd Skeleton::getPositions() const { - size_t index = 0; - size_t dof = getNumDofs(); - Eigen::VectorXd pos(dof); + Eigen::VectorXd q(getNumDofs()); - for (size_t i = 0; i < dof; ++i) + for (const auto& bodyNode : mBodyNodes) { - pos[index++] - = mGenCoordInfos[i].joint->getPosition(mGenCoordInfos[i].localIndex); - } + const Joint* joint = bodyNode->getParentJoint(); + const size_t dof = joint->getNumDofs(); - assert(index == dof); + if (dof) + { + size_t index = joint->getDof(0)->getIndexInSkeleton(); + q.segment(index, dof) = joint->getPositions(); + } + } - return pos; + return q; } //============================================================================== @@ -580,16 +572,8 @@ Eigen::VectorXd Skeleton::getPositionSegment( { Eigen::VectorXd q(_id.size()); - Joint* joint; - size_t localIndex; - for (size_t i = 0; i < _id.size(); ++i) - { - joint = mGenCoordInfos[_id[i]].joint; - localIndex = mGenCoordInfos[_id[i]].localIndex; - - q[i] = joint->getPosition(localIndex); - } + q[i] = mDofs[_id[i]]->getPosition(); return q; } @@ -598,16 +582,8 @@ Eigen::VectorXd Skeleton::getPositionSegment( void Skeleton::setPositionSegment(const std::vector& _id, const Eigen::VectorXd& _positions) { - Joint* joint; - size_t localIndex; - for (size_t i = 0; i < _id.size(); ++i) - { - joint = mGenCoordInfos[_id[i]].joint; - localIndex = mGenCoordInfos[_id[i]].localIndex; - - joint->setPosition(localIndex, _positions[i]); - } + mDofs[_id[i]]->setPosition(_positions[i]); } //============================================================================== @@ -622,8 +598,7 @@ void Skeleton::setPositionLowerLimit(size_t _index, double _position) { assert(_index < getNumDofs()); - mGenCoordInfos[_index].joint->setPositionLowerLimit( - mGenCoordInfos[_index].localIndex, _position); + mDofs[_index]->setPositionLowerLimit(_position); } //============================================================================== @@ -631,8 +606,7 @@ double Skeleton::getPositionLowerLimit(size_t _index) { assert(_index getPositionLowerLimit( - mGenCoordInfos[_index].localIndex); + return mDofs[_index]->getPositionLowerLimit(); } //============================================================================== @@ -640,8 +614,7 @@ void Skeleton::setPositionUpperLimit(size_t _index, double _position) { assert(_index < getNumDofs()); - mGenCoordInfos[_index].joint->setPositionUpperLimit( - mGenCoordInfos[_index].localIndex, _position); + mDofs[_index]->setPositionUpperLimit(_position); } //============================================================================== @@ -649,8 +622,7 @@ double Skeleton::getPositionUpperLimit(size_t _index) { assert(_index getPositionUpperLimit( - mGenCoordInfos[_index].localIndex); + return mDofs[_index]->getPositionUpperLimit(); } //============================================================================== @@ -658,8 +630,7 @@ void Skeleton::setVelocity(size_t _index, double _velocity) { assert(_index < getNumDofs()); - mGenCoordInfos[_index].joint->setVelocity(mGenCoordInfos[_index].localIndex, - _velocity); + mDofs[_index]->setVelocity(_velocity); } //============================================================================== @@ -667,28 +638,21 @@ double Skeleton::getVelocity(size_t _index) const { assert(_index getVelocity( - mGenCoordInfos[_index].localIndex); + return mDofs[_index]->getVelocity(); } //============================================================================== -void Skeleton::setVelocities(const Eigen::VectorXd& _genVels) +void Skeleton::setVelocities(const Eigen::VectorXd& _velocities) { - size_t index = 0; - size_t dof = 0; - Joint* joint; - - for (size_t i = 0; i < mBodyNodes.size(); ++i) + for (const auto& bodyNode : mBodyNodes) { - joint = mBodyNodes[i]->getParentJoint(); - - dof = joint->getNumDofs(); + Joint* joint = bodyNode->getParentJoint(); + const size_t dof = joint->getNumDofs(); if (dof) { - joint->setVelocities(_genVels.segment(index, dof)); - - index += dof; + size_t index = joint->getDof(0)->getIndexInSkeleton(); + joint->setVelocities(_velocities.segment(index, dof)); } } } @@ -696,19 +660,21 @@ void Skeleton::setVelocities(const Eigen::VectorXd& _genVels) //============================================================================== Eigen::VectorXd Skeleton::getVelocities() const { - size_t index = 0; - size_t dof = getNumDofs(); - Eigen::VectorXd vel(dof); + Eigen::VectorXd dq(getNumDofs()); - for (size_t i = 0; i < dof; ++i) + for (const auto& bodyNode : mBodyNodes) { - vel[index++] - = mGenCoordInfos[i].joint->getVelocity(mGenCoordInfos[i].localIndex); - } + const Joint* joint = bodyNode->getParentJoint(); + const size_t dof = joint->getNumDofs(); - assert(index == dof); + if (dof) + { + size_t index = joint->getDof(0)->getIndexInSkeleton(); + dq.segment(index, dof) = joint->getVelocities(); + } + } - return vel; + return dq; } //============================================================================== @@ -723,8 +689,7 @@ void Skeleton::setVelocityLowerLimit(size_t _index, double _velocity) { assert(_index < getNumDofs()); - mGenCoordInfos[_index].joint->setVelocityLowerLimit( - mGenCoordInfos[_index].localIndex, _velocity); + mDofs[_index]->setVelocityLowerLimit(_velocity); } //============================================================================== @@ -732,8 +697,7 @@ double Skeleton::getVelocityLowerLimit(size_t _index) { assert(_index getVelocityLowerLimit( - mGenCoordInfos[_index].localIndex); + return mDofs[_index]->getVelocityLowerLimit(); } //============================================================================== @@ -741,8 +705,7 @@ void Skeleton::setVelocityUpperLimit(size_t _index, double _velocity) { assert(_index < getNumDofs()); - mGenCoordInfos[_index].joint->setVelocityUpperLimit( - mGenCoordInfos[_index].localIndex, _velocity); + mDofs[_index]->setVelocityUpperLimit(_velocity); } //============================================================================== @@ -750,8 +713,7 @@ double Skeleton::getVelocityUpperLimit(size_t _index) { assert(_index getVelocityUpperLimit( - mGenCoordInfos[_index].localIndex); + return mDofs[_index]->getVelocityUpperLimit(); } //============================================================================== @@ -759,8 +721,7 @@ void Skeleton::setAcceleration(size_t _index, double _acceleration) { assert(_index < getNumDofs()); - mGenCoordInfos[_index].joint->setAcceleration( - mGenCoordInfos[_index].localIndex, _acceleration); + mDofs[_index]->setAcceleration(_acceleration); } //============================================================================== @@ -768,28 +729,21 @@ double Skeleton::getAcceleration(size_t _index) const { assert(_index getAcceleration( - mGenCoordInfos[_index].localIndex); + return mDofs[_index]->getAcceleration(); } //============================================================================== void Skeleton::setAccelerations(const Eigen::VectorXd& _accelerations) { - size_t index = 0; - size_t dof = 0; - Joint* joint; - - for (size_t i = 0; i < mBodyNodes.size(); ++i) + for (const auto& bodyNode : mBodyNodes) { - joint = mBodyNodes[i]->getParentJoint(); - - dof = joint->getNumDofs(); + Joint* joint = bodyNode->getParentJoint(); + const size_t dof = joint->getNumDofs(); if (dof) { + size_t index = joint->getDof(0)->getIndexInSkeleton(); joint->setAccelerations(_accelerations.segment(index, dof)); - - index += dof; } } } @@ -797,19 +751,21 @@ void Skeleton::setAccelerations(const Eigen::VectorXd& _accelerations) //============================================================================== Eigen::VectorXd Skeleton::getAccelerations() const { - size_t index = 0; - size_t dof = getNumDofs(); - Eigen::VectorXd acc(dof); + Eigen::VectorXd ddq(getNumDofs()); - for (size_t i = 0; i < dof; ++i) + for (const auto& bodyNode : mBodyNodes) { - acc[index++] - = mGenCoordInfos[i].joint->getAcceleration(mGenCoordInfos[i].localIndex); - } + const Joint* joint = bodyNode->getParentJoint(); + const size_t dof = joint->getNumDofs(); - assert(index == dof); + if (dof) + { + size_t index = joint->getDof(0)->getIndexInSkeleton(); + ddq.segment(index, dof) = joint->getAccelerations(); + } + } - return acc; + return ddq; } //============================================================================== @@ -824,8 +780,7 @@ void Skeleton::setForce(size_t _index, double _force) { assert(_index < getNumDofs()); - mGenCoordInfos[_index].joint->setForce( - mGenCoordInfos[_index].localIndex, _force); + mDofs[_index]->setEffort(_force); } //============================================================================== @@ -833,28 +788,21 @@ double Skeleton::getForce(size_t _index) { assert(_index getForce( - mGenCoordInfos[_index].localIndex); + return mDofs[_index]->getEffort(); } //============================================================================== void Skeleton::setForces(const Eigen::VectorXd& _forces) { - size_t index = 0; - size_t dof = 0; - Joint* joint; - - for (size_t i = 0; i < mBodyNodes.size(); ++i) + for (const auto& bodyNode : mBodyNodes) { - joint = mBodyNodes[i]->getParentJoint(); - - dof = joint->getNumDofs(); + Joint* joint = bodyNode->getParentJoint(); + const size_t dof = joint->getNumDofs(); if (dof) { + size_t index = joint->getDof(0)->getIndexInSkeleton(); joint->setForces(_forces.segment(index, dof)); - - index += dof; } } } @@ -862,19 +810,21 @@ void Skeleton::setForces(const Eigen::VectorXd& _forces) //============================================================================== Eigen::VectorXd Skeleton::getForces() const { - size_t index = 0; - size_t dof = getNumDofs(); - Eigen::VectorXd force(dof); + Eigen::VectorXd forces(getNumDofs()); - for (size_t i = 0; i < dof; ++i) + for (const auto& bodyNode : mBodyNodes) { - force[index++] - = mGenCoordInfos[i].joint->getForce(mGenCoordInfos[i].localIndex); - } + const Joint* joint = bodyNode->getParentJoint(); + const size_t dof = joint->getNumDofs(); - assert(index == dof); + if (dof) + { + size_t index = joint->getDof(0)->getIndexInSkeleton(); + forces.segment(index, dof) = joint->getForces(); + } + } - return force; + return forces; } //============================================================================== @@ -887,9 +837,7 @@ void Skeleton::resetForces() for (size_t i = 0; i < mSoftBodyNodes.size(); ++i) { for (size_t j = 0; j < mSoftBodyNodes[i]->getNumPointMasses(); ++j) - { mSoftBodyNodes[i]->getPointMass(j)->resetForces(); - } } } @@ -898,8 +846,7 @@ void Skeleton::setForceLowerLimit(size_t _index, double _force) { assert(_index < getNumDofs()); - mGenCoordInfos[_index].joint->setForceLowerLimit( - mGenCoordInfos[_index].localIndex, _force); + mDofs[_index]->setEffortLowerLimit(_force); } //============================================================================== @@ -907,8 +854,7 @@ double Skeleton::getForceLowerLimit(size_t _index) { assert(_index getForceLowerLimit( - mGenCoordInfos[_index].localIndex); + return mDofs[_index]->getEffortLowerLimit(); } //============================================================================== @@ -916,8 +862,7 @@ void Skeleton::setForceUpperLimit(size_t _index, double _force) { assert(_index < getNumDofs()); - mGenCoordInfos[_index].joint->setForceUpperLimit( - mGenCoordInfos[_index].localIndex, _force); + mDofs[_index]->setEffortUpperLimit(_force); } //============================================================================== @@ -925,21 +870,22 @@ double Skeleton::getForceUpperLimit(size_t _index) { assert(_index getForceUpperLimit( - mGenCoordInfos[_index].localIndex); + return mDofs[_index]->getVelocityUpperLimit(); } //============================================================================== Eigen::VectorXd Skeleton::getVelocityChanges() const { - size_t index = 0; - size_t dof = getNumDofs(); + const size_t dof = getNumDofs(); Eigen::VectorXd velChange(dof); + size_t index = 0; for (size_t i = 0; i < dof; ++i) { - velChange[index++] = mGenCoordInfos[i].joint->getVelocityChange( - mGenCoordInfos[i].localIndex); + Joint* joint = mDofs[i]->getJoint(); + size_t localIndex = mDofs[i]->getIndexInJoint(); + + velChange[index++] = joint->getVelocityChange(localIndex); } assert(index == dof); @@ -956,16 +902,18 @@ void Skeleton::setConstraintImpulses(const Eigen::VectorXd& _impulses) //============================================================================== void Skeleton::setJointConstraintImpulses(const Eigen::VectorXd& _impulses) { - size_t index = 0; - size_t dof = getNumDofs(); + const size_t dof = getNumDofs(); + size_t index = 0; for (size_t i = 0; i < dof; ++i) { - mGenCoordInfos[i].joint->setConstraintImpulse( - mGenCoordInfos[i].localIndex, _impulses[index++]); + Joint* joint = mDofs[i]->getJoint(); + size_t localIndex = mDofs[i]->getIndexInJoint(); + + joint->setConstraintImpulse(localIndex, _impulses[index++]); } - assert(index == getNumDofs()); + assert(index == dof); } //============================================================================== @@ -977,15 +925,16 @@ Eigen::VectorXd Skeleton::getConstraintImpulses() const //============================================================================== Eigen::VectorXd Skeleton::getJointConstraintImpulses() const { - size_t index = 0; - size_t dof = getNumDofs(); + const size_t dof = getNumDofs(); Eigen::VectorXd impulse(dof); + size_t index = 0; for (size_t i = 0; i < dof; ++i) { - impulse[index++] - = mGenCoordInfos[i].joint->getConstraintImpulse( - mGenCoordInfos[i].localIndex); + Joint* joint = mDofs[i]->getJoint(); + size_t localIndex = mDofs[i]->getIndexInJoint(); + + impulse[index++] = joint->getConstraintImpulse(localIndex); } assert(index == dof); @@ -1181,7 +1130,7 @@ const Eigen::VectorXd& Skeleton::getExternalForces() //============================================================================== const Eigen::VectorXd& Skeleton::getConstraintForces() { - size_t dof = getNumDofs(); + const size_t dof = getNumDofs(); mFc = Eigen::VectorXd::Zero(dof); // Body constraint impulses @@ -1195,8 +1144,10 @@ const Eigen::VectorXd& Skeleton::getConstraintForces() size_t index = 0; for (size_t i = 0; i < dof; ++i) { - mFc[index++] += mGenCoordInfos[i].joint->getConstraintImpulse( - mGenCoordInfos[i].localIndex); + Joint* joint = mDofs[i]->getJoint(); + size_t localIndex = mDofs[i]->getIndexInJoint(); + + mFc[index++] += joint->getConstraintImpulse(localIndex); } assert(index == dof); diff --git a/dart/dynamics/Skeleton.h b/dart/dynamics/Skeleton.h index 13f71f417c77e..7450bd9248056 100644 --- a/dart/dynamics/Skeleton.h +++ b/dart/dynamics/Skeleton.h @@ -68,7 +68,7 @@ struct GenCoordInfo { Joint* joint; size_t localIndex; -}; +} DEPRECATED(4.3); /// class Skeleton class Skeleton @@ -226,7 +226,12 @@ class Skeleton /// Return degrees of freedom of this skeleton size_t getNumDofs() const; - /// + /// \brief Return _index-th GenCoordInfo + /// \warning GenCoordInfo is deprecated so this function is not necessary + /// anymore. Please use DegreeOfFreedom by calling getDof(). We will keep this + /// function until the next major version up only for backward compatibility + /// in minor version ups. + DEPRECATED(4.3) GenCoordInfo getGenCoordInfo(size_t _index) const; //---------------------------------------------------------------------------- @@ -239,11 +244,10 @@ class Skeleton /// Get a single position double getPosition(size_t _index) const; - /// Set configurations defined in terms of generalized coordinates and update - /// Cartesian terms of body nodes using following parameters. + /// Set generalized positions void setPositions(const Eigen::VectorXd& _positions); - /// Get positions + /// Get generalized positions Eigen::VectorXd getPositions() const; /// Set the configuration of this skeleton described in generalized @@ -282,11 +286,9 @@ class Skeleton double getVelocity(size_t _index) const; /// Set generalized velocities - /// \param[in] _updateVels True to update spacial velocities of body nodes - /// \param[in] _updateAccs True to update spacial accelerations of body nodes - void setVelocities(const Eigen::VectorXd& _genVels); + void setVelocities(const Eigen::VectorXd& _velocities); - /// Get velocities + /// Get generalized velocities Eigen::VectorXd getVelocities() const; /// Set zero all the velocities @@ -682,14 +684,17 @@ class Skeleton /// Name std::string mName; - /// Degrees of freedom + /// Number of degrees of freedom (aka generalized coordinates) size_t mDof; - /// + /// \brief Array of GenCoordInfo objects + /// \warning GenCoordInfo is deprecated because the functionality is replaced + /// by DegreeOfFreedom. + DEPRECATED(4.3) std::vector mGenCoordInfos; - // TODO(MXG): Now that we have the DegreeOfFreedom class, this vector is - // probably outdated. We can instead have a std::vector, - // because each DegreeOfFreedom has all the information that GenCoordInfo has + + /// Array of DegreeOfFreedom objects within all the joints in this Skeleton + std::vector mDofs; /// True if self collision check is enabled bool mEnabledSelfCollisionCheck; diff --git a/dart/utils/FileInfoDof.cpp b/dart/utils/FileInfoDof.cpp index a328dbc1ff750..6d7f1860cf707 100644 --- a/dart/utils/FileInfoDof.cpp +++ b/dart/utils/FileInfoDof.cpp @@ -39,6 +39,7 @@ #include #include +#include "dart/dynamics/DegreeOfFreedom.h" #include "dart/dynamics/Skeleton.h" #include "dart/dynamics/Joint.h" #include "dart/simulation/Recording.h" @@ -129,8 +130,11 @@ bool FileInfoDof::saveFile(const char* _fName, size_t _start, size_t _end, for (size_t i = 0; i < mSkel->getNumDofs(); i++) { - dynamics::GenCoordInfo info = mSkel->getGenCoordInfo(i); - outFile << info.joint->getName() << "." << info.localIndex << ' '; + const dynamics::DegreeOfFreedom* dof = mSkel->getDof(i); + const dynamics::Joint* joint = dof->getJoint(); + const size_t localIndex = dof->getIndexInJoint(); + + outFile << joint->getName() << "." << localIndex << ' '; } outFile << std::endl; diff --git a/unittests/testNameManagement.cpp b/unittests/testNameManagement.cpp index bf3e52b7d2b20..f053a05cf3a92 100644 --- a/unittests/testNameManagement.cpp +++ b/unittests/testNameManagement.cpp @@ -157,13 +157,10 @@ TEST(NameManagement, Basic) EXPECT_TRUE(joint3->getDof(4) == skel->getDof(8)); EXPECT_TRUE(joint3->getDof(5) == skel->getDof(9)); - // Test that GenCoordInfos agree with the DegreeOfFreedom classes + // Test whether the return of getIndexInSkeleton() and the index of the + // corresponding DegreeOfFreedom in the Skeleton are same for (size_t i = 0; i < skel->getNumDofs(); ++i) - { EXPECT_TRUE(skel->getDof(i)->getIndexInSkeleton() == i); - EXPECT_TRUE(skel->getDof(i)->getIndexInJoint() == - skel->getGenCoordInfo(i).localIndex); - } // Test whether all the joint names are still unique EXPECT_FALSE(joint1->getName() == joint2->getName()); From 43605e53e7f320d290053523da981a7b3c17725f Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 1 Jan 2015 00:30:39 -0500 Subject: [PATCH 11/30] Rename Skeleton::mDof to Skeleton::mNumOfDofs for name consistency --- dart/dynamics/Skeleton.cpp | 10 +++++----- dart/dynamics/Skeleton.h | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index 2651872c29c63..0f0b4b3fc31de 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -58,7 +58,7 @@ namespace dynamics { //============================================================================== Skeleton::Skeleton(const std::string& _name) : mName(_name), - mDof(0), + mNumDofs(0), mEnabledSelfCollisionCheck(false), mEnabledAdjacentBodyCheck(false), mNameMgrForBodyNodes("BodyNode"), @@ -450,7 +450,7 @@ void Skeleton::init(double _timeStep, const Eigen::Vector3d& _gravity) // Initialize body nodes and generalized coordinates mDofs.clear(); - mDof = 0; + mNumDofs = 0; const size_t numBodyNodes = getNumBodyNodes(); for (size_t i = 0; i < numBodyNodes; ++i) { @@ -461,11 +461,11 @@ void Skeleton::init(double _timeStep, const Eigen::Vector3d& _gravity) for (size_t j = 0; j < numDofsOfJoint; ++j) { mDofs.push_back(joint->getDof(j)); - joint->setIndexInSkeleton(j, mDof + j); + joint->setIndexInSkeleton(j, mNumDofs + j); } bodyNode->init(this); - mDof += joint->getNumDofs(); + mNumDofs += joint->getNumDofs(); } // Compute transformations, velocities, and partial accelerations @@ -503,7 +503,7 @@ size_t Skeleton::getDof() const //============================================================================== size_t Skeleton::getNumDofs() const { - return mDof; + return mNumDofs; } //============================================================================== diff --git a/dart/dynamics/Skeleton.h b/dart/dynamics/Skeleton.h index 7450bd9248056..a5384ba2c3301 100644 --- a/dart/dynamics/Skeleton.h +++ b/dart/dynamics/Skeleton.h @@ -685,7 +685,7 @@ class Skeleton std::string mName; /// Number of degrees of freedom (aka generalized coordinates) - size_t mDof; + size_t mNumDofs; /// \brief Array of GenCoordInfo objects /// \warning GenCoordInfo is deprecated because the functionality is replaced From b27fbea65157f57c976456a7f0304e01b52d48f6 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 1 Jan 2015 01:19:14 -0500 Subject: [PATCH 12/30] Change parameter name for name consistency with the member functions --- dart/dynamics/UniversalJoint.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/dart/dynamics/UniversalJoint.cpp b/dart/dynamics/UniversalJoint.cpp index 33a4ecdf6e3e0..b9d14a14aa124 100644 --- a/dart/dynamics/UniversalJoint.cpp +++ b/dart/dynamics/UniversalJoint.cpp @@ -45,12 +45,11 @@ namespace dart { namespace dynamics { //============================================================================== -UniversalJoint::UniversalJoint(const Eigen::Vector3d& _axis0, - const Eigen::Vector3d& _axis1, +UniversalJoint::UniversalJoint(const Eigen::Vector3d& _axis1, const Eigen::Vector3d& _axis2, const std::string& _name) : MultiDofJoint(_name) { - mAxis[0] = _axis0.normalized(); + mAxis[0] = _axis2.normalized(); mAxis[1] = _axis1.normalized(); updateDegreeOfFreedomNames(); From df5e4613239f1f056a796f2f7a9b65f682d0158e Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 1 Jan 2015 01:20:39 -0500 Subject: [PATCH 13/30] Add missing change from the previous commit --- dart/dynamics/UniversalJoint.h | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/dart/dynamics/UniversalJoint.h b/dart/dynamics/UniversalJoint.h index 8a7ee2389ae59..cb86e50302e47 100644 --- a/dart/dynamics/UniversalJoint.h +++ b/dart/dynamics/UniversalJoint.h @@ -51,12 +51,9 @@ class UniversalJoint : public MultiDofJoint<2> { public: /// Constructor - UniversalJoint(const Eigen::Vector3d& _axis0 = Eigen::Vector3d::UnitX(), - const Eigen::Vector3d& _axis1 = Eigen::Vector3d::UnitY(), + UniversalJoint(const Eigen::Vector3d& _axis1 = Eigen::Vector3d::UnitX(), + const Eigen::Vector3d& _axis2 = Eigen::Vector3d::UnitY(), const std::string& _name = "Universal joint"); - // TODO(MXG): We should be more consistent about indexing starting from 0 or 1 - // In the constructor arguments, we start with axis0, but in the member - // functions we start with axis1. /// Destructor virtual ~UniversalJoint(); From ddc5882b47dd6aec74f1ca76e147ac73a5ebe381 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 2 Jan 2015 12:23:43 -0500 Subject: [PATCH 14/30] Fix style --- dart/dynamics/BallJoint.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/dart/dynamics/BallJoint.cpp b/dart/dynamics/BallJoint.cpp index 002cac80602b9..f75f00cf074d6 100644 --- a/dart/dynamics/BallJoint.cpp +++ b/dart/dynamics/BallJoint.cpp @@ -69,9 +69,9 @@ void BallJoint::integratePositions(double _dt) //============================================================================== void BallJoint::updateDegreeOfFreedomNames() { - mDofs[0]->setName(mName+"_x"); - mDofs[1]->setName(mName+"_y"); - mDofs[2]->setName(mName+"_z"); + mDofs[0]->setName(mName + "_x"); + mDofs[1]->setName(mName + "_y"); + mDofs[2]->setName(mName + "_z"); } //============================================================================== From 4f11c66cd2b6018f9858f02ecca62a946eec57bf Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Fri, 2 Jan 2015 17:57:39 -0500 Subject: [PATCH 15/30] removed unnecessary namespace specification --- dart/dynamics/Skeleton.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/dart/dynamics/Skeleton.h b/dart/dynamics/Skeleton.h index a5384ba2c3301..e04de097f88e0 100644 --- a/dart/dynamics/Skeleton.h +++ b/dart/dynamics/Skeleton.h @@ -610,10 +610,10 @@ class Skeleton protected: /// Register a joint with the Skeleton. Internal use only. - void registerJoint(dart::dynamics::Joint* _newJoint); + void registerJoint(Joint* _newJoint); /// Remove a joint from the Skeleton. Internal use only. - void unregisterJoint(dart::dynamics::Joint* _oldJoint); + void unregisterJoint(Joint* _oldJoint); /// Update mass matrix of the skeleton. void updateMassMatrix(); From 80af6dea272bde4176c4fda2a9356fccca19fe76 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Fri, 2 Jan 2015 17:57:53 -0500 Subject: [PATCH 16/30] added ability to parse DOFs -- needs testing --- dart/utils/SkelParser.cpp | 76 +++++++++++++++++++++++++++++++++++++++ dart/utils/SkelParser.h | 5 +++ 2 files changed, 81 insertions(+) diff --git a/dart/utils/SkelParser.cpp b/dart/utils/SkelParser.cpp index 92771a82758dc..b2b6c42cd7966 100644 --- a/dart/utils/SkelParser.cpp +++ b/dart/utils/SkelParser.cpp @@ -884,6 +884,14 @@ dynamics::Joint* SkelParser::readJoint( std::string name = getAttribute(_jointElement, "name"); newJoint->setName(name); + //-------------------------------------------------------------------------- + // Degrees of Freedom + ElementEnumerator DofElements(_jointElement, "dof"); + while(DofElements.next()) + { + readDegreeOfFreedom(DofElements.get(), newJoint); + } + //-------------------------------------------------------------------------- // parent SkelBodyNode softParentBodyNode; @@ -983,6 +991,74 @@ dynamics::Joint* SkelParser::readJoint( return newJoint; } +template +static void setDofAttributes(tinyxml2::XMLElement* xmlElement, + dart::dynamics::DegreeOfFreedom* dof) +{ + double defaultMin = -DART_DBL_INF, defaultMax = DART_DBL_INF, defaultVal = 0; + + (dof->*setLimits)(xmlElement->QueryDoubleAttribute("lower", &defaultMin), + xmlElement->QueryDoubleAttribute("upper", &defaultMax)); + + (dof->*setValue)(xmlElement->QueryDoubleAttribute("initial", &defaultVal)); +} + +void SkelParser::readDegreeOfFreedom(tinyxml2::XMLElement* _dofElement, + dynamics::Joint* _dartJoint) +{ + if(NULL==_dartJoint || NULL==_dofElement) + return; + + size_t localIndex = _dofElement->IntAttribute("number"); + + if(localIndex >= _dartJoint->getNumDofs()) + { + dtwarn << "[SkelParser::readDegreeOfFreedom] Joint named '" + << _dartJoint->getName() << "' contains dof element with invalid " + << "number attribute [" << localIndex << "]. It must be less than " + << _dartJoint->getNumDofs() << ".\n"; + return; + } + + dart::dynamics::DegreeOfFreedom* dof = _dartJoint->getDof(localIndex); + const char* name = _dofElement->Attribute("name"); + if(name) + dof->setName(std::string(name)); + + if(hasElement(_dofElement, "position")) + { + tinyxml2::XMLElement* posElement = getElement(_dofElement, "position"); + setDofAttributes< + &dart::dynamics::DegreeOfFreedom::setPositionLimits, + &dart::dynamics::DegreeOfFreedom::setPosition>(posElement, dof); + } + + if(hasElement(_dofElement, "velocity")) + { + tinyxml2::XMLElement* velElement = getElement(_dofElement, "velocity"); + setDofAttributes< + &dart::dynamics::DegreeOfFreedom::setVelocityLimits, + &dart::dynamics::DegreeOfFreedom::setVelocity>(velElement, dof); + } + + if(hasElement(_dofElement, "acceleration")) + { + tinyxml2::XMLElement* accElement = getElement(_dofElement, "acceleration"); + setDofAttributes< + &dart::dynamics::DegreeOfFreedom::setAccelerationLimits, + &dart::dynamics::DegreeOfFreedom::setAcceleration>(accElement, dof); + } + + if(hasElement(_dofElement, "effort")) + { + tinyxml2::XMLElement* effortElement = getElement(_dofElement, "effort"); + setDofAttributes< + &dart::dynamics::DegreeOfFreedom::setEffortLimits, + &dart::dynamics::DegreeOfFreedom::setEffort>(effortElement, dof); + } +} + dynamics::WeldJoint* SkelParser::readWeldJoint( tinyxml2::XMLElement* _jointElement) { assert(_jointElement != NULL); diff --git a/dart/utils/SkelParser.h b/dart/utils/SkelParser.h index b40caccca3a2e..57397063a0951 100644 --- a/dart/utils/SkelParser.h +++ b/dart/utils/SkelParser.h @@ -128,6 +128,11 @@ class SkelParser const std::vector >& _softBodyNodes); + /// + static void readDegreeOfFreedom( + tinyxml2::XMLElement* _dofElement, + dart::dynamics::Joint* _dartJoint); + /// static dynamics::PrismaticJoint* readPrismaticJoint( tinyxml2::XMLElement* _jointElement); From 8bd330f2ddcf23cf03eb98626ddfa20359c763fa Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Fri, 2 Jan 2015 19:01:30 -0500 Subject: [PATCH 17/30] creating test xml file --- data/skel/test/dof_attribute_test.skel | 205 +++++++++++++++++++++++++ 1 file changed, 205 insertions(+) create mode 100644 data/skel/test/dof_attribute_test.skel diff --git a/data/skel/test/dof_attribute_test.skel b/data/skel/test/dof_attribute_test.skel new file mode 100644 index 0000000000000..8f2b14fe627ae --- /dev/null +++ b/data/skel/test/dof_attribute_test.skel @@ -0,0 +1,205 @@ + + + + + 0.001 + 0 -9.81 0 + fcl_mesh + + + + 1 + 0 0 0 0 0 0 + + 5 + 0 0.5 0 + + + 0 0.5 0 0 0 0 + + + 0.1 1.0 0.1 + + + + + 0 0.5 0 0 0 0 + + + 0.1 1.0 0.1 + + + + + + 1 + 0 1.0 0 0 0 0 + + 5 + 0 0.5 0 + + + 0 0.5 0 0 0 0 + + + 0.1 1.0 0.1 + + + + + 0 0.5 0 0 0 0 + + + 0.1 1.0 0.1 + + + + + + 1 + 0 1.0 0 0 0 0 + + 5 + 0 0.5 0 + + + 0 0.5 0 0 0 0 + + + 0.1 1.0 0.1 + + + + + 0 0.5 0 0 0 0 + + + 0.1 1.0 0.1 + + + + + + 1 + 0 1.0 0 0 0 0 + + 5 + 0 0.5 0 + + + 0 0.5 0 0 0 0 + + + 0.1 1.0 0.1 + + + + + 0 0.5 0 0 0 0 + + + 0.1 1.0 0.1 + + + + + + 1 + 0 1.0 0 0 0 0 + + 5 + 0 0.5 0 + + + 0 0.5 0 0 0 0 + + + 0.1 1.0 0.1 + + + + + 0 0.5 0 0 0 0 + + + 0.1 1.0 0.1 + + + + + + world + link 1 + -0 0 0 0 0 0 + + 1 0 0 + + + + link 1 + link 2 + -0 0 0 0 0 0 + + 1 0 0 + + -1.57 + +1.57 + + + + + + link 2 + link 3 + 0 0 0 0 0 0 + + 1 0 0 + + + + + + + + + + link 3 + link 4 + 0 0 0 0 0 0 + + 1 0 0 + + -1 + +1 + + + + 0 1 0 + + -2 + +2 + + + + + link 4 + link 5 + 0 0 0 0 0 0 + + 1 0 0 + + + 0 1 0 + + + + + + + + + + + + + From 37c4f040a7e92210539eb63d12492b9fd51f225d Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Fri, 2 Jan 2015 20:43:32 -0500 Subject: [PATCH 18/30] fixed important typos --- dart/dynamics/DegreeOfFreedom.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/dart/dynamics/DegreeOfFreedom.cpp b/dart/dynamics/DegreeOfFreedom.cpp index 38875e53c51db..5f509b3f07105 100644 --- a/dart/dynamics/DegreeOfFreedom.cpp +++ b/dart/dynamics/DegreeOfFreedom.cpp @@ -188,7 +188,7 @@ void DegreeOfFreedom::setVelocityUpperLimit(double _limit) //============================================================================== double DegreeOfFreedom::getVelocityUpperLimit() const { - return mJoint->getVelocityLowerLimit(mIndexInJoint); + return mJoint->getVelocityUpperLimit(mIndexInJoint); } //============================================================================== @@ -289,7 +289,7 @@ void DegreeOfFreedom::setEffortLowerLimit(double _limit) //============================================================================== double DegreeOfFreedom::getEffortLowerLimit() const { - return mJoint->getForce(mIndexInJoint); + return mJoint->getForceLowerLimit(mIndexInJoint); } //============================================================================== From 311d2d7609cb6a31f46994d5637565dcd77bc31a Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Fri, 2 Jan 2015 22:01:15 -0500 Subject: [PATCH 19/30] added ability to prevent automatic DOF name changing --- dart/dynamics/BallJoint.cpp | 9 ++++++--- dart/dynamics/DegreeOfFreedom.cpp | 13 +++++++++++++ dart/dynamics/DegreeOfFreedom.h | 12 ++++++++++++ dart/dynamics/EulerJoint.cpp | 5 ++++- dart/dynamics/FreeJoint.cpp | 18 ++++++++++++------ dart/dynamics/PlanarJoint.cpp | 5 ++++- dart/dynamics/SingleDofJoint.cpp | 3 ++- dart/dynamics/TranslationalJoint.cpp | 9 ++++++--- dart/dynamics/UniversalJoint.cpp | 6 ++++-- 9 files changed, 63 insertions(+), 17 deletions(-) diff --git a/dart/dynamics/BallJoint.cpp b/dart/dynamics/BallJoint.cpp index f75f00cf074d6..4abb5c130bae1 100644 --- a/dart/dynamics/BallJoint.cpp +++ b/dart/dynamics/BallJoint.cpp @@ -69,9 +69,12 @@ void BallJoint::integratePositions(double _dt) //============================================================================== void BallJoint::updateDegreeOfFreedomNames() { - mDofs[0]->setName(mName + "_x"); - mDofs[1]->setName(mName + "_y"); - mDofs[2]->setName(mName + "_z"); + if(!mDofs[0]->isNamePreserved()) + mDofs[0]->setName(mName + "_x"); + if(!mDofs[1]->isNamePreserved()) + mDofs[1]->setName(mName + "_y"); + if(!mDofs[2]->isNamePreserved()) + mDofs[2]->setName(mName + "_z"); } //============================================================================== diff --git a/dart/dynamics/DegreeOfFreedom.cpp b/dart/dynamics/DegreeOfFreedom.cpp index 5f509b3f07105..ccf5bb7d57bca 100644 --- a/dart/dynamics/DegreeOfFreedom.cpp +++ b/dart/dynamics/DegreeOfFreedom.cpp @@ -66,6 +66,18 @@ const std::string& DegreeOfFreedom::getName() const return mName; } +//============================================================================== +void DegreeOfFreedom::preserveName(bool _preserve) +{ + mNamePreserved = _preserve; +} + +//============================================================================== +bool DegreeOfFreedom::isNamePreserved() const +{ + return mNamePreserved; +} + //============================================================================== size_t DegreeOfFreedom::getIndexInSkeleton() const { @@ -357,6 +369,7 @@ DegreeOfFreedom::DegreeOfFreedom(Joint* _joint, const std::string& _name, size_t _indexInJoint) : mName(_name), + mNamePreserved(false), mIndexInJoint(_indexInJoint), mIndexInSkeleton(0), mJoint(_joint) diff --git a/dart/dynamics/DegreeOfFreedom.h b/dart/dynamics/DegreeOfFreedom.h index 9837e4b2b869b..351ce6520df84 100644 --- a/dart/dynamics/DegreeOfFreedom.h +++ b/dart/dynamics/DegreeOfFreedom.h @@ -66,6 +66,15 @@ class DegreeOfFreedom /// Get the name of this DegreeOfFreedom const std::string& getName() const; + /// Prevent Joint::updateDegreeOfFreedomNames() from changing the name of this + /// degree of freedom. This is useful if you (the user) have customized the + /// name for this DegreeOfFreedom and want to prevent DART from automatically + /// updating its name if its parent Joint properties ever change. + void preserveName(bool _preserve); + + /// Check whether DegreeOfFreedom::lockName(bool) is activate + bool isNamePreserved() const; + /// Get this DegreeOfFreedom's index within its Skeleton size_t getIndexInSkeleton() const; @@ -243,6 +252,9 @@ class DegreeOfFreedom /// The default name can be renamed by setName() as well. std::string mName; + /// True if DegreeOfFreedom::lockName(bool) is active + bool mNamePreserved; + /// \brief Index of this DegreeOfFreedom within its Joint /// /// The index is determined when this DegreeOfFreedom is created by the Joint diff --git a/dart/dynamics/EulerJoint.cpp b/dart/dynamics/EulerJoint.cpp index ff2d94123e222..d82c29d088843 100644 --- a/dart/dynamics/EulerJoint.cpp +++ b/dart/dynamics/EulerJoint.cpp @@ -107,7 +107,10 @@ void EulerJoint::updateDegreeOfFreedomNames() if (affixes.size() == 3) { for (size_t i = 0; i < 3; ++i) - mDofs[i]->setName(mName + affixes[i]); + { + if(!mDofs[i]->isNamePreserved()) + mDofs[i]->setName(mName + affixes[i]); + } } } diff --git a/dart/dynamics/FreeJoint.cpp b/dart/dynamics/FreeJoint.cpp index bc547db625e25..773c815ff0d33 100644 --- a/dart/dynamics/FreeJoint.cpp +++ b/dart/dynamics/FreeJoint.cpp @@ -71,12 +71,18 @@ void FreeJoint::integratePositions(double _dt) //============================================================================== void FreeJoint::updateDegreeOfFreedomNames() { - mDofs[0]->setName(mName + "_rot_x"); - mDofs[1]->setName(mName + "_rot_y"); - mDofs[2]->setName(mName + "_rot_z"); - mDofs[3]->setName(mName + "_pos_x"); - mDofs[4]->setName(mName + "_pos_y"); - mDofs[5]->setName(mName + "_pos_z"); + if(!mDofs[0]->isNamePreserved()) + mDofs[0]->setName(mName + "_rot_x"); + if(!mDofs[1]->isNamePreserved()) + mDofs[1]->setName(mName + "_rot_y"); + if(!mDofs[2]->isNamePreserved()) + mDofs[2]->setName(mName + "_rot_z"); + if(!mDofs[3]->isNamePreserved()) + mDofs[3]->setName(mName + "_pos_x"); + if(!mDofs[4]->isNamePreserved()) + mDofs[4]->setName(mName + "_pos_y"); + if(!mDofs[5]->isNamePreserved()) + mDofs[5]->setName(mName + "_pos_z"); } //============================================================================== diff --git a/dart/dynamics/PlanarJoint.cpp b/dart/dynamics/PlanarJoint.cpp index 76a6dd69443c8..09156d2b620cb 100644 --- a/dart/dynamics/PlanarJoint.cpp +++ b/dart/dynamics/PlanarJoint.cpp @@ -176,7 +176,10 @@ void PlanarJoint::updateDegreeOfFreedomNames() if (affixes.size() == 2) { for (size_t i = 0; i < 2; ++i) - mDofs[i]->setName(mName+affixes[i]); + { + if (!mDofs[i]->isNamePreserved()) + mDofs[i]->setName(mName+affixes[i]); + } } } diff --git a/dart/dynamics/SingleDofJoint.cpp b/dart/dynamics/SingleDofJoint.cpp index 7f4b83b8121cb..7530b22f86629 100644 --- a/dart/dynamics/SingleDofJoint.cpp +++ b/dart/dynamics/SingleDofJoint.cpp @@ -724,7 +724,8 @@ double SingleDofJoint::getPotentialEnergy() const void SingleDofJoint::updateDegreeOfFreedomNames() { // Same name as the joint it belongs to. - mDof->setName(mName); + if(!mDof->isNamePreserved()) + mDof->setName(mName); } //============================================================================== diff --git a/dart/dynamics/TranslationalJoint.cpp b/dart/dynamics/TranslationalJoint.cpp index e820909b25473..07fbeceaf08fe 100644 --- a/dart/dynamics/TranslationalJoint.cpp +++ b/dart/dynamics/TranslationalJoint.cpp @@ -59,9 +59,12 @@ TranslationalJoint::~TranslationalJoint() //============================================================================== void TranslationalJoint::updateDegreeOfFreedomNames() { - mDofs[0]->setName(mName + "_x"); - mDofs[1]->setName(mName + "_y"); - mDofs[2]->setName(mName + "_z"); + if(!mDofs[0]->isNamePreserved()) + mDofs[0]->setName(mName + "_x"); + if(!mDofs[1]->isNamePreserved()) + mDofs[1]->setName(mName + "_y"); + if(!mDofs[2]->isNamePreserved()) + mDofs[2]->setName(mName + "_z"); } //============================================================================== diff --git a/dart/dynamics/UniversalJoint.cpp b/dart/dynamics/UniversalJoint.cpp index b9d14a14aa124..d9660c8dacada 100644 --- a/dart/dynamics/UniversalJoint.cpp +++ b/dart/dynamics/UniversalJoint.cpp @@ -87,8 +87,10 @@ const Eigen::Vector3d& UniversalJoint::getAxis2() const //============================================================================== void UniversalJoint::updateDegreeOfFreedomNames() { - mDofs[0]->setName(mName + "_1"); - mDofs[1]->setName(mName + "_2"); + if(!mDofs[0]->isNamePreserved()) + mDofs[0]->setName(mName + "_1"); + if(!mDofs[1]->isNamePreserved()) + mDofs[1]->setName(mName + "_2"); } //============================================================================== From 24d301d57058391c374895895c9da090b37127f7 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Fri, 2 Jan 2015 22:01:27 -0500 Subject: [PATCH 20/30] finished test for DOF parsing -- passing --- dart/utils/SkelParser.cpp | 55 ++++++++++++------ data/skel/test/dof_attribute_test.skel | 29 +++++----- unittests/testParser.cpp | 78 ++++++++++++++++++++++++++ 3 files changed, 131 insertions(+), 31 deletions(-) diff --git a/dart/utils/SkelParser.cpp b/dart/utils/SkelParser.cpp index b2b6c42cd7966..ecc00a23ff14f 100644 --- a/dart/utils/SkelParser.cpp +++ b/dart/utils/SkelParser.cpp @@ -331,6 +331,11 @@ dynamics::Skeleton* SkelParser::readSkeleton( newSkeleton->addBodyNode((*it).bodyNode); } + // No longer need to preserve the dof names. User can turn this back on if + // they desire. + for(size_t i=0; igetNumDofs(); ++i) + newSkeleton->getDof(i)->preserveName(false); + return newSkeleton; } @@ -884,14 +889,6 @@ dynamics::Joint* SkelParser::readJoint( std::string name = getAttribute(_jointElement, "name"); newJoint->setName(name); - //-------------------------------------------------------------------------- - // Degrees of Freedom - ElementEnumerator DofElements(_jointElement, "dof"); - while(DofElements.next()) - { - readDegreeOfFreedom(DofElements.get(), newJoint); - } - //-------------------------------------------------------------------------- // parent SkelBodyNode softParentBodyNode; @@ -988,20 +985,33 @@ dynamics::Joint* SkelParser::readJoint( newJoint->setTransformFromChildBodyNode(childToJoint); newJoint->setTransformFromParentBodyNode(parentToJoint); + //-------------------------------------------------------------------------- + // Degrees of Freedom + ElementEnumerator DofElements(_jointElement, "dof"); + while(DofElements.next()) + readDegreeOfFreedom(DofElements.get(), newJoint); + return newJoint; } template + std::pair (dart::dynamics::DegreeOfFreedom::*getLimits)() const, + void (dart::dynamics::DegreeOfFreedom::*setValue)(double), + double (dart::dynamics::DegreeOfFreedom::*getValue)() const> static void setDofAttributes(tinyxml2::XMLElement* xmlElement, dart::dynamics::DegreeOfFreedom* dof) { - double defaultMin = -DART_DBL_INF, defaultMax = DART_DBL_INF, defaultVal = 0; + std::pair limit_vals = (dof->*getLimits)(); + double defaultMin = limit_vals.first, + defaultMax = limit_vals.second, + defaultVal = (dof->*getValue)(); - (dof->*setLimits)(xmlElement->QueryDoubleAttribute("lower", &defaultMin), - xmlElement->QueryDoubleAttribute("upper", &defaultMax)); + xmlElement->QueryDoubleAttribute("lower", &defaultMin); + xmlElement->QueryDoubleAttribute("upper", &defaultMax); + xmlElement->QueryDoubleAttribute("initial", &defaultVal); - (dof->*setValue)(xmlElement->QueryDoubleAttribute("initial", &defaultVal)); + (dof->*setLimits)(defaultMin, defaultMax); + (dof->*setValue)(defaultVal); } void SkelParser::readDegreeOfFreedom(tinyxml2::XMLElement* _dofElement, @@ -1024,14 +1034,19 @@ void SkelParser::readDegreeOfFreedom(tinyxml2::XMLElement* _dofElement, dart::dynamics::DegreeOfFreedom* dof = _dartJoint->getDof(localIndex); const char* name = _dofElement->Attribute("name"); if(name) + { dof->setName(std::string(name)); + dof->preserveName(true); + } if(hasElement(_dofElement, "position")) { tinyxml2::XMLElement* posElement = getElement(_dofElement, "position"); setDofAttributes< &dart::dynamics::DegreeOfFreedom::setPositionLimits, - &dart::dynamics::DegreeOfFreedom::setPosition>(posElement, dof); + &dart::dynamics::DegreeOfFreedom::getPositionLimits, + &dart::dynamics::DegreeOfFreedom::setPosition, + &dart::dynamics::DegreeOfFreedom::getPosition>(posElement, dof); } if(hasElement(_dofElement, "velocity")) @@ -1039,7 +1054,9 @@ void SkelParser::readDegreeOfFreedom(tinyxml2::XMLElement* _dofElement, tinyxml2::XMLElement* velElement = getElement(_dofElement, "velocity"); setDofAttributes< &dart::dynamics::DegreeOfFreedom::setVelocityLimits, - &dart::dynamics::DegreeOfFreedom::setVelocity>(velElement, dof); + &dart::dynamics::DegreeOfFreedom::getVelocityLimits, + &dart::dynamics::DegreeOfFreedom::setVelocity, + &dart::dynamics::DegreeOfFreedom::getVelocity>(velElement, dof); } if(hasElement(_dofElement, "acceleration")) @@ -1047,7 +1064,9 @@ void SkelParser::readDegreeOfFreedom(tinyxml2::XMLElement* _dofElement, tinyxml2::XMLElement* accElement = getElement(_dofElement, "acceleration"); setDofAttributes< &dart::dynamics::DegreeOfFreedom::setAccelerationLimits, - &dart::dynamics::DegreeOfFreedom::setAcceleration>(accElement, dof); + &dart::dynamics::DegreeOfFreedom::getAccelerationLimits, + &dart::dynamics::DegreeOfFreedom::setAcceleration, + &dart::dynamics::DegreeOfFreedom::getAcceleration>(accElement, dof); } if(hasElement(_dofElement, "effort")) @@ -1055,7 +1074,9 @@ void SkelParser::readDegreeOfFreedom(tinyxml2::XMLElement* _dofElement, tinyxml2::XMLElement* effortElement = getElement(_dofElement, "effort"); setDofAttributes< &dart::dynamics::DegreeOfFreedom::setEffortLimits, - &dart::dynamics::DegreeOfFreedom::setEffort>(effortElement, dof); + &dart::dynamics::DegreeOfFreedom::getEffortLimits, + &dart::dynamics::DegreeOfFreedom::setEffort, + &dart::dynamics::DegreeOfFreedom::getEffort>(effortElement, dof); } } diff --git a/data/skel/test/dof_attribute_test.skel b/data/skel/test/dof_attribute_test.skel index 8f2b14fe627ae..078142d6decc9 100644 --- a/data/skel/test/dof_attribute_test.skel +++ b/data/skel/test/dof_attribute_test.skel @@ -6,7 +6,7 @@ 0 -9.81 0 fcl_mesh - + 1 0 0 0 0 0 0 @@ -180,24 +180,25 @@ +2 + + + + + + - + link 4 link 5 0 0 0 0 0 0 - - 1 0 0 - - - 0 1 0 - - - - + xyz + + + - - - + + + diff --git a/unittests/testParser.cpp b/unittests/testParser.cpp index 233c8ba8007e4..96484b724d7f3 100644 --- a/unittests/testParser.cpp +++ b/unittests/testParser.cpp @@ -182,6 +182,7 @@ TEST(SKEL_PARSER, RIGID_SOFT_BODIES) Skeleton* skel1 = world->getSkeleton("skeleton 1"); Skeleton* softSkel1 = dynamic_cast(skel1); + // TODO(MXG): ^This cast looks like it's outdated EXPECT_TRUE(softSkel1 != NULL); EXPECT_EQ(softSkel1->getNumBodyNodes(), 2); EXPECT_EQ(softSkel1->getNumRigidBodyNodes(), 1); @@ -313,6 +314,83 @@ TEST(SKEL_PARSER, PLANAR_JOINT) delete world; } +//============================================================================== +TEST(SKEL_PARSER, DOF_ATTRIBUTES) +{ + World* world = SkelParser::readWorld( + DART_DATA_PATH"/skel/test/dof_attribute_test.skel"); + EXPECT_TRUE(world != NULL); + + Skeleton* skel1 = world->getSkeleton("skeleton 1"); + + // Test for no dof elements being specified + Joint* joint0 = skel1->getJoint("joint0"); + EXPECT_EQ(joint0->getDof(0)->getPositionLowerLimit(), -DART_DBL_INF); + EXPECT_EQ(joint0->getDof(0)->getPositionUpperLimit(), DART_DBL_INF); + EXPECT_EQ(joint0->getDof(0)->getPosition(), 0); + + EXPECT_EQ(joint0->getDof(0)->getVelocityLowerLimit(), -DART_DBL_INF); + EXPECT_EQ(joint0->getDof(0)->getVelocityUpperLimit(), DART_DBL_INF); + EXPECT_EQ(joint0->getDof(0)->getVelocity(), 0); + + EXPECT_EQ(joint0->getDof(0)->getName(), joint0->getName()); + + // Test for only a dof name being changed + Joint* joint1 = skel1->getJoint("joint1"); + EXPECT_EQ(joint1->getDof(0)->getPositionLowerLimit(), -1.57); + EXPECT_EQ(joint1->getDof(0)->getPositionUpperLimit(), 1.57); + EXPECT_EQ(joint1->getDof(0)->getName(), "customJoint1"); + + // Test for when dof attributes (but not a name) are specified + Joint* joint2 = skel1->getJoint("joint2"); + EXPECT_EQ(joint2->getDof(0)->getName(), joint2->getName()); + + EXPECT_EQ(joint2->getDof(0)->getPositionLowerLimit(), -1); + EXPECT_EQ(joint2->getDof(0)->getPositionUpperLimit(), 1); + EXPECT_EQ(joint2->getDof(0)->getPosition(), 0); + + EXPECT_EQ(joint2->getDof(0)->getVelocityLowerLimit(), -2); + EXPECT_EQ(joint2->getDof(0)->getVelocityUpperLimit(), 2); + EXPECT_EQ(joint2->getDof(0)->getVelocity(), 0); + + EXPECT_EQ(joint2->getDof(0)->getAccelerationLowerLimit(), -3); + EXPECT_EQ(joint2->getDof(0)->getAccelerationUpperLimit(), 3); + EXPECT_EQ(joint2->getDof(0)->getAcceleration(), 0); + + EXPECT_EQ(joint2->getDof(0)->getEffortLowerLimit(), -4); + EXPECT_EQ(joint2->getDof(0)->getEffortUpperLimit(), 4); + EXPECT_EQ(joint2->getDof(0)->getEffort(), 0); + + // Test for mixture of old method and new method + // Note: If there is a conflict, the data given in the dof element will win + Joint* joint3 = skel1->getJoint("joint3"); + EXPECT_EQ(joint3->getDof(0)->getName(), joint3->getName()+"_1"); + EXPECT_EQ(joint3->getDof(0)->getPositionLowerLimit(), -1); + EXPECT_EQ(joint3->getDof(0)->getPositionUpperLimit(), 1); + EXPECT_EQ(joint3->getDof(0)->getPosition(), 5); + + EXPECT_EQ(joint3->getDof(1)->getName(), joint3->getName()+"_2"); + EXPECT_EQ(joint3->getDof(1)->getPositionLowerLimit(), -2); + EXPECT_EQ(joint3->getDof(1)->getPositionUpperLimit(), 2); + EXPECT_EQ(joint3->getDof(1)->getPosition(), -5); + + // Test for only some of the DOFs being specified + Joint* joint4 = skel1->getJoint("joint4"); + EXPECT_EQ(joint4->getDof(0)->getName(), "joint4_1"); + EXPECT_EQ(joint4->getDof(0)->getPositionLowerLimit(), -1); + EXPECT_EQ(joint4->getDof(0)->getPositionUpperLimit(), 1); + EXPECT_EQ(joint4->getDof(0)->getVelocityLowerLimit(), -10); + EXPECT_EQ(joint4->getDof(0)->getVelocityUpperLimit(), 10); + + EXPECT_EQ(joint4->getDof(1)->getName(), joint4->getName()+"_y"); + + EXPECT_EQ(joint4->getDof(2)->getName(), "joint4_3"); + EXPECT_EQ(joint4->getDof(2)->getPositionLowerLimit(), -2); + EXPECT_EQ(joint4->getDof(2)->getPositionUpperLimit(), 2); + EXPECT_EQ(joint4->getDof(2)->getVelocityLowerLimit(), -20); + EXPECT_EQ(joint4->getDof(2)->getVelocityUpperLimit(), 20); +} + /******************************************************************************/ int main(int argc, char* argv[]) { From 913d7fd02ef9b437d40544cf7cba8b5d5a47025c Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Sat, 3 Jan 2015 14:07:23 -0500 Subject: [PATCH 21/30] calls to DOF::setName() will, by default, set the name to be preserved --- dart/dynamics/BallJoint.cpp | 6 +++--- dart/dynamics/DegreeOfFreedom.cpp | 5 ++++- dart/dynamics/DegreeOfFreedom.h | 7 ++++++- dart/dynamics/EulerJoint.cpp | 2 +- dart/dynamics/FreeJoint.cpp | 12 ++++++------ dart/dynamics/PlanarJoint.cpp | 2 +- dart/dynamics/SingleDofJoint.cpp | 2 +- dart/dynamics/TranslationalJoint.cpp | 6 +++--- dart/dynamics/UniversalJoint.cpp | 4 ++-- dart/utils/SkelParser.cpp | 1 - 10 files changed, 27 insertions(+), 20 deletions(-) diff --git a/dart/dynamics/BallJoint.cpp b/dart/dynamics/BallJoint.cpp index 4abb5c130bae1..ff3e2df14d613 100644 --- a/dart/dynamics/BallJoint.cpp +++ b/dart/dynamics/BallJoint.cpp @@ -70,11 +70,11 @@ void BallJoint::integratePositions(double _dt) void BallJoint::updateDegreeOfFreedomNames() { if(!mDofs[0]->isNamePreserved()) - mDofs[0]->setName(mName + "_x"); + mDofs[0]->setName(mName + "_x", false); if(!mDofs[1]->isNamePreserved()) - mDofs[1]->setName(mName + "_y"); + mDofs[1]->setName(mName + "_y", false); if(!mDofs[2]->isNamePreserved()) - mDofs[2]->setName(mName + "_z"); + mDofs[2]->setName(mName + "_z", false); } //============================================================================== diff --git a/dart/dynamics/DegreeOfFreedom.cpp b/dart/dynamics/DegreeOfFreedom.cpp index ccf5bb7d57bca..c3999d84abbb4 100644 --- a/dart/dynamics/DegreeOfFreedom.cpp +++ b/dart/dynamics/DegreeOfFreedom.cpp @@ -42,8 +42,11 @@ namespace dart { namespace dynamics { //============================================================================== -const std::string& DegreeOfFreedom::setName(const std::string& _name) +const std::string& DegreeOfFreedom::setName(const std::string& _name, + bool _preserveName) { + preserveName(_preserveName); + if (mName == _name) return mName; diff --git a/dart/dynamics/DegreeOfFreedom.h b/dart/dynamics/DegreeOfFreedom.h index 351ce6520df84..d4f30f3e0f713 100644 --- a/dart/dynamics/DegreeOfFreedom.h +++ b/dart/dynamics/DegreeOfFreedom.h @@ -61,7 +61,12 @@ class DegreeOfFreedom friend class dart::dynamics::Skeleton; /// Change the name of this DegreeOfFreedom - const std::string& setName(const std::string& _name); + /// + /// The _preserveName argument will be passed to the preserveName(bool) + /// function. Set _preserveName to true when customizing the name of the + /// DegreeOfFreedom; that way the name will not be overwritten if the Joint + /// name changes. + const std::string& setName(const std::string& _name, bool _preserveName=true); /// Get the name of this DegreeOfFreedom const std::string& getName() const; diff --git a/dart/dynamics/EulerJoint.cpp b/dart/dynamics/EulerJoint.cpp index d82c29d088843..045dbc08f8aa2 100644 --- a/dart/dynamics/EulerJoint.cpp +++ b/dart/dynamics/EulerJoint.cpp @@ -109,7 +109,7 @@ void EulerJoint::updateDegreeOfFreedomNames() for (size_t i = 0; i < 3; ++i) { if(!mDofs[i]->isNamePreserved()) - mDofs[i]->setName(mName + affixes[i]); + mDofs[i]->setName(mName + affixes[i], false); } } } diff --git a/dart/dynamics/FreeJoint.cpp b/dart/dynamics/FreeJoint.cpp index 773c815ff0d33..6b3e561ff0b01 100644 --- a/dart/dynamics/FreeJoint.cpp +++ b/dart/dynamics/FreeJoint.cpp @@ -72,17 +72,17 @@ void FreeJoint::integratePositions(double _dt) void FreeJoint::updateDegreeOfFreedomNames() { if(!mDofs[0]->isNamePreserved()) - mDofs[0]->setName(mName + "_rot_x"); + mDofs[0]->setName(mName + "_rot_x", false); if(!mDofs[1]->isNamePreserved()) - mDofs[1]->setName(mName + "_rot_y"); + mDofs[1]->setName(mName + "_rot_y", false); if(!mDofs[2]->isNamePreserved()) - mDofs[2]->setName(mName + "_rot_z"); + mDofs[2]->setName(mName + "_rot_z", false); if(!mDofs[3]->isNamePreserved()) - mDofs[3]->setName(mName + "_pos_x"); + mDofs[3]->setName(mName + "_pos_x", false); if(!mDofs[4]->isNamePreserved()) - mDofs[4]->setName(mName + "_pos_y"); + mDofs[4]->setName(mName + "_pos_y", false); if(!mDofs[5]->isNamePreserved()) - mDofs[5]->setName(mName + "_pos_z"); + mDofs[5]->setName(mName + "_pos_z", false); } //============================================================================== diff --git a/dart/dynamics/PlanarJoint.cpp b/dart/dynamics/PlanarJoint.cpp index 09156d2b620cb..3da59f39f726b 100644 --- a/dart/dynamics/PlanarJoint.cpp +++ b/dart/dynamics/PlanarJoint.cpp @@ -178,7 +178,7 @@ void PlanarJoint::updateDegreeOfFreedomNames() for (size_t i = 0; i < 2; ++i) { if (!mDofs[i]->isNamePreserved()) - mDofs[i]->setName(mName+affixes[i]); + mDofs[i]->setName(mName+affixes[i], false); } } } diff --git a/dart/dynamics/SingleDofJoint.cpp b/dart/dynamics/SingleDofJoint.cpp index 7530b22f86629..521a9e04c5ad0 100644 --- a/dart/dynamics/SingleDofJoint.cpp +++ b/dart/dynamics/SingleDofJoint.cpp @@ -725,7 +725,7 @@ void SingleDofJoint::updateDegreeOfFreedomNames() { // Same name as the joint it belongs to. if(!mDof->isNamePreserved()) - mDof->setName(mName); + mDof->setName(mName, false); } //============================================================================== diff --git a/dart/dynamics/TranslationalJoint.cpp b/dart/dynamics/TranslationalJoint.cpp index 07fbeceaf08fe..d3d14eba3f829 100644 --- a/dart/dynamics/TranslationalJoint.cpp +++ b/dart/dynamics/TranslationalJoint.cpp @@ -60,11 +60,11 @@ TranslationalJoint::~TranslationalJoint() void TranslationalJoint::updateDegreeOfFreedomNames() { if(!mDofs[0]->isNamePreserved()) - mDofs[0]->setName(mName + "_x"); + mDofs[0]->setName(mName + "_x", false); if(!mDofs[1]->isNamePreserved()) - mDofs[1]->setName(mName + "_y"); + mDofs[1]->setName(mName + "_y", false); if(!mDofs[2]->isNamePreserved()) - mDofs[2]->setName(mName + "_z"); + mDofs[2]->setName(mName + "_z", false); } //============================================================================== diff --git a/dart/dynamics/UniversalJoint.cpp b/dart/dynamics/UniversalJoint.cpp index d9660c8dacada..f44882161ffd1 100644 --- a/dart/dynamics/UniversalJoint.cpp +++ b/dart/dynamics/UniversalJoint.cpp @@ -88,9 +88,9 @@ const Eigen::Vector3d& UniversalJoint::getAxis2() const void UniversalJoint::updateDegreeOfFreedomNames() { if(!mDofs[0]->isNamePreserved()) - mDofs[0]->setName(mName + "_1"); + mDofs[0]->setName(mName + "_1", false); if(!mDofs[1]->isNamePreserved()) - mDofs[1]->setName(mName + "_2"); + mDofs[1]->setName(mName + "_2", false); } //============================================================================== diff --git a/dart/utils/SkelParser.cpp b/dart/utils/SkelParser.cpp index ecc00a23ff14f..a20b748d219d9 100644 --- a/dart/utils/SkelParser.cpp +++ b/dart/utils/SkelParser.cpp @@ -1036,7 +1036,6 @@ void SkelParser::readDegreeOfFreedom(tinyxml2::XMLElement* _dofElement, if(name) { dof->setName(std::string(name)); - dof->preserveName(true); } if(hasElement(_dofElement, "position")) From f7e48406a7184c0432d9072b44d3d680cdbedd9c Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Sat, 3 Jan 2015 14:20:56 -0500 Subject: [PATCH 22/30] when custom DOF names are given in a skel file, keep them preserved --- dart/utils/SkelParser.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/dart/utils/SkelParser.cpp b/dart/utils/SkelParser.cpp index a20b748d219d9..71b7fa9f7c09f 100644 --- a/dart/utils/SkelParser.cpp +++ b/dart/utils/SkelParser.cpp @@ -331,11 +331,6 @@ dynamics::Skeleton* SkelParser::readSkeleton( newSkeleton->addBodyNode((*it).bodyNode); } - // No longer need to preserve the dof names. User can turn this back on if - // they desire. - for(size_t i=0; igetNumDofs(); ++i) - newSkeleton->getDof(i)->preserveName(false); - return newSkeleton; } From 0b0de72655b988988a5fc6db25c49b45df80ca6d Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Sat, 3 Jan 2015 15:02:21 -0500 Subject: [PATCH 23/30] better error checking in the xml parsing --- dart/utils/SkelParser.cpp | 102 ++++++++++++++++++------- data/skel/test/dof_attribute_test.skel | 10 +-- 2 files changed, 80 insertions(+), 32 deletions(-) diff --git a/dart/utils/SkelParser.cpp b/dart/utils/SkelParser.cpp index 71b7fa9f7c09f..f5d8936e66b62 100644 --- a/dart/utils/SkelParser.cpp +++ b/dart/utils/SkelParser.cpp @@ -989,24 +989,46 @@ dynamics::Joint* SkelParser::readJoint( return newJoint; } +static void getDofAttributeIfItExists(const std::string& attribute, + double& value, + const std::string& element_type, + const tinyxml2::XMLElement* xmlElement, + const dart::dynamics::DegreeOfFreedom* dof) +{ + if(xmlElement->QueryDoubleAttribute(attribute.c_str(), &value)== + tinyxml2::XML_WRONG_ATTRIBUTE_TYPE) + { + dterr << "Invalid type for '" << attribute << "' attribute of '" + << element_type << "' element in the '" + << dof->getName() << "' dof of '" << dof->getJoint()->getName() + << "'.\n"; + } +} + template (dart::dynamics::DegreeOfFreedom::*getLimits)() const, void (dart::dynamics::DegreeOfFreedom::*setValue)(double), double (dart::dynamics::DegreeOfFreedom::*getValue)() const> -static void setDofAttributes(tinyxml2::XMLElement* xmlElement, - dart::dynamics::DegreeOfFreedom* dof) +static void setDofAttributes(tinyxml2::XMLElement* _dofElement, + dart::dynamics::DegreeOfFreedom* _dof, + const std::string& _element_type) { - std::pair limit_vals = (dof->*getLimits)(); - double defaultMin = limit_vals.first, - defaultMax = limit_vals.second, - defaultVal = (dof->*getValue)(); - - xmlElement->QueryDoubleAttribute("lower", &defaultMin); - xmlElement->QueryDoubleAttribute("upper", &defaultMax); - xmlElement->QueryDoubleAttribute("initial", &defaultVal); - - (dof->*setLimits)(defaultMin, defaultMax); - (dof->*setValue)(defaultVal); + const tinyxml2::XMLElement* xmlElement = + getElement(_dofElement, _element_type); + std::pair limit_vals = (_dof->*getLimits)(); + double defaultLower = limit_vals.first, + defaultUpper = limit_vals.second, + defaultInitial = (_dof->*getValue)(); + + getDofAttributeIfItExists("lower", defaultLower, + _element_type, xmlElement, _dof); + getDofAttributeIfItExists("upper", defaultUpper, + _element_type, xmlElement, _dof); + getDofAttributeIfItExists("initial", defaultInitial, + _element_type, xmlElement, _dof); + + (_dof->*setLimits)(defaultLower, defaultUpper); + (_dof->*setValue)(defaultInitial); } void SkelParser::readDegreeOfFreedom(tinyxml2::XMLElement* _dofElement, @@ -1015,16 +1037,42 @@ void SkelParser::readDegreeOfFreedom(tinyxml2::XMLElement* _dofElement, if(NULL==_dartJoint || NULL==_dofElement) return; - size_t localIndex = _dofElement->IntAttribute("number"); + int localIndex = -1; + int xml_err = _dofElement->QueryIntAttribute("local_index", &localIndex); + + // If the localIndex is out of bounds, quit + if(localIndex >= (int)_dartJoint->getNumDofs()) + { + dterr << "[SkelParser::readDegreeOfFreedom] Joint named '" + << _dartJoint->getName() << "' contains dof element with invalid " + << "number attribute [" << localIndex << "]. It must be less than " + << _dartJoint->getNumDofs() << ".\n"; + return; + } - if(localIndex >= _dartJoint->getNumDofs()) + // If no localIndex was found, report an error and quit + if(localIndex == -1 && _dartJoint->getNumDofs() > 1) { - dtwarn << "[SkelParser::readDegreeOfFreedom] Joint named '" - << _dartJoint->getName() << "' contains dof element with invalid " - << "number attribute [" << localIndex << "]. It must be less than " - << _dartJoint->getNumDofs() << ".\n"; + if(tinyxml2::XML_NO_ATTRIBUTE == xml_err) + { + dterr << "[SkelParser::readDegreeOfFreedom] Joint named '" + << _dartJoint->getName() << "' has " << _dartJoint->getNumDofs() + << " DOFs, but the xml contains a dof element without its " + << "local_index specified. For Joints with multiple DOFs, all dof " + << "elements must specify their local_index attribute.\n"; + } + else if(tinyxml2::XML_WRONG_ATTRIBUTE_TYPE == xml_err) + { + dterr << "[SkelParser::readDegreeOfFreedom] Joint named '" + << _dartJoint->getName() << "' has a dof element with a wrongly " + << "formatted local_index attribute.\n"; + } + return; } + // Unless the joint is a single-dof joint + else if(localIndex == -1 && _dartJoint->getNumDofs() == 1) + localIndex = 0; dart::dynamics::DegreeOfFreedom* dof = _dartJoint->getDof(localIndex); const char* name = _dofElement->Attribute("name"); @@ -1035,42 +1083,42 @@ void SkelParser::readDegreeOfFreedom(tinyxml2::XMLElement* _dofElement, if(hasElement(_dofElement, "position")) { - tinyxml2::XMLElement* posElement = getElement(_dofElement, "position"); setDofAttributes< &dart::dynamics::DegreeOfFreedom::setPositionLimits, &dart::dynamics::DegreeOfFreedom::getPositionLimits, &dart::dynamics::DegreeOfFreedom::setPosition, - &dart::dynamics::DegreeOfFreedom::getPosition>(posElement, dof); + &dart::dynamics::DegreeOfFreedom::getPosition>(_dofElement, dof, + "position"); } if(hasElement(_dofElement, "velocity")) { - tinyxml2::XMLElement* velElement = getElement(_dofElement, "velocity"); setDofAttributes< &dart::dynamics::DegreeOfFreedom::setVelocityLimits, &dart::dynamics::DegreeOfFreedom::getVelocityLimits, &dart::dynamics::DegreeOfFreedom::setVelocity, - &dart::dynamics::DegreeOfFreedom::getVelocity>(velElement, dof); + &dart::dynamics::DegreeOfFreedom::getVelocity>(_dofElement, dof, + "velocity"); } if(hasElement(_dofElement, "acceleration")) { - tinyxml2::XMLElement* accElement = getElement(_dofElement, "acceleration"); setDofAttributes< &dart::dynamics::DegreeOfFreedom::setAccelerationLimits, &dart::dynamics::DegreeOfFreedom::getAccelerationLimits, &dart::dynamics::DegreeOfFreedom::setAcceleration, - &dart::dynamics::DegreeOfFreedom::getAcceleration>(accElement, dof); + &dart::dynamics::DegreeOfFreedom::getAcceleration>(_dofElement, dof, + "acceleration"); } if(hasElement(_dofElement, "effort")) { - tinyxml2::XMLElement* effortElement = getElement(_dofElement, "effort"); setDofAttributes< &dart::dynamics::DegreeOfFreedom::setEffortLimits, &dart::dynamics::DegreeOfFreedom::getEffortLimits, &dart::dynamics::DegreeOfFreedom::setEffort, - &dart::dynamics::DegreeOfFreedom::getEffort>(effortElement, dof); + &dart::dynamics::DegreeOfFreedom::getEffort>(_dofElement, dof, + "effort"); } } diff --git a/data/skel/test/dof_attribute_test.skel b/data/skel/test/dof_attribute_test.skel index 078142d6decc9..dac9c8f9c4ab4 100644 --- a/data/skel/test/dof_attribute_test.skel +++ b/data/skel/test/dof_attribute_test.skel @@ -155,7 +155,7 @@ 1 0 0 - + @@ -180,10 +180,10 @@ +2 - + - + @@ -192,11 +192,11 @@ link 5 0 0 0 0 0 0 xyz - + - + From 5317dc655336f04693d16f8c0722881b5d204f8e Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Mon, 5 Jan 2015 19:39:23 -0500 Subject: [PATCH 24/30] added copyright to unit test --- unittests/testNameManagement.cpp | 35 ++++++++++++++++++++++++++++++++ 1 file changed, 35 insertions(+) diff --git a/unittests/testNameManagement.cpp b/unittests/testNameManagement.cpp index f053a05cf3a92..f81501183291d 100644 --- a/unittests/testNameManagement.cpp +++ b/unittests/testNameManagement.cpp @@ -1,3 +1,38 @@ +/* + * Copyright (c) 2014, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Michael X. Grey + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ #include #include From 88836b2fc5dfc7925c5e1ef2295d7679ce1e40ce Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 6 Jan 2015 00:09:35 -0500 Subject: [PATCH 25/30] Style fix --- unittests/testParser.cpp | 209 ++++++++++++++++++++------------------- 1 file changed, 108 insertions(+), 101 deletions(-) diff --git a/unittests/testParser.cpp b/unittests/testParser.cpp index 96484b724d7f3..96cd951fe96f6 100644 --- a/unittests/testParser.cpp +++ b/unittests/testParser.cpp @@ -53,121 +53,128 @@ using namespace dynamics; using namespace simulation; using namespace utils; -/******************************************************************************/ -TEST(SKEL_PARSER, DATA_STRUCTUER) +//============================================================================== +TEST(Parser, DataStructure) { - bool v1 = true; - int v2 = -3; - unsigned int v3 = 1; - float v4 = -3.140f; - double v5 = 1.4576640; - char v6 = 'd'; - Eigen::Vector2d v7 = Eigen::Vector2d::Ones(); - Eigen::Vector3d v8 = Eigen::Vector3d::Ones(); - //Eigen::Vector3d v9 = Eigen::Vector3d::Ones(); - //math::SO3 v10; - Eigen::Isometry3d v11 = Eigen::Isometry3d::Identity(); - - std::string str1 = toString(v1); - std::string str2 = toString(v2); - std::string str3 = toString(v3); - std::string str4 = toString(v4); - std::string str5 = toString(v5); - std::string str6 = toString(v6); - std::string str7 = toString(v7); - std::string str8 = toString(v8); - //std::string str9 = toString(v9); - //std::string str10 = toString(v10); - std::string str11 = toString(v11); - - bool b = toBool(str1); - int i = toInt(str2); - unsigned int ui = toUInt(str3); - float f = toFloat(str4); - double d = toDouble(str5); - char c = toChar(str6); - Eigen::Vector2d vec2 = toVector2d(str7); - Eigen::Vector3d vec3 = toVector3d(str8); - //Eigen::Vector3d valso3 = toVector3d(str9); - //math::SO3 valSO3 = toSO3(str10); - Eigen::Isometry3d valSE3 = toIsometry3d(str11); - - EXPECT_EQ(b, v1); - EXPECT_EQ(i, v2); - EXPECT_EQ(ui, v3); - EXPECT_EQ(f, v4); - EXPECT_EQ(d, v5); - EXPECT_EQ(c, v6); - for (int i = 0; i < 2; i++) { - EXPECT_EQ(vec2[i], v7[i]); - } - EXPECT_EQ(vec3, v8); - //EXPECT_EQ(valso3, v9); - //EXPECT_EQ(valSO3, v10); - for (int i = 0; i < 4; ++i) - for (int j = 0; j < 4; ++j) - EXPECT_EQ(valSE3(i,j), v11(i,j)); + bool v1 = true; + int v2 = -3; + unsigned int v3 = 1; + float v4 = -3.140f; + double v5 = 1.4576640; + char v6 = 'd'; + Eigen::Vector2d v7 = Eigen::Vector2d::Ones(); + Eigen::Vector3d v8 = Eigen::Vector3d::Ones(); + //Eigen::Vector3d v9 = Eigen::Vector3d::Ones(); + //math::SO3 v10; + Eigen::Isometry3d v11 = Eigen::Isometry3d::Identity(); + + std::string str1 = toString(v1); + std::string str2 = toString(v2); + std::string str3 = toString(v3); + std::string str4 = toString(v4); + std::string str5 = toString(v5); + std::string str6 = toString(v6); + std::string str7 = toString(v7); + std::string str8 = toString(v8); + //std::string str9 = toString(v9); + //std::string str10 = toString(v10); + std::string str11 = toString(v11); + + bool b = toBool(str1); + int i = toInt(str2); + unsigned int ui = toUInt(str3); + float f = toFloat(str4); + double d = toDouble(str5); + char c = toChar(str6); + Eigen::Vector2d vec2 = toVector2d(str7); + Eigen::Vector3d vec3 = toVector3d(str8); + //Eigen::Vector3d valso3 = toVector3d(str9); + //math::SO3 valSO3 = toSO3(str10); + Eigen::Isometry3d valSE3 = toIsometry3d(str11); + + EXPECT_EQ(b, v1); + EXPECT_EQ(i, v2); + EXPECT_EQ(ui, v3); + EXPECT_EQ(f, v4); + EXPECT_EQ(d, v5); + EXPECT_EQ(c, v6); + for (int i = 0; i < 2; i++) + EXPECT_EQ(vec2[i], v7[i]); + EXPECT_EQ(vec3, v8); + //EXPECT_EQ(valso3, v9); + //EXPECT_EQ(valSO3, v10); + for (int i = 0; i < 4; ++i) + { + for (int j = 0; j < 4; ++j) + EXPECT_EQ(valSE3(i,j), v11(i,j)); + } } -TEST(SKEL_PARSER, EMPTY) +//============================================================================== +TEST(Parser, EmptyWorld) { - World* world = SkelParser::readWorld(DART_DATA_PATH"skel/test/empty.skel"); + World* world = SkelParser::readWorld(DART_DATA_PATH"skel/test/empty.skel"); - EXPECT_TRUE(world != NULL); - EXPECT_EQ(world->getTimeStep(), 0.001); - EXPECT_EQ(world->getGravity()(0), 0); - EXPECT_EQ(world->getGravity()(1), 0); - EXPECT_EQ(world->getGravity()(2), -9.81); - EXPECT_EQ(world->getNumSkeletons(), 0); + EXPECT_TRUE(world != NULL); + EXPECT_EQ(world->getTimeStep(), 0.001); + EXPECT_EQ(world->getGravity()(0), 0); + EXPECT_EQ(world->getGravity()(1), 0); + EXPECT_EQ(world->getGravity()(2), -9.81); + EXPECT_EQ(world->getNumSkeletons(), 0); - EXPECT_EQ(world->getTime(), 0); - world->step(); - EXPECT_EQ(world->getTime(), world->getTimeStep()); + EXPECT_EQ(world->getTime(), 0); + world->step(); + EXPECT_EQ(world->getTime(), world->getTimeStep()); - delete world; + delete world; } -TEST(SKEL_PARSER, PENDULUM) +//============================================================================== +TEST(Parser, SinglePendulum) { - World* world = SkelParser::readWorld(DART_DATA_PATH"skel/test/single_pendulum.skel"); + World* world = SkelParser::readWorld( + DART_DATA_PATH"skel/test/single_pendulum.skel"); - EXPECT_TRUE(world != NULL); - EXPECT_EQ(world->getTimeStep(), 0.001); - EXPECT_EQ(world->getGravity()(0), 0); - EXPECT_EQ(world->getGravity()(1), -9.81); - EXPECT_EQ(world->getGravity()(2), 0); - EXPECT_EQ(world->getNumSkeletons(), 1); + EXPECT_TRUE(world != NULL); + EXPECT_EQ(world->getTimeStep(), 0.001); + EXPECT_EQ(world->getGravity()(0), 0); + EXPECT_EQ(world->getGravity()(1), -9.81); + EXPECT_EQ(world->getGravity()(2), 0); + EXPECT_EQ(world->getNumSkeletons(), 1); - Skeleton* skel1 = world->getSkeleton("single_pendulum"); + Skeleton* skel1 = world->getSkeleton("single_pendulum"); - EXPECT_EQ(skel1->getNumBodyNodes(), 1); + EXPECT_EQ(skel1->getNumBodyNodes(), 1); - world->step(); + world->step(); - delete world; + delete world; } -TEST(SKEL_PARSER, SERIAL_CAHIN) +//============================================================================== +TEST(Parser, SerialChain) { - World* world = SkelParser::readWorld(DART_DATA_PATH"skel/test/serial_chain_ball_joint.skel"); + World* world = SkelParser::readWorld( + DART_DATA_PATH"skel/test/serial_chain_ball_joint.skel"); - EXPECT_TRUE(world != NULL); - EXPECT_EQ(world->getTimeStep(), 0.001); - EXPECT_EQ(world->getGravity()(0), 0); - EXPECT_EQ(world->getGravity()(1), -9.81); - EXPECT_EQ(world->getGravity()(2), 0); - EXPECT_EQ(world->getNumSkeletons(), 1); + EXPECT_TRUE(world != NULL); + EXPECT_EQ(world->getTimeStep(), 0.001); + EXPECT_EQ(world->getGravity()(0), 0); + EXPECT_EQ(world->getGravity()(1), -9.81); + EXPECT_EQ(world->getGravity()(2), 0); + EXPECT_EQ(world->getNumSkeletons(), 1); - Skeleton* skel1 = world->getSkeleton("skeleton 1"); + Skeleton* skel1 = world->getSkeleton("skeleton 1"); - EXPECT_EQ(skel1->getNumBodyNodes(), 10); + EXPECT_EQ(skel1->getNumBodyNodes(), 10); - world->step(); + world->step(); - delete world; + delete world; } -TEST(SKEL_PARSER, RIGID_SOFT_BODIES) +//============================================================================== +TEST(Parser, RigidAndSoftBodies) { using namespace dart; using namespace math; @@ -175,8 +182,7 @@ TEST(SKEL_PARSER, RIGID_SOFT_BODIES) using namespace simulation; using namespace utils; - World* world - = SkelParser::readWorld( + World* world = SkelParser::readWorld( DART_DATA_PATH"skel/test/test_articulated_bodies.skel"); EXPECT_TRUE(world != NULL); @@ -197,7 +203,7 @@ TEST(SKEL_PARSER, RIGID_SOFT_BODIES) } //============================================================================== -TEST(SKEL_PARSER, PLANAR_JOINT) +TEST(Parser, PlanarJoint) { using namespace dart; using namespace math; @@ -315,10 +321,10 @@ TEST(SKEL_PARSER, PLANAR_JOINT) } //============================================================================== -TEST(SKEL_PARSER, DOF_ATTRIBUTES) +TEST(Parser, DofAttributes) { World* world = SkelParser::readWorld( - DART_DATA_PATH"/skel/test/dof_attribute_test.skel"); + DART_DATA_PATH"/skel/test/dof_attribute_test.skel"); EXPECT_TRUE(world != NULL); Skeleton* skel1 = world->getSkeleton("skeleton 1"); @@ -364,12 +370,12 @@ TEST(SKEL_PARSER, DOF_ATTRIBUTES) // Test for mixture of old method and new method // Note: If there is a conflict, the data given in the dof element will win Joint* joint3 = skel1->getJoint("joint3"); - EXPECT_EQ(joint3->getDof(0)->getName(), joint3->getName()+"_1"); + EXPECT_EQ(joint3->getDof(0)->getName(), joint3->getName() + "_1"); EXPECT_EQ(joint3->getDof(0)->getPositionLowerLimit(), -1); EXPECT_EQ(joint3->getDof(0)->getPositionUpperLimit(), 1); EXPECT_EQ(joint3->getDof(0)->getPosition(), 5); - EXPECT_EQ(joint3->getDof(1)->getName(), joint3->getName()+"_2"); + EXPECT_EQ(joint3->getDof(1)->getName(), joint3->getName() + "_2"); EXPECT_EQ(joint3->getDof(1)->getPositionLowerLimit(), -2); EXPECT_EQ(joint3->getDof(1)->getPositionUpperLimit(), 2); EXPECT_EQ(joint3->getDof(1)->getPosition(), -5); @@ -382,7 +388,7 @@ TEST(SKEL_PARSER, DOF_ATTRIBUTES) EXPECT_EQ(joint4->getDof(0)->getVelocityLowerLimit(), -10); EXPECT_EQ(joint4->getDof(0)->getVelocityUpperLimit(), 10); - EXPECT_EQ(joint4->getDof(1)->getName(), joint4->getName()+"_y"); + EXPECT_EQ(joint4->getDof(1)->getName(), joint4->getName() + "_y"); EXPECT_EQ(joint4->getDof(2)->getName(), "joint4_3"); EXPECT_EQ(joint4->getDof(2)->getPositionLowerLimit(), -2); @@ -391,9 +397,10 @@ TEST(SKEL_PARSER, DOF_ATTRIBUTES) EXPECT_EQ(joint4->getDof(2)->getVelocityUpperLimit(), 20); } -/******************************************************************************/ +//============================================================================== int main(int argc, char* argv[]) { - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); } + From 81d68ac73308b180e806ba6b2aea88794b321d03 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 6 Jan 2015 00:10:07 -0500 Subject: [PATCH 26/30] Remove outdated code in testParser.cpp --- unittests/testParser.cpp | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/unittests/testParser.cpp b/unittests/testParser.cpp index 96cd951fe96f6..0479054918de5 100644 --- a/unittests/testParser.cpp +++ b/unittests/testParser.cpp @@ -183,18 +183,16 @@ TEST(Parser, RigidAndSoftBodies) using namespace utils; World* world = SkelParser::readWorld( - DART_DATA_PATH"skel/test/test_articulated_bodies.skel"); + DART_DATA_PATH"skel/test/test_articulated_bodies.skel"); EXPECT_TRUE(world != NULL); Skeleton* skel1 = world->getSkeleton("skeleton 1"); - Skeleton* softSkel1 = dynamic_cast(skel1); - // TODO(MXG): ^This cast looks like it's outdated - EXPECT_TRUE(softSkel1 != NULL); - EXPECT_EQ(softSkel1->getNumBodyNodes(), 2); - EXPECT_EQ(softSkel1->getNumRigidBodyNodes(), 1); - EXPECT_EQ(softSkel1->getNumSoftBodyNodes(), 1); - - SoftBodyNode* sbn = softSkel1->getSoftBodyNode(0); + EXPECT_TRUE(skel1 != NULL); + EXPECT_EQ(skel1->getNumBodyNodes(), 2); + EXPECT_EQ(skel1->getNumRigidBodyNodes(), 1); + EXPECT_EQ(skel1->getNumSoftBodyNodes(), 1); + + SoftBodyNode* sbn = skel1->getSoftBodyNode(0); EXPECT_TRUE(sbn->getNumPointMasses() > 0); world->step(); From 79b9edf379ef7d9ef036e1c82eba321a50490548 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 6 Jan 2015 00:13:50 -0500 Subject: [PATCH 27/30] Style fix in PlanarJoint.cpp --- dart/dynamics/PlanarJoint.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dart/dynamics/PlanarJoint.cpp b/dart/dynamics/PlanarJoint.cpp index 3da59f39f726b..1255c6eb13add 100644 --- a/dart/dynamics/PlanarJoint.cpp +++ b/dart/dynamics/PlanarJoint.cpp @@ -178,7 +178,7 @@ void PlanarJoint::updateDegreeOfFreedomNames() for (size_t i = 0; i < 2; ++i) { if (!mDofs[i]->isNamePreserved()) - mDofs[i]->setName(mName+affixes[i], false); + mDofs[i]->setName(mName + affixes[i], false); } } } From ce2277c733016c637081254832c390b50a7a2286 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 6 Jan 2015 00:24:14 -0500 Subject: [PATCH 28/30] Remove unnecessary namespace specification --- dart/dynamics/DegreeOfFreedom.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/dart/dynamics/DegreeOfFreedom.h b/dart/dynamics/DegreeOfFreedom.h index d4f30f3e0f713..e82828158a3da 100644 --- a/dart/dynamics/DegreeOfFreedom.h +++ b/dart/dynamics/DegreeOfFreedom.h @@ -55,10 +55,10 @@ class DegreeOfFreedom { public: - friend class dart::dynamics::Joint; - friend class dart::dynamics::SingleDofJoint; - template friend class dart::dynamics::MultiDofJoint; - friend class dart::dynamics::Skeleton; + friend class Joint; + friend class SingleDofJoint; + template friend class MultiDofJoint; + friend class Skeleton; /// Change the name of this DegreeOfFreedom /// From d083568ba7e038fca4052031fe639b4e790c1666 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Fri, 9 Jan 2015 21:53:31 -0500 Subject: [PATCH 29/30] converted usage of 'effort' to 'force' --- dart/dynamics/DegreeOfFreedom.cpp | 28 +++++++++++++------------- dart/dynamics/DegreeOfFreedom.h | 25 +++++++++-------------- dart/dynamics/Skeleton.cpp | 10 ++++----- dart/utils/SkelParser.cpp | 12 +++++------ data/skel/test/dof_attribute_test.skel | 2 +- unittests/testParser.cpp | 6 +++--- 6 files changed, 39 insertions(+), 44 deletions(-) diff --git a/dart/dynamics/DegreeOfFreedom.cpp b/dart/dynamics/DegreeOfFreedom.cpp index c3999d84abbb4..3cc5115930dd9 100644 --- a/dart/dynamics/DegreeOfFreedom.cpp +++ b/dart/dynamics/DegreeOfFreedom.cpp @@ -265,56 +265,56 @@ double DegreeOfFreedom::getAccelerationUpperLimit() const } //============================================================================== -void DegreeOfFreedom::setEffort(double _effort) +void DegreeOfFreedom::setForce(double _force) { - mJoint->setForce(mIndexInJoint, _effort); + mJoint->setForce(mIndexInJoint, _force); } //============================================================================== -double DegreeOfFreedom::getEffort() const +double DegreeOfFreedom::getForce() const { return mJoint->getForce(mIndexInJoint); } //============================================================================== -void DegreeOfFreedom::setEffortLimits(double _lowerLimit, double _upperLimit) +void DegreeOfFreedom::setForceLimits(double _lowerLimit, double _upperLimit) { - setEffortLowerLimit(_lowerLimit); - setEffortUpperLimit(_upperLimit); + setForceLowerLimit(_lowerLimit); + setForceUpperLimit(_upperLimit); } //============================================================================== -void DegreeOfFreedom::setEffortLimits(const std::pair &_limits) +void DegreeOfFreedom::setForceLimits(const std::pair &_limits) { - setEffortLimits(_limits.first, _limits.second); + setForceLimits(_limits.first, _limits.second); } //============================================================================== -std::pair DegreeOfFreedom::getEffortLimits() const +std::pair DegreeOfFreedom::getForceLimits() const { - return std::pair(getEffortLowerLimit(), getEffortUpperLimit()); + return std::pair(getForceLowerLimit(), getForceUpperLimit()); } //============================================================================== -void DegreeOfFreedom::setEffortLowerLimit(double _limit) +void DegreeOfFreedom::setForceLowerLimit(double _limit) { mJoint->setForceLowerLimit(mIndexInJoint, _limit); } //============================================================================== -double DegreeOfFreedom::getEffortLowerLimit() const +double DegreeOfFreedom::getForceLowerLimit() const { return mJoint->getForceLowerLimit(mIndexInJoint); } //============================================================================== -void DegreeOfFreedom::setEffortUpperLimit(double _limit) +void DegreeOfFreedom::setForceUpperLimit(double _limit) { mJoint->setForceUpperLimit(mIndexInJoint, _limit); } //============================================================================== -double DegreeOfFreedom::getEffortUpperLimit() const +double DegreeOfFreedom::getForceUpperLimit() const { return mJoint->getForceUpperLimit(mIndexInJoint); } diff --git a/dart/dynamics/DegreeOfFreedom.h b/dart/dynamics/DegreeOfFreedom.h index e82828158a3da..fa1c09d9e4028 100644 --- a/dart/dynamics/DegreeOfFreedom.h +++ b/dart/dynamics/DegreeOfFreedom.h @@ -173,39 +173,34 @@ class DegreeOfFreedom /// Get the upper acceleration limit of this DegreeOfFreedom double getAccelerationUpperLimit() const; - // -- Effort/Force functions ------------------------------------------------- - // Note: In these functions, the word "Effort" is being used instead of - // "Force". Currently "Force" is being used throughout DART to refer to the - // Generalized Force of a Generalized Coordinate, but I propose we use the - // word "Effort" instead. We can change the names of these functions later if - // it's decided that "Effort" is not a good word. + // -- Force functions -------------------------------------------------------- /// Set the generalized force of this DegreeOfFreedom - void setEffort(double _effort); + void setForce(double _force); /// Get the generalized force of this DegreeOfFreedom - double getEffort() const; + double getForce() const; /// Set the generalized force limits of this DegreeOfFreedom - void setEffortLimits(double _lowerLimit, double _upperLimit); + void setForceLimits(double _lowerLimit, double _upperLimit); /// Set the generalized force limits of this DegreeOfFreedom - void setEffortLimits(const std::pair& _limits); + void setForceLimits(const std::pair& _limits); /// Get the generalized force limits of this DegreeOfFreedom - std::pair getEffortLimits() const; + std::pair getForceLimits() const; /// Set the lower generalized force limit of this DegreeOfFreedom - void setEffortLowerLimit(double _limit); + void setForceLowerLimit(double _limit); /// Get the lower generalized force limit of this DegreeOfFreedom - double getEffortLowerLimit() const; + double getForceLowerLimit() const; /// Set the upper generalized force limit of this DegreeOfFreedom - void setEffortUpperLimit(double _limit); + void setForceUpperLimit(double _limit); /// Get the upper generalized force limit of this DegreeOfFreedom - double getEffortUpperLimit() const; + double getForceUpperLimit() const; // -- Relationships ---------------------------------------------------------- diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index 0f0b4b3fc31de..c4fd3adff30bb 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -780,7 +780,7 @@ void Skeleton::setForce(size_t _index, double _force) { assert(_index < getNumDofs()); - mDofs[_index]->setEffort(_force); + mDofs[_index]->setForce(_force); } //============================================================================== @@ -788,7 +788,7 @@ double Skeleton::getForce(size_t _index) { assert(_index getEffort(); + return mDofs[_index]->getForce(); } //============================================================================== @@ -846,7 +846,7 @@ void Skeleton::setForceLowerLimit(size_t _index, double _force) { assert(_index < getNumDofs()); - mDofs[_index]->setEffortLowerLimit(_force); + mDofs[_index]->setForceLowerLimit(_force); } //============================================================================== @@ -854,7 +854,7 @@ double Skeleton::getForceLowerLimit(size_t _index) { assert(_index getEffortLowerLimit(); + return mDofs[_index]->getForceLowerLimit(); } //============================================================================== @@ -862,7 +862,7 @@ void Skeleton::setForceUpperLimit(size_t _index, double _force) { assert(_index < getNumDofs()); - mDofs[_index]->setEffortUpperLimit(_force); + mDofs[_index]->setForceUpperLimit(_force); } //============================================================================== diff --git a/dart/utils/SkelParser.cpp b/dart/utils/SkelParser.cpp index f5d8936e66b62..80a5a25d10bc9 100644 --- a/dart/utils/SkelParser.cpp +++ b/dart/utils/SkelParser.cpp @@ -1111,14 +1111,14 @@ void SkelParser::readDegreeOfFreedom(tinyxml2::XMLElement* _dofElement, "acceleration"); } - if(hasElement(_dofElement, "effort")) + if(hasElement(_dofElement, "force")) { setDofAttributes< - &dart::dynamics::DegreeOfFreedom::setEffortLimits, - &dart::dynamics::DegreeOfFreedom::getEffortLimits, - &dart::dynamics::DegreeOfFreedom::setEffort, - &dart::dynamics::DegreeOfFreedom::getEffort>(_dofElement, dof, - "effort"); + &dart::dynamics::DegreeOfFreedom::setForceLimits, + &dart::dynamics::DegreeOfFreedom::getForceLimits, + &dart::dynamics::DegreeOfFreedom::setForce, + &dart::dynamics::DegreeOfFreedom::getForce>(_dofElement, dof, + "force"); } } diff --git a/data/skel/test/dof_attribute_test.skel b/data/skel/test/dof_attribute_test.skel index dac9c8f9c4ab4..cf7127a77d8fd 100644 --- a/data/skel/test/dof_attribute_test.skel +++ b/data/skel/test/dof_attribute_test.skel @@ -159,7 +159,7 @@ - + diff --git a/unittests/testParser.cpp b/unittests/testParser.cpp index 0479054918de5..cf06385a49759 100644 --- a/unittests/testParser.cpp +++ b/unittests/testParser.cpp @@ -361,9 +361,9 @@ TEST(Parser, DofAttributes) EXPECT_EQ(joint2->getDof(0)->getAccelerationUpperLimit(), 3); EXPECT_EQ(joint2->getDof(0)->getAcceleration(), 0); - EXPECT_EQ(joint2->getDof(0)->getEffortLowerLimit(), -4); - EXPECT_EQ(joint2->getDof(0)->getEffortUpperLimit(), 4); - EXPECT_EQ(joint2->getDof(0)->getEffort(), 0); + EXPECT_EQ(joint2->getDof(0)->getForceLowerLimit(), -4); + EXPECT_EQ(joint2->getDof(0)->getForceUpperLimit(), 4); + EXPECT_EQ(joint2->getDof(0)->getForce(), 0); // Test for mixture of old method and new method // Note: If there is a conflict, the data given in the dof element will win From df8fe62d9a8a2c69673c4069e385769e8c672807 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Tue, 13 Jan 2015 19:33:52 -0500 Subject: [PATCH 30/30] corrected minor merge error --- dart/dynamics/MultiDofJoint.h | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/dart/dynamics/MultiDofJoint.h b/dart/dynamics/MultiDofJoint.h index d00a3f06e7fa2..8c833f96e12ac 100644 --- a/dart/dynamics/MultiDofJoint.h +++ b/dart/dynamics/MultiDofJoint.h @@ -74,7 +74,7 @@ class MultiDofJoint : public Joint virtual size_t getDof() const; // Documentation inherited - virtual size_t getNumDofs() const; + virtual DegreeOfFreedom* getDof(size_t index); // Documentation inherited virtual const DegreeOfFreedom* getDof(size_t _index) const; @@ -88,12 +88,6 @@ class MultiDofJoint : public Joint // Documentation inherited virtual size_t getIndexInSkeleton(size_t _index) const; - // Documentation inherited - virtual void setIndexInSkeleton(size_t _index, size_t _indexInSkeleton); - - // Documentation inherited - virtual size_t getIndexInSkeleton(size_t _index) const; - //---------------------------------------------------------------------------- // Command //----------------------------------------------------------------------------