From 2cfd9c56d34896abb07aaa4c2533a50af5e3d6df Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Fri, 22 Apr 2016 15:24:26 -0400 Subject: [PATCH 1/4] Refactored Marker class into being a FixedJacobianNode --- dart/collision/dart/DARTCollide.cpp | 1 + dart/constraint/ContactConstraint.cpp | 1 + dart/dynamics/BodyNode.cpp | 71 ++++------------ dart/dynamics/BodyNode.h | 32 +++----- dart/dynamics/EndEffector.h | 5 +- dart/dynamics/Marker.cpp | 114 +++++++++++++------------- dart/dynamics/Marker.h | 95 ++++++++------------- dart/dynamics/MetaSkeleton.h | 2 +- dart/dynamics/Skeleton.cpp | 41 +-------- dart/dynamics/Skeleton.h | 21 +---- dart/dynamics/detail/BodyNodeAspect.h | 4 - dart/dynamics/detail/MarkerAspect.h | 72 ++++++++++++++++ dart/gui/SimWindow.cpp | 1 + dart/utils/SkelParser.cpp | 21 +++-- dart/utils/VskParser.cpp | 4 +- dart/utils/VskParser.h | 1 + 16 files changed, 209 insertions(+), 277 deletions(-) create mode 100644 dart/dynamics/detail/MarkerAspect.h diff --git a/dart/collision/dart/DARTCollide.cpp b/dart/collision/dart/DARTCollide.cpp index 7e453ad2f5a92..360ab4d9d80ad 100644 --- a/dart/collision/dart/DARTCollide.cpp +++ b/dart/collision/dart/DARTCollide.cpp @@ -43,6 +43,7 @@ #include "dart/dynamics/EllipsoidShape.h" #include "dart/dynamics/CylinderShape.h" #include "dart/dynamics/BodyNode.h" +#include "dart/math/Helpers.h" namespace dart { namespace collision { diff --git a/dart/constraint/ContactConstraint.cpp b/dart/constraint/ContactConstraint.cpp index 960a195fc853e..a8f587851a36b 100644 --- a/dart/constraint/ContactConstraint.cpp +++ b/dart/constraint/ContactConstraint.cpp @@ -43,6 +43,7 @@ #include "dart/dynamics/Skeleton.h" #include "dart/collision/CollisionObject.h" #include "dart/lcpsolver/lcp.h" +#include "dart/math/Helpers.h" #define DART_EPSILON 1e-6 #define DART_ERROR_ALLOWANCE 0.0 diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index c91cd39fda42e..444511a7e68de 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -243,13 +243,6 @@ BodyNode::~BodyNode() mNodeMap.clear(); mNodeDestructors.clear(); - // Release markers - for (std::vector::const_iterator it = mMarkers.begin(); - it != mMarkers.end(); ++it) - { - delete (*it); - } - delete mParentJoint; } @@ -329,17 +322,6 @@ void BodyNode::setAspectProperties(const AspectProperties& properties) setGravityMode(properties.mGravityMode); setFrictionCoeff(properties.mFrictionCoeff); setRestitutionCoeff(properties.mRestitutionCoeff); - - // TODO(MXG): Make Markers into Nodes before DART 6.0 - mAspectProperties.mMarkerProperties = properties.mMarkerProperties; - // Remove current markers - for(Marker* marker : mMarkers) - delete marker; - - // Create new markers - mMarkers.clear(); - for(const Marker::Properties& marker : mAspectProperties.mMarkerProperties) - addMarker(new Marker(marker, this)); } //============================================================================== @@ -899,6 +881,9 @@ const Joint* BodyNode::getChildJoint(size_t _index) const return const_cast(this)->getChildJoint(_index); } +//============================================================================== +DART_BAKE_SPECIALIZED_NODE_DEFINITIONS( BodyNode, ShapeNode ) + //============================================================================== ShapeNode* BodyNode::createShapeNode(const ShapePtr& shape) { @@ -925,24 +910,6 @@ ShapeNode* BodyNode::createShapeNode(const ShapePtr& shape, const char* name) return createShapeNode(shape, std::string(name)); } -//============================================================================== -size_t BodyNode::getNumShapeNodes() const -{ - return getNumNodes(); -} - -//============================================================================== -ShapeNode* BodyNode::getShapeNode(size_t index) -{ - return getNode(index); -} - -//============================================================================== -const ShapeNode* BodyNode::getShapeNode(size_t index) const -{ - return getNode(index); -} - //============================================================================== const std::vector BodyNode::getShapeNodes() { @@ -977,6 +944,9 @@ void BodyNode::removeAllShapeNodes() shapeNode->remove(); } +//============================================================================== +DART_BAKE_SPECIALIZED_NODE_DEFINITIONS( BodyNode, EndEffector ) + //============================================================================== EndEffector* BodyNode::createEndEffector(const std::string& _name) { @@ -993,30 +963,19 @@ EndEffector* BodyNode::createEndEffector(const char* _name) } //============================================================================== -void BodyNode::addMarker(Marker* _marker) -{ - mMarkers.push_back(_marker); - const SkeletonPtr& skel = getSkeleton(); - if(skel) - skel->addEntryToMarkerNameMgr(_marker); -} - -//============================================================================== -size_t BodyNode::getNumMarkers() const -{ - return mMarkers.size(); -} +DART_BAKE_SPECIALIZED_NODE_DEFINITIONS( BodyNode, Marker ) //============================================================================== -Marker* BodyNode::getMarker(size_t _index) +Marker* BodyNode::createMarker(const std::string& name, + const Eigen::Vector3d& position, + const Eigen::Vector4d& color) { - return common::getVectorObjectIfAvailable(_index, mMarkers); -} + Marker::BasicProperties properties; + properties.mName = name; + properties.mRelativeTf.translation() = position; + properties.mColor = color; -//============================================================================== -const Marker* BodyNode::getMarker(size_t _index) const -{ - return common::getVectorObjectIfAvailable(_index, mMarkers); + return createNode(properties); } //============================================================================== diff --git a/dart/dynamics/BodyNode.h b/dart/dynamics/BodyNode.h index b3f770b6b0fc8..1a01b02887704 100644 --- a/dart/dynamics/BodyNode.h +++ b/dart/dynamics/BodyNode.h @@ -535,6 +535,8 @@ class BodyNode : template NodeType* createNode(Args&&... args); + DART_BAKE_SPECIALIZED_NODE_DECLARATIONS( ShapeNode ) + /// Create an ShapeNode attached to this BodyNode. Pass a /// ShapeNode::Properties argument into its constructor. If automaticName is /// true, then the mName field of properties will be ignored, and the @@ -555,15 +557,6 @@ class BodyNode : /// Create an ShapeNode with the specified name ShapeNode* createShapeNode(const ShapePtr& shape, const char* name); - /// Return the number of all the ShapeNodes in this BodyNode - size_t getNumShapeNodes() const; - - /// Return the index-th ShapeNode - ShapeNode* getShapeNode(size_t index); - - /// Return the index-th (const) ShapeNode - const ShapeNode* getShapeNode(size_t index) const; - /// Return the list of ShapeNodes const std::vector getShapeNodes(); @@ -599,6 +592,8 @@ class BodyNode : template void removeAllShapeNodesWith(); + DART_BAKE_SPECIALIZED_NODE_DECLARATIONS( EndEffector ) + /// Create an EndEffector attached to this BodyNode. Pass an /// EndEffector::Properties argument into this function. template @@ -610,17 +605,13 @@ class BodyNode : /// Create an EndEffector with the specified name EndEffector* createEndEffector(const char* _name); - /// Add a marker into the bodynode - void addMarker(Marker* _marker); - - /// Return the number of markers of the bodynode - size_t getNumMarkers() const; + DART_BAKE_SPECIALIZED_NODE_DECLARATIONS( Marker ) - /// Return _index-th marker of the bodynode - Marker* getMarker(size_t _index); - - /// Return (const) _index-th marker of the bodynode - const Marker* getMarker(size_t _index) const; + /// Create a Marker with the given fields + Marker* createMarker( + const std::string& name = "marker", + const Eigen::Vector3d& position = Eigen::Vector3d::Zero(), + const Eigen::Vector4d& color = Eigen::Vector4d::Constant(1.0)); // Documentation inherited bool dependsOn(size_t _genCoordIndex) const override; @@ -1071,9 +1062,6 @@ class BodyNode : /// allows some performance optimizations. std::set mNonBodyNodeEntities; - /// List of markers associated - std::vector mMarkers; - /// A increasingly sorted list of dependent dof indices. std::vector mDependentGenCoordIndices; diff --git a/dart/dynamics/EndEffector.h b/dart/dynamics/EndEffector.h index be94396ddeaf2..ebc05604b7ffa 100644 --- a/dart/dynamics/EndEffector.h +++ b/dart/dynamics/EndEffector.h @@ -158,9 +158,8 @@ class EndEffector final : /// Constructor used by the Skeleton class explicit EndEffector(BodyNode* parent, const BasicProperties& properties); - /// Create a clone of this BodyNode. This may only be called by the Skeleton - /// class. - virtual Node* cloneNode(BodyNode* _parent) const override; + // Documentation inherited + Node* cloneNode(BodyNode* _parent) const override; public: diff --git a/dart/dynamics/Marker.cpp b/dart/dynamics/Marker.cpp index 2c3c8f23ae3cf..ef9b5ac17a5b6 100644 --- a/dart/dynamics/Marker.cpp +++ b/dart/dynamics/Marker.cpp @@ -46,127 +46,125 @@ namespace dynamics { int Marker::msMarkerCount = 0; //============================================================================== -Marker::Properties::Properties(const std::string& name, - const Eigen::Vector3d& offset, - const Eigen::Vector4d& color, - ConstraintType type) - : mName(name), mOffset(offset), mColor(color), mType(type) -{ - // Do nothing -} +// These declarations are needed for linking to work +constexpr Marker::ConstraintType Marker::NO; +constexpr Marker::ConstraintType Marker::HARD; +constexpr Marker::ConstraintType Marker::SOFT; + +namespace detail { //============================================================================== -Marker::Marker(const std::string& name, - const Eigen::Vector3d& offset, - const Eigen::Vector4d& color, - BodyNode* bodyNode, - ConstraintType type) - : mProperties(name, offset, color, type), - mBodyNode(bodyNode), - mSkelIndex(0), - mID(Marker::msMarkerCount++) +MarkerProperties::MarkerProperties(const Eigen::Vector4d& color, + ConstraintType type) + : mColor(color), + mType(type) { // Do nothing } +} // namespace detail + //============================================================================== -Marker::~Marker() +void Marker::setAspectProperties(const AspectProperties& properties) { - // Do nothing + setColor(properties.mColor); + setConstraintType(properties.mType); } //============================================================================== BodyNode* Marker::getBodyNode() { - return mBodyNode; + return getBodyNodePtr(); } //============================================================================== const BodyNode* Marker::getBodyNode() const { - return mBodyNode; + return getBodyNodePtr(); } //============================================================================== -const Eigen::Vector3d& Marker::getLocalPosition() const +Eigen::Vector3d Marker::getLocalPosition() const { - return mProperties.mOffset; + return getRelativeTransform().translation(); } //============================================================================== void Marker::setLocalPosition(const Eigen::Vector3d& offset) { - mProperties.mOffset = offset; + Eigen::Isometry3d tf = getRelativeTransform(); + tf.translation() = offset; + setRelativeTransform(tf); } //============================================================================== Eigen::Vector3d Marker::getWorldPosition() const { - return mBodyNode->getTransform() * mProperties.mOffset; + return getWorldTransform().translation(); } //============================================================================== -void Marker::setSkeletonIndex(int index) +int Marker::getID() const { - setIndexInSkeleton(index); + return mID; } //============================================================================== -void Marker::setIndexInSkeleton(int index) +void Marker::setConstraintType(Marker::ConstraintType type) { - mSkelIndex = index; -} + if(type == mAspectProperties.mType) + return; -//============================================================================== -int Marker::getIndexInSkeleton() const -{ - return mSkelIndex; + mAspectProperties.mType = type; + incrementVersion(); } //============================================================================== -int Marker::getID() const +Marker::ConstraintType Marker::getConstraintType() const { - return mID; + return mAspectProperties.mType; } //============================================================================== -void Marker::setName(const std::string& name) +void Marker::setColor(const Eigen::Vector4d& color) { - mProperties.mName = name; -} + if(color == mAspectProperties.mColor) + return; -//============================================================================== -const std::string& Marker::getName() const -{ - return mProperties.mName; + mAspectProperties.mColor = color; + incrementVersion(); } //============================================================================== -void Marker::setConstraintType(Marker::ConstraintType type) +const Eigen::Vector4d& Marker::getColor() const { - mProperties.mType = type; + return mAspectProperties.mColor; } //============================================================================== -Marker::ConstraintType Marker::getConstraintType() const +Marker::Marker(BodyNode* parent, const BasicProperties& properties) + : Entity(ConstructFrame), + Frame(parent), + FixedFrame(parent, properties.mRelativeTf), + common::EmbedPropertiesOnTopOf< + Marker, detail::MarkerProperties, FixedJacobianNode>( + parent, properties.mRelativeTf), + mID(Marker::msMarkerCount++) { - return mProperties.mType; + createAspect(); + setCompositeProperties(properties); } //============================================================================== -const Eigen::Vector4d& Marker::getColor() const +Node* Marker::cloneNode(BodyNode* parent) const { - return mProperties.mColor; -} + Marker* marker = new Marker(parent, BasicProperties()); + marker->duplicateAspects(this); -//============================================================================== -Marker::Marker(const Properties& properties, BodyNode* parent) - : mProperties(properties), - mBodyNode(parent), - mSkelIndex(0), - mID(Marker::msMarkerCount++) -{ - // Do nothing + if(mIK) + marker->mIK = mIK->clone(marker); + + return marker; } } // namespace dynamics diff --git a/dart/dynamics/Marker.h b/dart/dynamics/Marker.h index 58a67b9b49d11..97ebc6950cefe 100644 --- a/dart/dynamics/Marker.h +++ b/dart/dynamics/Marker.h @@ -40,58 +40,53 @@ #include #include #include "dart/common/Deprecated.h" -#include "dart/math/Helpers.h" +#include "dart/dynamics/detail/MarkerAspect.h" +#include "dart/dynamics/FixedJacobianNode.h" namespace dart { namespace dynamics { class BodyNode; -class Marker +class Marker final : + public common::EmbedPropertiesOnTopOf< + Marker, detail::MarkerProperties, + FixedJacobianNode> { public: - enum ConstraintType - { - NO, - HARD, - SOFT - }; - - struct Properties - { - std::string mName; - Eigen::Vector3d mOffset; - Eigen::Vector4d mColor; - ConstraintType mType; - - Properties(const std::string& name = "", - const Eigen::Vector3d& offset = Eigen::Vector3d::Zero(), - const Eigen::Vector4d& color = Color::White(1.0), - ConstraintType type = NO); - - // To get byte-aligned Eigen vectors - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; - - /// Constructor - Marker(const std::string& name, - const Eigen::Vector3d& offset, - const Eigen::Vector4d& color, - BodyNode* bodyNode, - ConstraintType type = NO); + using ConstraintType = detail::MarkerProperties::ConstraintType; + static constexpr ConstraintType NO = detail::MarkerProperties::NO; + static constexpr ConstraintType HARD = detail::MarkerProperties::HARD; + static constexpr ConstraintType SOFT = detail::MarkerProperties::SOFT; + + using BasicProperties = common::Composite::MakeProperties< + NameAspect, + FixedFrame, + Marker>; + + using Properties = common::Composite::Properties; /// Destructor - virtual ~Marker(); + virtual ~Marker() = default; + + /// Set the AspectProperties of this Marker + void setAspectProperties(const AspectProperties& properties); /// Get the BodyNode this Marker belongs to + /// + /// Deprecated: Use getBodyNodePtr() instead + DEPRECATED(6.0) BodyNode* getBodyNode(); /// Get the (const) BodyNode this Marker belongs to + /// + /// Deprecated: Use getBodyNodePtr() instead + DEPRECATED(6.0) const BodyNode* getBodyNode() const; /// Get position of this marker in the parent body node coordinates - const Eigen::Vector3d& getLocalPosition() const; + Eigen::Vector3d getLocalPosition() const; /// Set position of this marker in the parent body node coordinates void setLocalPosition(const Eigen::Vector3d& offset); @@ -99,52 +94,30 @@ class Marker /// Get position in the world coordinates Eigen::Vector3d getWorldPosition() const; - /// Deprecated; please use setIndexInSkeleton() instead - DEPRECATED(6.0) - void setSkeletonIndex(int index); - - /// Set index in skeleton this marker is belongs to - void setIndexInSkeleton(int index); - // TODO(JS): This function is not called by any. Remove? - - /// Get index in skeleton this marker is belongs to - int getIndexInSkeleton() const; - // TODO(JS): This function is not called by any. Remove? - /// Get global unique ID int getID() const; - /// Set name of this marker - void setName(const std::string& name); - - /// Get name of this marker - const std::string& getName() const; - /// Set constraint type. which will be useful for inverse kinematics void setConstraintType(ConstraintType type); /// Get constraint type. which will be useful for inverse kinematics ConstraintType getConstraintType() const; + /// Set the color of this Marker + void setColor(const Eigen::Vector4d& color); + /// Return color of this Marker const Eigen::Vector4d& getColor() const; - friend class Skeleton; friend class BodyNode; protected: /// Constructor used by BodyNode - Marker(const Properties& properties, BodyNode* parent); - - /// \brief Properties of this Marker - Properties mProperties; - - /// \brief BodyNode this marker belongs to - BodyNode* mBodyNode; + Marker(BodyNode* parent, const BasicProperties& properties); - /// \brief position in the model class marker vector. - int mSkelIndex; + // Documentation inherited + Node* cloneNode(BodyNode* parent) const override; private: /// Unique ID of this marker globally. diff --git a/dart/dynamics/MetaSkeleton.h b/dart/dynamics/MetaSkeleton.h index 53e4aa36fce5c..bce6dc60bab5a 100644 --- a/dart/dynamics/MetaSkeleton.h +++ b/dart/dynamics/MetaSkeleton.h @@ -55,8 +55,8 @@ class BodyNode; class SoftBodyNode; class PointMass; class Joint; -class Marker; class DegreeOfFreedom; +class Marker; /// MetaSkeleton is a pure abstract base class that provides a common interface /// for obtaining data (such as Jacobians and Mass Matrices) from groups of diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index adec9741a9792..9c9e296ef17ab 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -587,8 +587,6 @@ const std::string& Skeleton::setName(const std::string& _name) "Skeleton::Joint | "+mAspectProperties.mName); mNameMgrForDofs.setManagerName( "Skeleton::DegreeOfFreedom | "+mAspectProperties.mName); - mNameMgrForMarkers.setManagerName( - "Skeleton::Marker | "+mAspectProperties.mName); for(auto& mgr : mNodeNameMgrMap) mgr.second.setManagerName( std::string("Skeleton::") + mgr.first.name() @@ -637,28 +635,6 @@ void Skeleton::addEntryToSoftBodyNodeNameMgr(SoftBodyNode* _newNode) mNameMgrForSoftBodyNodes.addName(_newNode->getName(), _newNode); } -//============================================================================== -void Skeleton::addMarkersOfBodyNode(BodyNode* _node) -{ - 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) - mNameMgrForMarkers.removeName(_node->getMarker(i)->getName()); -} - -//============================================================================== -const std::string& Skeleton::addEntryToMarkerNameMgr(Marker* _newMarker) -{ - _newMarker->mProperties.mName = mNameMgrForMarkers.issueNewNameAndAdd( - _newMarker->getName(), _newMarker); - return _newMarker->mProperties.mName; -} - //============================================================================== void Skeleton::enableSelfCollision(bool _enableAdjacentBodyCheck) { @@ -1310,22 +1286,7 @@ void Skeleton::clearIK() } //============================================================================== -size_t Skeleton::getNumMarkers() const -{ - return mNameMgrForMarkers.getCount(); -} - -//============================================================================== -Marker* Skeleton::getMarker(const std::string& _name) -{ - return mNameMgrForMarkers.getObject(_name); -} - -//============================================================================== -const Marker* Skeleton::getMarker(const std::string& _name) const -{ - return const_cast(this)->getMarker(_name); -} +DART_BAKE_SPECIALIZED_NODE_SKEL_DEFINITIONS( Skeleton, Marker ) //============================================================================== DART_BAKE_SPECIALIZED_NODE_SKEL_DEFINITIONS( Skeleton, ShapeNode ) diff --git a/dart/dynamics/Skeleton.h b/dart/dynamics/Skeleton.h index 52a621a387ff3..7c250617577d5 100644 --- a/dart/dynamics/Skeleton.h +++ b/dart/dynamics/Skeleton.h @@ -437,14 +437,7 @@ class Skeleton : /// nullptr void clearIK(); - /// Get total number of markers in this Skeleton - size_t getNumMarkers() const; - - /// Get marker whose name is _name - Marker* getMarker(const std::string& _name); - - /// Get const marker whose name is _name - const Marker* getMarker(const std::string& _name) const; + DART_BAKE_SPECIALIZED_NODE_SKEL_DECLARATIONS( Marker ) DART_BAKE_SPECIALIZED_NODE_SKEL_DECLARATIONS( ShapeNode ) @@ -1054,15 +1047,6 @@ class Skeleton : /// Add a SoftBodyNode to the SoftBodyNode NameManager void addEntryToSoftBodyNodeNameMgr(SoftBodyNode* _newNode); - /// Add entries for all the Markers belonging to BodyNode _node - void addMarkersOfBodyNode(BodyNode* _node); - - /// Remove entries for all the Markers belonging to BodyNode _node - void removeMarkersOfBodyNode(BodyNode* _node); - - /// Add a Marker entry - const std::string& addEntryToMarkerNameMgr(Marker* _newMarker); - protected: /// The resource-managing pointer to this Skeleton @@ -1083,9 +1067,6 @@ class Skeleton : /// NameManager for tracking SoftBodyNodes dart::common::NameManager mNameMgrForSoftBodyNodes; - /// NameManager for tracking Markers - dart::common::NameManager mNameMgrForMarkers; - /// WholeBodyIK module for this Skeleton std::shared_ptr mWholeBodyIK; diff --git a/dart/dynamics/detail/BodyNodeAspect.h b/dart/dynamics/detail/BodyNodeAspect.h index d0fbed27ca942..2f708a1756bf8 100644 --- a/dart/dynamics/detail/BodyNodeAspect.h +++ b/dart/dynamics/detail/BodyNodeAspect.h @@ -40,7 +40,6 @@ #include "dart/dynamics/Entity.h" #include "dart/dynamics/Inertia.h" #include "dart/dynamics/Node.h" -#include "dart/dynamics/Marker.h" #include "dart/common/ProxyAspect.h" #include "dart/common/EmbeddedAspect.h" @@ -89,9 +88,6 @@ struct BodyNodeAspectProperties /// Gravity will be applied if true bool mGravityMode; - /// Properties of the Markers belonging to this BodyNode - std::vector mMarkerProperties; - /// Constructor BodyNodeAspectProperties( const std::string& name = "BodyNode", diff --git a/dart/dynamics/detail/MarkerAspect.h b/dart/dynamics/detail/MarkerAspect.h new file mode 100644 index 0000000000000..afd8a99f99737 --- /dev/null +++ b/dart/dynamics/detail/MarkerAspect.h @@ -0,0 +1,72 @@ +/* + * Copyright (c) 2016, 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_DETAIL_MARKER_H_ +#define DART_DYNAMICS_DETAIL_MARKER_H_ + +#include +#include "dart/math/Helpers.h" + +namespace dart { +namespace dynamics { +namespace detail { + +struct MarkerProperties +{ + enum ConstraintType + { + NO, + HARD, + SOFT + }; + + Eigen::Vector4d mColor; + ConstraintType mType; + + MarkerProperties( + const Eigen::Vector4d& color = Color::White(1.0), + ConstraintType type = NO); + + // To get byte-aligned Eigen vectors + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + + +} // namespace detail +} // namespace dynamics +} // namespace dart + +#endif // DART_DYNAMICS_DETAIL_MARKER_H_ diff --git a/dart/gui/SimWindow.cpp b/dart/gui/SimWindow.cpp index 8d20f55458bdc..529b7b034ff81 100644 --- a/dart/gui/SimWindow.cpp +++ b/dart/gui/SimWindow.cpp @@ -55,6 +55,7 @@ #include "dart/dynamics/MeshShape.h" #include "dart/dynamics/SoftMeshShape.h" #include "dart/dynamics/LineSegmentShape.h" +#include "dart/dynamics/Marker.h" #include "dart/constraint/ConstraintSolver.h" #include "dart/collision/CollisionDetector.h" #include "dart/gui/LoadGlut.h" diff --git a/dart/utils/SkelParser.cpp b/dart/utils/SkelParser.cpp index b6105fc53ccda..c9473c7abc1e4 100644 --- a/dart/utils/SkelParser.cpp +++ b/dart/utils/SkelParser.cpp @@ -122,6 +122,7 @@ struct SkelBodyNode { BodyPropPtr properties; Eigen::Isometry3d initTransform; + std::vector markers; std::string type; EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; @@ -179,7 +180,7 @@ dynamics::ShapePtr readShape( const common::ResourceRetrieverPtr& retriever); /// Read marker -dynamics::Marker::Properties readMarker( +dynamics::Marker::BasicProperties readMarker( tinyxml2::XMLElement* _markerElement); void readJoint( @@ -318,7 +319,7 @@ dynamics::ShapePtr readShape( const common::Uri& baseUri, const common::ResourceRetrieverPtr& retriever); -dynamics::Marker::Properties readMarker( +dynamics::Marker::BasicProperties readMarker( tinyxml2::XMLElement* _markerElement); void readJoint(tinyxml2::XMLElement* _jointElement, @@ -1118,18 +1119,18 @@ SkelBodyNode readBodyNode( } } + SkelBodyNode skelBodyNode; + skelBodyNode.properties = newBodyNode; + skelBodyNode.initTransform = initTransform; + //-------------------------------------------------------------------------- // marker ElementEnumerator markers(_bodyNodeElement, "marker"); while (markers.next()) { - newBodyNode->mMarkerProperties.push_back(readMarker(markers.get())); + skelBodyNode.markers.push_back(readMarker(markers.get())); } - SkelBodyNode skelBodyNode; - skelBodyNode.properties = newBodyNode; - skelBodyNode.initTransform = initTransform; - return skelBodyNode; } @@ -1327,7 +1328,7 @@ dynamics::ShapePtr readShape( } //============================================================================== -dynamics::Marker::Properties readMarker( +dynamics::Marker::BasicProperties readMarker( tinyxml2::XMLElement* _markerElement) { // Name attribute @@ -1338,7 +1339,9 @@ dynamics::Marker::Properties readMarker( if (hasElement(_markerElement, "offset")) offset = getValueVector3d(_markerElement, "offset"); - dynamics::Marker::Properties newMarker(name, offset); + dynamics::Marker::BasicProperties newMarker; + newMarker.mName = name; + newMarker.mRelativeTf.translation() = offset; return newMarker; } diff --git a/dart/utils/VskParser.cpp b/dart/utils/VskParser.cpp index 3a0506c7294bf..a2d77f3d01056 100644 --- a/dart/utils/VskParser.cpp +++ b/dart/utils/VskParser.cpp @@ -863,9 +863,7 @@ bool readMarker(const tinyxml2::XMLElement* markerEle, return false; } - dynamics::Marker* marker = new dynamics::Marker(name, position, rgba, - bodyNode); - bodyNode->addMarker(marker); + bodyNode->createMarker(name, position, rgba); return true; } diff --git a/dart/utils/VskParser.h b/dart/utils/VskParser.h index 3693a847c14a8..0d89b8d7d3d31 100644 --- a/dart/utils/VskParser.h +++ b/dart/utils/VskParser.h @@ -41,6 +41,7 @@ #include "dart/common/ResourceRetriever.h" #include "dart/common/Uri.h" #include "dart/dynamics/Skeleton.h" +#include "dart/math/Helpers.h" namespace dart { namespace utils { From 316a583e5e5a5c23360678c44fdf3834bd3573da Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Fri, 22 Apr 2016 20:52:29 -0400 Subject: [PATCH 2/4] Using Marker Properties to construct Markers in SkelParser --- dart/utils/SkelParser.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/dart/utils/SkelParser.cpp b/dart/utils/SkelParser.cpp index c9473c7abc1e4..5a382cf2d7fe2 100644 --- a/dart/utils/SkelParser.cpp +++ b/dart/utils/SkelParser.cpp @@ -916,6 +916,10 @@ bool createJointAndNodePair(dynamics::SkeletonPtr skeleton, newJoint->setAccelerations(joint.acceleration); newJoint->setForces(joint.force); + dynamics::BodyNode* bn = pair.second; + for(std::size_t i=0; i < body.markers.size(); ++i) + bn->createNode(body.markers[i]); + return true; } From bc7fe49e4dc1a1a1cb6be101cdf4b1e40b285514 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sat, 23 Apr 2016 21:30:07 -0400 Subject: [PATCH 3/4] Minor updates for code quality --- dart/dynamics/Marker.cpp | 2 -- dart/dynamics/Marker.h | 4 ---- dart/dynamics/detail/MarkerAspect.h | 7 +++---- dart/utils/VskParser.h | 2 +- 4 files changed, 4 insertions(+), 11 deletions(-) diff --git a/dart/dynamics/Marker.cpp b/dart/dynamics/Marker.cpp index ef9b5ac17a5b6..d59345d6e0233 100644 --- a/dart/dynamics/Marker.cpp +++ b/dart/dynamics/Marker.cpp @@ -36,8 +36,6 @@ #include "dart/dynamics/Marker.h" -#include - #include "dart/dynamics/BodyNode.h" namespace dart { diff --git a/dart/dynamics/Marker.h b/dart/dynamics/Marker.h index 97ebc6950cefe..752565741453a 100644 --- a/dart/dynamics/Marker.h +++ b/dart/dynamics/Marker.h @@ -37,7 +37,6 @@ #ifndef DART_DYNAMICS_MARKER_H_ #define DART_DYNAMICS_MARKER_H_ -#include #include #include "dart/common/Deprecated.h" #include "dart/dynamics/detail/MarkerAspect.h" @@ -131,11 +130,8 @@ class Marker final : EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -// TODO: Marker class should be refactored into a Node once pull request #531 is -// finished. } // namespace dynamics } // namespace dart #endif // DART_DYNAMICS_MARKER_H_ - diff --git a/dart/dynamics/detail/MarkerAspect.h b/dart/dynamics/detail/MarkerAspect.h index afd8a99f99737..f7d7e40f69e6d 100644 --- a/dart/dynamics/detail/MarkerAspect.h +++ b/dart/dynamics/detail/MarkerAspect.h @@ -34,8 +34,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_DYNAMICS_DETAIL_MARKER_H_ -#define DART_DYNAMICS_DETAIL_MARKER_H_ +#ifndef DART_DYNAMICS_DETAIL_MARKERASPECT_H_ +#define DART_DYNAMICS_DETAIL_MARKERASPECT_H_ #include #include "dart/math/Helpers.h" @@ -64,9 +64,8 @@ struct MarkerProperties EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; - } // namespace detail } // namespace dynamics } // namespace dart -#endif // DART_DYNAMICS_DETAIL_MARKER_H_ +#endif // DART_DYNAMICS_DETAIL_MARKERASPECT_H_ diff --git a/dart/utils/VskParser.h b/dart/utils/VskParser.h index 0d89b8d7d3d31..cbd04152838c1 100644 --- a/dart/utils/VskParser.h +++ b/dart/utils/VskParser.h @@ -40,8 +40,8 @@ #include "dart/common/ResourceRetriever.h" #include "dart/common/Uri.h" +#include "dart/math/Constants.h" #include "dart/dynamics/Skeleton.h" -#include "dart/math/Helpers.h" namespace dart { namespace utils { From 9d9a8a0366384d667008dcc54db2b7237666329a Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Mon, 25 Apr 2016 13:08:03 -0400 Subject: [PATCH 4/4] Standardizing the create functions for built-in Node types --- dart/dynamics/BodyNode.cpp | 13 +++++++++++++ dart/dynamics/BodyNode.h | 9 ++++++--- dart/dynamics/Skeleton.h | 3 ++- dart/dynamics/detail/BodyNode.h | 8 -------- 4 files changed, 21 insertions(+), 12 deletions(-) diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index 444511a7e68de..b7c3720c2ffc5 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -947,6 +947,13 @@ void BodyNode::removeAllShapeNodes() //============================================================================== DART_BAKE_SPECIALIZED_NODE_DEFINITIONS( BodyNode, EndEffector ) +//============================================================================== +EndEffector* BodyNode::createEndEffector( + const EndEffector::BasicProperties& _properties) +{ + return createNode(_properties); +} + //============================================================================== EndEffector* BodyNode::createEndEffector(const std::string& _name) { @@ -978,6 +985,12 @@ Marker* BodyNode::createMarker(const std::string& name, return createNode(properties); } +//============================================================================== +Marker* BodyNode::createMarker(const Marker::BasicProperties& properties) +{ + return createNode(properties); +} + //============================================================================== bool BodyNode::dependsOn(size_t _genCoordIndex) const { diff --git a/dart/dynamics/BodyNode.h b/dart/dynamics/BodyNode.h index 1a01b02887704..1689e524313ee 100644 --- a/dart/dynamics/BodyNode.h +++ b/dart/dynamics/BodyNode.h @@ -76,7 +76,7 @@ class Marker; /// BodyNode of the BodyNode. class BodyNode : public detail::BodyNodeCompositeBase, - public virtual BodyNodeSpecializedFor, + public virtual BodyNodeSpecializedFor, public SkeletonRefCountingBase, public TemplatedJacobianNode { @@ -596,8 +596,8 @@ class BodyNode : /// Create an EndEffector attached to this BodyNode. Pass an /// EndEffector::Properties argument into this function. - template - EndEffector* createEndEffector(const EndEffectorProperties& _properties); + EndEffector* createEndEffector( + const EndEffector::BasicProperties& _properties); /// Create an EndEffector with the specified name EndEffector* createEndEffector(const std::string& _name = "EndEffector"); @@ -613,6 +613,9 @@ class BodyNode : const Eigen::Vector3d& position = Eigen::Vector3d::Zero(), const Eigen::Vector4d& color = Eigen::Vector4d::Constant(1.0)); + /// Create a Marker given its basic properties + Marker* createMarker(const Marker::BasicProperties& properties); + // Documentation inherited bool dependsOn(size_t _genCoordIndex) const override; diff --git a/dart/dynamics/Skeleton.h b/dart/dynamics/Skeleton.h index 7c250617577d5..689c6360fda37 100644 --- a/dart/dynamics/Skeleton.h +++ b/dart/dynamics/Skeleton.h @@ -47,6 +47,7 @@ #include "dart/dynamics/Joint.h" #include "dart/dynamics/ShapeNode.h" #include "dart/dynamics/EndEffector.h" +#include "dart/dynamics/Marker.h" #include "dart/dynamics/detail/BodyNodeAspect.h" #include "dart/dynamics/SpecializedNodeManager.h" #include "dart/dynamics/detail/SkeletonAspect.h" @@ -58,7 +59,7 @@ namespace dynamics { class Skeleton : public virtual common::VersionCounter, public MetaSkeleton, - public SkeletonSpecializedFor, + public SkeletonSpecializedFor, public detail::SkeletonAspectBase { public: diff --git a/dart/dynamics/detail/BodyNode.h b/dart/dynamics/detail/BodyNode.h index b39789d8a6da5..9974631c84218 100644 --- a/dart/dynamics/detail/BodyNode.h +++ b/dart/dynamics/detail/BodyNode.h @@ -240,14 +240,6 @@ void BodyNode::removeAllShapeNodesWith() shapeNode->remove(); } -//============================================================================== -template -EndEffector* BodyNode::createEndEffector( - const EndEffectorProperties& _properties) -{ - return createNode(_properties); -} - } // namespace dynamics } // namespace dart