From 09feabf1ef4c364d2fe6b63ebd2c6d2b43cf2257 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Fri, 18 Mar 2016 18:00:48 -0400 Subject: [PATCH 01/62] moved SoftBodyNode properties into an Addon --- dart/dynamics/BodyNode.cpp | 7 + dart/dynamics/BodyNode.h | 3 + dart/dynamics/Entity.cpp | 7 + dart/dynamics/Entity.h | 3 + dart/dynamics/Frame.cpp | 7 + dart/dynamics/Frame.h | 3 + dart/dynamics/PointMass.cpp | 14 +- dart/dynamics/SoftBodyNode.cpp | 200 ++++++++++-------- dart/dynamics/SoftBodyNode.h | 88 ++------ dart/dynamics/detail/SoftBodyNodeProperties.h | 150 +++++++++++++ 10 files changed, 326 insertions(+), 156 deletions(-) create mode 100644 dart/dynamics/detail/SoftBodyNodeProperties.h diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index ddbcbf1445766..1becd7d21fcb8 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -1231,6 +1231,13 @@ BodyNode::BodyNode(BodyNode* _parentBodyNode, Joint* _parentJoint, _parentBodyNode->addChildBodyNode(this); } +//============================================================================== +BodyNode::BodyNode(const std::tuple& args) + : BodyNode(std::get<0>(args), std::get<1>(args), std::get<2>(args)) +{ + // The initializer list is delegating the construction +} + //============================================================================== BodyNode* BodyNode::clone(BodyNode* _parentBodyNode, Joint* _parentJoint, bool cloneNodes) const diff --git a/dart/dynamics/BodyNode.h b/dart/dynamics/BodyNode.h index bcadf0a526e62..162fe1967f112 100644 --- a/dart/dynamics/BodyNode.h +++ b/dart/dynamics/BodyNode.h @@ -870,6 +870,9 @@ class BodyNode : BodyNode(BodyNode* _parentBodyNode, Joint* _parentJoint, const Properties& _properties); + /// Delegating constructor + BodyNode(const std::tuple& args); + /// Create a clone of this BodyNode. This may only be called by the Skeleton /// class. virtual BodyNode* clone(BodyNode* _parentBodyNode, Joint* _parentJoint, diff --git a/dart/dynamics/Entity.cpp b/dart/dynamics/Entity.cpp index b6829cc0cd6ae..2b3ee9989ef8d 100644 --- a/dart/dynamics/Entity.cpp +++ b/dart/dynamics/Entity.cpp @@ -79,6 +79,13 @@ Entity::Entity(Frame* _refFrame, const std::string& _name, bool _quiet) changeParentFrame(_refFrame); } +//============================================================================== +Entity::Entity() + : Entity(ConstructAbstract) +{ + // Delegated to Entity(ConstructAbstract_t) +} + //============================================================================== Entity::~Entity() { diff --git a/dart/dynamics/Entity.h b/dart/dynamics/Entity.h index 205b3f646088e..1d155c6184e64 100644 --- a/dart/dynamics/Entity.h +++ b/dart/dynamics/Entity.h @@ -97,6 +97,9 @@ class Entity : public virtual common::Subject /// Constructor for typical usage explicit Entity(Frame* _refFrame, const std::string& _name, bool _quiet); + /// Default constructor, delegates to Entity(ConstructAbstract_t) + Entity(); + Entity(const Entity&) = delete; /// Destructor diff --git a/dart/dynamics/Frame.cpp b/dart/dynamics/Frame.cpp index 7938841f89198..7358968e73f33 100644 --- a/dart/dynamics/Frame.cpp +++ b/dart/dynamics/Frame.cpp @@ -532,6 +532,13 @@ Frame::Frame(Frame* _refFrame, const std::string& _name) changeParentFrame(_refFrame); } +//============================================================================== +Frame::Frame() + : Frame(ConstructAbstract) +{ + // Delegated to Frame(ConstructAbstract) +} + //============================================================================== Frame::Frame(ConstructAbstract_t) : Entity(Entity::ConstructAbstract), diff --git a/dart/dynamics/Frame.h b/dart/dynamics/Frame.h index ceed644258338..58e500249a5d8 100644 --- a/dart/dynamics/Frame.h +++ b/dart/dynamics/Frame.h @@ -263,6 +263,9 @@ class Frame : public virtual Entity /// Constructor for typical usage explicit Frame(Frame* _refFrame, const std::string& _name); + /// Default constructor, delegates to Frame(ConstructAbstract_t) + Frame(); + /// Constructor for use by pure abstract classes explicit Frame(ConstructAbstract_t); diff --git a/dart/dynamics/PointMass.cpp b/dart/dynamics/PointMass.cpp index cfe4dfb9ab0ad..a020756ce95a7 100644 --- a/dart/dynamics/PointMass.cpp +++ b/dart/dynamics/PointMass.cpp @@ -147,13 +147,13 @@ size_t PointMass::getIndexInSoftBodyNode() const void PointMass::setMass(double _mass) { assert(0.0 < _mass); - mParentSoftBodyNode->mSoftP.mPointProps[mIndex].mMass = _mass; + mParentSoftBodyNode->getSoftAndInc().mPointProps[mIndex].mMass = _mass; } //============================================================================== double PointMass::getMass() const { - return mParentSoftBodyNode->mSoftP.mPointProps[mIndex].mMass; + return mParentSoftBodyNode->getSoft().mPointProps[mIndex].mMass; } //============================================================================== @@ -189,14 +189,14 @@ void PointMass::addConnectedPointMass(PointMass* _pointMass) { assert(_pointMass != nullptr); - mParentSoftBodyNode->mSoftP.mPointProps[mIndex]. + mParentSoftBodyNode->getSoftAndInc().mPointProps[mIndex]. mConnectedPointMassIndices.push_back(_pointMass->mIndex); } //============================================================================== size_t PointMass::getNumConnectedPointMasses() const { - return mParentSoftBodyNode->mSoftP.mPointProps[mIndex]. + return mParentSoftBodyNode->getSoft().mPointProps[mIndex]. mConnectedPointMassIndices.size(); } @@ -206,7 +206,7 @@ PointMass* PointMass::getConnectedPointMass(size_t _idx) assert(_idx < getNumConnectedPointMasses()); return mParentSoftBodyNode->mPointMasses[ - mParentSoftBodyNode->mSoftP.mPointProps[mIndex]. + mParentSoftBodyNode->getSoft().mPointProps[mIndex]. mConnectedPointMassIndices[_idx]]; } @@ -532,14 +532,14 @@ void PointMass::clearConstraintImpulse() //============================================================================== void PointMass::setRestingPosition(const Eigen::Vector3d& _p) { - mParentSoftBodyNode->mSoftP.mPointProps[mIndex].mX0 = _p; + mParentSoftBodyNode->getSoftAndInc().mPointProps[mIndex].mX0 = _p; mNotifier->notifyTransformUpdate(); } //============================================================================== const Eigen::Vector3d& PointMass::getRestingPosition() const { - return mParentSoftBodyNode->mSoftP.mPointProps[mIndex].mX0; + return mParentSoftBodyNode->getSoft().mPointProps[mIndex].mX0; } //============================================================================== diff --git a/dart/dynamics/SoftBodyNode.cpp b/dart/dynamics/SoftBodyNode.cpp index de0b9e62df4c0..429e30c8082fd 100644 --- a/dart/dynamics/SoftBodyNode.cpp +++ b/dart/dynamics/SoftBodyNode.cpp @@ -54,9 +54,10 @@ namespace dart { namespace dynamics { +namespace detail { //============================================================================== -SoftBodyNode::UniqueProperties::UniqueProperties( +SoftBodyNodeUniqueProperties::SoftBodyNodeUniqueProperties( double _Kv, double _Ke, double _DampCoeff, const std::vector& _points, const std::vector& _faces) @@ -70,14 +71,14 @@ SoftBodyNode::UniqueProperties::UniqueProperties( } //============================================================================== -void SoftBodyNode::UniqueProperties::addPointMass( +void SoftBodyNodeUniqueProperties::addPointMass( const PointMass::Properties& _properties) { mPointProps.push_back(_properties); } //============================================================================== -bool SoftBodyNode::UniqueProperties::connectPointMasses(size_t i1, size_t i2) +bool SoftBodyNodeUniqueProperties::connectPointMasses(size_t i1, size_t i2) { if(i1 >= mPointProps.size() || i2 >= mPointProps.size()) { @@ -100,7 +101,7 @@ bool SoftBodyNode::UniqueProperties::connectPointMasses(size_t i1, size_t i2) } //============================================================================== -void SoftBodyNode::UniqueProperties::addFace(const Eigen::Vector3i& _newFace) +void SoftBodyNodeUniqueProperties::addFace(const Eigen::Vector3i& _newFace) { assert(_newFace[0] != _newFace[1]); assert(_newFace[1] != _newFace[2]); @@ -112,15 +113,24 @@ void SoftBodyNode::UniqueProperties::addFace(const Eigen::Vector3i& _newFace) } //============================================================================== -SoftBodyNode::Properties::Properties( +SoftBodyNodeProperties::SoftBodyNodeProperties( const BodyNode::Properties& _bodyProperties, - const SoftBodyNode::UniqueProperties& _softProperties) + const SoftBodyNodeUniqueProperties& _softProperties) : BodyNode::Properties(_bodyProperties), - SoftBodyNode::UniqueProperties(_softProperties) + SoftBodyNodeUniqueProperties(_softProperties) { // Do nothing } +//============================================================================== +void SoftBodyNodePropertiesUpdate(SoftBodyAddon* addon) +{ + if(SoftBodyNode* sbn = addon->getManager()) + sbn->configureSoftMeshShape(addon->getProperties()); +} + +} // namespace detail + //============================================================================== SoftBodyNode::~SoftBodyNode() { @@ -142,70 +152,13 @@ void SoftBodyNode::setProperties(const Properties& _properties) //============================================================================== void SoftBodyNode::setProperties(const UniqueProperties& _properties) { - size_t newCount = _properties.mPointProps.size(); - size_t oldCount = mPointMasses.size(); - if(newCount < oldCount) - { - for(size_t i = newCount; i < oldCount; ++i) - delete mPointMasses[i]; - mPointMasses.resize(newCount); - mSoftP.mPointProps.resize(newCount); - } - else if(oldCount < newCount) - { - mPointMasses.resize(newCount); - mSoftP.mPointProps.resize(newCount); - for(size_t i = oldCount; i < newCount; ++i) - { - mPointMasses[i] = new PointMass(this); - mPointMasses[i]->mIndex = i; - mPointMasses[i]->init(); - } - } - - const std::vector& allProps = _properties.mPointProps; - for(size_t i=0; isetRestingPosition(props.mX0); - p->setMass(props.mMass); - - mSoftP.mPointProps[i].mConnectedPointMassIndices = - props.mConnectedPointMassIndices; - } - - setVertexSpringStiffness(_properties.mKv); - setEdgeSpringStiffness(_properties.mKe); - setDampingCoefficient(_properties.mDampCoeff); - mSoftP.mFaces = _properties.mFaces; - - // Create a new SoftMeshShape for this SoftBodyNode - auto softMesh = std::shared_ptr(new SoftMeshShape(this)); - auto newSoftShapeNode - = createShapeNodeWith( - softMesh, "soft mesh"); - - // Copy the properties of the previous soft shape, if it exists - ShapeNodePtr softNode = mSoftShapeNode.lock(); - if(softNode) - { - newSoftShapeNode->getVisualAddon()->setColor( - softNode->getVisualAddon()->getRGBA()); - - newSoftShapeNode->setRelativeTransform( - softNode->getRelativeTransform()); - - softNode->remove(); - } - - mSoftShapeNode = newSoftShapeNode; + getSoftBodyAddon()->setProperties(_properties); } //============================================================================== SoftBodyNode::Properties SoftBodyNode::getSoftBodyNodeProperties() const { - return Properties(getBodyNodeProperties(), mSoftP); + return Properties(getBodyNodeProperties(), getSoft()); } //============================================================================== @@ -261,9 +214,11 @@ SoftBodyNode::SoftBodyNode(BodyNode* _parentBodyNode, const Properties& _properties) : Entity(Frame::World(), "", false), Frame(Frame::World(), ""), - BodyNode(_parentBodyNode, _parentJoint, _properties), + Base(std::make_tuple(_parentBodyNode, _parentJoint, _properties), + common::NoArg), mSoftShapeNode(nullptr) { + createSoftBodyAddon(); mNotifier = new PointMassNotifier(this, "PointMassNotifier"); setProperties(_properties); } @@ -283,6 +238,72 @@ BodyNode* SoftBodyNode::clone(BodyNode* _parentBodyNode, return clonedBn; } +//============================================================================== +void SoftBodyNode::configureSoftMeshShape( + const UniqueProperties& softProperties) +{ + UniqueProperties& mSoftP = getSoftBodyAddon()->_getProperties(); + + size_t newCount = softProperties.mPointProps.size(); + size_t oldCount = mPointMasses.size(); + if(newCount < oldCount) + { + for(size_t i = newCount; i < oldCount; ++i) + delete mPointMasses[i]; + mPointMasses.resize(newCount); + mSoftP.mPointProps.resize(newCount); + } + else if(oldCount < newCount) + { + mPointMasses.resize(newCount); + mSoftP.mPointProps.resize(newCount); + for(size_t i = oldCount; i < newCount; ++i) + { + mPointMasses[i] = new PointMass(this); + mPointMasses[i]->mIndex = i; + mPointMasses[i]->init(); + } + } + + const std::vector& allProps = softProperties.mPointProps; + for(size_t i=0; isetRestingPosition(props.mX0); + p->setMass(props.mMass); + + mSoftP.mPointProps[i].mConnectedPointMassIndices = + props.mConnectedPointMassIndices; + } + + setVertexSpringStiffness(softProperties.mKv); + setEdgeSpringStiffness(softProperties.mKe); + setDampingCoefficient(softProperties.mDampCoeff); + mSoftP.mFaces = softProperties.mFaces; + + // Create a new SoftMeshShape for this SoftBodyNode + auto softMesh = std::shared_ptr(new SoftMeshShape(this)); + auto newSoftShapeNode + = createShapeNodeWith( + softMesh, "soft mesh"); + + // Copy the properties of the previous soft shape, if it exists + ShapeNodePtr softNode = mSoftShapeNode.lock(); + if(softNode) + { + newSoftShapeNode->getVisualAddon()->setColor( + softNode->getVisualAddon()->getRGBA()); + + newSoftShapeNode->setRelativeTransform( + softNode->getRelativeTransform()); + + softNode->remove(); + } + + mSoftShapeNode = newSoftShapeNode; +} + //============================================================================== void SoftBodyNode::init(const SkeletonPtr& _skeleton) { @@ -356,46 +377,46 @@ double SoftBodyNode::getMass() const void SoftBodyNode::setVertexSpringStiffness(double _kv) { assert(0.0 <= _kv); - mSoftP.mKv = _kv; + getSoftAndInc().mKv = _kv; } //============================================================================== double SoftBodyNode::getVertexSpringStiffness() const { - return mSoftP.mKv; + return getSoft().mKv; } //============================================================================== void SoftBodyNode::setEdgeSpringStiffness(double _ke) { assert(0.0 <= _ke); - mSoftP.mKe = _ke; + getSoftAndInc().mKe = _ke; } //============================================================================== double SoftBodyNode::getEdgeSpringStiffness() const { - return mSoftP.mKe; + return getSoft().mKe; } //============================================================================== void SoftBodyNode::setDampingCoefficient(double _damp) { assert(_damp >= 0.0); - mSoftP.mDampCoeff = _damp; + getSoftAndInc().mDampCoeff = _damp; } //============================================================================== double SoftBodyNode::getDampingCoefficient() const { - return mSoftP.mDampCoeff; + return getSoft().mDampCoeff; } //============================================================================== void SoftBodyNode::removeAllPointMasses() { mPointMasses.clear(); - mSoftP.mPointProps.clear(); + getSoftAndInc().mPointProps.clear(); } //============================================================================== @@ -403,7 +424,7 @@ PointMass* SoftBodyNode::addPointMass(const PointMass::Properties& _properties) { mPointMasses.push_back(new PointMass(this)); mPointMasses.back()->mIndex = mPointMasses.size()-1; - mSoftP.mPointProps.push_back(_properties); + getSoftAndInc().mPointProps.push_back(_properties); return mPointMasses.back(); } @@ -421,20 +442,20 @@ void SoftBodyNode::connectPointMasses(size_t _idx1, size_t _idx2) //============================================================================== void SoftBodyNode::addFace(const Eigen::Vector3i& _face) { - mSoftP.addFace(_face); + getSoftAndInc().addFace(_face); } //============================================================================== const Eigen::Vector3i& SoftBodyNode::getFace(size_t _idx) const { - assert(_idx < mSoftP.mFaces.size()); - return mSoftP.mFaces[_idx]; + assert(_idx < getSoft().mFaces.size()); + return getSoft().mFaces[_idx]; } //============================================================================== size_t SoftBodyNode::getNumFaces() const { - return mSoftP.mFaces.size(); + return getSoft().mFaces.size(); } //============================================================================== @@ -1181,20 +1202,20 @@ void SoftBodyNode::draw(renderer::RenderInterface* _ri, Eigen::Vector3d pos; Eigen::Vector3d pos_normalized; - for (size_t i = 0; i < mSoftP.mFaces.size(); ++i) + for (size_t i = 0; i < getSoft().mFaces.size(); ++i) { glEnable(GL_AUTO_NORMAL); glBegin(GL_TRIANGLES); - pos = mPointMasses[mSoftP.mFaces[i](0)]->getLocalPosition(); + pos = mPointMasses[getSoft().mFaces[i](0)]->getLocalPosition(); pos_normalized = pos.normalized(); glNormal3f(pos_normalized(0), pos_normalized(1), pos_normalized(2)); glVertex3f(pos(0), pos(1), pos(2)); - pos = mPointMasses[mSoftP.mFaces[i](1)]->getLocalPosition(); + pos = mPointMasses[getSoft().mFaces[i](1)]->getLocalPosition(); pos_normalized = pos.normalized(); glNormal3f(pos_normalized(0), pos_normalized(1), pos_normalized(2)); glVertex3f(pos(0), pos(1), pos(2)); - pos = mPointMasses[mSoftP.mFaces[i](2)]->getLocalPosition(); + pos = mPointMasses[getSoft().mFaces[i](2)]->getLocalPosition(); pos_normalized = pos.normalized(); glNormal3f(pos_normalized(0), pos_normalized(1), pos_normalized(2)); glVertex3f(pos(0), pos(1), pos(2)); @@ -1215,6 +1236,19 @@ void SoftBodyNode::draw(renderer::RenderInterface* _ri, _ri->popMatrix(); } +//============================================================================== +SoftBodyNode::UniqueProperties& SoftBodyNode::getSoftAndInc() +{ + getSoftBodyAddon()->incrementVersion(); + return getSoftBodyAddon()->_getProperties(); +} + +//============================================================================== +const SoftBodyNode::UniqueProperties& SoftBodyNode::getSoft() const +{ + return getSoftBodyAddon()->_getProperties(); +} + //============================================================================== void SoftBodyNode::_addPiToArtInertia(const Eigen::Vector3d& _p, double _Pi) const { diff --git a/dart/dynamics/SoftBodyNode.h b/dart/dynamics/SoftBodyNode.h index ffd3a8aa04865..2d469ba320396 100644 --- a/dart/dynamics/SoftBodyNode.h +++ b/dart/dynamics/SoftBodyNode.h @@ -37,86 +37,30 @@ #ifndef DART_DYNAMICS_SOFTBODYNODE_H_ #define DART_DYNAMICS_SOFTBODYNODE_H_ -const double DART_DEFAULT_VERTEX_STIFFNESS = 1.0; -const double DART_DEFAULT_EDGE_STIFNESS = 1.0; -const double DART_DEFAULT_DAMPING_COEFF = 0.01; - -#include -#include - -#include - -#include "dart/dynamics/BodyNode.h" -#include "dart/dynamics/PointMass.h" +#include "dart/dynamics/detail/SoftBodyNodeProperties.h" namespace dart { namespace dynamics { -class PointMass; -class PointMassNotifier; -class SoftMeshShape; - /// SoftBodyNode represent a soft body that has one deformable skin /// /// This class is implementation of Sumit Jain and C. Karen Liu's paper: /// http://www.cc.gatech.edu/graphics/projects/Sumit/homepage/projects/softcontacts/index.html -class SoftBodyNode : public BodyNode +class SoftBodyNode : public detail::SoftBodyNodeBase { public: + friend class Skeleton; friend class PointMass; friend class PointMassNotifier; + friend void detail::SoftBodyNodePropertiesUpdate(SoftBodyAddon *addon); - struct UniqueProperties - { - /// Spring stiffness for vertex deformation restoring spring force of the - /// point masses - double mKv; - - /// Spring stiffness for edge deformation restoring spring force of the - /// point masses - double mKe; - - /// Damping coefficient - double mDampCoeff; - - /// Array of Properties for PointMasses - std::vector mPointProps; - - // TODO(JS): Let's remove this because this is rendering part - /// \brief Tri-mesh indexes for rendering. - std::vector mFaces; - - UniqueProperties( - double _Kv = DART_DEFAULT_VERTEX_STIFFNESS, - double _Ke = DART_DEFAULT_EDGE_STIFNESS, - double _DampCoeff = DART_DEFAULT_DAMPING_COEFF, - const std::vector& _points = - std::vector(), - const std::vector& _faces = - std::vector()); + using UniqueProperties = detail::SoftBodyNodeUniqueProperties; + using Properties = detail::SoftBodyNodeProperties; + using Addon = detail::SoftBodyAddon; + using Base = detail::SoftBodyNodeBase; - virtual ~UniqueProperties() = default; - - /// Add a PointMass to this Properties struct - void addPointMass(const PointMass::Properties& _properties); - - /// Connect two PointMasses together in this Properties struct - bool connectPointMasses(size_t i1, size_t i2); - - /// Add a face to this Properties struct - void addFace(const Eigen::Vector3i& _newFace); - }; - - struct Properties : BodyNode::Properties, UniqueProperties - { - Properties( - const BodyNode::Properties& _bodyProperties = BodyNode::Properties(), - const SoftBodyNode::UniqueProperties& _softProperties = - SoftBodyNode::UniqueProperties()); - - virtual ~Properties() = default; - }; + DART_BAKE_SPECIALIZED_ADDON_IRREGULAR(detail::SoftBodyAddon, SoftBodyAddon) /// \brief virtual ~SoftBodyNode(); @@ -209,6 +153,10 @@ class SoftBodyNode : public BodyNode virtual BodyNode* clone(BodyNode* _parentBodyNode, Joint* _parentJoint, bool cloneNodes) const override; + /// Used by SoftBodyAddon to have this SoftBodyNode reconstruct its + /// SoftMeshShape + void configureSoftMeshShape(const UniqueProperties& softProperties); + //-------------------------------------------------------------------------- // Sub-functions for Recursive Kinematics Algorithms //-------------------------------------------------------------------------- @@ -346,6 +294,14 @@ class SoftBodyNode : public BodyNode bool _useDefaultColor = true, int _depth = 0) const override; protected: + + /// Get a reference to the UniqueProperties of this SoftBodyNode and increment + /// its version. + UniqueProperties& getSoftAndInc(); + + /// Get a reference to the UniqueProperties of this SoftBodyNode + const UniqueProperties& getSoft() const; + /// \brief List of point masses composing deformable mesh. std::vector mPointMasses; @@ -353,7 +309,7 @@ class SoftBodyNode : public BodyNode PointMassNotifier* mNotifier; /// SoftBodyNode Properties - UniqueProperties mSoftP; +// UniqueProperties mSoftP; /// \brief Soft mesh shape belonging to this node. WeakShapeNodePtr mSoftShapeNode; diff --git a/dart/dynamics/detail/SoftBodyNodeProperties.h b/dart/dynamics/detail/SoftBodyNodeProperties.h new file mode 100644 index 0000000000000..adb10d6338f2a --- /dev/null +++ b/dart/dynamics/detail/SoftBodyNodeProperties.h @@ -0,0 +1,150 @@ +/* + * 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_SOFTBODYNODEPROPERTIES_H_ +#define DART_DYNAMICS_DETAIL_SOFTBODYNODEPROPERTIES_H_ + +#include "dart/dynamics/BodyNode.h" +#include "dart/dynamics/PointMass.h" +#include "dart/common/RequiresAddon.h" + +namespace dart { +namespace dynamics { + +const double DART_DEFAULT_VERTEX_STIFFNESS = 1.0; +const double DART_DEFAULT_EDGE_STIFNESS = 1.0; +const double DART_DEFAULT_DAMPING_COEFF = 0.01; + +class SoftBodyNode; +class SoftMeshShape; + +namespace detail { + +//============================================================================== +class SoftBodyAddon; + +//============================================================================== +struct SoftBodyNodeUniqueProperties +{ + /// Spring stiffness for vertex deformation restoring spring force of the + /// point masses + double mKv; + + /// Spring stiffness for edge deformation restoring spring force of the + /// point masses + double mKe; + + /// Damping coefficient + double mDampCoeff; + + /// Array of Properties for PointMasses + std::vector mPointProps; + + // TODO(JS): Let's remove this because this is rendering part + /// \brief Tri-mesh indexes for rendering. + std::vector mFaces; + + SoftBodyNodeUniqueProperties( + double _Kv = DART_DEFAULT_VERTEX_STIFFNESS, + double _Ke = DART_DEFAULT_EDGE_STIFNESS, + double _DampCoeff = DART_DEFAULT_DAMPING_COEFF, + const std::vector& _points = + std::vector(), + const std::vector& _faces = + std::vector()); + + virtual ~SoftBodyNodeUniqueProperties() = default; + + /// Add a PointMass to this Properties struct + void addPointMass(const PointMass::Properties& _properties); + + /// Connect two PointMasses together in this Properties struct + bool connectPointMasses(size_t i1, size_t i2); + + /// Add a face to this Properties struct + void addFace(const Eigen::Vector3i& _newFace); +}; + +//============================================================================== +struct SoftBodyNodeProperties + : BodyNode::Properties, SoftBodyNodeUniqueProperties +{ + SoftBodyNodeProperties( + const BodyNode::Properties& _bodyProperties = BodyNode::Properties(), + const SoftBodyNodeUniqueProperties& _softProperties = + SoftBodyNodeUniqueProperties()); + + virtual ~SoftBodyNodeProperties() = default; +}; + +//============================================================================== +void SoftBodyNodePropertiesUpdate(SoftBodyAddon* addon); + +//============================================================================== +class SoftBodyAddon final : + public common::AddonWithVersionedProperties< + SoftBodyAddon, SoftBodyNodeUniqueProperties, SoftBodyNode, + &SoftBodyNodePropertiesUpdate > +{ +public: + + friend class dart::dynamics::SoftBodyNode; + + DART_COMMON_ADDON_PROPERTY_CONSTRUCTOR(SoftBodyAddon, SoftBodyNodePropertiesUpdate) + + DART_COMMON_SET_GET_ADDON_PROPERTY(double, Kv) + DART_COMMON_SET_GET_ADDON_PROPERTY(double, Ke) + DART_COMMON_SET_GET_ADDON_PROPERTY(double, DampCoeff) + +protected: + + /// Get a direct reference to the Properties of this Addon + inline PropertiesData& _getProperties() { return mProperties; } + + /// Get a direct const reference to the Properties of this Addon + inline const PropertiesData& _getProperties() const { return mProperties; } + +}; + +//============================================================================== +using SoftBodyNodeBase = common::AddonManagerJoiner< + BodyNode, common::RequiresAddon >; + +} // namespace detail +} // namespace dynamics +} // namespace dart + +#endif // DART_DYNAMICS_DETAIL_SOFTBODYNODEPROPERTIES_H_ From 4d4f84c1951965031f657f0e2489ebd5690fb120 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Fri, 18 Mar 2016 19:21:30 -0400 Subject: [PATCH 02/62] working on making sure that auto-updating works correctly for SoftBodyNode --- dart/dynamics/PointMass.cpp | 98 ++++++++++++------- dart/dynamics/PointMass.h | 40 +++++--- dart/dynamics/SoftBodyNode.cpp | 51 ++++++---- dart/dynamics/SoftBodyNode.h | 7 +- dart/dynamics/detail/SoftBodyNodeProperties.h | 20 +++- 5 files changed, 143 insertions(+), 73 deletions(-) diff --git a/dart/dynamics/PointMass.cpp b/dart/dynamics/PointMass.cpp index a020756ce95a7..2b1b2c47823bd 100644 --- a/dart/dynamics/PointMass.cpp +++ b/dart/dynamics/PointMass.cpp @@ -49,6 +49,20 @@ using namespace Eigen; namespace dart { namespace dynamics { +//============================================================================== +PointMass::State::State( + const Vector3d& positions, + const Vector3d& velocities, + const Vector3d& accelerations, + const Vector3d& forces) + : mPositions(positions), + mVelocities(velocities), + mAccelerations(accelerations), + mForces(forces) +{ + // Do nothing +} + //============================================================================== PointMass::Properties::Properties( const Vector3d& _X0, @@ -93,13 +107,9 @@ void PointMass::Properties::setMass(double _mass) PointMass::PointMass(SoftBodyNode* _softBodyNode) : // mIndexInSkeleton(Eigen::Matrix::Zero()), mParentSoftBodyNode(_softBodyNode), - mPositions(Eigen::Vector3d::Zero()), mPositionDeriv(Eigen::Vector3d::Zero()), - mVelocities(Eigen::Vector3d::Zero()), mVelocitiesDeriv(Eigen::Vector3d::Zero()), - mAccelerations(Eigen::Vector3d::Zero()), mAccelerationsDeriv(Eigen::Vector3d::Zero()), - mForces(Eigen::Vector3d::Zero()), mForcesDeriv(Eigen::Vector3d::Zero()), mVelocityChanges(Eigen::Vector3d::Zero()), // mImpulse(Eigen::Vector3d::Zero()), @@ -137,6 +147,18 @@ PointMass::~PointMass() delete mShape; } +//============================================================================== +PointMass::State& PointMass::getState() +{ + return mParentSoftBodyNode->getPointState(mIndex); +} + +//============================================================================== +const PointMass::State& PointMass::getState() const +{ + return mParentSoftBodyNode->getPointState(mIndex); +} + //============================================================================== size_t PointMass::getIndexInSoftBodyNode() const { @@ -147,13 +169,13 @@ size_t PointMass::getIndexInSoftBodyNode() const void PointMass::setMass(double _mass) { assert(0.0 < _mass); - mParentSoftBodyNode->getSoftAndInc().mPointProps[mIndex].mMass = _mass; + mParentSoftBodyNode->getSoftPropertiesAndInc().mPointProps[mIndex].mMass = _mass; } //============================================================================== double PointMass::getMass() const { - return mParentSoftBodyNode->getSoft().mPointProps[mIndex].mMass; + return mParentSoftBodyNode->getSoftProperties().mPointProps[mIndex].mMass; } //============================================================================== @@ -189,14 +211,14 @@ void PointMass::addConnectedPointMass(PointMass* _pointMass) { assert(_pointMass != nullptr); - mParentSoftBodyNode->getSoftAndInc().mPointProps[mIndex]. + mParentSoftBodyNode->getSoftPropertiesAndInc().mPointProps[mIndex]. mConnectedPointMassIndices.push_back(_pointMass->mIndex); } //============================================================================== size_t PointMass::getNumConnectedPointMasses() const { - return mParentSoftBodyNode->getSoft().mPointProps[mIndex]. + return mParentSoftBodyNode->getSoftProperties().mPointProps[mIndex]. mConnectedPointMassIndices.size(); } @@ -206,7 +228,7 @@ PointMass* PointMass::getConnectedPointMass(size_t _idx) assert(_idx < getNumConnectedPointMasses()); return mParentSoftBodyNode->mPointMasses[ - mParentSoftBodyNode->getSoft().mPointProps[mIndex]. + mParentSoftBodyNode->getSoftProperties().mPointProps[mIndex]. mConnectedPointMassIndices[_idx]]; } @@ -255,7 +277,7 @@ void PointMass::setPosition(size_t _index, double _position) { assert(_index < 3); - mPositions[_index] = _position; + getState().mPositions[_index] = _position; mNotifier->notifyTransformUpdate(); } @@ -264,26 +286,26 @@ double PointMass::getPosition(size_t _index) const { assert(_index < 3); - return mPositions[_index]; + return getState().mPositions[_index]; } //============================================================================== void PointMass::setPositions(const Vector3d& _positions) { - mPositions = _positions; + getState().mPositions = _positions; mNotifier->notifyTransformUpdate(); } //============================================================================== const Vector3d& PointMass::getPositions() const { - return mPositions; + return getState().mPositions; } //============================================================================== void PointMass::resetPositions() { - mPositions.setZero(); + getState().mPositions.setZero(); mNotifier->notifyTransformUpdate(); } @@ -292,7 +314,7 @@ void PointMass::setVelocity(size_t _index, double _velocity) { assert(_index < 3); - mVelocities[_index] = _velocity; + getState().mVelocities[_index] = _velocity; mNotifier->notifyVelocityUpdate(); } @@ -301,26 +323,26 @@ double PointMass::getVelocity(size_t _index) const { assert(_index < 3); - return mVelocities[_index]; + return getState().mVelocities[_index]; } //============================================================================== void PointMass::setVelocities(const Vector3d& _velocities) { - mVelocities = _velocities; + getState().mVelocities = _velocities; mNotifier->notifyVelocityUpdate(); } //============================================================================== const Vector3d& PointMass::getVelocities() const { - return mVelocities; + return getState().mVelocities; } //============================================================================== void PointMass::resetVelocities() { - mVelocities.setZero(); + getState().mVelocities.setZero(); mNotifier->notifyVelocityUpdate(); } @@ -329,7 +351,7 @@ void PointMass::setAcceleration(size_t _index, double _acceleration) { assert(_index < 3); - mAccelerations[_index] = _acceleration; + getState().mAccelerations[_index] = _acceleration; mNotifier->notifyAccelerationUpdate(); } @@ -338,20 +360,20 @@ double PointMass::getAcceleration(size_t _index) const { assert(_index < 3); - return mAccelerations[_index]; + return getState().mAccelerations[_index]; } //============================================================================== void PointMass::setAccelerations(const Eigen::Vector3d& _accelerations) { - mAccelerations = _accelerations; + getState().mAccelerations = _accelerations; mNotifier->notifyAccelerationUpdate(); } //============================================================================== const Vector3d& PointMass::getAccelerations() const { - return mAccelerations; + return getState().mAccelerations; } //============================================================================== @@ -365,7 +387,7 @@ const Vector3d& PointMass::getPartialAccelerations() const //============================================================================== void PointMass::resetAccelerations() { - mAccelerations.setZero(); + getState().mAccelerations.setZero(); mNotifier->notifyAccelerationUpdate(); } @@ -374,7 +396,7 @@ void PointMass::setForce(size_t _index, double _force) { assert(_index < 3); - mForces[_index] = _force; + getState().mForces[_index] = _force; } //============================================================================== @@ -382,25 +404,25 @@ double PointMass::getForce(size_t _index) { assert(_index < 3); - return mForces[_index]; + return getState().mForces[_index]; } //============================================================================== void PointMass::setForces(const Vector3d& _forces) { - mForces = _forces; + getState().mForces = _forces; } //============================================================================== const Vector3d& PointMass::getForces() const { - return mForces; + return getState().mForces; } //============================================================================== void PointMass::resetForces() { - mForces.setZero(); + getState().mForces.setZero(); } //============================================================================== @@ -532,14 +554,14 @@ void PointMass::clearConstraintImpulse() //============================================================================== void PointMass::setRestingPosition(const Eigen::Vector3d& _p) { - mParentSoftBodyNode->getSoftAndInc().mPointProps[mIndex].mX0 = _p; + mParentSoftBodyNode->getSoftPropertiesAndInc().mPointProps[mIndex].mX0 = _p; mNotifier->notifyTransformUpdate(); } //============================================================================== const Eigen::Vector3d& PointMass::getRestingPosition() const { - return mParentSoftBodyNode->getSoft().mPointProps[mIndex].mX0; + return mParentSoftBodyNode->getSoftProperties().mPointProps[mIndex].mX0; } //============================================================================== @@ -753,7 +775,7 @@ void PointMass::updateJointForceID(double /*_timeStep*/, double /*_withSpringForces*/) { // tau = f - mForces = mF; + getState().mForces = mF; // TODO: need to add spring and damping forces } @@ -773,20 +795,22 @@ void PointMass::updateBiasForceFD(double _dt, const Eigen::Vector3d& _gravity) } assert(!math::isNan(mB)); + const State& state = getState(); + // Cache data: alpha double kv = mParentSoftBodyNode->getVertexSpringStiffness(); double ke = mParentSoftBodyNode->getEdgeSpringStiffness(); double kd = mParentSoftBodyNode->getDampingCoefficient(); int nN = getNumConnectedPointMasses(); - mAlpha = mForces + mAlpha = state.mForces - (kv + nN * ke) * getPositions() - (_dt * (kv + nN * ke) + kd) * getVelocities() - getMass() * getPartialAccelerations() - mB; for (size_t i = 0; i < getNumConnectedPointMasses(); ++i) { - mAlpha += ke * (getConnectedPointMass(i)->mPositions - + _dt * getConnectedPointMass(i)->mVelocities); + const State& i_state = getConnectedPointMass(i)->getState(); + mAlpha += ke * (i_state.mPositions + _dt * i_state.mVelocities); } assert(!math::isNan(mAlpha)); @@ -892,7 +916,7 @@ void PointMass::updateConstrainedTermsFD(double _timeStep) setAccelerations( getAccelerations() + mVelocityChanges / _timeStep ); // 3. tau = tau + imp / dt - mForces.noalias() += mConstraintImpulses / _timeStep; + getState().mForces.noalias() += mConstraintImpulses / _timeStep; /// // mA += mDelV / _timeStep; @@ -932,7 +956,7 @@ void PointMass::aggregateAugMassMatrix(Eigen::MatrixXd& /*_MCol*/, int /*_col*/, //============================================================================== void PointMass::updateInvMassMatrix() { - mBiasForceForInvMeta = mForces; + mBiasForceForInvMeta = getState().mForces; } //============================================================================== diff --git a/dart/dynamics/PointMass.h b/dart/dynamics/PointMass.h index 22ad4859ad0e2..804222d124b4d 100644 --- a/dart/dynamics/PointMass.h +++ b/dart/dynamics/PointMass.h @@ -61,6 +61,29 @@ class PointMass : public common::Subject public: friend class SoftBodyNode; + /// State for each PointMass + struct State + { + /// Position + Eigen::Vector3d mPositions; + + /// Generalized velocity + Eigen::Vector3d mVelocities; + + /// Generalized acceleration + Eigen::Vector3d mAccelerations; + + /// Generalized force + Eigen::Vector3d mForces; + + /// Default constructor + State(const Eigen::Vector3d& positions = Eigen::Vector3d::Zero(), + const Eigen::Vector3d& velocities = Eigen::Vector3d::Zero(), + const Eigen::Vector3d& accelerations = Eigen::Vector3d::Zero(), + const Eigen::Vector3d& forces = Eigen::Vector3d::Zero()); + }; + + /// Properties for each PointMass struct Properties { /// Resting position viewed in the parent SoftBodyNode frame @@ -128,6 +151,12 @@ class PointMass : public common::Subject /// Default destructor virtual ~PointMass(); + /// State of this PointMass + State& getState(); + + /// State of this PointMass + const State& getState() const; + /// size_t getIndexInSoftBodyNode() const; @@ -529,9 +558,6 @@ class PointMass : public common::Subject // Configuration //---------------------------------------------------------------------------- - /// Position - Eigen::Vector3d mPositions; - /// Derivatives w.r.t. an arbitrary scalr variable Eigen::Vector3d mPositionDeriv; @@ -539,9 +565,6 @@ class PointMass : public common::Subject // Velocity //---------------------------------------------------------------------------- - /// Generalized velocity - Eigen::Vector3d mVelocities; - /// Derivatives w.r.t. an arbitrary scalr variable Eigen::Vector3d mVelocitiesDeriv; @@ -549,9 +572,6 @@ class PointMass : public common::Subject // Acceleration //---------------------------------------------------------------------------- - /// Generalized acceleration - Eigen::Vector3d mAccelerations; - /// Derivatives w.r.t. an arbitrary scalr variable Eigen::Vector3d mAccelerationsDeriv; @@ -559,8 +579,6 @@ class PointMass : public common::Subject // Force //---------------------------------------------------------------------------- - /// Generalized force - Eigen::Vector3d mForces; /// Derivatives w.r.t. an arbitrary scalr variable Eigen::Vector3d mForcesDeriv; diff --git a/dart/dynamics/SoftBodyNode.cpp b/dart/dynamics/SoftBodyNode.cpp index 429e30c8082fd..56adf2c3effa8 100644 --- a/dart/dynamics/SoftBodyNode.cpp +++ b/dart/dynamics/SoftBodyNode.cpp @@ -125,6 +125,7 @@ SoftBodyNodeProperties::SoftBodyNodeProperties( //============================================================================== void SoftBodyNodePropertiesUpdate(SoftBodyAddon* addon) { + static_assert(false, "This should actually update kinematics things, I don't think the configuring is necessary"); if(SoftBodyNode* sbn = addon->getManager()) sbn->configureSoftMeshShape(addon->getProperties()); } @@ -158,7 +159,7 @@ void SoftBodyNode::setProperties(const UniqueProperties& _properties) //============================================================================== SoftBodyNode::Properties SoftBodyNode::getSoftBodyNodeProperties() const { - return Properties(getBodyNodeProperties(), getSoft()); + return Properties(getBodyNodeProperties(), getSoftProperties()); } //============================================================================== @@ -282,11 +283,12 @@ void SoftBodyNode::configureSoftMeshShape( setDampingCoefficient(softProperties.mDampCoeff); mSoftP.mFaces = softProperties.mFaces; + static_assert(false, "This is all very wrong and needs to be fixed"); // Create a new SoftMeshShape for this SoftBodyNode auto softMesh = std::shared_ptr(new SoftMeshShape(this)); auto newSoftShapeNode = createShapeNodeWith( - softMesh, "soft mesh"); + softMesh, getName()+"_SoftMeshShape"); // Copy the properties of the previous soft shape, if it exists ShapeNodePtr softNode = mSoftShapeNode.lock(); @@ -301,6 +303,8 @@ void SoftBodyNode::configureSoftMeshShape( softNode->remove(); } + getSoftBodyAddon()->_getState().mPointStates.resize( + allProps.size(), PointMass::State()); mSoftShapeNode = newSoftShapeNode; } @@ -377,46 +381,46 @@ double SoftBodyNode::getMass() const void SoftBodyNode::setVertexSpringStiffness(double _kv) { assert(0.0 <= _kv); - getSoftAndInc().mKv = _kv; + getSoftPropertiesAndInc().mKv = _kv; } //============================================================================== double SoftBodyNode::getVertexSpringStiffness() const { - return getSoft().mKv; + return getSoftProperties().mKv; } //============================================================================== void SoftBodyNode::setEdgeSpringStiffness(double _ke) { assert(0.0 <= _ke); - getSoftAndInc().mKe = _ke; + getSoftPropertiesAndInc().mKe = _ke; } //============================================================================== double SoftBodyNode::getEdgeSpringStiffness() const { - return getSoft().mKe; + return getSoftProperties().mKe; } //============================================================================== void SoftBodyNode::setDampingCoefficient(double _damp) { assert(_damp >= 0.0); - getSoftAndInc().mDampCoeff = _damp; + getSoftPropertiesAndInc().mDampCoeff = _damp; } //============================================================================== double SoftBodyNode::getDampingCoefficient() const { - return getSoft().mDampCoeff; + return getSoftProperties().mDampCoeff; } //============================================================================== void SoftBodyNode::removeAllPointMasses() { mPointMasses.clear(); - getSoftAndInc().mPointProps.clear(); + getSoftPropertiesAndInc().mPointProps.clear(); } //============================================================================== @@ -424,7 +428,8 @@ PointMass* SoftBodyNode::addPointMass(const PointMass::Properties& _properties) { mPointMasses.push_back(new PointMass(this)); mPointMasses.back()->mIndex = mPointMasses.size()-1; - getSoftAndInc().mPointProps.push_back(_properties); + getSoftPropertiesAndInc().mPointProps.push_back(_properties); + configureSoftMeshShape(getSoftProperties()); return mPointMasses.back(); } @@ -442,20 +447,20 @@ void SoftBodyNode::connectPointMasses(size_t _idx1, size_t _idx2) //============================================================================== void SoftBodyNode::addFace(const Eigen::Vector3i& _face) { - getSoftAndInc().addFace(_face); + getSoftPropertiesAndInc().addFace(_face); } //============================================================================== const Eigen::Vector3i& SoftBodyNode::getFace(size_t _idx) const { - assert(_idx < getSoft().mFaces.size()); - return getSoft().mFaces[_idx]; + assert(_idx < getSoftProperties().mFaces.size()); + return getSoftProperties().mFaces[_idx]; } //============================================================================== size_t SoftBodyNode::getNumFaces() const { - return getSoft().mFaces.size(); + return getSoftProperties().mFaces.size(); } //============================================================================== @@ -1202,20 +1207,20 @@ void SoftBodyNode::draw(renderer::RenderInterface* _ri, Eigen::Vector3d pos; Eigen::Vector3d pos_normalized; - for (size_t i = 0; i < getSoft().mFaces.size(); ++i) + for (size_t i = 0; i < getSoftProperties().mFaces.size(); ++i) { glEnable(GL_AUTO_NORMAL); glBegin(GL_TRIANGLES); - pos = mPointMasses[getSoft().mFaces[i](0)]->getLocalPosition(); + pos = mPointMasses[getSoftProperties().mFaces[i](0)]->getLocalPosition(); pos_normalized = pos.normalized(); glNormal3f(pos_normalized(0), pos_normalized(1), pos_normalized(2)); glVertex3f(pos(0), pos(1), pos(2)); - pos = mPointMasses[getSoft().mFaces[i](1)]->getLocalPosition(); + pos = mPointMasses[getSoftProperties().mFaces[i](1)]->getLocalPosition(); pos_normalized = pos.normalized(); glNormal3f(pos_normalized(0), pos_normalized(1), pos_normalized(2)); glVertex3f(pos(0), pos(1), pos(2)); - pos = mPointMasses[getSoft().mFaces[i](2)]->getLocalPosition(); + pos = mPointMasses[getSoftProperties().mFaces[i](2)]->getLocalPosition(); pos_normalized = pos.normalized(); glNormal3f(pos_normalized(0), pos_normalized(1), pos_normalized(2)); glVertex3f(pos(0), pos(1), pos(2)); @@ -1237,14 +1242,20 @@ void SoftBodyNode::draw(renderer::RenderInterface* _ri, } //============================================================================== -SoftBodyNode::UniqueProperties& SoftBodyNode::getSoftAndInc() +PointMass::State& SoftBodyNode::getPointState(size_t index) +{ + return getSoftBodyAddon()->_getState().mPointStates.at(index); +} + +//============================================================================== +SoftBodyNode::UniqueProperties& SoftBodyNode::getSoftPropertiesAndInc() { getSoftBodyAddon()->incrementVersion(); return getSoftBodyAddon()->_getProperties(); } //============================================================================== -const SoftBodyNode::UniqueProperties& SoftBodyNode::getSoft() const +const SoftBodyNode::UniqueProperties& SoftBodyNode::getSoftProperties() const { return getSoftBodyAddon()->_getProperties(); } diff --git a/dart/dynamics/SoftBodyNode.h b/dart/dynamics/SoftBodyNode.h index 2d469ba320396..d8c7f31767c07 100644 --- a/dart/dynamics/SoftBodyNode.h +++ b/dart/dynamics/SoftBodyNode.h @@ -295,12 +295,15 @@ class SoftBodyNode : public detail::SoftBodyNodeBase protected: + /// Get a reference to the State of a PointMass + PointMass::State& getPointState(size_t index); + /// Get a reference to the UniqueProperties of this SoftBodyNode and increment /// its version. - UniqueProperties& getSoftAndInc(); + UniqueProperties& getSoftPropertiesAndInc(); /// Get a reference to the UniqueProperties of this SoftBodyNode - const UniqueProperties& getSoft() const; + const UniqueProperties& getSoftProperties() const; /// \brief List of point masses composing deformable mesh. std::vector mPointMasses; diff --git a/dart/dynamics/detail/SoftBodyNodeProperties.h b/dart/dynamics/detail/SoftBodyNodeProperties.h index adb10d6338f2a..302ef43b9b976 100644 --- a/dart/dynamics/detail/SoftBodyNodeProperties.h +++ b/dart/dynamics/detail/SoftBodyNodeProperties.h @@ -56,6 +56,13 @@ namespace detail { //============================================================================== class SoftBodyAddon; +//============================================================================== +struct SoftBodyNodeUniqueState +{ + /// Array of States for PointMasses + std::vector mPointStates; +}; + //============================================================================== struct SoftBodyNodeUniqueProperties { @@ -115,15 +122,16 @@ void SoftBodyNodePropertiesUpdate(SoftBodyAddon* addon); //============================================================================== class SoftBodyAddon final : - public common::AddonWithVersionedProperties< - SoftBodyAddon, SoftBodyNodeUniqueProperties, SoftBodyNode, + public common::AddonWithStateAndVersionedProperties< + SoftBodyAddon, SoftBodyNodeUniqueState, SoftBodyNodeUniqueProperties, + SoftBodyNode, &common::detail::NoOp, &SoftBodyNodePropertiesUpdate > { public: friend class dart::dynamics::SoftBodyNode; - DART_COMMON_ADDON_PROPERTY_CONSTRUCTOR(SoftBodyAddon, SoftBodyNodePropertiesUpdate) + DART_COMMON_ADDON_STATE_PROPERTY_CONSTRUCTORS(SoftBodyAddon) DART_COMMON_SET_GET_ADDON_PROPERTY(double, Kv) DART_COMMON_SET_GET_ADDON_PROPERTY(double, Ke) @@ -131,6 +139,12 @@ class SoftBodyAddon final : protected: + /// Get a direct reference to the State of this Addon + inline StateData& _getState() { return mState; } + + /// Get a direct reference to the State of this Addon + inline const StateData& _getState() const { return mState; } + /// Get a direct reference to the Properties of this Addon inline PropertiesData& _getProperties() { return mProperties; } From c341b9723f209ecdf863fd644fb897987ac2ca96 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Mon, 21 Mar 2016 14:15:34 -0400 Subject: [PATCH 03/62] corrected the way states and properties are managed for SoftBodyNode --- dart/dynamics/PointMass.cpp | 27 +++--- dart/dynamics/PointMass.h | 2 +- dart/dynamics/SoftBodyNode.cpp | 95 ++++++++++--------- dart/dynamics/SoftBodyNode.h | 5 +- dart/dynamics/SoftMeshShape.cpp | 6 +- dart/dynamics/SoftMeshShape.h | 5 +- dart/dynamics/detail/SoftBodyNodeProperties.h | 6 +- 7 files changed, 83 insertions(+), 63 deletions(-) diff --git a/dart/dynamics/PointMass.cpp b/dart/dynamics/PointMass.cpp index 2b1b2c47823bd..b3630fcec5c96 100644 --- a/dart/dynamics/PointMass.cpp +++ b/dart/dynamics/PointMass.cpp @@ -671,19 +671,24 @@ Eigen::Vector3d PointMass::getWorldAcceleration() const //============================================================================== void PointMass::init() { - // Dependen generalized coordinate setting - int parentDof = mParentSoftBodyNode->getNumDependentGenCoords(); + mDependentGenCoordIndices = mParentSoftBodyNode->getDependentGenCoordIndices(); - mDependentGenCoordIndices.resize(parentDof + 3); - for (int i = 0; i < parentDof; ++i) - { - mDependentGenCoordIndices[i] - = mParentSoftBodyNode->getDependentGenCoordIndex(i); - } + // TODO(MXG): The old code below would result in undefined behavior if the + // coordinates were actually used for anything. + +// // Dependen generalized coordinate setting +// int parentDof = mParentSoftBodyNode->getNumDependentGenCoords(); + +// mDependentGenCoordIndices.resize(parentDof + 3); +// for (int i = 0; i < parentDof; ++i) +// { +// mDependentGenCoordIndices[i] +// = mParentSoftBodyNode->getDependentGenCoordIndex(i); +// } -// mDependentGenCoordIndices[parentDof] = mIndexInSkeleton[0]; -// mDependentGenCoordIndices[parentDof + 1] = mIndexInSkeleton[1]; -// mDependentGenCoordIndices[parentDof + 2] = mIndexInSkeleton[2]; +//// mDependentGenCoordIndices[parentDof] = mIndexInSkeleton[0]; +//// mDependentGenCoordIndices[parentDof + 1] = mIndexInSkeleton[1]; +//// mDependentGenCoordIndices[parentDof + 2] = mIndexInSkeleton[2]; } //============================================================================== diff --git a/dart/dynamics/PointMass.h b/dart/dynamics/PointMass.h index 804222d124b4d..29ab40530e9ba 100644 --- a/dart/dynamics/PointMass.h +++ b/dart/dynamics/PointMass.h @@ -641,7 +641,7 @@ class PointMass : public common::Subject Eigen::Vector3d mFext; /// A increasingly sorted list of dependent dof indices. - std::vector mDependentGenCoordIndices; + std::vector mDependentGenCoordIndices; /// Whether the node is currently in collision with another node. bool mIsColliding; diff --git a/dart/dynamics/SoftBodyNode.cpp b/dart/dynamics/SoftBodyNode.cpp index 56adf2c3effa8..eda8fec5e5f84 100644 --- a/dart/dynamics/SoftBodyNode.cpp +++ b/dart/dynamics/SoftBodyNode.cpp @@ -122,12 +122,21 @@ SoftBodyNodeProperties::SoftBodyNodeProperties( // Do nothing } +//============================================================================== +void SoftBodyNodeStateUpdate(SoftBodyAddon* addon) +{ + if(SoftBodyNode* sbn = addon->getManager()) + sbn->mNotifier->notifyTransformUpdate(); +} + //============================================================================== void SoftBodyNodePropertiesUpdate(SoftBodyAddon* addon) { - static_assert(false, "This should actually update kinematics things, I don't think the configuring is necessary"); if(SoftBodyNode* sbn = addon->getManager()) - sbn->configureSoftMeshShape(addon->getProperties()); + { + sbn->configurePointMasses(sbn->mSoftShapeNode.lock()); + SoftBodyNodeStateUpdate(addon); + } } } // namespace detail @@ -221,7 +230,18 @@ SoftBodyNode::SoftBodyNode(BodyNode* _parentBodyNode, { createSoftBodyAddon(); mNotifier = new PointMassNotifier(this, "PointMassNotifier"); - setProperties(_properties); + std::cout << "Creating and assigning mSoftShapeNode" << std::endl; + ShapeNode* softNode = createShapeNodeWith< + VisualAddon, CollisionAddon, DynamicsAddon>( + std::make_shared(this), getName()+"_SoftMeshShape"); + mSoftShapeNode = softNode; + + // Dev's Note: We do this workaround (instead of just using setProperties(~)) + // because mSoftShapeNode cannot be used until init(SkeletonPtr) has been + // called on this BodyNode, but that happens after construction is finished. + getSoftBodyAddon()->mProperties = _properties; + configurePointMasses(softNode); + mNotifier->notifyTransformUpdate(); } //============================================================================== @@ -240,24 +260,26 @@ BodyNode* SoftBodyNode::clone(BodyNode* _parentBodyNode, } //============================================================================== -void SoftBodyNode::configureSoftMeshShape( - const UniqueProperties& softProperties) +void SoftBodyNode::configurePointMasses(ShapeNode* softNode) { - UniqueProperties& mSoftP = getSoftBodyAddon()->_getProperties(); + const UniqueProperties& softProperties = getSoftBodyAddon()->getProperties(); size_t newCount = softProperties.mPointProps.size(); size_t oldCount = mPointMasses.size(); + + if(newCount == oldCount) + return; + + // Adjust the number of PointMass objects since that has changed if(newCount < oldCount) { for(size_t i = newCount; i < oldCount; ++i) delete mPointMasses[i]; mPointMasses.resize(newCount); - mSoftP.mPointProps.resize(newCount); } else if(oldCount < newCount) { mPointMasses.resize(newCount); - mSoftP.mPointProps.resize(newCount); for(size_t i = oldCount; i < newCount; ++i) { mPointMasses[i] = new PointMass(this); @@ -266,46 +288,33 @@ void SoftBodyNode::configureSoftMeshShape( } } - const std::vector& allProps = softProperties.mPointProps; - for(size_t i=0; isetRestingPosition(props.mX0); - p->setMass(props.mMass); - - mSoftP.mPointProps[i].mConnectedPointMassIndices = - props.mConnectedPointMassIndices; - } - - setVertexSpringStiffness(softProperties.mKv); - setEdgeSpringStiffness(softProperties.mKe); - setDampingCoefficient(softProperties.mDampCoeff); - mSoftP.mFaces = softProperties.mFaces; - - static_assert(false, "This is all very wrong and needs to be fixed"); - // Create a new SoftMeshShape for this SoftBodyNode - auto softMesh = std::shared_ptr(new SoftMeshShape(this)); - auto newSoftShapeNode - = createShapeNodeWith( - softMesh, getName()+"_SoftMeshShape"); + // Resize the number of States in the Addon + getSoftBodyAddon()->_getState().mPointStates.resize( + softProperties.mPointProps.size(), PointMass::State()); - // Copy the properties of the previous soft shape, if it exists - ShapeNodePtr softNode = mSoftShapeNode.lock(); + // Access the SoftMeshShape and reallocate its meshes if(softNode) { - newSoftShapeNode->getVisualAddon()->setColor( - softNode->getVisualAddon()->getRGBA()); - - newSoftShapeNode->setRelativeTransform( - softNode->getRelativeTransform()); + std::shared_ptr softShape = + std::dynamic_pointer_cast(softNode->getShape()); - softNode->remove(); + if(softShape) + softShape->_buildMesh(); } + else + { + dtwarn << "[SoftBodyNode::configurePointMasses] The ShapeNode containing " + << "the SoftMeshShape for the SoftBodyNode named [" << getName() + << "] (" << this << ") has been removed. The soft body features for " + << "this SoftBodyNode cannot be used unless you recreate the " + << "SoftMeshShape.\n"; - getSoftBodyAddon()->_getState().mPointStates.resize( - allProps.size(), PointMass::State()); - mSoftShapeNode = newSoftShapeNode; + std::cout << "ShapeNodes: " << std::endl; + for(size_t i=0; i < getNumShapeNodes(); ++i) + { + std::cout << "- " << i << ") " << getShapeNode(i)->getName() << std::endl; + } + } } //============================================================================== @@ -429,7 +438,7 @@ PointMass* SoftBodyNode::addPointMass(const PointMass::Properties& _properties) mPointMasses.push_back(new PointMass(this)); mPointMasses.back()->mIndex = mPointMasses.size()-1; getSoftPropertiesAndInc().mPointProps.push_back(_properties); - configureSoftMeshShape(getSoftProperties()); + configurePointMasses(mSoftShapeNode.lock()); return mPointMasses.back(); } diff --git a/dart/dynamics/SoftBodyNode.h b/dart/dynamics/SoftBodyNode.h index d8c7f31767c07..d4f1a61b7cd65 100644 --- a/dart/dynamics/SoftBodyNode.h +++ b/dart/dynamics/SoftBodyNode.h @@ -53,7 +53,8 @@ class SoftBodyNode : public detail::SoftBodyNodeBase friend class Skeleton; friend class PointMass; friend class PointMassNotifier; - friend void detail::SoftBodyNodePropertiesUpdate(SoftBodyAddon *addon); + friend void detail::SoftBodyNodeStateUpdate(SoftBodyAddon* addon); + friend void detail::SoftBodyNodePropertiesUpdate(SoftBodyAddon* addon); using UniqueProperties = detail::SoftBodyNodeUniqueProperties; using Properties = detail::SoftBodyNodeProperties; @@ -155,7 +156,7 @@ class SoftBodyNode : public detail::SoftBodyNodeBase /// Used by SoftBodyAddon to have this SoftBodyNode reconstruct its /// SoftMeshShape - void configureSoftMeshShape(const UniqueProperties& softProperties); + void configurePointMasses(ShapeNode* softNode); //-------------------------------------------------------------------------- // Sub-functions for Recursive Kinematics Algorithms diff --git a/dart/dynamics/SoftMeshShape.cpp b/dart/dynamics/SoftMeshShape.cpp index aceb0d2e1418a..ac713f034adee 100644 --- a/dart/dynamics/SoftMeshShape.cpp +++ b/dart/dynamics/SoftMeshShape.cpp @@ -58,12 +58,12 @@ SoftMeshShape::SoftMeshShape(SoftBodyNode* _softBodyNode) SoftMeshShape::~SoftMeshShape() { - delete mAssimpMesh; + // Do nothing } const aiMesh* SoftMeshShape::getAssimpMesh() const { - return mAssimpMesh; + return mAssimpMesh.get(); } const SoftBodyNode* SoftMeshShape::getSoftBodyNode() const @@ -97,7 +97,7 @@ void SoftMeshShape::_buildMesh() int nFaces = mSoftBodyNode->getNumFaces(); // Create new aiMesh - mAssimpMesh = new aiMesh(); + mAssimpMesh = std::unique_ptr(new aiMesh()); // Set vertices and normals mAssimpMesh->mNumVertices = nVertices; diff --git a/dart/dynamics/SoftMeshShape.h b/dart/dynamics/SoftMeshShape.h index eea133f8e3195..f4540133bb2f6 100644 --- a/dart/dynamics/SoftMeshShape.h +++ b/dart/dynamics/SoftMeshShape.h @@ -50,6 +50,9 @@ class SoftBodyNode; class SoftMeshShape : public Shape { public: + + friend class SoftBodyNode; + /// \brief Constructor. explicit SoftMeshShape(SoftBodyNode* _softBodyNode); @@ -85,7 +88,7 @@ class SoftMeshShape : public Shape SoftBodyNode* mSoftBodyNode; /// \brief - aiMesh* mAssimpMesh; + std::unique_ptr mAssimpMesh; }; } // namespace dynamics diff --git a/dart/dynamics/detail/SoftBodyNodeProperties.h b/dart/dynamics/detail/SoftBodyNodeProperties.h index 302ef43b9b976..4262339841c51 100644 --- a/dart/dynamics/detail/SoftBodyNodeProperties.h +++ b/dart/dynamics/detail/SoftBodyNodeProperties.h @@ -117,6 +117,9 @@ struct SoftBodyNodeProperties virtual ~SoftBodyNodeProperties() = default; }; +//============================================================================== +void SoftBodyNodeStateUpdate(SoftBodyAddon* addon); + //============================================================================== void SoftBodyNodePropertiesUpdate(SoftBodyAddon* addon); @@ -124,8 +127,7 @@ void SoftBodyNodePropertiesUpdate(SoftBodyAddon* addon); class SoftBodyAddon final : public common::AddonWithStateAndVersionedProperties< SoftBodyAddon, SoftBodyNodeUniqueState, SoftBodyNodeUniqueProperties, - SoftBodyNode, &common::detail::NoOp, - &SoftBodyNodePropertiesUpdate > + SoftBodyNode, &SoftBodyNodeStateUpdate, &SoftBodyNodePropertiesUpdate > { public: From f9bce3bbc0be16fc5f2f4924d9b61ebebeb60e3d Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Mon, 21 Mar 2016 16:25:37 -0400 Subject: [PATCH 04/62] Successfully recording SoftBodyNode states via Configuration plus AddonManager::State --- dart/dynamics/Skeleton.h | 7 ++ osgDart/examples/osgSoftBodies.cpp | 176 ++++++++++++++++++++++++++++- 2 files changed, 182 insertions(+), 1 deletion(-) diff --git a/dart/dynamics/Skeleton.h b/dart/dynamics/Skeleton.h index 3f4e598fe57bc..a0f868ded9395 100644 --- a/dart/dynamics/Skeleton.h +++ b/dart/dynamics/Skeleton.h @@ -127,6 +127,13 @@ class Skeleton : bool operator!=(const Configuration& other) const; }; + + + struct State + { + + }; + /// The Properties of this Skeleton which are independent of the components /// within the Skeleton, such as its BodyNodes and Joints. This does not /// include any Properties of the Skeleton's Addons. diff --git a/osgDart/examples/osgSoftBodies.cpp b/osgDart/examples/osgSoftBodies.cpp index f08d0b4510e39..d39d38cc6196a 100644 --- a/osgDart/examples/osgSoftBodies.cpp +++ b/osgDart/examples/osgSoftBodies.cpp @@ -41,6 +41,178 @@ #include +using namespace dart::dynamics; + +class RecordingWorld : public osgDart::WorldNode +{ +public: + + RecordingWorld(const dart::simulation::WorldPtr& world) + : osgDart::WorldNode(world) + { + grabTimeSlice(); + mCurrentIndex = 0; + } + + void grabTimeSlice() + { + TimeSlice slice; + slice.reserve(mWorld->getNumSkeletons()); + + for(size_t i=0; i < mWorld->getNumSkeletons(); ++i) + { + const SkeletonPtr& skeleton = mWorld->getSkeleton(i); + State state; + state.mConfig = skeleton->getConfiguration(); + state.mAddonStates.reserve(skeleton->getNumBodyNodes()); + + for(size_t j=0; j < skeleton->getNumBodyNodes(); ++j) + { + BodyNode* bn = skeleton->getBodyNode(j); + state.mAddonStates.push_back(bn->getAddonStates()); + } + + slice.push_back(state); + } + + mHistory.push_back(slice); + } + + void customPostStep() override + { + if(mCurrentIndex < mHistory.size()-1) + mHistory.resize(mCurrentIndex+1); + + grabTimeSlice(); + ++mCurrentIndex; + } + + void moveTo(size_t index) + { + mViewer->simulate(false); + + if(mHistory.empty()) + return; + + if(index >= mHistory.size()) + index = mHistory.size() - 1; + + std::cout << "Moving to time step #" << index << std::endl; + + const TimeSlice& slice = mHistory[index]; + for(size_t i=0; i < slice.size(); ++i) + { + const State& state = slice[i]; + const SkeletonPtr& skeleton = mWorld->getSkeleton(i); + + skeleton->setConfiguration(state.mConfig); + + for(size_t j=0; j < skeleton->getNumBodyNodes(); ++j) + { + BodyNode* bn = skeleton->getBodyNode(j); + bn->setAddonStates(state.mAddonStates[j]); + } + } + + mCurrentIndex = index; + } + + void moveForward(int delta) + { + moveTo(mCurrentIndex+delta); + } + + void moveBackward(int delta) + { + if(mCurrentIndex > 0) + moveTo(mCurrentIndex-delta); + } + + void restart() + { + moveTo(0); + } + + void moveToEnd() + { + moveTo(mHistory.size()-1); + } + + struct State + { + Skeleton::Configuration mConfig; + std::vector mAddonStates; + }; + + using TimeSlice = std::vector; + using History = std::vector; + + History mHistory; + + size_t mCurrentIndex; +}; + +class RecordingEventHandler : public osgGA::GUIEventHandler +{ +public: + + RecordingEventHandler(RecordingWorld* rec) + : mRecWorld(rec) + { + // Do nothing + } + + virtual bool handle(const osgGA::GUIEventAdapter& ea, + osgGA::GUIActionAdapter&) override + { + if(!mRecWorld) + return false; + + if(ea.getEventType() == osgGA::GUIEventAdapter::KEYDOWN) + { + if(ea.getKey() == '[') + { + mRecWorld->moveBackward(1); + return true; + } + + if(ea.getKey() == ']') + { + mRecWorld->moveForward(1); + return true; + } + + if(ea.getKey() == '{') + { + mRecWorld->moveBackward(10); + return true; + } + + if(ea.getKey() == '}') + { + mRecWorld->moveForward(10); + return true; + } + + if(ea.getKey() == 'r') + { + mRecWorld->restart(); + return true; + } + + if(ea.getKey() == '\\') + { + mRecWorld->moveToEnd(); + return true; + } + } + + return false; + } + + RecordingWorld* mRecWorld; +}; + int main() { using namespace dart::dynamics; @@ -48,13 +220,15 @@ int main() dart::simulation::WorldPtr world = dart::utils::SkelParser::readWorld(DART_DATA_PATH"skel/softBodies.skel"); - osg::ref_ptr node = new osgDart::WorldNode(world); +// osg::ref_ptr node = new osgDart::WorldNode(world); + osg::ref_ptr node = new RecordingWorld(world); node->simulate(true); node->setNumStepsPerCycle(15); osgDart::Viewer viewer; viewer.addWorldNode(node); + viewer.addEventHandler(new RecordingEventHandler(node)); std::cout << viewer.getInstructions() << std::endl; From 733f21920ecd1baa0c8ec8bd6341180c965cafd7 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Mon, 28 Mar 2016 13:52:58 -0400 Subject: [PATCH 05/62] Renamed Addon to Aspect and AddonManager to Composite --- apps/addDeleteSkels/MyWindow.cpp | 8 +- apps/hardcodedDesign/Main.cpp | 18 +- apps/rigidLoop/Main.cpp | 4 +- apps/rigidShapes/MyWindow.cpp | 24 +- apps/softBodies/Main.cpp | 4 +- dart/collision/dart/DARTCollisionDetector.cpp | 12 +- dart/collision/fcl/FCLCollisionNode.cpp | 2 +- .../fcl_mesh/FCLMeshCollisionNode.cpp | 10 +- dart/common/AddonManager.h | 210 -------- dart/common/{Addon.cpp => Aspect.cpp} | 18 +- dart/common/{Addon.h => Aspect.h} | 148 +++--- ...AddonWithVersion.h => AspectWithVersion.h} | 62 +-- .../{AddonManager.cpp => Composite.cpp} | 122 ++--- dart/common/Composite.h | 210 ++++++++ ...AddonManagerJoiner.h => CompositeJoiner.h} | 44 +- dart/common/Empty.h | 2 +- dart/common/Extensible.h | 4 +- .../{RequiresAddon.h => RequiresAspect.h} | 38 +- ...lizedForAddon.h => SpecializedForAspect.h} | 108 ++-- dart/common/detail/{Addon.h => Aspect.h} | 24 +- ...AddonWithVersion.h => AspectWithVersion.h} | 144 ++--- .../detail/{AddonManager.h => Composite.h} | 114 ++-- ...AddonManagerJoiner.h => CompositeJoiner.h} | 36 +- .../{RequiresAddon.h => RequiresAspect.h} | 14 +- dart/common/detail/SpecializedForAddon.h | 329 ------------ dart/common/detail/SpecializedForAspect.h | 329 ++++++++++++ .../detail/TemplateJoinerDispatchMacro.h | 2 +- dart/dynamics/BodyNode.cpp | 20 +- dart/dynamics/BodyNode.h | 34 +- dart/dynamics/EndEffector.cpp | 24 +- dart/dynamics/EndEffector.h | 34 +- dart/dynamics/Entity.h | 2 +- dart/dynamics/EulerJoint.cpp | 12 +- dart/dynamics/EulerJoint.h | 6 +- dart/dynamics/Joint.cpp | 8 +- dart/dynamics/Joint.h | 22 +- dart/dynamics/MultiDofJoint.h | 8 +- dart/dynamics/PlanarJoint.cpp | 50 +- dart/dynamics/PlanarJoint.h | 6 +- dart/dynamics/PrismaticJoint.cpp | 8 +- dart/dynamics/PrismaticJoint.h | 6 +- dart/dynamics/RevoluteJoint.cpp | 8 +- dart/dynamics/RevoluteJoint.h | 6 +- dart/dynamics/ScrewJoint.cpp | 12 +- dart/dynamics/ScrewJoint.h | 6 +- dart/dynamics/ShapeFrame.cpp | 66 +-- dart/dynamics/ShapeFrame.h | 106 ++-- dart/dynamics/ShapeNode.cpp | 14 +- dart/dynamics/ShapeNode.h | 16 +- dart/dynamics/SimpleFrame.cpp | 4 +- dart/dynamics/SingleDofJoint.cpp | 142 ++--- dart/dynamics/SingleDofJoint.h | 6 +- dart/dynamics/Skeleton.cpp | 4 +- dart/dynamics/Skeleton.h | 18 +- dart/dynamics/SoftBodyNode.cpp | 38 +- dart/dynamics/SoftBodyNode.h | 10 +- dart/dynamics/UniversalJoint.cpp | 12 +- dart/dynamics/UniversalJoint.h | 6 +- dart/dynamics/detail/BasicNodeManager.h | 90 ++-- dart/dynamics/detail/BodyNode.h | 24 +- dart/dynamics/detail/BodyNodeProperties.h | 10 +- dart/dynamics/detail/EulerJointProperties.h | 16 +- dart/dynamics/detail/MultiDofJoint.h | 152 +++--- .../dynamics/detail/MultiDofJointProperties.h | 72 +-- .../dynamics/detail/PlanarJointProperties.cpp | 8 +- dart/dynamics/detail/PlanarJointProperties.h | 22 +- .../detail/PrismaticJointProperties.cpp | 4 +- .../detail/PrismaticJointProperties.h | 14 +- .../detail/RevoluteJointProperties.cpp | 4 +- .../dynamics/detail/RevoluteJointProperties.h | 14 +- dart/dynamics/detail/ScrewJointProperties.cpp | 4 +- dart/dynamics/detail/ScrewJointProperties.h | 16 +- .../detail/SingleDofJointProperties.cpp | 2 +- .../detail/SingleDofJointProperties.h | 50 +- dart/dynamics/detail/SoftBodyNodeProperties.h | 34 +- dart/dynamics/detail/SpecializedNodeManager.h | 2 +- .../detail/UniversalJointProperties.cpp | 8 +- .../detail/UniversalJointProperties.h | 14 +- dart/renderer/OpenGLRenderInterface.cpp | 14 +- dart/utils/SkelParser.cpp | 18 +- dart/utils/VskParser.cpp | 16 +- dart/utils/sdf/SdfParser.cpp | 12 +- dart/utils/urdf/DartLoader.cpp | 10 +- osgDart/DragAndDrop.cpp | 4 +- osgDart/InteractiveFrame.cpp | 10 +- osgDart/SupportPolygonVisual.cpp | 6 +- osgDart/examples/osgAtlasPuppet.cpp | 8 +- osgDart/examples/osgDragAndDrop.cpp | 6 +- osgDart/examples/osgHuboPuppet.cpp | 4 +- .../examples/osgOperationalSpaceControl.cpp | 4 +- osgDart/examples/osgSoftBodies.cpp | 8 +- osgDart/render/BoxShapeNode.cpp | 16 +- osgDart/render/CylinderShapeNode.cpp | 16 +- osgDart/render/EllipsoidShapeNode.cpp | 16 +- osgDart/render/LineSegmentShapeNode.cpp | 16 +- osgDart/render/MeshShapeNode.cpp | 6 +- osgDart/render/PlaneShapeNode.cpp | 16 +- osgDart/render/ShapeNode.cpp | 10 +- osgDart/render/ShapeNode.h | 10 +- osgDart/render/SoftMeshShapeNode.cpp | 20 +- tutorials/tutorialBiped-Finished.cpp | 8 +- tutorials/tutorialBiped.cpp | 8 +- tutorials/tutorialCollisions-Finished.cpp | 24 +- tutorials/tutorialCollisions.cpp | 12 +- tutorials/tutorialDominoes-Finished.cpp | 6 +- tutorials/tutorialDominoes.cpp | 6 +- tutorials/tutorialMultiPendulum-Finished.cpp | 26 +- tutorials/tutorialMultiPendulum.cpp | 12 +- unittests/TestHelpers.h | 34 +- unittests/testAddon.cpp | 502 +++++++++--------- unittests/testInverseKinematics.cpp | 4 +- unittests/testJoints.cpp | 2 +- unittests/testSdfParser.cpp | 4 +- 113 files changed, 2104 insertions(+), 2104 deletions(-) delete mode 100644 dart/common/AddonManager.h rename dart/common/{Addon.cpp => Aspect.cpp} (83%) rename dart/common/{Addon.h => Aspect.h} (52%) rename dart/common/{AddonWithVersion.h => AspectWithVersion.h} (72%) rename dart/common/{AddonManager.cpp => Composite.cpp} (65%) create mode 100644 dart/common/Composite.h rename dart/common/{AddonManagerJoiner.h => CompositeJoiner.h} (80%) rename dart/common/{RequiresAddon.h => RequiresAspect.h} (68%) rename dart/common/{SpecializedForAddon.h => SpecializedForAspect.h} (58%) rename dart/common/detail/{Addon.h => Aspect.h} (84%) rename dart/common/detail/{AddonWithVersion.h => AspectWithVersion.h} (71%) rename dart/common/detail/{AddonManager.h => Composite.h} (67%) rename dart/common/detail/{AddonManagerJoiner.h => CompositeJoiner.h} (75%) rename dart/common/detail/{RequiresAddon.h => RequiresAspect.h} (86%) delete mode 100644 dart/common/detail/SpecializedForAddon.h create mode 100644 dart/common/detail/SpecializedForAspect.h diff --git a/apps/addDeleteSkels/MyWindow.cpp b/apps/addDeleteSkels/MyWindow.cpp index dd36b1692ee37..b293077930913 100644 --- a/apps/addDeleteSkels/MyWindow.cpp +++ b/apps/addDeleteSkels/MyWindow.cpp @@ -104,10 +104,10 @@ void MyWindow::spawnCube(const Eigen::Vector3d& _position, = newCubeSkeleton->createJointAndBodyNodePair( nullptr, joint, body); auto shapeNode = pair.second->createShapeNodeWith< - dart::dynamics::VisualAddon, - dart::dynamics::CollisionAddon, - dart::dynamics::DynamicsAddon>(newBoxShape); - shapeNode->getVisualAddon()->setColor(dart::math::randomVector<3>(0.0, 1.0)); + dart::dynamics::VisualAspect, + dart::dynamics::CollisionAspect, + dart::dynamics::DynamicsAspect>(newBoxShape); + shapeNode->getVisualAspect()->setColor(dart::math::randomVector<3>(0.0, 1.0)); mWorld->addSkeleton(newCubeSkeleton); } diff --git a/apps/hardcodedDesign/Main.cpp b/apps/hardcodedDesign/Main.cpp index 811f4a0407e72..173744c292799 100644 --- a/apps/hardcodedDesign/Main.cpp +++ b/apps/hardcodedDesign/Main.cpp @@ -73,9 +73,9 @@ int main(int argc, char* argv[]) { LeftLegSkel->createJointAndBodyNodePair( nullptr, joint, body); pair.second->createShapeNodeWith< - dart::dynamics::VisualAddon, - dart::dynamics::CollisionAddon, - dart::dynamics::DynamicsAddon>(shape); + dart::dynamics::VisualAspect, + dart::dynamics::CollisionAspect, + dart::dynamics::DynamicsAspect>(shape); dart::dynamics::BodyNode* parent = pair.second; // BodyNode 2: Left Hip Roll (LHR) whose parent is: LHY @@ -93,9 +93,9 @@ int main(int argc, char* argv[]) { parent, joint, body); pair1.first->setAxis(Eigen::Vector3d(1.0, 0.0, 0.0)); auto shapeNode1 = pair1.second->createShapeNodeWith< - dart::dynamics::VisualAddon, - dart::dynamics::CollisionAddon, - dart::dynamics::DynamicsAddon>(shape); + dart::dynamics::VisualAspect, + dart::dynamics::CollisionAspect, + dart::dynamics::DynamicsAspect>(shape); shapeNode1->setRelativeTranslation(Eigen::Vector3d(0.0, 0.0, 0.5)); pair1.second->setLocalCOM(shapeNode1->getRelativeTranslation()); pair1.second->setMass(mass); @@ -115,9 +115,9 @@ int main(int argc, char* argv[]) { LeftLegSkel->createJointAndBodyNodePair( LeftLegSkel->getBodyNode(1), joint, body); auto shapeNode2 = pair2.second->createShapeNodeWith< - dart::dynamics::VisualAddon, - dart::dynamics::CollisionAddon, - dart::dynamics::DynamicsAddon>(shape); + dart::dynamics::VisualAspect, + dart::dynamics::CollisionAspect, + dart::dynamics::DynamicsAspect>(shape); shapeNode2->setRelativeTranslation(Eigen::Vector3d(0.0, 0.0, 0.5)); pair2.second->setLocalCOM(shapeNode2->getRelativeTranslation()); pair2.second->setMass(mass); diff --git a/apps/rigidLoop/Main.cpp b/apps/rigidLoop/Main.cpp index be833793cd39e..19bc2878b6e3a 100644 --- a/apps/rigidLoop/Main.cpp +++ b/apps/rigidLoop/Main.cpp @@ -70,8 +70,8 @@ int main(int argc, char* argv[]) // create a ball joint constraint BodyNode* bd1 = myWorld->getSkeleton(0)->getBodyNode("link 6"); BodyNode* bd2 = myWorld->getSkeleton(0)->getBodyNode("link 10"); - bd1->getShapeNode(0)->getVisualAddon()->setColor(Eigen::Vector3d(1.0, 0.0, 0.0)); - bd2->getShapeNode(0)->getVisualAddon()->setColor(Eigen::Vector3d(1.0, 0.0, 0.0)); + bd1->getShapeNode(0)->getVisualAspect()->setColor(Eigen::Vector3d(1.0, 0.0, 0.0)); + bd2->getShapeNode(0)->getVisualAspect()->setColor(Eigen::Vector3d(1.0, 0.0, 0.0)); Eigen::Vector3d offset(0.0, 0.025, 0.0); Eigen::Vector3d jointPos = bd1->getTransform() * offset; BallJointConstraintPtr cl = diff --git a/apps/rigidShapes/MyWindow.cpp b/apps/rigidShapes/MyWindow.cpp index c552f1a0e9d85..3fc7ae8925163 100644 --- a/apps/rigidShapes/MyWindow.cpp +++ b/apps/rigidShapes/MyWindow.cpp @@ -186,10 +186,10 @@ void MyWindow::spawnBox(const Eigen::Isometry3d& _T, auto pair = newSkeleton->createJointAndBodyNodePair( nullptr, jointProp, bodyProp); auto shapeNode = pair.second->createShapeNodeWith< - dynamics::VisualAddon, - dynamics::CollisionAddon, - dynamics::DynamicsAddon>(newShape); - shapeNode->getVisualAddon()->setColor(math::randomVector<3>(0.0, 1.0)); + dynamics::VisualAspect, + dynamics::CollisionAspect, + dynamics::DynamicsAspect>(newShape); + shapeNode->getVisualAspect()->setColor(math::randomVector<3>(0.0, 1.0)); mWorld->addSkeleton(newSkeleton); } @@ -214,10 +214,10 @@ void MyWindow::spawnEllipsoid(const Eigen::Isometry3d& _T, auto pair = newSkeleton->createJointAndBodyNodePair( nullptr, jointProp, bodyProp); auto shapeNode = pair.second->createShapeNodeWith< - dynamics::VisualAddon, - dynamics::CollisionAddon, - dynamics::DynamicsAddon>(newShape); - shapeNode->getVisualAddon()->setColor(math::randomVector<3>(0.0, 1.0)); + dynamics::VisualAspect, + dynamics::CollisionAspect, + dynamics::DynamicsAspect>(newShape); + shapeNode->getVisualAspect()->setColor(math::randomVector<3>(0.0, 1.0)); mWorld->addSkeleton(newSkeleton); } @@ -243,10 +243,10 @@ void MyWindow::spawnCylinder(const Eigen::Isometry3d& _T, auto pair = newSkeleton->createJointAndBodyNodePair( nullptr, jointProp, bodyProp); auto shapeNode = pair.second->createShapeNodeWith< - dynamics::VisualAddon, - dynamics::CollisionAddon, - dynamics::DynamicsAddon>(newShape); - shapeNode->getVisualAddon()->setColor(math::randomVector<3>(0.0, 1.0)); + dynamics::VisualAspect, + dynamics::CollisionAspect, + dynamics::DynamicsAspect>(newShape); + shapeNode->getVisualAspect()->setColor(math::randomVector<3>(0.0, 1.0)); mWorld->addSkeleton(newSkeleton); } diff --git a/apps/softBodies/Main.cpp b/apps/softBodies/Main.cpp index 82e5d9ba04efe..04c54377969fe 100644 --- a/apps/softBodies/Main.cpp +++ b/apps/softBodies/Main.cpp @@ -62,9 +62,9 @@ int main(int argc, char* argv[]) { dart::dynamics::BodyNode* bn = skel->getBodyNode(j); Eigen::Vector3d color = dart::Color::Random(); - auto shapeNodes = bn->getShapeNodesWith(); + auto shapeNodes = bn->getShapeNodesWith(); for(auto shapeNode : shapeNodes) - shapeNode->getVisualAddon(true)->setColor(color); + shapeNode->getVisualAspect(true)->setColor(color); } } diff --git a/dart/collision/dart/DARTCollisionDetector.cpp b/dart/collision/dart/DARTCollisionDetector.cpp index 373102882e269..c8f5f2b46aaac 100644 --- a/dart/collision/dart/DARTCollisionDetector.cpp +++ b/dart/collision/dart/DARTCollisionDetector.cpp @@ -77,10 +77,10 @@ bool DARTCollisionDetector::detectCollision(bool /*_checkAllCollisions*/, if (!isCollidable(collNode1, collNode2)) continue; - auto collShapeNodes1 = BodyNode1->getShapeNodesWith(); + auto collShapeNodes1 = BodyNode1->getShapeNodesWith(); for (auto shapeNode1 : collShapeNodes1) { - auto collShapeNodes2 = BodyNode2->getShapeNodesWith(); + auto collShapeNodes2 = BodyNode2->getShapeNodesWith(); for (auto shapeNode2 : collShapeNodes2) { int currContactNum = mContacts.size(); @@ -147,16 +147,16 @@ bool DARTCollisionDetector::detectCollision(CollisionNode* _collNode1, { auto shapeNode1 = BodyNode1->getNode(i); - auto collisionAddon = shapeNode1->get(); - if (!collisionAddon) + auto collisionAspect = shapeNode1->get(); + if (!collisionAspect) continue; for (auto j = 0u; j < BodyNode2->getNumNodes(); ++j) { auto shapeNode2 = BodyNode2->getNode(j); - auto collisionAddon = shapeNode2->get(); - if (!collisionAddon) + auto collisionAspect = shapeNode2->get(); + if (!collisionAspect) continue; collide(shapeNode1->getShape(), shapeNode1->getWorldTransform(), diff --git a/dart/collision/fcl/FCLCollisionNode.cpp b/dart/collision/fcl/FCLCollisionNode.cpp index 5edc5a71bba09..50d565fbb52a4 100644 --- a/dart/collision/fcl/FCLCollisionNode.cpp +++ b/dart/collision/fcl/FCLCollisionNode.cpp @@ -347,7 +347,7 @@ FCLCollisionNode::FCLCollisionNode(dynamics::BodyNode* _bodyNode) using dynamics::MeshShape; using dynamics::SoftMeshShape; - auto collShapeNodes = _bodyNode->getShapeNodesWith(); + auto collShapeNodes = _bodyNode->getShapeNodesWith(); for (auto shapeNode : collShapeNodes) { auto shape = shapeNode->getShape(); diff --git a/dart/collision/fcl_mesh/FCLMeshCollisionNode.cpp b/dart/collision/fcl_mesh/FCLMeshCollisionNode.cpp index 8e96f655ab87b..0be4d6149151f 100644 --- a/dart/collision/fcl_mesh/FCLMeshCollisionNode.cpp +++ b/dart/collision/fcl_mesh/FCLMeshCollisionNode.cpp @@ -71,7 +71,7 @@ FCLMeshCollisionNode::FCLMeshCollisionNode(dynamics::BodyNode* _bodyNode) using dart::dynamics::SoftMeshShape; // Create meshes according to types of the shapes - auto collShapeNodes = _bodyNode->getShapeNodesWith(); + auto collShapeNodes = _bodyNode->getShapeNodesWith(); for (auto shapeNode : collShapeNodes) { auto shape = shapeNode->getShape(); @@ -161,8 +161,8 @@ bool FCLMeshCollisionNode::detectCollision(FCLMeshCollisionNode* _otherNode, auto bodyNode1 = this->getBodyNode(); auto bodyNode2 = _otherNode->getBodyNode(); - auto collShapeNodes1 = bodyNode1->getShapeNodesWith(); - auto collShapeNodes2 = bodyNode2->getShapeNodesWith(); + auto collShapeNodes1 = bodyNode1->getShapeNodesWith(); + auto collShapeNodes2 = bodyNode2->getShapeNodesWith(); for (size_t i = 0; i < mMeshes.size(); i++) { @@ -319,8 +319,8 @@ void FCLMeshCollisionNode::updateShape() { auto shapeNode = mBodyNode->getNode(i); - auto collisionAddon = shapeNode->get(); - if (!collisionAddon) + auto collisionAspect = shapeNode->get(); + if (!collisionAspect) continue; auto shape = shapeNode->getShape(); diff --git a/dart/common/AddonManager.h b/dart/common/AddonManager.h deleted file mode 100644 index 74c1b4a5fa0a6..0000000000000 --- a/dart/common/AddonManager.h +++ /dev/null @@ -1,210 +0,0 @@ -/* - * Copyright (c) 2015, 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_COMMON_ADDONMANAGER_H_ -#define DART_COMMON_ADDONMANAGER_H_ - -#include -#include -#include -#include - -#include "dart/common/Addon.h" - -namespace dart { -namespace common { - -/// AddonManager is a base class that should be virtually inherited by any class -/// that wants to be able to manage Addons. -/// -/// The base AddonManager class is completely agnostic to what kind of Addons it -/// is given. Addons are stored in a std::map, so access to its Addons happens -/// on average in log(N) time. Most often, a class that accepts Addons will have -/// certain Addon types that it will need to access frequently, and it would be -/// beneficial to have constant-time access to those Addon types. To get -/// constant-time access to specific Addon types, you can use the templated -/// class SpecializedForAddon. -class AddonManager -{ -public: - - using StateMap = std::map< std::type_index, std::unique_ptr >; - using State = ExtensibleMapHolder; - - using PropertiesMap = std::map< std::type_index, std::unique_ptr >; - using Properties = ExtensibleMapHolder; - - using AddonMap = std::map< std::type_index, std::unique_ptr >; - using RequiredAddonSet = std::unordered_set; - - /// Virtual destructor - virtual ~AddonManager() = default; - - /// Default constructor - AddonManager() = default; - - /// It is currently unsafe to copy an AddonManager - // TODO(MXG): Consider making this safe by cloning Addons into the new copy - AddonManager(const AddonManager&) = delete; - - /// It is currently unsafe to move an AddonManager - AddonManager(AddonManager&&) = delete; - - /// It is currently unsafe to copy an AddonManager - AddonManager& operator=(const AddonManager&) = delete; - - /// It is currently unsafe to move an AddonManager - AddonManager& operator=(AddonManager&&) = delete; - - /// Check if this AddonManager currently has a certain type of Addon - template - bool has() const; - - /// Get a certain type of Addon from this AddonManager - template - T* get(); - - /// Get a certain type of Addon from this AddonManager - template - const T* get() const; - - /// Make a clone of the addon and place the clone into this AddonManager. If - /// an Addon of the same type already exists in this AddonManager, the - /// existing Addon will be destroyed. - template - void set(const T* addon); - - /// Use move semantics to place addon into this AddonManager. If an Addon of - /// the same type already exists in this AddonManager, the existing Addon will - /// be destroyed. - template - void set(std::unique_ptr&& addon); - - /// Construct an Addon inside of this AddonManager - template - T* create(Args&&... args); - - /// Remove an Addon from this AddonManager. - template - void erase(); - - /// Remove an Addon from this AddonManager, but return its unique_ptr instead - /// of letting it be deleted. This allows you to safely use move semantics to - /// transfer an Addon between two AddonManagers. - template - std::unique_ptr release(); - - /// Check if this Manager is specialized for a specific type of Addon. - /// - /// By default, this simply returns false. - template - static constexpr bool isSpecializedFor(); - - /// Check if this Manager requires this specific type of Addon - template - bool requires() const; - - /// Set the states of the addons in this AddonManager based on the given - /// AddonManager::State. The states of any Addon types that do not exist - /// within this manager will be ignored. - void setAddonStates(const State& newStates); - - /// Get the states of the addons inside of this AddonManager - State getAddonStates() const; - - /// Fill outgoingStates with the states of the addons inside this AddonManager - void copyAddonStatesTo(State& outgoingStates) const; - - /// Set the properties of the addons in this AddonManager based on the given - /// AddonManager::Properties. The properties of any Addon types that do not - /// exist within this manager will be ignored. - void setAddonProperties(const Properties& newProperties); - - /// Get the properties of the addons inside of this AddonManager - Properties getAddonProperties() const; - - /// Fill outgoingProperties with the properties of the addons inside this - /// AddonManager - void copyAddonPropertiesTo(Properties& outgoingProperties) const; - - /// Give this AddonManager a copy of each Addon from otherManager - void duplicateAddons(const AddonManager* fromManager); - - /// Make the Addons of this AddonManager match the Addons of otherManager. Any - /// Addons in this AddonManager which do not exist in otherManager will be - /// erased. - void matchAddons(const AddonManager* otherManager); - -protected: - - template struct type { }; - - /// Become the AddonManager of the given Addon. This allows derived - /// AddonManager types to call the protected Addon::setManager function. - void becomeManager(Addon* addon, bool transfer); - - /// Non-templated version of set(const T*) - void _set(std::type_index type_idx, const Addon* addon); - - /// Non-templated version of set(std::unqiue_ptr&&) - void _set(std::type_index type_idx, std::unique_ptr addon); - - /// A map that relates the type of Addon to its pointer - AddonMap mAddonMap; - - /// A set containing type information for Addons which are not allowed to - /// leave this manager. - RequiredAddonSet mRequiredAddons; -}; - -//============================================================================== -/// Attach an arbitrary number of Addons to the specified AddonManager type. -// TODO(MXG): Consider putting this functionality into the AddonManager class -// itself. Also consider allowing the user to specify arguments for the -// constructors of the Addons. -template -void createAddons(T* /*mgr*/); - -//============================================================================== -template -void createAddons(T* mgr); - -} // namespace common -} // namespace dart - -#include "dart/common/detail/AddonManager.h" - -#endif // DART_COMMON_ADDONMANAGER_H_ diff --git a/dart/common/Addon.cpp b/dart/common/Aspect.cpp similarity index 83% rename from dart/common/Addon.cpp rename to dart/common/Aspect.cpp index f6a1226f50aaa..ce51eb0b5b3d4 100644 --- a/dart/common/Addon.cpp +++ b/dart/common/Aspect.cpp @@ -38,49 +38,49 @@ #include #include -#include "dart/common/Addon.h" +#include "dart/common/Aspect.h" #include "dart/common/Console.h" namespace dart { namespace common { //============================================================================== -void Addon::setAddonState(const State& /*otherState*/) +void Aspect::setAspectState(const State& /*otherState*/) { // Do nothing } //============================================================================== -const Addon::State* Addon::getAddonState() const +const Aspect::State* Aspect::getAspectState() const { return nullptr; } //============================================================================== -void Addon::setAddonProperties(const Properties& /*someProperties*/) +void Aspect::setAspectProperties(const Properties& /*someProperties*/) { // Do nothing } //============================================================================== -const Addon::Properties* Addon::getAddonProperties() const +const Aspect::Properties* Aspect::getAspectProperties() const { return nullptr; } //============================================================================== -Addon::Addon(AddonManager* manager) +Aspect::Aspect(Composite* manager) { if(nullptr == manager) { - dterr << "[Addon::constructor] You are not allowed to construct an Addon " - << "outside of an AddonManager!\n"; + dterr << "[Aspect::constructor] You are not allowed to construct an Aspect " + << "outside of an Composite!\n"; assert(false); } } //============================================================================== -void Addon::setManager(AddonManager* /*newManager*/, bool /*transfer*/) +void Aspect::setManager(Composite* /*newManager*/, bool /*transfer*/) { // Do nothing } diff --git a/dart/common/Addon.h b/dart/common/Aspect.h similarity index 52% rename from dart/common/Addon.h rename to dart/common/Aspect.h index 46c7f15db62cb..0885be17a1890 100644 --- a/dart/common/Addon.h +++ b/dart/common/Aspect.h @@ -34,8 +34,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_COMMON_ADDON_H_ -#define DART_COMMON_ADDON_H_ +#ifndef DART_COMMON_ASPECT_H_ +#define DART_COMMON_ASPECT_H_ #include @@ -45,24 +45,24 @@ namespace dart { namespace common { -class AddonManager; +class Composite; -class Addon +class Aspect { public: - friend class AddonManager; + friend class Composite; - /// If your Addon has a State class, then that State class should inherit this - /// Addon::State class. This allows us to safely serialize, store, and clone - /// the states of arbitrary Addon extensions. If your Addon is stateless, then + /// If your Aspect has a State class, then that State class should inherit this + /// Aspect::State class. This allows us to safely serialize, store, and clone + /// the states of arbitrary Aspect extensions. If your Aspect is stateless, then /// you do not have to worry about extending this class, because - /// Addon::getState() will simply return a nullptr by default, which is taken + /// Aspect::getState() will simply return a nullptr by default, which is taken /// to indicate that it is stateless. /// /// The distinction between the State class and the Properties class is that - /// State will get stored in AddonManager::State whereas Properties will get - /// stored in AddonManager::Properties. Typically Properties are values that + /// State will get stored in Composite::State whereas Properties will get + /// stored in Composite::Properties. Typically Properties are values that /// only change rarely if ever, whereas State contains values that might /// change as often as every time step. class State : public Extensible { }; @@ -72,16 +72,16 @@ class Addon template using StateMixer = ExtensibleMixer; - /// If your Addon has a Properties class, then it should inherit this - /// Addon::Properties class. This allows us to safely serialize, store, and - /// clone the properties of arbitrary Addon extensions. If your Addon has no + /// If your Aspect has a Properties class, then it should inherit this + /// Aspect::Properties class. This allows us to safely serialize, store, and + /// clone the properties of arbitrary Aspect extensions. If your Aspect has no /// properties, then you do not have to worry about extending this class, - /// because Addon::getProperties() will simply return a nullptr by default, + /// because Aspect::getProperties() will simply return a nullptr by default, /// which is taken to indicate that it has no properties. /// /// The distinction between the State class and the Properties class is that - /// State will get stored in AddonManager::State whereas Properties will get - /// stored in AddonManager::Properties. Typically Properties are values that + /// State will get stored in Composite::State whereas Properties will get + /// stored in Composite::Properties. Typically Properties are values that /// only change rarely if ever, whereas State contains values that might /// change as often as every time step. class Properties : public Extensible { }; @@ -92,64 +92,64 @@ class Addon using PropertiesMixer = ExtensibleMixer; /// Virtual destructor - virtual ~Addon() = default; + virtual ~Aspect() = default; - /// Clone this Addon into a new manager - virtual std::unique_ptr cloneAddon(AddonManager* newManager) const = 0; + /// Clone this Aspect into a new manager + virtual std::unique_ptr cloneAspect(Composite* newManager) const = 0; - /// Set the State of this Addon. By default, this does nothing. - virtual void setAddonState(const State& otherState); + /// Set the State of this Aspect. By default, this does nothing. + virtual void setAspectState(const State& otherState); - /// Get the State of this Addon. By default, this returns a nullptr which - /// implies that the Addon is stateless. - virtual const State* getAddonState() const; + /// Get the State of this Aspect. By default, this returns a nullptr which + /// implies that the Aspect is stateless. + virtual const State* getAspectState() const; - /// Set the Properties of this Addon. By default, this does nothing. - virtual void setAddonProperties(const Properties& someProperties); + /// Set the Properties of this Aspect. By default, this does nothing. + virtual void setAspectProperties(const Properties& someProperties); - /// Get the Properties of this Addon. By default, this returns a nullptr - /// which implies that the Addon has no properties. - virtual const Properties* getAddonProperties() const; + /// Get the Properties of this Aspect. By default, this returns a nullptr + /// which implies that the Aspect has no properties. + virtual const Properties* getAspectProperties() const; protected: /// Constructor /// - /// We require the AddonManager argument in this constructor to make it clear - /// to extensions that they must have an AddonManager argument in their + /// We require the Composite argument in this constructor to make it clear + /// to extensions that they must have an Composite argument in their /// constructors. - Addon(AddonManager* manager); + Aspect(Composite* manager); - /// This function will be triggered (1) after the Addon has been created - /// [transfer will be false] and (2) after the Addon has been transferred - /// to a new AddonManager [transfer will be true]. You should override this - /// function if your Addon requires special handling in either of those cases. + /// This function will be triggered (1) after the Aspect has been created + /// [transfer will be false] and (2) after the Aspect has been transferred + /// to a new Composite [transfer will be true]. You should override this + /// function if your Aspect requires special handling in either of those cases. /// By default, this function does nothing. - virtual void setManager(AddonManager* newManager, bool transfer); + virtual void setManager(Composite* newManager, bool transfer); }; //============================================================================== template -class ManagerTrackingAddon : public Addon +class ManagerTrackingAspect : public Aspect { public: /// Default constructor - ManagerTrackingAddon(AddonManager* mgr); + ManagerTrackingAspect(Composite* mgr); - /// Get the Manager of this Addon + /// Get the Manager of this Aspect ManagerType* getManager(); - /// Get the Manager of this Addon + /// Get the Manager of this Aspect const ManagerType* getManager() const; protected: /// Grab the new manager - void setManager(AddonManager* newManager, bool transfer); + void setManager(Composite* newManager, bool transfer); - /// Pointer to the current Manager of this Addon + /// Pointer to the current Manager of this Aspect ManagerType* mManager; }; @@ -158,44 +158,44 @@ class ManagerTrackingAddon : public Addon } // namespace dart //============================================================================== -#define DART_COMMON_ADDON_PROPERTY_CONSTRUCTOR( ClassName, UpdatePropertiesMacro )\ +#define DART_COMMON_ASPECT_PROPERTY_CONSTRUCTOR( ClassName, UpdatePropertiesMacro )\ ClassName (const ClassName &) = delete;\ - inline ClassName (dart::common::AddonManager* mgr, const PropertiesData& properties = PropertiesData())\ - : AddonWithVersionedProperties< Base, Derived, PropertiesData, ManagerType, UpdatePropertiesMacro>(mgr, properties) { } + inline ClassName (dart::common::Composite* mgr, const PropertiesData& properties = PropertiesData())\ + : AspectWithVersionedProperties< Base, Derived, PropertiesData, ManagerType, UpdatePropertiesMacro>(mgr, properties) { } //============================================================================== -#define DART_COMMON_JOINT_ADDON_CONSTRUCTOR( ClassName )\ - DART_COMMON_ADDON_PROPERTY_CONSTRUCTOR( ClassName, &detail::JointPropertyUpdate ) +#define DART_COMMON_JOINT_ASPECT_CONSTRUCTOR( ClassName )\ + DART_COMMON_ASPECT_PROPERTY_CONSTRUCTOR( ClassName, &detail::JointPropertyUpdate ) //============================================================================== -#define DART_COMMON_ADDON_STATE_PROPERTY_CONSTRUCTORS(ClassName)\ +#define DART_COMMON_ASPECT_STATE_PROPERTY_CONSTRUCTORS(ClassName)\ ClassName (const ClassName &) = delete;\ - inline ClassName (dart::common::AddonManager* mgr, const StateData& state = StateData(), const PropertiesData& properties = PropertiesData())\ - : AddonImplementation(mgr, state, properties) { }\ - inline ClassName (dart::common::AddonManager* mgr, const PropertiesData& properties, const StateData state = StateData())\ - : AddonImplementation(mgr, properties, state) { } + inline ClassName (dart::common::Composite* mgr, const StateData& state = StateData(), const PropertiesData& properties = PropertiesData())\ + : AspectImplementation(mgr, state, properties) { }\ + inline ClassName (dart::common::Composite* mgr, const PropertiesData& properties, const StateData state = StateData())\ + : AspectImplementation(mgr, properties, state) { } //============================================================================== -#define DART_COMMON_SET_ADDON_PROPERTY_CUSTOM( Type, Name, Update )\ +#define DART_COMMON_SET_ASPECT_PROPERTY_CUSTOM( Type, Name, Update )\ inline void set ## Name (const Type & value)\ { mProperties.m ## Name = value; Update(); } //============================================================================== -#define DART_COMMON_SET_ADDON_PROPERTY( Type, Name )\ - DART_COMMON_SET_ADDON_PROPERTY_CUSTOM( Type, Name, notifyPropertiesUpdate ) +#define DART_COMMON_SET_ASPECT_PROPERTY( Type, Name )\ + DART_COMMON_SET_ASPECT_PROPERTY_CUSTOM( Type, Name, notifyPropertiesUpdate ) //============================================================================== -#define DART_COMMON_GET_ADDON_PROPERTY( Type, Name )\ +#define DART_COMMON_GET_ASPECT_PROPERTY( Type, Name )\ inline const Type& get ## Name () const\ { return mProperties.m ## Name; } //============================================================================== -#define DART_COMMON_SET_GET_ADDON_PROPERTY( Type, Name )\ - DART_COMMON_SET_ADDON_PROPERTY( Type, Name )\ - DART_COMMON_GET_ADDON_PROPERTY( Type, Name ) +#define DART_COMMON_SET_GET_ASPECT_PROPERTY( Type, Name )\ + DART_COMMON_SET_ASPECT_PROPERTY( Type, Name )\ + DART_COMMON_GET_ASPECT_PROPERTY( Type, Name ) //============================================================================== -#define DART_COMMON_SET_ADDON_PROPERTY_ARRAY( Class, SingleType, VectorType, SingleName, PluralName, Size )\ +#define DART_COMMON_SET_ASPECT_PROPERTY_ARRAY( Class, SingleType, VectorType, SingleName, PluralName, Size )\ void set ## SingleName (size_t index, const SingleType & value)\ {\ if( index >= Size )\ @@ -214,7 +214,7 @@ class ManagerTrackingAddon : public Addon } //============================================================================== -#define DART_COMMON_GET_ADDON_PROPERTY_ARRAY(Class, SingleType, VectorType, SingleName, PluralName, Size)\ +#define DART_COMMON_GET_ASPECT_PROPERTY_ARRAY(Class, SingleType, VectorType, SingleName, PluralName, Size)\ inline const SingleType& get ## SingleName (size_t index) const\ {\ if(index >= Size)\ @@ -231,22 +231,22 @@ class ManagerTrackingAddon : public Addon } //============================================================================== -#define DART_COMMON_IRREGULAR_SET_GET_ADDON_PROPERTY_ARRAY( Class, SingleType, VectorType, SingleName, PluralName, Size )\ - DART_COMMON_SET_ADDON_PROPERTY_ARRAY( Class, SingleType, VectorType, SingleName, PluralName, Size )\ - DART_COMMON_GET_ADDON_PROPERTY_ARRAY( Class, SingleType, VectorType, SingleName, PluralName, Size ) +#define DART_COMMON_IRREGULAR_SET_GET_ASPECT_PROPERTY_ARRAY( Class, SingleType, VectorType, SingleName, PluralName, Size )\ + DART_COMMON_SET_ASPECT_PROPERTY_ARRAY( Class, SingleType, VectorType, SingleName, PluralName, Size )\ + DART_COMMON_GET_ASPECT_PROPERTY_ARRAY( Class, SingleType, VectorType, SingleName, PluralName, Size ) //============================================================================== -#define DART_COMMON_SET_GET_ADDON_PROPERTY_ARRAY( Class, SingleType, VectorType, SingleName, Size )\ - DART_COMMON_IRREGULAR_SET_GET_ADDON_PROPERTY_ARRAY( Class, SingleType, VectorType, SingleName, SingleName ## s, Size ) +#define DART_COMMON_SET_GET_ASPECT_PROPERTY_ARRAY( Class, SingleType, VectorType, SingleName, Size )\ + DART_COMMON_IRREGULAR_SET_GET_ASPECT_PROPERTY_ARRAY( Class, SingleType, VectorType, SingleName, SingleName ## s, Size ) //============================================================================== -#define DART_COMMON_IRREGULAR_SET_GET_MULTIDOF_ADDON( SingleType, VectorType, SingleName, PluralName )\ - DART_COMMON_IRREGULAR_SET_GET_ADDON_PROPERTY_ARRAY( MultiDofJointAddon, SingleType, VectorType, SingleName, PluralName, DOF ) +#define DART_COMMON_IRREGULAR_SET_GET_MULTIDOF_ASPECT( SingleType, VectorType, SingleName, PluralName )\ + DART_COMMON_IRREGULAR_SET_GET_ASPECT_PROPERTY_ARRAY( MultiDofJointAspect, SingleType, VectorType, SingleName, PluralName, DOF ) //============================================================================== -#define DART_COMMON_SET_GET_MULTIDOF_ADDON( SingleType, VectorType, SingleName )\ - DART_COMMON_IRREGULAR_SET_GET_MULTIDOF_ADDON( SingleType, VectorType, SingleName, SingleName ## s ) +#define DART_COMMON_SET_GET_MULTIDOF_ASPECT( SingleType, VectorType, SingleName )\ + DART_COMMON_IRREGULAR_SET_GET_MULTIDOF_ASPECT( SingleType, VectorType, SingleName, SingleName ## s ) -#include "dart/common/detail/Addon.h" +#include "dart/common/detail/Aspect.h" -#endif // DART_COMMON_ADDON_H_ +#endif // DART_COMMON_ASPECT_H_ diff --git a/dart/common/AddonWithVersion.h b/dart/common/AspectWithVersion.h similarity index 72% rename from dart/common/AddonWithVersion.h rename to dart/common/AspectWithVersion.h index dfcd859fddd0d..11d25b68e8ec2 100644 --- a/dart/common/AddonWithVersion.h +++ b/dart/common/AspectWithVersion.h @@ -34,10 +34,10 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_COMMON_ADDONWITHVERSION_H_ -#define DART_COMMON_ADDONWITHVERSION_H_ +#ifndef DART_COMMON_ASPECTWITHVERSION_H_ +#define DART_COMMON_ASPECTWITHVERSION_H_ -#include "dart/common/detail/AddonWithVersion.h" +#include "dart/common/detail/AspectWithVersion.h" namespace dart { namespace common { @@ -45,29 +45,29 @@ namespace common { //============================================================================== template > -using AddonWithState = - detail::AddonWithState, DerivedT, StateDataT, ManagerT, updateState>; +using AspectWithState = + detail::AspectWithState, DerivedT, StateDataT, ManagerT, updateState>; //============================================================================== template > -using AddonWithVersionedProperties = - detail::AddonWithVersionedProperties, DerivedT, PropertiesDataT, ManagerT, updateProperties>; +using AspectWithVersionedProperties = + detail::AspectWithVersionedProperties, DerivedT, PropertiesDataT, ManagerT, updateProperties>; //============================================================================== template , void (*updateProperties)(DerivedT*) = updateState> -class AddonWithStateAndVersionedProperties : - public detail::AddonWithVersionedProperties< - AddonWithState, +class AspectWithStateAndVersionedProperties : + public detail::AspectWithVersionedProperties< + AspectWithState, DerivedT, PropertiesDataT, ManagerT, updateProperties> { public: @@ -76,42 +76,42 @@ class AddonWithStateAndVersionedProperties : using StateData = StateDataT; using PropertiesData = PropertiesDataT; using ManagerType = ManagerT; - using State = common::Addon::StateMixer; - using Properties = common::Addon::PropertiesMixer; + using State = common::Aspect::StateMixer; + using Properties = common::Aspect::PropertiesMixer; constexpr static void (*UpdateState)(Derived*) = updateState; constexpr static void (*UpdateProperties)(Derived*) = updateProperties; - using AddonStateImplementation = AddonWithState< + using AspectStateImplementation = AspectWithState< Derived, StateData, ManagerType, updateState>; - using AddonPropertiesImplementation = detail::AddonWithVersionedProperties< - AddonStateImplementation, + using AspectPropertiesImplementation = detail::AspectWithVersionedProperties< + AspectStateImplementation, Derived, PropertiesData, ManagerType, updateProperties>; - using AddonImplementation = AddonWithStateAndVersionedProperties< + using AspectImplementation = AspectWithStateAndVersionedProperties< DerivedT, StateDataT, PropertiesDataT, ManagerT, updateState, updateProperties>; - AddonWithStateAndVersionedProperties() = delete; - AddonWithStateAndVersionedProperties( - const AddonWithStateAndVersionedProperties&) = delete; + AspectWithStateAndVersionedProperties() = delete; + AspectWithStateAndVersionedProperties( + const AspectWithStateAndVersionedProperties&) = delete; /// Construct using a StateData and a PropertiesData instance - AddonWithStateAndVersionedProperties( - common::AddonManager* mgr, + AspectWithStateAndVersionedProperties( + common::Composite* mgr, const StateData& state = StateData(), const PropertiesData& properties = PropertiesData()) - : AddonPropertiesImplementation(mgr, properties, state) + : AspectPropertiesImplementation(mgr, properties, state) { // Do nothing } /// Construct using a PropertiesData and a StateData instance - AddonWithStateAndVersionedProperties( - common::AddonManager* mgr, + AspectWithStateAndVersionedProperties( + common::Composite* mgr, const PropertiesData& properties, const StateData& state = StateData()) - : AddonPropertiesImplementation(mgr, properties, state) + : AspectPropertiesImplementation(mgr, properties, state) { // Do nothing } @@ -131,7 +131,7 @@ template -constexpr void (*AddonWithStateAndVersionedProperties::UpdateState) (DerivedT*); @@ -142,11 +142,11 @@ template -constexpr void (*AddonWithStateAndVersionedProperties::UpdateProperties) (DerivedT*); } // namespace common } // namespace dart -#endif // DART_COMMON_ADDONWITHVERSION_H_ +#endif // DART_COMMON_ASPECTWITHVERSION_H_ diff --git a/dart/common/AddonManager.cpp b/dart/common/Composite.cpp similarity index 65% rename from dart/common/AddonManager.cpp rename to dart/common/Composite.cpp index 16be91e672105..672e0f4f4d16d 100644 --- a/dart/common/AddonManager.cpp +++ b/dart/common/Composite.cpp @@ -38,33 +38,33 @@ #include #include "dart/common/Console.h" -#include "dart/common/AddonManager.h" +#include "dart/common/Composite.h" namespace dart { namespace common { //============================================================================== -void AddonManager::setAddonStates(const State& newStates) +void Composite::setAspectStates(const State& newStates) { const StateMap& stateMap = newStates.getMap(); - AddonMap::iterator addons = mAddonMap.begin(); + AspectMap::iterator aspects = mAspectMap.begin(); StateMap::const_iterator states = stateMap.begin(); - while( mAddonMap.end() != addons && stateMap.end() != states ) + while( mAspectMap.end() != aspects && stateMap.end() != states ) { - if( addons->first == states->first ) + if( aspects->first == states->first ) { - Addon* addon = addons->second.get(); - if(addon && states->second) - addon->setAddonState(*states->second); + Aspect* aspect = aspects->second.get(); + if(aspect && states->second) + aspect->setAspectState(*states->second); - ++addons; + ++aspects; ++states; } - else if( addons->first < states->first ) + else if( aspects->first < states->first ) { - ++addons; + ++aspects; } else { @@ -74,19 +74,19 @@ void AddonManager::setAddonStates(const State& newStates) } //============================================================================== -template -void extractMapData(MapType& outgoingMap, const AddonManager::AddonMap& addonMap) +template +void extractMapData(MapType& outgoingMap, const Composite::AspectMap& aspectMap) { // TODO(MXG): Consider placing this function in a header so it can be utilized // by anything that needs to transfer data between maps of extensibles // This method allows us to avoid dynamic allocation (cloning) whenever possible. - for(const auto& addon : addonMap) + for(const auto& aspect : aspectMap) { - if(nullptr == addon.second) + if(nullptr == aspect.second) continue; - const DataType* data = (addon.second.get()->*getData)(); + const DataType* data = (aspect.second.get()->*getData)(); if(data) { // Attempt to insert a nullptr to see whether this entry exists while also @@ -95,7 +95,7 @@ void extractMapData(MapType& outgoingMap, const AddonManager::AddonMap& addonMap // see if the entry already exists and then searching the map again in // order to insert the entry if it didn't already exist. std::pair insertion = - outgoingMap.insert(typename MapType::value_type(addon.first, nullptr)); + outgoingMap.insert(typename MapType::value_type(aspect.first, nullptr)); typename MapType::iterator& it = insertion.first; bool existed = !insertion.second; @@ -124,43 +124,43 @@ void extractMapData(MapType& outgoingMap, const AddonManager::AddonMap& addonMap } //============================================================================== -AddonManager::State AddonManager::getAddonStates() const +Composite::State Composite::getAspectStates() const { State states; - copyAddonStatesTo(states); + copyAspectStatesTo(states); return states; } //============================================================================== -void AddonManager::copyAddonStatesTo(State& outgoingStates) const +void Composite::copyAspectStatesTo(State& outgoingStates) const { StateMap& states = outgoingStates.getMap(); - extractMapData(states, mAddonMap); + extractMapData(states, mAspectMap); } //============================================================================== -void AddonManager::setAddonProperties(const Properties& newProperties) +void Composite::setAspectProperties(const Properties& newProperties) { const PropertiesMap& propertiesMap = newProperties.getMap(); - AddonMap::iterator addons = mAddonMap.begin(); + AspectMap::iterator aspects = mAspectMap.begin(); PropertiesMap::const_iterator props = propertiesMap.begin(); - while( mAddonMap.end() != addons && propertiesMap.end() != props ) + while( mAspectMap.end() != aspects && propertiesMap.end() != props ) { - if( addons->first == props->first ) + if( aspects->first == props->first ) { - Addon* addon = addons->second.get(); - if(addon) - addon->setAddonProperties(*props->second); + Aspect* aspect = aspects->second.get(); + if(aspect) + aspect->setAspectProperties(*props->second); - ++addons; + ++aspects; ++props; } - else if( addons->first < props->first ) + else if( aspects->first < props->first ) { - ++addons; + ++aspects; } else { @@ -170,30 +170,30 @@ void AddonManager::setAddonProperties(const Properties& newProperties) } //============================================================================== -AddonManager::Properties AddonManager::getAddonProperties() const +Composite::Properties Composite::getAspectProperties() const { Properties properties; - copyAddonPropertiesTo(properties); + copyAspectPropertiesTo(properties); return properties; } //============================================================================== -void AddonManager::copyAddonPropertiesTo( +void Composite::copyAspectPropertiesTo( Properties& outgoingProperties) const { PropertiesMap& properties = outgoingProperties.getMap(); - extractMapData( - properties, mAddonMap); + extractMapData( + properties, mAspectMap); } //============================================================================== -void AddonManager::duplicateAddons(const AddonManager* fromManager) +void Composite::duplicateAspects(const Composite* fromManager) { if(nullptr == fromManager) { - dterr << "[AddonManager::duplicateAddons] You have asked to duplicate the " - << "Addons of a nullptr, which is not allowed!\n"; + dterr << "[Composite::duplicateAspects] You have asked to duplicate the " + << "Aspects of a nullptr, which is not allowed!\n"; assert(false); return; } @@ -201,16 +201,16 @@ void AddonManager::duplicateAddons(const AddonManager* fromManager) if(this == fromManager) return; - const AddonMap& otherMap = fromManager->mAddonMap; + const AspectMap& otherMap = fromManager->mAspectMap; - AddonMap::iterator receiving = mAddonMap.begin(); - AddonMap::const_iterator incoming = otherMap.begin(); + AspectMap::iterator receiving = mAspectMap.begin(); + AspectMap::const_iterator incoming = otherMap.begin(); while( otherMap.end() != incoming ) { - if( mAddonMap.end() == receiving ) + if( mAspectMap.end() == receiving ) { - // If we've reached the end of this Manager's AddonMap, then we should + // If we've reached the end of this Manager's AspectMap, then we should // just add each entry _set(incoming->first, incoming->second.get()); ++incoming; @@ -230,7 +230,7 @@ void AddonManager::duplicateAddons(const AddonManager* fromManager) else { // If this Manager does not have an entry corresponding to the incoming - // Addon, then we must create it + // Aspect, then we must create it _set(incoming->first, incoming->second.get()); ++incoming; } @@ -238,48 +238,48 @@ void AddonManager::duplicateAddons(const AddonManager* fromManager) } //============================================================================== -void AddonManager::matchAddons(const AddonManager* otherManager) +void Composite::matchAspects(const Composite* otherManager) { if(nullptr == otherManager) { - dterr << "[AddonManager::matchAddons] You have asked to match the Addons " + dterr << "[Composite::matchAspects] You have asked to match the Aspects " << "of a nullptr, which is not allowed!\n"; assert(false); return; } - for(auto& addon : mAddonMap) - addon.second = nullptr; + for(auto& aspect : mAspectMap) + aspect.second = nullptr; - duplicateAddons(otherManager); + duplicateAspects(otherManager); } //============================================================================== -void AddonManager::becomeManager(Addon* addon, bool transfer) +void Composite::becomeManager(Aspect* aspect, bool transfer) { - if(addon) - addon->setManager(this, transfer); + if(aspect) + aspect->setManager(this, transfer); } //============================================================================== -void AddonManager::_set(std::type_index type_idx, const Addon* addon) +void Composite::_set(std::type_index type_idx, const Aspect* aspect) { - if(addon) + if(aspect) { - mAddonMap[type_idx] = addon->cloneAddon(this); - becomeManager(mAddonMap[type_idx].get(), false); + mAspectMap[type_idx] = aspect->cloneAspect(this); + becomeManager(mAspectMap[type_idx].get(), false); } else { - mAddonMap[type_idx] = nullptr; + mAspectMap[type_idx] = nullptr; } } //============================================================================== -void AddonManager::_set(std::type_index type_idx, std::unique_ptr addon) +void Composite::_set(std::type_index type_idx, std::unique_ptr aspect) { - mAddonMap[type_idx] = std::move(addon); - becomeManager(mAddonMap[type_idx].get(), true); + mAspectMap[type_idx] = std::move(aspect); + becomeManager(mAspectMap[type_idx].get(), true); } } // namespace common diff --git a/dart/common/Composite.h b/dart/common/Composite.h new file mode 100644 index 0000000000000..3aedca5c46a54 --- /dev/null +++ b/dart/common/Composite.h @@ -0,0 +1,210 @@ +/* + * Copyright (c) 2015, 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_COMMON_COMPOSITE_H_ +#define DART_COMMON_COMPOSITE_H_ + +#include +#include +#include +#include + +#include "dart/common/Aspect.h" + +namespace dart { +namespace common { + +/// Composite is a base class that should be virtually inherited by any class +/// that wants to be able to manage Aspects. +/// +/// The base Composite class is completely agnostic to what kind of Aspects it +/// is given. Aspects are stored in a std::map, so access to its Aspects happens +/// on average in log(N) time. Most often, a class that accepts Aspects will have +/// certain Aspect types that it will need to access frequently, and it would be +/// beneficial to have constant-time access to those Aspect types. To get +/// constant-time access to specific Aspect types, you can use the templated +/// class SpecializedForAspect. +class Composite +{ +public: + + using StateMap = std::map< std::type_index, std::unique_ptr >; + using State = ExtensibleMapHolder; + + using PropertiesMap = std::map< std::type_index, std::unique_ptr >; + using Properties = ExtensibleMapHolder; + + using AspectMap = std::map< std::type_index, std::unique_ptr >; + using RequiredAspectSet = std::unordered_set; + + /// Virtual destructor + virtual ~Composite() = default; + + /// Default constructor + Composite() = default; + + /// It is currently unsafe to copy an Composite + // TODO(MXG): Consider making this safe by cloning Aspects into the new copy + Composite(const Composite&) = delete; + + /// It is currently unsafe to move an Composite + Composite(Composite&&) = delete; + + /// It is currently unsafe to copy an Composite + Composite& operator=(const Composite&) = delete; + + /// It is currently unsafe to move an Composite + Composite& operator=(Composite&&) = delete; + + /// Check if this Composite currently has a certain type of Aspect + template + bool has() const; + + /// Get a certain type of Aspect from this Composite + template + T* get(); + + /// Get a certain type of Aspect from this Composite + template + const T* get() const; + + /// Make a clone of the aspect and place the clone into this Composite. If + /// an Aspect of the same type already exists in this Composite, the + /// existing Aspect will be destroyed. + template + void set(const T* aspect); + + /// Use move semantics to place aspect into this Composite. If an Aspect of + /// the same type already exists in this Composite, the existing Aspect will + /// be destroyed. + template + void set(std::unique_ptr&& aspect); + + /// Construct an Aspect inside of this Composite + template + T* create(Args&&... args); + + /// Remove an Aspect from this Composite. + template + void erase(); + + /// Remove an Aspect from this Composite, but return its unique_ptr instead + /// of letting it be deleted. This allows you to safely use move semantics to + /// transfer an Aspect between two Composites. + template + std::unique_ptr release(); + + /// Check if this Manager is specialized for a specific type of Aspect. + /// + /// By default, this simply returns false. + template + static constexpr bool isSpecializedFor(); + + /// Check if this Manager requires this specific type of Aspect + template + bool requires() const; + + /// Set the states of the aspects in this Composite based on the given + /// Composite::State. The states of any Aspect types that do not exist + /// within this manager will be ignored. + void setAspectStates(const State& newStates); + + /// Get the states of the aspects inside of this Composite + State getAspectStates() const; + + /// Fill outgoingStates with the states of the aspects inside this Composite + void copyAspectStatesTo(State& outgoingStates) const; + + /// Set the properties of the aspects in this Composite based on the given + /// Composite::Properties. The properties of any Aspect types that do not + /// exist within this manager will be ignored. + void setAspectProperties(const Properties& newProperties); + + /// Get the properties of the aspects inside of this Composite + Properties getAspectProperties() const; + + /// Fill outgoingProperties with the properties of the aspects inside this + /// Composite + void copyAspectPropertiesTo(Properties& outgoingProperties) const; + + /// Give this Composite a copy of each Aspect from otherManager + void duplicateAspects(const Composite* fromManager); + + /// Make the Aspects of this Composite match the Aspects of otherManager. Any + /// Aspects in this Composite which do not exist in otherManager will be + /// erased. + void matchAspects(const Composite* otherManager); + +protected: + + template struct type { }; + + /// Become the Composite of the given Aspect. This allows derived + /// Composite types to call the protected Aspect::setManager function. + void becomeManager(Aspect* aspect, bool transfer); + + /// Non-templated version of set(const T*) + void _set(std::type_index type_idx, const Aspect* aspect); + + /// Non-templated version of set(std::unqiue_ptr&&) + void _set(std::type_index type_idx, std::unique_ptr aspect); + + /// A map that relates the type of Aspect to its pointer + AspectMap mAspectMap; + + /// A set containing type information for Aspects which are not allowed to + /// leave this manager. + RequiredAspectSet mRequiredAspects; +}; + +//============================================================================== +/// Attach an arbitrary number of Aspects to the specified Composite type. +// TODO(MXG): Consider putting this functionality into the Composite class +// itself. Also consider allowing the user to specify arguments for the +// constructors of the Aspects. +template +void createAspects(T* /*mgr*/); + +//============================================================================== +template +void createAspects(T* mgr); + +} // namespace common +} // namespace dart + +#include "dart/common/detail/Composite.h" + +#endif // DART_COMMON_COMPOSITE_H_ diff --git a/dart/common/AddonManagerJoiner.h b/dart/common/CompositeJoiner.h similarity index 80% rename from dart/common/AddonManagerJoiner.h rename to dart/common/CompositeJoiner.h index 5ac57d06a31e2..23042456cd6a0 100644 --- a/dart/common/AddonManagerJoiner.h +++ b/dart/common/CompositeJoiner.h @@ -34,10 +34,10 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_COMMON_ADDONMANAGERJOINER_H_ -#define DART_COMMON_ADDONMANAGERJOINER_H_ +#ifndef DART_COMMON_COMPOSITEJOINER_H_ +#define DART_COMMON_COMPOSITEJOINER_H_ -#include "dart/common/AddonManager.h" +#include "dart/common/Composite.h" #include "dart/common/Empty.h" namespace dart { @@ -45,23 +45,23 @@ namespace common { /// Terminator for the variadic template template -class AddonManagerJoiner { }; +class CompositeJoiner { }; /// Special case of only having 1 class: we do nothing but inherit it. template -class AddonManagerJoiner : public Base1 { }; +class CompositeJoiner : public Base1 { }; -/// AddonManagerJoiner allows classes that inherit from various -/// SpecializedForAddon types to be inherited by a single derived class. +/// CompositeJoiner allows classes that inherit from various +/// SpecializedForAspect types to be inherited by a single derived class. /// This class solves the diamond-of-death problem for multiple -/// SpecializedForAddon inheritance. +/// SpecializedForAspect inheritance. template -class AddonManagerJoiner : public Base1, public Base2 +class CompositeJoiner : public Base1, public Base2 { public: /// Default constructor - AddonManagerJoiner() = default; + CompositeJoiner() = default; /// This constructor allows one argument to be passed to the Base1 constructor /// and arbitrarily many arguments to be passed to the Base2 constructor. @@ -72,17 +72,17 @@ class AddonManagerJoiner : public Base1, public Base2 // then, this is the best we can offer due to fundamental limitations of // variadic templates in C++11. template - AddonManagerJoiner(Base1Arg&& arg1, Base2Args&&... args2); + CompositeJoiner(Base1Arg&& arg1, Base2Args&&... args2); /// This constructor passes one argument to the Base1 constructor and no /// arguments to the Base2 constructor. template - AddonManagerJoiner(Base1Arg&& arg1, NoArg_t); + CompositeJoiner(Base1Arg&& arg1, NoArg_t); /// This constructor passes no arguments to the Base1 constructor and /// arbitrarily many arguments to the Base2 constructor. template - AddonManagerJoiner(NoArg_t, Base2Args&&... args2); + CompositeJoiner(NoArg_t, Base2Args&&... args2); // Documentation inherited template @@ -98,11 +98,11 @@ class AddonManagerJoiner : public Base1, public Base2 // Documentation inherited template - void set(const T* addon); + void set(const T* aspect); // Documentation inherited template - void set(std::unique_ptr&& addon); + void set(std::unique_ptr&& aspect); // Documentation inherited template @@ -122,16 +122,16 @@ class AddonManagerJoiner : public Base1, public Base2 }; -/// This is the variadic version of the AddonManagerJoiner class which allows +/// This is the variadic version of the CompositeJoiner class which allows /// you to include arbitrarily many base classes in the joining. template -class AddonManagerJoiner : - public AddonManagerJoiner< Base1, AddonManagerJoiner > +class CompositeJoiner : + public CompositeJoiner< Base1, CompositeJoiner > { public: /// Default constructor - AddonManagerJoiner() = default; + CompositeJoiner() = default; /// This constructor allows one argument to be passed to each Base class's /// constructor (except the final Base class, which accepts arbitrarily many @@ -144,14 +144,14 @@ class AddonManagerJoiner : // then, this is the best we can offer due to fundamental limitations of // variadic templates in C++11. template - AddonManagerJoiner(Args&&... args); + CompositeJoiner(Args&&... args); }; } // namespace common } // namespace dart -#include "dart/common/detail/AddonManagerJoiner.h" +#include "dart/common/detail/CompositeJoiner.h" -#endif // DART_COMMON_ADDONMANAGERJOINER_H_ +#endif // DART_COMMON_COMPOSITEJOINER_H_ diff --git a/dart/common/Empty.h b/dart/common/Empty.h index c0d3eff3839e3..b7192b845ebdb 100644 --- a/dart/common/Empty.h +++ b/dart/common/Empty.h @@ -45,7 +45,7 @@ namespace common { struct Empty { }; /// Used to tag arguments as blank for in variadic joiner classes such as -/// common::AddonManagerJoiner and dynamics::NodeManagerJoiner +/// common::CompositeJoiner and dynamics::NodeManagerJoiner enum NoArg_t { NoArg }; } // namespace common diff --git a/dart/common/Extensible.h b/dart/common/Extensible.h index 5918b945b7272..232e6b3d79c40 100644 --- a/dart/common/Extensible.h +++ b/dart/common/Extensible.h @@ -126,7 +126,7 @@ class ExtensibleMixer : public T, public Mixin }; /// MapHolder is a templated wrapper class that is used to allow maps of -/// Addon::State and Addon::Properties to be handled in a semantically +/// Aspect::State and Aspect::Properties to be handled in a semantically /// palatable way. template class ExtensibleMapHolder final @@ -168,7 +168,7 @@ class ExtensibleMapHolder final private: - /// A map containing the collection of States for the Addon + /// A map containing the collection of States for the Aspect MapType mMap; }; diff --git a/dart/common/RequiresAddon.h b/dart/common/RequiresAspect.h similarity index 68% rename from dart/common/RequiresAddon.h rename to dart/common/RequiresAspect.h index 15b58a772d980..c7282b1cec5d3 100644 --- a/dart/common/RequiresAddon.h +++ b/dart/common/RequiresAspect.h @@ -34,44 +34,44 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_COMMON_REQUIRESADDON_H_ -#define DART_COMMON_REQUIRESADDON_H_ +#ifndef DART_COMMON_REQUIRESASPECT_H_ +#define DART_COMMON_REQUIRESASPECT_H_ -#include "dart/common/SpecializedForAddon.h" +#include "dart/common/SpecializedForAspect.h" namespace dart { namespace common { //============================================================================== -/// RequiresAddon allows classes that inherit AddonManager to know which Addons +/// RequiresAspect allows classes that inherit Composite to know which Aspects /// are required for their operation. This guarantees that there is no way for -/// a required Addon do not get unexpectedly removed from their manager. +/// a required Aspect do not get unexpectedly removed from their manager. /// -/// Required Addons are also automatically specialized for. -template -class RequiresAddon { }; +/// Required Aspects are also automatically specialized for. +template +class RequiresAspect { }; //============================================================================== -template -class RequiresAddon : public virtual SpecializedForAddon +template +class RequiresAspect : public virtual SpecializedForAspect { public: - /// Default constructor. This is where the base AddonManager is informed that - /// the Addon type is required. - RequiresAddon(); + /// Default constructor. This is where the base Composite is informed that + /// the Aspect type is required. + RequiresAspect(); }; //============================================================================== -template -class RequiresAddon : - public AddonManagerJoiner< Virtual< RequiresAddon >, - Virtual< RequiresAddon > > { }; +template +class RequiresAspect : + public CompositeJoiner< Virtual< RequiresAspect >, + Virtual< RequiresAspect > > { }; } // namespace common } // namespace dart -#include "dart/common/detail/RequiresAddon.h" +#include "dart/common/detail/RequiresAspect.h" -#endif // DART_COMMON_REQUIRESADDON_H_ +#endif // DART_COMMON_REQUIRESASPECT_H_ diff --git a/dart/common/SpecializedForAddon.h b/dart/common/SpecializedForAspect.h similarity index 58% rename from dart/common/SpecializedForAddon.h rename to dart/common/SpecializedForAspect.h index 465be208c27db..b315f2abb3bfd 100644 --- a/dart/common/SpecializedForAddon.h +++ b/dart/common/SpecializedForAspect.h @@ -34,162 +34,162 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_COMMON_SPECIALIZEDFORADDON_H_ -#define DART_COMMON_SPECIALIZEDFORADDON_H_ +#ifndef DART_COMMON_SPECIALIZEDFORASPECT_H_ +#define DART_COMMON_SPECIALIZEDFORASPECT_H_ -#include "dart/common/AddonManager.h" -#include "dart/common/AddonManagerJoiner.h" +#include "dart/common/Composite.h" +#include "dart/common/CompositeJoiner.h" #include "dart/common/Virtual.h" namespace dart { namespace common { /// Declaration of the variadic template -template -class SpecializedForAddon { }; +template +class SpecializedForAspect { }; //============================================================================== -/// SpecializedForAddon allows classes that inherit AddonManager to have -/// constant-time access to a specific type of Addon -template -class SpecializedForAddon : public virtual AddonManager +/// SpecializedForAspect allows classes that inherit Composite to have +/// constant-time access to a specific type of Aspect +template +class SpecializedForAspect : public virtual Composite { public: /// Default Constructor - SpecializedForAddon(); + SpecializedForAspect(); - /// Check if this AddonManager currently has a certain type of Addon + /// Check if this Composite currently has a certain type of Aspect template bool has() const; - /// Get a certain type of Addon from this AddonManager + /// Get a certain type of Aspect from this Composite template T* get(); - /// Get a certain type of Addon from this AddonManager + /// Get a certain type of Aspect from this Composite template const T* get() const; - /// Make a clone of the addon and place the clone into this AddonManager. If - /// an Addon of the same type already exists in this AddonManager, the - /// existing Addon will be destroyed. + /// Make a clone of the aspect and place the clone into this Composite. If + /// an Aspect of the same type already exists in this Composite, the + /// existing Aspect will be destroyed. template - void set(const T* addon); + void set(const T* aspect); - /// Use move semantics to place an addon into this AddonManager. If an Addon - /// of the same type already exists in this AddonManager, the existing Addon + /// Use move semantics to place an aspect into this Composite. If an Aspect + /// of the same type already exists in this Composite, the existing Aspect /// will be destroyed. template - void set(std::unique_ptr&& addon); + void set(std::unique_ptr&& aspect); - /// Construct an Addon inside of this AddonManager + /// Construct an Aspect inside of this Composite template T* create(Args&&... args); - /// Remove an Addon from this AddonManager + /// Remove an Aspect from this Composite template void erase(); - /// Remove an Addon from this AddonManager, but return its unique_ptr instead + /// Remove an Aspect from this Composite, but return its unique_ptr instead /// of letting it be deleted. This allows you to safely use move semantics to - /// transfer an Addon between two AddonManagers. + /// transfer an Aspect between two Composites. template std::unique_ptr release(); - /// Check if this Manager is specialized for a specific type of Addon + /// Check if this Manager is specialized for a specific type of Aspect template static constexpr bool isSpecializedFor(); protected: - /// Redirect to AddonManager::has() + /// Redirect to Composite::has() template bool _has(type) const; /// Specialized implementation of has() - bool _has(type) const; + bool _has(type) const; - /// Redirect to AddonManager::get() + /// Redirect to Composite::get() template T* _get(type); /// Specialized implementation of get() - SpecAddon* _get(type); + SpecAspect* _get(type); - /// Redirect to AddonManager::get() + /// Redirect to Composite::get() template const T* _get(type) const; /// Specialized implementation of get() - const SpecAddon* _get(type) const; + const SpecAspect* _get(type) const; - /// Redirect to AddonManager::set() + /// Redirect to Composite::set() /// /// Using the type tag for this is not be necessary, but it helps to avoid /// confusion between the set() versus _set() function. template - void _set(type, const T* addon); + void _set(type, const T* aspect); /// Specialized implementation of set() - void _set(type, const SpecAddon* addon); + void _set(type, const SpecAspect* aspect); - /// Redirect to AddonManager::set() + /// Redirect to Composite::set() /// /// Using the type tag for this is not be necessary, but it helps to avoid /// confusion between the set() versus _set() function. template - void _set(type, std::unique_ptr&& addon); + void _set(type, std::unique_ptr&& aspect); /// Specialized implementation of set() - void _set(type, std::unique_ptr&& addon); + void _set(type, std::unique_ptr&& aspect); - /// Redirect to AddonManager::create() + /// Redirect to Composite::create() template T* _create(type, Args&&... args); /// Specialized implementation of create() template - SpecAddon* _create(type, Args&&... args); + SpecAspect* _create(type, Args&&... args); - /// Redirect to AddonManager::erase() + /// Redirect to Composite::erase() template void _erase(type); /// Specialized implementation of erase() - void _erase(type); + void _erase(type); - /// Redirect to AddonManager::release() + /// Redirect to Composite::release() template std::unique_ptr _release(type); /// Specialized implementation of release() - std::unique_ptr _release(type); + std::unique_ptr _release(type); /// Return false template static constexpr bool _isSpecializedFor(type); /// Return true - static constexpr bool _isSpecializedFor(type); + static constexpr bool _isSpecializedFor(type); - /// Iterator that points to the Addon of this SpecializedForAddon - AddonManager::AddonMap::iterator mSpecAddonIterator; + /// Iterator that points to the Aspect of this SpecializedForAspect + Composite::AspectMap::iterator mSpecAspectIterator; }; //============================================================================== -/// This is the variadic version of the SpecializedForAddon class which +/// This is the variadic version of the SpecializedForAspect class which /// allows you to include arbitrarily many specialized types in the /// specialization. -template -class SpecializedForAddon : - public AddonManagerJoiner< Virtual< SpecializedForAddon >, - Virtual< SpecializedForAddon > > { }; +template +class SpecializedForAspect : + public CompositeJoiner< Virtual< SpecializedForAspect >, + Virtual< SpecializedForAspect > > { }; } // namespace common } // namespace dart -#include "dart/common/detail/SpecializedForAddon.h" +#include "dart/common/detail/SpecializedForAspect.h" -#endif // DART_COMMON_SPECIALIZEDFORADDON_H_ +#endif // DART_COMMON_SPECIALIZEDFORASPECT_H_ diff --git a/dart/common/detail/Addon.h b/dart/common/detail/Aspect.h similarity index 84% rename from dart/common/detail/Addon.h rename to dart/common/detail/Aspect.h index 86573342e5903..a8fa53cb8bb6e 100644 --- a/dart/common/detail/Addon.h +++ b/dart/common/detail/Aspect.h @@ -34,22 +34,22 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_COMMON_DETAIL_ADDON_H_ -#define DART_COMMON_DETAIL_ADDON_H_ +#ifndef DART_COMMON_DETAIL_ASPECT_H_ +#define DART_COMMON_DETAIL_ASPECT_H_ #include #include "dart/common/Console.h" -#include "dart/common/Addon.h" +#include "dart/common/Aspect.h" namespace dart { namespace common { //============================================================================== template -ManagerTrackingAddon::ManagerTrackingAddon( - AddonManager* mgr) - : Addon(mgr), +ManagerTrackingAspect::ManagerTrackingAspect( + Composite* mgr) + : Aspect(mgr), mManager(nullptr) // This will be set later when the Manager calls setManager { // Do nothing @@ -57,29 +57,29 @@ ManagerTrackingAddon::ManagerTrackingAddon( //============================================================================== template -ManagerType* ManagerTrackingAddon::getManager() +ManagerType* ManagerTrackingAspect::getManager() { return mManager; } //============================================================================== template -const ManagerType* ManagerTrackingAddon::getManager() const +const ManagerType* ManagerTrackingAspect::getManager() const { return mManager; } //============================================================================== template -void ManagerTrackingAddon::setManager( - AddonManager* newManager, bool) +void ManagerTrackingAspect::setManager( + Composite* newManager, bool) { mManager = dynamic_cast(newManager); if(nullptr == mManager) { dterr << "[" << typeid(*this).name() << "::setManager] Attempting to use a " << "[" << typeid(newManager).name() << "] type manager, but this " - << "Addon is only designed to be attached to a [" + << "Aspect is only designed to be attached to a [" << typeid(ManagerType).name() << "] type manager. This may cause " << "undefined behavior!\n"; assert(false); @@ -89,4 +89,4 @@ void ManagerTrackingAddon::setManager( } // namespace common } // namespace dart -#endif // DART_COMMON_DETAIL_ADDON_H_ +#endif // DART_COMMON_DETAIL_ASPECT_H_ diff --git a/dart/common/detail/AddonWithVersion.h b/dart/common/detail/AspectWithVersion.h similarity index 71% rename from dart/common/detail/AddonWithVersion.h rename to dart/common/detail/AspectWithVersion.h index 92f632c31f836..746152a80570f 100644 --- a/dart/common/detail/AddonWithVersion.h +++ b/dart/common/detail/AspectWithVersion.h @@ -34,10 +34,10 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_COMMON_DETAIL_ADDONWITHVERSION_H_ -#define DART_COMMON_DETAIL_ADDONWITHVERSION_H_ +#ifndef DART_COMMON_DETAIL_ASPECTWITHVERSION_H_ +#define DART_COMMON_DETAIL_ASPECTWITHVERSION_H_ -#include "dart/common/Addon.h" +#include "dart/common/Aspect.h" #include "dart/common/StlHelpers.h" namespace dart { @@ -45,12 +45,12 @@ namespace common { namespace detail { //============================================================================== -/// AddonWithProtectedState generates implementations of the State managing -/// functions for an Addon class. +/// AspectWithProtectedState generates implementations of the State managing +/// functions for an Aspect class. template > -class AddonWithState : public BaseT +class AspectWithState : public BaseT { public: @@ -58,21 +58,21 @@ class AddonWithState : public BaseT using Derived = DerivedT; using StateData = StateDataT; using ManagerType = ManagerT; - using State = Addon::StateMixer; + using State = Aspect::StateMixer; constexpr static void (*UpdateState)(Derived*) = updateState; - using AddonImplementation = AddonWithState< + using AspectImplementation = AspectWithState< Base, Derived, StateData, ManagerT, updateState>; - AddonWithState() = delete; - AddonWithState(const AddonWithState&) = delete; + AspectWithState() = delete; + AspectWithState(const AspectWithState&) = delete; /// Construct using a StateData instance - AddonWithState(AddonManager* mgr, const StateData& state = StateData()); + AspectWithState(Composite* mgr, const StateData& state = StateData()); - /// Construct this Addon and pass args into the constructor of the Base class + /// Construct this Aspect and pass args into the constructor of the Base class template - AddonWithState(AddonManager* mgr, const StateData& state, + AspectWithState(Composite* mgr, const StateData& state, BaseArgs&&... args) : Base(mgr, std::forward(args)...), mState(state) @@ -81,34 +81,34 @@ class AddonWithState : public BaseT } // Documentation inherited - void setAddonState(const Addon::State& otherState) override final; + void setAspectState(const Aspect::State& otherState) override final; // Documentation inherited - const Addon::State* getAddonState() const override final; + const Aspect::State* getAspectState() const override final; - /// Set the State of this Addon + /// Set the State of this Aspect void setState(const StateData& state); - /// Get the State of this Addon + /// Get the State of this Aspect const State& getState() const; // Documentation inherited - std::unique_ptr cloneAddon( - AddonManager* newManager) const override; + std::unique_ptr cloneAspect( + Composite* newManager) const override; protected: - /// State of this Addon + /// State of this Aspect State mState; }; //============================================================================== -/// AddonWithProtectedProperties generates implementations of the Property -/// managing functions for an Addon class. +/// AspectWithProtectedProperties generates implementations of the Property +/// managing functions for an Aspect class. template > -class AddonWithVersionedProperties : public BaseT +class AspectWithVersionedProperties : public BaseT { public: @@ -116,23 +116,23 @@ class AddonWithVersionedProperties : public BaseT using Derived = DerivedT; using PropertiesData = PropertiesDataT; using ManagerType = ManagerT; - using Properties = Addon::PropertiesMixer; + using Properties = Aspect::PropertiesMixer; constexpr static void (*UpdateProperties)(Derived*) = updateProperties; - using AddonImplementation = AddonWithVersionedProperties< + using AspectImplementation = AspectWithVersionedProperties< Base, Derived, PropertiesData, ManagerT, updateProperties>; - AddonWithVersionedProperties() = delete; - AddonWithVersionedProperties(const AddonWithVersionedProperties&) = delete; + AspectWithVersionedProperties() = delete; + AspectWithVersionedProperties(const AspectWithVersionedProperties&) = delete; /// Construct using a PropertiesData instance - AddonWithVersionedProperties( - AddonManager* mgr, const PropertiesData& properties = PropertiesData()); + AspectWithVersionedProperties( + Composite* mgr, const PropertiesData& properties = PropertiesData()); - /// Construct this Addon and pass args into the constructor of the Base class + /// Construct this Aspect and pass args into the constructor of the Base class template - AddonWithVersionedProperties( - AddonManager* mgr, const PropertiesData& properties, BaseArgs&&... args) + AspectWithVersionedProperties( + Composite* mgr, const PropertiesData& properties, BaseArgs&&... args) : Base(mgr, std::forward(args)...), mProperties(properties) { @@ -140,22 +140,22 @@ class AddonWithVersionedProperties : public BaseT } // Documentation inherited - void setAddonProperties(const Addon::Properties& someProperties) override final; + void setAspectProperties(const Aspect::Properties& someProperties) override final; // Documentation inherited - const Addon::Properties* getAddonProperties() const override final; + const Aspect::Properties* getAspectProperties() const override final; - /// Set the Properties of this Addon + /// Set the Properties of this Aspect void setProperties(const PropertiesData& properties); - /// Get the Properties of this Addon + /// Get the Properties of this Aspect const Properties& getProperties() const; // Documentation inherited - std::unique_ptr cloneAddon( - AddonManager* newManager) const override; + std::unique_ptr cloneAspect( + Composite* newManager) const override; - /// Increment the version of this Addon and its Manager + /// Increment the version of this Aspect and its Manager size_t incrementVersion(); /// Call UpdateProperties(this) and incrementVersion() @@ -163,7 +163,7 @@ class AddonWithVersionedProperties : public BaseT protected: - /// Properties of this Addon + /// Properties of this Aspect Properties mProperties; }; @@ -177,15 +177,15 @@ class AddonWithVersionedProperties : public BaseT // template -constexpr void (*AddonWithState< +constexpr void (*AspectWithState< BaseT, DerivedT, StateDataT, ManagerT, updateState>::UpdateState)( DerivedT*); //============================================================================== template -AddonWithState:: -AddonWithState(AddonManager* mgr, const StateDataT& state) +AspectWithState:: +AspectWithState(Composite* mgr, const StateDataT& state) : BaseT(mgr), mState(state) { @@ -195,8 +195,8 @@ AddonWithState(AddonManager* mgr, const StateDataT& state) //============================================================================== template -void AddonWithState:: -setAddonState(const Addon::State& otherState) +void AspectWithState:: +setAspectState(const Aspect::State& otherState) { setState(static_cast(otherState)); } @@ -204,9 +204,9 @@ setAddonState(const Addon::State& otherState) //============================================================================== template -const Addon::State* -AddonWithState:: -getAddonState() const +const Aspect::State* +AspectWithState:: +getAspectState() const { return &mState; } @@ -214,7 +214,7 @@ getAddonState() const //============================================================================== template -void AddonWithState:: +void AspectWithState:: setState(const StateData& state) { static_cast(mState) = state; @@ -224,7 +224,7 @@ setState(const StateData& state) //============================================================================== template -auto AddonWithState:: +auto AspectWithState:: getState() const -> const State& { return mState; @@ -233,9 +233,9 @@ getState() const -> const State& //============================================================================== template -std::unique_ptr -AddonWithState:: - cloneAddon(AddonManager* newManager) const +std::unique_ptr +AspectWithState:: + cloneAspect(Composite* newManager) const { return common::make_unique(newManager, mState); } @@ -249,17 +249,17 @@ AddonWithState:: // template -constexpr void (*AddonWithVersionedProperties:: UpdateProperties)(DerivedT*); //============================================================================== template -AddonWithVersionedProperties:: -AddonWithVersionedProperties( - AddonManager* mgr, const PropertiesData& properties) +AspectWithVersionedProperties( + Composite* mgr, const PropertiesData& properties) : BaseT(mgr), mProperties(properties) { @@ -269,9 +269,9 @@ AddonWithVersionedProperties( //============================================================================== template -void AddonWithVersionedProperties:: -setAddonProperties(const Addon::Properties& someProperties) +setAspectProperties(const Aspect::Properties& someProperties) { setProperties(static_cast(someProperties)); } @@ -279,10 +279,10 @@ setAddonProperties(const Addon::Properties& someProperties) //============================================================================== template -const Addon::Properties* -AddonWithVersionedProperties:: -getAddonProperties() const +getAspectProperties() const { return &mProperties; } @@ -290,7 +290,7 @@ getAddonProperties() const //============================================================================== template -void AddonWithVersionedProperties:: setProperties(const PropertiesData& properties) { @@ -301,7 +301,7 @@ setProperties(const PropertiesData& properties) //============================================================================== template -auto AddonWithVersionedProperties:: getProperties() const -> const Properties& { @@ -311,10 +311,10 @@ getProperties() const -> const Properties& //============================================================================== template -std::unique_ptr -AddonWithVersionedProperties +AspectWithVersionedProperties:: -cloneAddon(AddonManager* newManager) const +cloneAspect(Composite* newManager) const { return common::make_unique(newManager, mProperties); } @@ -322,7 +322,7 @@ cloneAddon(AddonManager* newManager) const //============================================================================== template -size_t AddonWithVersionedProperties::incrementVersion() { if(ManagerType* mgr = this->getManager()) @@ -334,7 +334,7 @@ size_t AddonWithVersionedProperties -void AddonWithVersionedProperties< +void AspectWithVersionedProperties< BaseT, DerivedT, PropertiesData, ManagerT, updateProperties>::notifyPropertiesUpdate() { @@ -346,4 +346,4 @@ void AddonWithVersionedProperties< } // namespace common } // namespace dart -#endif // DART_COMMON_DETAIL_ADDONWITHVERSION_H_ +#endif // DART_COMMON_DETAIL_ASPECTWITHVERSION_H_ diff --git a/dart/common/detail/AddonManager.h b/dart/common/detail/Composite.h similarity index 67% rename from dart/common/detail/AddonManager.h rename to dart/common/detail/Composite.h index 409481a6246a8..a4c1e7c89c93c 100644 --- a/dart/common/detail/AddonManager.h +++ b/dart/common/detail/Composite.h @@ -34,16 +34,16 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_COMMON_DETAIL_ADDONMANAGER_H_ -#define DART_COMMON_DETAIL_ADDONMANAGER_H_ +#ifndef DART_COMMON_DETAIL_COMPOSITE_H_ +#define DART_COMMON_DETAIL_COMPOSITE_H_ -#include "dart/common/AddonManager.h" +#include "dart/common/Composite.h" -#define DART_COMMON_CHECK_ILLEGAL_ADDON_ERASE( Func, T, ReturnType )\ +#define DART_COMMON_CHECK_ILLEGAL_ASPECT_ERASE( Func, T, ReturnType )\ if(requires< T >())\ {\ - dterr << "[AddonManager::" #Func << "] Illegal request to remove required "\ - << "Addon [" << typeid(T).name() << "]!\n";\ + dterr << "[Composite::" #Func << "] Illegal request to remove required "\ + << "Aspect [" << typeid(T).name() << "]!\n";\ assert(false);\ return ReturnType ;\ } @@ -53,17 +53,17 @@ namespace common { //============================================================================== template -bool AddonManager::has() const +bool Composite::has() const { return (get() != nullptr); } //============================================================================== template -T* AddonManager::get() +T* Composite::get() { - AddonMap::iterator it = mAddonMap.find( typeid(T) ); - if(mAddonMap.end() == it) + AspectMap::iterator it = mAspectMap.find( typeid(T) ); + if(mAspectMap.end() == it) return nullptr; return static_cast(it->second.get()); @@ -71,54 +71,54 @@ T* AddonManager::get() //============================================================================== template -const T* AddonManager::get() const +const T* Composite::get() const { - return const_cast(this)->get(); + return const_cast(this)->get(); } //============================================================================== template -void AddonManager::set(const T* addon) +void Composite::set(const T* aspect) { - _set(typeid(T), addon); + _set(typeid(T), aspect); } //============================================================================== template -void AddonManager::set(std::unique_ptr&& addon) +void Composite::set(std::unique_ptr&& aspect) { - _set(typeid(T), std::move(addon)); + _set(typeid(T), std::move(aspect)); } //============================================================================== template -T* AddonManager::create(Args&&... args) +T* Composite::create(Args&&... args) { - T* addon = new T(this, std::forward(args)...); - mAddonMap[typeid(T)] = std::unique_ptr(addon); - becomeManager(addon, false); + T* aspect = new T(this, std::forward(args)...); + mAspectMap[typeid(T)] = std::unique_ptr(aspect); + becomeManager(aspect, false); - return addon; + return aspect; } //============================================================================== template -void AddonManager::erase() +void Composite::erase() { - AddonMap::iterator it = mAddonMap.find( typeid(T) ); - DART_COMMON_CHECK_ILLEGAL_ADDON_ERASE(erase, T, DART_BLANK) - if(mAddonMap.end() != it) + AspectMap::iterator it = mAspectMap.find( typeid(T) ); + DART_COMMON_CHECK_ILLEGAL_ASPECT_ERASE(erase, T, DART_BLANK) + if(mAspectMap.end() != it) it->second = nullptr; } //============================================================================== template -std::unique_ptr AddonManager::release() +std::unique_ptr Composite::release() { std::unique_ptr extraction = nullptr; - AddonMap::iterator it = mAddonMap.find( typeid(T) ); - DART_COMMON_CHECK_ILLEGAL_ADDON_ERASE(release, T, nullptr) - if(mAddonMap.end() != it) + AspectMap::iterator it = mAspectMap.find( typeid(T) ); + DART_COMMON_CHECK_ILLEGAL_ASPECT_ERASE(release, T, nullptr) + if(mAspectMap.end() != it) extraction = std::unique_ptr(static_cast(it->second.release())); return extraction; @@ -126,93 +126,93 @@ std::unique_ptr AddonManager::release() //============================================================================== template -constexpr bool AddonManager::isSpecializedFor() +constexpr bool Composite::isSpecializedFor() { return false; } //============================================================================== template -bool AddonManager::requires() const +bool Composite::requires() const { - return (mRequiredAddons.find(typeid(T)) != mRequiredAddons.end()); + return (mRequiredAspects.find(typeid(T)) != mRequiredAspects.end()); } //============================================================================== template -void createAddons(T* /*mgr*/) +void createAspects(T* /*mgr*/) { // Do nothing } //============================================================================== -template -void createAddons(T* mgr) +template +void createAspects(T* mgr) { - mgr->template create(); + mgr->template create(); - createAddons(mgr); + createAspects(mgr); } } // namespace common } // namespace dart //============================================================================== -// Create non-template alternatives to AddonManager functions -#define DART_BAKE_SPECIALIZED_ADDON_IRREGULAR( TypeName, AddonName ) \ - inline bool has ## AddonName () const \ +// Create non-template alternatives to Composite functions +#define DART_BAKE_SPECIALIZED_ASPECT_IRREGULAR( TypeName, AspectName ) \ + inline bool has ## AspectName () const \ { \ return this->template has(); \ } \ \ - inline TypeName * get ## AddonName () \ + inline TypeName * get ## AspectName () \ { \ return this->template get(); \ } \ \ - inline const TypeName* get ## AddonName () const \ + inline const TypeName* get ## AspectName () const \ { \ return this->template get(); \ } \ \ - inline TypeName * get ## AddonName (const bool createIfNull) \ + inline TypeName * get ## AspectName (const bool createIfNull) \ { \ - TypeName* addon = get ## AddonName(); \ + TypeName* aspect = get ## AspectName(); \ \ - if (createIfNull && nullptr == addon) \ - return create ## AddonName(); \ + if (createIfNull && nullptr == aspect) \ + return create ## AspectName(); \ \ - return addon; \ + return aspect; \ } \ \ - inline void set ## AddonName (const TypeName * addon) \ + inline void set ## AspectName (const TypeName * aspect) \ { \ - this->template set(addon); \ + this->template set(aspect); \ } \ \ - inline void set ## AddonName (std::unique_ptr< TypeName >&& addon) \ + inline void set ## AspectName (std::unique_ptr< TypeName >&& aspect) \ { \ - this->template set(std::move(addon)); \ + this->template set(std::move(aspect)); \ } \ \ template \ - inline TypeName * create ## AddonName (Args&&... args) \ + inline TypeName * create ## AspectName (Args&&... args) \ { \ return this->template create(std::forward(args)...); \ } \ \ - inline void erase ## AddonName () \ + inline void erase ## AspectName () \ { \ this->template erase(); \ } \ \ - inline std::unique_ptr< TypeName > release ## AddonName () \ + inline std::unique_ptr< TypeName > release ## AspectName () \ { \ return this->template release(); \ } //============================================================================== -#define DART_BAKE_SPECIALIZED_ADDON(AddonName)\ - DART_BAKE_SPECIALIZED_ADDON_IRREGULAR(AddonName, AddonName); +#define DART_BAKE_SPECIALIZED_ASPECT(AspectName)\ + DART_BAKE_SPECIALIZED_ASPECT_IRREGULAR(AspectName, AspectName); -#endif // DART_COMMON_DETAIL_ADDONMANAGER_H_ +#endif // DART_COMMON_DETAIL_COMPOSITE_H_ diff --git a/dart/common/detail/AddonManagerJoiner.h b/dart/common/detail/CompositeJoiner.h similarity index 75% rename from dart/common/detail/AddonManagerJoiner.h rename to dart/common/detail/CompositeJoiner.h index 7e0e3b9418ddb..3bc374807447a 100644 --- a/dart/common/detail/AddonManagerJoiner.h +++ b/dart/common/detail/CompositeJoiner.h @@ -34,10 +34,10 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_COMMON_DETAIL_ADDONMANAGERJOINER_H_ -#define DART_COMMON_DETAIL_ADDONMANAGERJOINER_H_ +#ifndef DART_COMMON_DETAIL_COMPOSITEJOINER_H_ +#define DART_COMMON_DETAIL_COMPOSITEJOINER_H_ -#include "dart/common/AddonManagerJoiner.h" +#include "dart/common/CompositeJoiner.h" #include "dart/common/detail/TemplateJoinerDispatchMacro.h" namespace dart { @@ -46,7 +46,7 @@ namespace common { //============================================================================== template template -AddonManagerJoiner::AddonManagerJoiner( +CompositeJoiner::CompositeJoiner( Base1Arg&& arg1, Base2Args&&... args2) : Base1(std::forward(arg1)), Base2(std::forward(args2)...) @@ -57,7 +57,7 @@ AddonManagerJoiner::AddonManagerJoiner( //============================================================================== template template -AddonManagerJoiner::AddonManagerJoiner( +CompositeJoiner::CompositeJoiner( Base1Arg&& arg1, NoArg_t) : Base1(std::forward(arg1)), Base2() @@ -68,7 +68,7 @@ AddonManagerJoiner::AddonManagerJoiner( //============================================================================== template template -AddonManagerJoiner::AddonManagerJoiner( +CompositeJoiner::CompositeJoiner( NoArg_t, Base2Args&&... args2) : Base1(), Base2(std::forward(args2)...) @@ -77,13 +77,13 @@ AddonManagerJoiner::AddonManagerJoiner( } //============================================================================== -DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(bool, AddonManagerJoiner, has, () const, ()) -DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(T*, AddonManagerJoiner, get, (), ()) -DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(const T*, AddonManagerJoiner, get, () const, ()) -DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(void, AddonManagerJoiner, set, (const T* addon), (addon)) -DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(void, AddonManagerJoiner, set, (std::unique_ptr&& addon), (std::move(addon))) -DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(void, AddonManagerJoiner, erase, (), ()) -DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(std::unique_ptr, AddonManagerJoiner, release, (), ()) +DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(bool, CompositeJoiner, has, () const, ()) +DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(T*, CompositeJoiner, get, (), ()) +DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(const T*, CompositeJoiner, get, () const, ()) +DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(void, CompositeJoiner, set, (const T* aspect), (aspect)) +DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(void, CompositeJoiner, set, (std::unique_ptr&& aspect), (std::move(aspect))) +DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(void, CompositeJoiner, erase, (), ()) +DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(std::unique_ptr, CompositeJoiner, release, (), ()) //============================================================================== // Because this function requires a comma inside of its template argument list, @@ -91,7 +91,7 @@ DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(std::unique_ptr, AddonManagerJ // implement it explicitly. template template -T* AddonManagerJoiner::create(Args&&... args) +T* CompositeJoiner::create(Args&&... args) { if(Base1::template isSpecializedFor()) return Base1::template create(std::forward(args)...); @@ -102,7 +102,7 @@ T* AddonManagerJoiner::create(Args&&... args) //============================================================================== template template -constexpr bool AddonManagerJoiner::isSpecializedFor() +constexpr bool CompositeJoiner::isSpecializedFor() { return (Base1::template isSpecializedFor() || Base2::template isSpecializedFor()); @@ -111,9 +111,9 @@ constexpr bool AddonManagerJoiner::isSpecializedFor() //============================================================================== template template -AddonManagerJoiner::AddonManagerJoiner( +CompositeJoiner::CompositeJoiner( Args&&... args) - : AddonManagerJoiner>( + : CompositeJoiner>( std::forward(args)...) { // Do nothing @@ -122,5 +122,5 @@ AddonManagerJoiner::AddonManagerJoiner( } // namespace common } // namespace dart -#endif // DART_COMMON_DETAIL_ADDONMANAGERJOINER_H_ +#endif // DART_COMMON_DETAIL_COMPOSITEJOINER_H_ diff --git a/dart/common/detail/RequiresAddon.h b/dart/common/detail/RequiresAspect.h similarity index 86% rename from dart/common/detail/RequiresAddon.h rename to dart/common/detail/RequiresAspect.h index ed319a7c16995..2f8cef0c65a2f 100644 --- a/dart/common/detail/RequiresAddon.h +++ b/dart/common/detail/RequiresAspect.h @@ -34,22 +34,22 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_COMMON_DETAIL_REQUIRESADDON_H_ -#define DART_COMMON_DETAIL_REQUIRESADDON_H_ +#ifndef DART_COMMON_DETAIL_REQUIRESASPECT_H_ +#define DART_COMMON_DETAIL_REQUIRESASPECT_H_ -#include "dart/common/RequiresAddon.h" +#include "dart/common/RequiresAspect.h" namespace dart { namespace common { //============================================================================== -template -RequiresAddon::RequiresAddon() +template +RequiresAspect::RequiresAspect() { - AddonManager::mRequiredAddons.insert(typeid(ReqAddon)); + Composite::mRequiredAspects.insert(typeid(ReqAspect)); } } // namespace common } // namespace dart -#endif // DART_COMMON_DETAIL_REQUIRESADDON_H_ +#endif // DART_COMMON_DETAIL_REQUIRESASPECT_H_ diff --git a/dart/common/detail/SpecializedForAddon.h b/dart/common/detail/SpecializedForAddon.h deleted file mode 100644 index 8d6aaa7d40d64..0000000000000 --- a/dart/common/detail/SpecializedForAddon.h +++ /dev/null @@ -1,329 +0,0 @@ -/* - * Copyright (c) 2015, 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_COMMON_DETAIL_SPECIALIZEDFORADDON_H_ -#define DART_COMMON_DETAIL_SPECIALIZEDFORADDON_H_ - -#include "dart/common/SpecializedForAddon.h" - -// This preprocessor token should only be used by the unittest that is -// responsible for checking that the specialized routines are being used to -// access specialized Addons -#ifdef DART_UNITTEST_SPECIALIZED_ADDON_ACCESS -bool usedSpecializedAddonAccess; -#endif // DART_UNITTEST_SPECIALIZED_ADDON_ACCESS - -namespace dart { -namespace common { - -//============================================================================== -template -SpecializedForAddon::SpecializedForAddon() -{ - mAddonMap[typeid( SpecAddon )] = nullptr; - mSpecAddonIterator = mAddonMap.find(typeid( SpecAddon )); -} - -//============================================================================== -template -template -bool SpecializedForAddon::has() const -{ - return _has(type()); -} - -//============================================================================== -template -template -T* SpecializedForAddon::get() -{ - return _get(type()); -} - -//============================================================================== -template -template -const T* SpecializedForAddon::get() const -{ - return _get(type()); -} - -//============================================================================== -template -template -void SpecializedForAddon::set(const T* addon) -{ - _set(type(), addon); -} - -//============================================================================== -template -template -void SpecializedForAddon::set(std::unique_ptr&& addon) -{ - _set(type(), std::move(addon)); -} - -//============================================================================== -template -template -T* SpecializedForAddon::create(Args&&... args) -{ - return _create(type(), std::forward(args)...); -} - -//============================================================================== -template -template -void SpecializedForAddon::erase() -{ - _erase(type()); -} - -//============================================================================== -template -template -std::unique_ptr SpecializedForAddon::release() -{ - return _release(type()); -} - -//============================================================================== -template -template -constexpr bool SpecializedForAddon::isSpecializedFor() -{ - return _isSpecializedFor(type()); -} - -//============================================================================== -template -template -bool SpecializedForAddon::_has(type) const -{ -#ifdef DART_UNITTEST_SPECIALIZED_ADDON_ACCESS - usedSpecializedAddonAccess = true; -#endif // DART_UNITTEST_SPECIALIZED_ADDON_ACCESS - - return AddonManager::has(); -} - -//============================================================================== -template -bool SpecializedForAddon::_has(type) const -{ -#ifdef DART_UNITTEST_SPECIALIZED_ADDON_ACCESS - usedSpecializedAddonAccess = true; -#endif // DART_UNITTEST_SPECIALIZED_ADDON_ACCESS - - return (mSpecAddonIterator->second.get() != nullptr); -} - -//============================================================================== -template -template -T* SpecializedForAddon::_get(type) -{ - return AddonManager::get(); -} - -//============================================================================== -template -SpecAddon* SpecializedForAddon::_get(type) -{ -#ifdef DART_UNITTEST_SPECIALIZED_ADDON_ACCESS - usedSpecializedAddonAccess = true; -#endif // DART_UNITTEST_SPECIALIZED_ADDON_ACCESS - - return static_cast(mSpecAddonIterator->second.get()); -} - -//============================================================================== -template -template -const T* SpecializedForAddon::_get(type) const -{ - return AddonManager::get(); -} - -//============================================================================== -template -const SpecAddon* SpecializedForAddon::_get(type) const -{ -#ifdef DART_UNITTEST_SPECIALIZED_ADDON_ACCESS - usedSpecializedAddonAccess = true; -#endif // DART_UNITTEST_SPECIALIZED_ADDON_ACCESS - - return static_cast(mSpecAddonIterator->second.get()); -} - -//============================================================================== -template -template -void SpecializedForAddon::_set(type, const T* addon) -{ - AddonManager::set(addon); -} - -//============================================================================== -template -void SpecializedForAddon::_set( - type, const SpecAddon* addon) -{ -#ifdef DART_UNITTEST_SPECIALIZED_ADDON_ACCESS - usedSpecializedAddonAccess = true; -#endif // DART_UNITTEST_SPECIALIZED_ADDON_ACCESS - - if(addon) - { - mSpecAddonIterator->second = addon->cloneAddon(this); - becomeManager(mSpecAddonIterator->second.get(), false); - } - else - { - mSpecAddonIterator->second = nullptr; - } -} - -//============================================================================== -template -template -void SpecializedForAddon::_set(type, std::unique_ptr&& addon) -{ - AddonManager::set(std::move(addon)); -} - -//============================================================================== -template -void SpecializedForAddon::_set( - type, std::unique_ptr&& addon) -{ -#ifdef DART_UNITTEST_SPECIALIZED_ADDON_ACCESS - usedSpecializedAddonAccess = true; -#endif // DART_UNITTEST_SPECIALIZED_ADDON_ACCESS - - mSpecAddonIterator->second = std::move(addon); - becomeManager(mSpecAddonIterator->second.get(), true); -} - -//============================================================================== -template -template -T* SpecializedForAddon::_create(type, Args&&... args) -{ - return AddonManager::create(std::forward(args)...); -} - -//============================================================================== -template -template -SpecAddon* SpecializedForAddon::_create( - type, Args&&... args) -{ -#ifdef DART_UNITTEST_SPECIALIZED_ADDON_ACCESS - usedSpecializedAddonAccess = true; -#endif // DART_UNITTEST_SPECIALIZED_ADDON_ACCESS - - SpecAddon* addon = new SpecAddon(this, std::forward(args)...); - mSpecAddonIterator->second = std::unique_ptr(addon); - becomeManager(addon, false); - - return addon; -} - -//============================================================================== -template -template -void SpecializedForAddon::_erase(type) -{ - AddonManager::erase(); -} - -//============================================================================== -template -void SpecializedForAddon::_erase(type) -{ -#ifdef DART_UNITTEST_SPECIALIZED_ADDON_ACCESS - usedSpecializedAddonAccess = true; -#endif // DART_UNITTEST_SPECIALIZED_ADDON_ACCESS - - DART_COMMON_CHECK_ILLEGAL_ADDON_ERASE(erase, SpecAddon, DART_BLANK); - mSpecAddonIterator->second = nullptr; -} - -//============================================================================== -template -template -std::unique_ptr SpecializedForAddon::_release(type) -{ - return AddonManager::release(); -} - -//============================================================================== -template -std::unique_ptr SpecializedForAddon::_release( - type) -{ -#ifdef DART_UNITTEST_SPECIALIZED_ADDON_ACCESS - usedSpecializedAddonAccess = true; -#endif // DART_UNITTEST_SPECIALIZED_ADDON_ACCESS - - DART_COMMON_CHECK_ILLEGAL_ADDON_ERASE(release, SpecAddon, nullptr); - std::unique_ptr extraction( - static_cast(mSpecAddonIterator->second.release())); - - return extraction; -} - -//============================================================================== -template -template -constexpr bool SpecializedForAddon::_isSpecializedFor(type) -{ - return false; -} - -//============================================================================== -template -constexpr bool SpecializedForAddon::_isSpecializedFor(type) -{ - return true; -} - -} // namespace common -} // namespace dart - -#endif // DART_COMMON_DETAIL_SPECIALIZEDFORADDON_H_ diff --git a/dart/common/detail/SpecializedForAspect.h b/dart/common/detail/SpecializedForAspect.h new file mode 100644 index 0000000000000..a6c9697cba536 --- /dev/null +++ b/dart/common/detail/SpecializedForAspect.h @@ -0,0 +1,329 @@ +/* + * Copyright (c) 2015, 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_COMMON_DETAIL_SPECIALIZEDFORASPECT_H_ +#define DART_COMMON_DETAIL_SPECIALIZEDFORASPECT_H_ + +#include "dart/common/SpecializedForAspect.h" + +// This preprocessor token should only be used by the unittest that is +// responsible for checking that the specialized routines are being used to +// access specialized Aspects +#ifdef DART_UNITTEST_SPECIALIZED_ASPECT_ACCESS +bool usedSpecializedAspectAccess; +#endif // DART_UNITTEST_SPECIALIZED_ASPECT_ACCESS + +namespace dart { +namespace common { + +//============================================================================== +template +SpecializedForAspect::SpecializedForAspect() +{ + mAspectMap[typeid( SpecAspect )] = nullptr; + mSpecAspectIterator = mAspectMap.find(typeid( SpecAspect )); +} + +//============================================================================== +template +template +bool SpecializedForAspect::has() const +{ + return _has(type()); +} + +//============================================================================== +template +template +T* SpecializedForAspect::get() +{ + return _get(type()); +} + +//============================================================================== +template +template +const T* SpecializedForAspect::get() const +{ + return _get(type()); +} + +//============================================================================== +template +template +void SpecializedForAspect::set(const T* aspect) +{ + _set(type(), aspect); +} + +//============================================================================== +template +template +void SpecializedForAspect::set(std::unique_ptr&& aspect) +{ + _set(type(), std::move(aspect)); +} + +//============================================================================== +template +template +T* SpecializedForAspect::create(Args&&... args) +{ + return _create(type(), std::forward(args)...); +} + +//============================================================================== +template +template +void SpecializedForAspect::erase() +{ + _erase(type()); +} + +//============================================================================== +template +template +std::unique_ptr SpecializedForAspect::release() +{ + return _release(type()); +} + +//============================================================================== +template +template +constexpr bool SpecializedForAspect::isSpecializedFor() +{ + return _isSpecializedFor(type()); +} + +//============================================================================== +template +template +bool SpecializedForAspect::_has(type) const +{ +#ifdef DART_UNITTEST_SPECIALIZED_ASPECT_ACCESS + usedSpecializedAspectAccess = true; +#endif // DART_UNITTEST_SPECIALIZED_ASPECT_ACCESS + + return Composite::has(); +} + +//============================================================================== +template +bool SpecializedForAspect::_has(type) const +{ +#ifdef DART_UNITTEST_SPECIALIZED_ASPECT_ACCESS + usedSpecializedAspectAccess = true; +#endif // DART_UNITTEST_SPECIALIZED_ASPECT_ACCESS + + return (mSpecAspectIterator->second.get() != nullptr); +} + +//============================================================================== +template +template +T* SpecializedForAspect::_get(type) +{ + return Composite::get(); +} + +//============================================================================== +template +SpecAspect* SpecializedForAspect::_get(type) +{ +#ifdef DART_UNITTEST_SPECIALIZED_ASPECT_ACCESS + usedSpecializedAspectAccess = true; +#endif // DART_UNITTEST_SPECIALIZED_ASPECT_ACCESS + + return static_cast(mSpecAspectIterator->second.get()); +} + +//============================================================================== +template +template +const T* SpecializedForAspect::_get(type) const +{ + return Composite::get(); +} + +//============================================================================== +template +const SpecAspect* SpecializedForAspect::_get(type) const +{ +#ifdef DART_UNITTEST_SPECIALIZED_ASPECT_ACCESS + usedSpecializedAspectAccess = true; +#endif // DART_UNITTEST_SPECIALIZED_ASPECT_ACCESS + + return static_cast(mSpecAspectIterator->second.get()); +} + +//============================================================================== +template +template +void SpecializedForAspect::_set(type, const T* aspect) +{ + Composite::set(aspect); +} + +//============================================================================== +template +void SpecializedForAspect::_set( + type, const SpecAspect* aspect) +{ +#ifdef DART_UNITTEST_SPECIALIZED_ASPECT_ACCESS + usedSpecializedAspectAccess = true; +#endif // DART_UNITTEST_SPECIALIZED_ASPECT_ACCESS + + if(aspect) + { + mSpecAspectIterator->second = aspect->cloneAspect(this); + becomeManager(mSpecAspectIterator->second.get(), false); + } + else + { + mSpecAspectIterator->second = nullptr; + } +} + +//============================================================================== +template +template +void SpecializedForAspect::_set(type, std::unique_ptr&& aspect) +{ + Composite::set(std::move(aspect)); +} + +//============================================================================== +template +void SpecializedForAspect::_set( + type, std::unique_ptr&& aspect) +{ +#ifdef DART_UNITTEST_SPECIALIZED_ASPECT_ACCESS + usedSpecializedAspectAccess = true; +#endif // DART_UNITTEST_SPECIALIZED_ASPECT_ACCESS + + mSpecAspectIterator->second = std::move(aspect); + becomeManager(mSpecAspectIterator->second.get(), true); +} + +//============================================================================== +template +template +T* SpecializedForAspect::_create(type, Args&&... args) +{ + return Composite::create(std::forward(args)...); +} + +//============================================================================== +template +template +SpecAspect* SpecializedForAspect::_create( + type, Args&&... args) +{ +#ifdef DART_UNITTEST_SPECIALIZED_ASPECT_ACCESS + usedSpecializedAspectAccess = true; +#endif // DART_UNITTEST_SPECIALIZED_ASPECT_ACCESS + + SpecAspect* aspect = new SpecAspect(this, std::forward(args)...); + mSpecAspectIterator->second = std::unique_ptr(aspect); + becomeManager(aspect, false); + + return aspect; +} + +//============================================================================== +template +template +void SpecializedForAspect::_erase(type) +{ + Composite::erase(); +} + +//============================================================================== +template +void SpecializedForAspect::_erase(type) +{ +#ifdef DART_UNITTEST_SPECIALIZED_ASPECT_ACCESS + usedSpecializedAspectAccess = true; +#endif // DART_UNITTEST_SPECIALIZED_ASPECT_ACCESS + + DART_COMMON_CHECK_ILLEGAL_ASPECT_ERASE(erase, SpecAspect, DART_BLANK); + mSpecAspectIterator->second = nullptr; +} + +//============================================================================== +template +template +std::unique_ptr SpecializedForAspect::_release(type) +{ + return Composite::release(); +} + +//============================================================================== +template +std::unique_ptr SpecializedForAspect::_release( + type) +{ +#ifdef DART_UNITTEST_SPECIALIZED_ASPECT_ACCESS + usedSpecializedAspectAccess = true; +#endif // DART_UNITTEST_SPECIALIZED_ASPECT_ACCESS + + DART_COMMON_CHECK_ILLEGAL_ASPECT_ERASE(release, SpecAspect, nullptr); + std::unique_ptr extraction( + static_cast(mSpecAspectIterator->second.release())); + + return extraction; +} + +//============================================================================== +template +template +constexpr bool SpecializedForAspect::_isSpecializedFor(type) +{ + return false; +} + +//============================================================================== +template +constexpr bool SpecializedForAspect::_isSpecializedFor(type) +{ + return true; +} + +} // namespace common +} // namespace dart + +#endif // DART_COMMON_DETAIL_SPECIALIZEDFORASPECT_H_ diff --git a/dart/common/detail/TemplateJoinerDispatchMacro.h b/dart/common/detail/TemplateJoinerDispatchMacro.h index 38e78718e16a6..b8b75b747c963 100644 --- a/dart/common/detail/TemplateJoinerDispatchMacro.h +++ b/dart/common/detail/TemplateJoinerDispatchMacro.h @@ -39,7 +39,7 @@ //============================================================================== /// This macro provides the implementation for most of the member functions in -/// common::AddonManagerJoiner, dynamics::NodeManagerJoinerForBodyNode, and +/// common::CompositeJoiner, dynamics::NodeManagerJoinerForBodyNode, and /// NodeManagerJoinerForSkeleton. The member functions of those classes share /// essentially the same logic, so it makes sense to have a single macro that /// provides the implementation for all of them. diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index 55453fbdfc7d3..ac01c0bb70dfc 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -144,10 +144,10 @@ BodyNodeProperties::BodyNodeProperties( BodyNodeExtendedProperties::BodyNodeExtendedProperties( const BodyNodeProperties& standardProperties, const NodeProperties& nodeProperties, - const AddonProperties& addonProperties) + const AspectProperties& aspectProperties) : BodyNodeProperties(standardProperties), mNodeProperties(nodeProperties), - mAddonProperties(addonProperties) + mAspectProperties(aspectProperties) { // Do nothing } @@ -156,11 +156,11 @@ BodyNodeExtendedProperties::BodyNodeExtendedProperties( BodyNodeExtendedProperties::BodyNodeExtendedProperties( BodyNodeProperties&& standardProperties, NodeProperties&& nodeProperties, - AddonProperties&& addonProperties) + AspectProperties&& aspectProperties) : BodyNodeProperties(std::move(standardProperties)) { mNodeProperties = std::move(nodeProperties); - mAddonProperties = std::move(addonProperties); + mAspectProperties = std::move(aspectProperties); } } // namespace detail @@ -188,7 +188,7 @@ void BodyNode::setProperties(const ExtendedProperties& _properties) setProperties(_properties.mNodeProperties); - setProperties(_properties.mAddonProperties); + setProperties(_properties.mAspectProperties); } //============================================================================== @@ -231,9 +231,9 @@ void BodyNode::setProperties(const NodeProperties& _properties) } //============================================================================== -void BodyNode::setProperties(const AddonProperties& _properties) +void BodyNode::setProperties(const AspectProperties& _properties) { - setAddonProperties(_properties); + setAspectProperties(_properties); } //============================================================================== @@ -298,7 +298,7 @@ BodyNode::ExtendedProperties BodyNode::getExtendedProperties() const { return ExtendedProperties(getBodyNodeProperties(), getAttachedNodeProperties(), - getAddonProperties()); + getAspectProperties()); } //============================================================================== @@ -1245,7 +1245,7 @@ BodyNode* BodyNode::clone(BodyNode* _parentBodyNode, Joint* _parentJoint, BodyNode* clonedBn = new BodyNode(_parentBodyNode, _parentJoint, getBodyNodeProperties()); - clonedBn->matchAddons(this); + clonedBn->matchAspects(this); if(cloneNodes) clonedBn->matchNodes(this); @@ -1398,7 +1398,7 @@ void BodyNode::draw(renderer::RenderInterface* ri, ri->transform(getRelativeTransform()); // _ri->pushName(???); TODO(MXG): What should we do about this for Frames? - auto shapeNodes = getShapeNodesWith(); + auto shapeNodes = getShapeNodesWith(); for (auto shapeNode : shapeNodes) shapeNode->draw(ri, color, useDefaultColor); // _ri.popName(); diff --git a/dart/dynamics/BodyNode.h b/dart/dynamics/BodyNode.h index 162fe1967f112..d875cc187778b 100644 --- a/dart/dynamics/BodyNode.h +++ b/dart/dynamics/BodyNode.h @@ -80,7 +80,7 @@ class Marker; /// BodyNode inherits Frame, and a parent Frame of a BodyNode is the parent /// BodyNode of the BodyNode. class BodyNode : - public virtual common::AddonManager, + public virtual common::Composite, public virtual BodyNodeSpecializedFor, public SkeletonRefCountingBase, public TemplatedJacobianNode @@ -98,7 +98,7 @@ class BodyNode : using NodePropertiesVector = common::ExtensibleVector< std::unique_ptr >; using NodePropertiesMap = std::map< std::type_index, std::unique_ptr >; using NodeProperties = common::ExtensibleMapHolder; - using AddonProperties = common::AddonManager::Properties; + using AspectProperties = common::Composite::Properties; using UniqueProperties = detail::BodyNodeUniqueProperties; using Properties = detail::BodyNodeProperties; @@ -115,8 +115,8 @@ class BodyNode : /// Set the Properties of the attached Nodes void setProperties(const NodeProperties& _properties); - /// Same as setAddonProperties() - void setProperties(const AddonProperties& _properties); + /// Same as setAspectProperties() + void setProperties(const AspectProperties& _properties); /// Set the Properties of this BodyNode void setProperties(const Properties& _properties); @@ -131,7 +131,7 @@ class BodyNode : NodeProperties getAttachedNodeProperties() const; /// The the full extended Properties of this BodyNode, including the - /// Properties of its Addons, its attached Nodes, and the BodyNode itself. + /// Properties of its Aspects, its attached Nodes, and the BodyNode itself. ExtendedProperties getExtendedProperties() const; /// Copy the Properties of another BodyNode @@ -564,30 +564,30 @@ class BodyNode : /// Remove all ShapeNodes from this BodyNode void removeAllShapeNodes(); - /// Create a ShapeNode with the specified Addons and an automatically assigned + /// Create a ShapeNode with the specified Aspects and an automatically assigned /// name: _ShapeNode_<#>. - template + template ShapeNode* createShapeNodeWith(const ShapePtr& shape); - /// Create a ShapeNode with the specified name and Addons - template + /// Create a ShapeNode with the specified name and Aspects + template ShapeNode* createShapeNodeWith(const ShapePtr& shape, const std::string& name); - /// Return the number of ShapeNodes containing given Addon in this BodyNode - template + /// Return the number of ShapeNodes containing given Aspect in this BodyNode + template size_t getNumShapeNodesWith() const; - /// Return the list of ShapeNodes containing given Addon - template + /// Return the list of ShapeNodes containing given Aspect + template const std::vector getShapeNodesWith(); - /// Return the list of ShapeNodes containing given Addon - template + /// Return the list of ShapeNodes containing given Aspect + template const std::vector getShapeNodesWith() const; - /// Remove all ShapeNodes containing given Addon from this BodyNode - template + /// Remove all ShapeNodes containing given Aspect from this BodyNode + template void removeAllShapeNodesWith(); /// Create an EndEffector attached to this BodyNode. Pass an diff --git a/dart/dynamics/EndEffector.cpp b/dart/dynamics/EndEffector.cpp index 619f40ee223a9..58cfa9b59a6ef 100644 --- a/dart/dynamics/EndEffector.cpp +++ b/dart/dynamics/EndEffector.cpp @@ -70,9 +70,9 @@ bool Support::isActive() const //============================================================================== EndEffector::StateData::StateData( const Eigen::Isometry3d& relativeTransform, - const common::AddonManager::State& addonStates) + const common::Composite::State& aspectStates) : mRelativeTransform(relativeTransform), - mAddonStates(addonStates) + mAspectStates(aspectStates) { // Do nothing } @@ -89,10 +89,10 @@ EndEffector::UniqueProperties::UniqueProperties( EndEffector::PropertiesData::PropertiesData( const Entity::Properties& _entityProperties, const UniqueProperties& _effectorProperties, - const common::AddonManager::Properties& _addonProperties) + const common::Composite::Properties& _aspectProperties) : Entity::Properties(_entityProperties), UniqueProperties(_effectorProperties), - mAddonProperties(_addonProperties) + mAspectProperties(_aspectProperties) { // Do nothing } @@ -101,20 +101,20 @@ EndEffector::PropertiesData::PropertiesData( void EndEffector::setState(const StateData& _state) { setRelativeTransform(_state.mRelativeTransform); - setAddonStates(_state.mAddonStates); + setAspectStates(_state.mAspectStates); } //============================================================================== void EndEffector::setState(StateData&& _state) { setRelativeTransform(_state.mRelativeTransform); - setAddonStates(std::move(_state.mAddonStates)); + setAspectStates(std::move(_state.mAspectStates)); } //============================================================================== EndEffector::StateData EndEffector::getEndEffectorState() const { - return StateData(mRelativeTf, getAddonStates()); + return StateData(mRelativeTf, getAspectStates()); } //============================================================================== @@ -122,7 +122,7 @@ void EndEffector::setProperties(const PropertiesData& _properties, bool _useNow) { Entity::setProperties(_properties); setProperties(static_cast(_properties), _useNow); - setAddonProperties(_properties.mAddonProperties); + setAspectProperties(_properties.mAspectProperties); } //============================================================================== @@ -136,7 +136,7 @@ void EndEffector::setProperties(const UniqueProperties& _properties, EndEffector::PropertiesData EndEffector::getEndEffectorProperties() const { return PropertiesData(getEntityProperties(), mEndEffectorP, - getAddonProperties()); + getAspectProperties()); } //============================================================================== @@ -199,7 +199,7 @@ void EndEffector::copyNodeStateTo(std::unique_ptr& outputState) con static_cast(outputState.get()); state->mRelativeTransform = mRelativeTf; - copyAddonStatesTo(state->mAddonStates); + copyAspectStatesTo(state->mAspectStates); } else { @@ -231,7 +231,7 @@ void EndEffector::copyNodePropertiesTo( static_cast(*properties) = getEntityProperties(); static_cast(*properties) = mEndEffectorP; - copyAddonPropertiesTo(properties->mAddonProperties); + copyAspectPropertiesTo(properties->mAspectProperties); } else { @@ -393,7 +393,7 @@ EndEffector::EndEffector(BodyNode* _parent, const PropertiesData& _properties) Node* EndEffector::cloneNode(BodyNode* _parent) const { EndEffector* ee = new EndEffector(_parent, PropertiesData()); - ee->duplicateAddons(this); + ee->duplicateAspects(this); ee->copy(this); diff --git a/dart/dynamics/EndEffector.h b/dart/dynamics/EndEffector.h index af206ffc3795a..31f89139fa9e1 100644 --- a/dart/dynamics/EndEffector.h +++ b/dart/dynamics/EndEffector.h @@ -37,9 +37,9 @@ #ifndef DART_DYNAMICS_ENDEFFECTOR_H_ #define DART_DYNAMICS_ENDEFFECTOR_H_ -#include "dart/common/Addon.h" -#include "dart/common/SpecializedForAddon.h" -#include "dart/common/AddonWithVersion.h" +#include "dart/common/Aspect.h" +#include "dart/common/SpecializedForAspect.h" +#include "dart/common/AspectWithVersion.h" #include "dart/dynamics/FixedFrame.h" #include "dart/dynamics/TemplatedJacobianNode.h" @@ -71,7 +71,7 @@ void SupportUpdate(Support* support); } // namespace detail class Support final : - public common::AddonWithStateAndVersionedProperties< + public common::AspectWithStateAndVersionedProperties< Support, detail::SupportStateData, detail::SupportPropertiesData, @@ -80,13 +80,13 @@ class Support final : { public: -// DART_COMMON_ADDON_STATE_PROPERTY_CONSTRUCTORS( Support, &detail::SupportUpdate, &detail::SupportUpdate ) - DART_COMMON_ADDON_STATE_PROPERTY_CONSTRUCTORS(Support) +// DART_COMMON_ASPECT_STATE_PROPERTY_CONSTRUCTORS( Support, &detail::SupportUpdate, &detail::SupportUpdate ) + DART_COMMON_ASPECT_STATE_PROPERTY_CONSTRUCTORS(Support) /// Set/Get the support geometry for this EndEffector. The SupportGeometry /// represents points in the EndEffector frame that can be used for contact /// when solving balancing or manipulation constraints. - DART_COMMON_SET_GET_ADDON_PROPERTY(math::SupportGeometry, Geometry) + DART_COMMON_SET_GET_ASPECT_PROPERTY(math::SupportGeometry, Geometry) // void setGeometry(const math::SupportGeometry&); // const math::SupportGeometry& getGeometry() const; @@ -100,7 +100,7 @@ class Support final : }; class EndEffector final : - public virtual common::SpecializedForAddon, + public virtual common::SpecializedForAspect, public FixedFrame, public AccessoryNode, public TemplatedJacobianNode @@ -114,16 +114,16 @@ class EndEffector final : { StateData(const Eigen::Isometry3d& relativeTransform = Eigen::Isometry3d::Identity(), - const common::AddonManager::State& addonStates = - common::AddonManager::State()); + const common::Composite::State& aspectStates = + common::Composite::State()); /// The current relative transform of the EndEffector // TODO(MXG): Consider moving this to a FixedFrame::State struct and then // inheriting that struct Eigen::Isometry3d mRelativeTransform; - /// The current states of the EndEffector's Addons - common::AddonManager::State mAddonStates; + /// The current states of the EndEffector's Aspects + common::Composite::State mAspectStates; }; using State = Node::StateMixer; @@ -145,11 +145,11 @@ class EndEffector final : PropertiesData( const Entity::Properties& _entityProperties = Entity::Properties(), const UniqueProperties& _effectorProperties = UniqueProperties(), - const common::AddonManager::Properties& _addonProperties = - common::AddonManager::Properties()); + const common::Composite::Properties& _aspectProperties = + common::Composite::Properties()); - /// The properties of the EndEffector's Addons - common::AddonManager::Properties mAddonProperties; + /// The properties of the EndEffector's Aspects + common::Composite::Properties mAspectProperties; }; using Properties = Node::PropertiesMixer; @@ -228,7 +228,7 @@ class EndEffector final : /// be set with setDefaultRelativeTransform() void resetRelativeTransform(); - DART_BAKE_SPECIALIZED_ADDON(Support) + DART_BAKE_SPECIALIZED_ASPECT(Support) // Documentation inherited bool dependsOn(size_t _genCoordIndex) const override; diff --git a/dart/dynamics/Entity.h b/dart/dynamics/Entity.h index 1d155c6184e64..96e1abd55a9c3 100644 --- a/dart/dynamics/Entity.h +++ b/dart/dynamics/Entity.h @@ -43,7 +43,7 @@ #include "dart/common/Subject.h" #include "dart/common/Signal.h" -#include "dart/common/AddonManager.h" +#include "dart/common/Composite.h" #include "dart/dynamics/Shape.h" #include "dart/dynamics/SmartPointer.h" diff --git a/dart/dynamics/EulerJoint.cpp b/dart/dynamics/EulerJoint.cpp index 563bd6dc7fa97..921d0abc69dad 100644 --- a/dart/dynamics/EulerJoint.cpp +++ b/dart/dynamics/EulerJoint.cpp @@ -63,13 +63,13 @@ void EulerJoint::setProperties(const Properties& _properties) //============================================================================== void EulerJoint::setProperties(const UniqueProperties& _properties) { - getEulerJointAddon()->setProperties(_properties); + getEulerJointAspect()->setProperties(_properties); } //============================================================================== EulerJoint::Properties EulerJoint::getEulerJointProperties() const { - return EulerJoint::Properties(getMultiDofJointProperties(), getEulerJointAddon()->getProperties()); + return EulerJoint::Properties(getMultiDofJointProperties(), getEulerJointAspect()->getProperties()); } //============================================================================== @@ -119,16 +119,16 @@ bool EulerJoint::isCyclic(size_t _index) const //============================================================================== void EulerJoint::setAxisOrder(EulerJoint::AxisOrder _order, bool _renameDofs) { - getEulerJointAddon()->setAxisOrder(_order); + getEulerJointAspect()->setAxisOrder(_order); if (_renameDofs) updateDegreeOfFreedomNames(); - // The EulerJoint::Addon will take care of notifying a position update + // The EulerJoint::Aspect will take care of notifying a position update } //============================================================================== EulerJoint::AxisOrder EulerJoint::getAxisOrder() const { - return getEulerJointAddon()->getAxisOrder(); + return getEulerJointAspect()->getAxisOrder(); } //============================================================================== @@ -283,7 +283,7 @@ Eigen::Matrix EulerJoint::getLocalJacobianStatic( EulerJoint::EulerJoint(const Properties& _properties) : detail::EulerJointBase(_properties, common::NoArg) { - createEulerJointAddon(_properties); + createEulerJointAspect(_properties); // Inherited Joint Properties must be set in the final joint class or else we // get pure virtual function calls diff --git a/dart/dynamics/EulerJoint.h b/dart/dynamics/EulerJoint.h index 391c403d39f33..c8e086dfd7801 100644 --- a/dart/dynamics/EulerJoint.h +++ b/dart/dynamics/EulerJoint.h @@ -51,10 +51,10 @@ class EulerJoint : public detail::EulerJointBase using AxisOrder = detail::AxisOrder; using UniqueProperties = detail::EulerJointUniqueProperties; using Properties = detail::EulerJointProperties; - using Addon = detail::EulerJointAddon; + using Aspect = detail::EulerJointAspect; using Base = detail::EulerJointBase; - DART_BAKE_SPECIALIZED_ADDON_IRREGULAR(Addon, EulerJointAddon) + DART_BAKE_SPECIALIZED_ASPECT_IRREGULAR(Aspect, EulerJointAspect) EulerJoint(const EulerJoint&) = delete; @@ -148,7 +148,7 @@ class EulerJoint : public detail::EulerJointBase Eigen::Matrix getLocalJacobianStatic( const Eigen::Vector3d& _positions) const override; - template friend void detail::JointPropertyUpdate(AddonType*); + template friend void detail::JointPropertyUpdate(AspectType*); protected: diff --git a/dart/dynamics/Joint.cpp b/dart/dynamics/Joint.cpp index 610a22287787f..e5fb1906f93f9 100644 --- a/dart/dynamics/Joint.cpp +++ b/dart/dynamics/Joint.cpp @@ -70,9 +70,9 @@ Joint::Properties::Properties(const std::string& _name, //============================================================================== Joint::ExtendedProperties::ExtendedProperties( const Properties& standardProperties, - const AddonProperties& addonProperties) + const AspectProperties& aspectProperties) : Properties(standardProperties), - mAddonProperties(addonProperties) + mAspectProperties(aspectProperties) { // Do nothing } @@ -80,9 +80,9 @@ Joint::ExtendedProperties::ExtendedProperties( //============================================================================== Joint::ExtendedProperties::ExtendedProperties( Properties&& standardProperties, - AddonProperties&& addonProperties) + AspectProperties&& aspectProperties) : Properties(std::move(standardProperties)), - mAddonProperties(std::move(addonProperties)) + mAspectProperties(std::move(aspectProperties)) { // Do nothing } diff --git a/dart/dynamics/Joint.h b/dart/dynamics/Joint.h index f7601be2b4981..48bb7d9662e09 100644 --- a/dart/dynamics/Joint.h +++ b/dart/dynamics/Joint.h @@ -45,7 +45,7 @@ #include "dart/common/Deprecated.h" #include "dart/common/Subject.h" #include "dart/common/VersionCounter.h" -#include "dart/common/AddonManager.h" +#include "dart/common/Composite.h" #include "dart/math/MathTypes.h" #include "dart/dynamics/SmartPointer.h" @@ -65,7 +65,7 @@ class DegreeOfFreedom; /// class Joint class Joint : public virtual common::Subject, public virtual common::VersionCounter, - public virtual common::AddonManager + public virtual common::Composite { public: @@ -160,22 +160,22 @@ class Joint : public virtual common::Subject, EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; - using AddonProperties = common::AddonManager::Properties; + using AspectProperties = common::Composite::Properties; struct ExtendedProperties : Properties { /// Composed constructor ExtendedProperties( const Properties& standardProperties = Properties(), - const AddonProperties& addonProperties = AddonProperties()); + const AspectProperties& aspectProperties = AspectProperties()); /// Composed move constructor ExtendedProperties( Properties&& standardProperties, - AddonProperties&& addonProperties); + AspectProperties&& aspectProperties); - /// Properties of all the Addons attached to this Joint - AddonProperties mAddonProperties; + /// Properties of all the Aspects attached to this Joint + AspectProperties mAspectProperties; }; /// Default actuator type @@ -998,11 +998,11 @@ class Joint : public virtual common::Subject, namespace detail { -template -void JointPropertyUpdate(AddonType* addon) +template +void JointPropertyUpdate(AspectType* aspect) { - addon->getManager()->notifyPositionUpdate(); - addon->getManager()->updateLocalJacobian(); + aspect->getManager()->notifyPositionUpdate(); + aspect->getManager()->updateLocalJacobian(); } } // namespace detail diff --git a/dart/dynamics/MultiDofJoint.h b/dart/dynamics/MultiDofJoint.h index ca3b8edb868bb..2219b39f07db8 100644 --- a/dart/dynamics/MultiDofJoint.h +++ b/dart/dynamics/MultiDofJoint.h @@ -47,7 +47,7 @@ #include "dart/dynamics/Joint.h" #include "dart/dynamics/Skeleton.h" #include "dart/dynamics/DegreeOfFreedom.h" -#include "dart/common/RequiresAddon.h" +#include "dart/common/RequiresAspect.h" #include "dart/dynamics/detail/MultiDofJointProperties.h" namespace dart { @@ -60,7 +60,7 @@ class Skeleton; template class MultiDofJoint : public Joint, - public virtual common::RequiresAddon< detail::MultiDofJointAddon > + public virtual common::RequiresAspect< detail::MultiDofJointAspect > { public: @@ -69,9 +69,9 @@ class MultiDofJoint : using UniqueProperties = detail::MultiDofJointUniqueProperties; using Properties = detail::MultiDofJointProperties; - using Addon = detail::MultiDofJointAddon; + using Aspect = detail::MultiDofJointAspect; - DART_BAKE_SPECIALIZED_ADDON_IRREGULAR( typename MultiDofJoint::Addon, MultiDofJointAddon ) + DART_BAKE_SPECIALIZED_ASPECT_IRREGULAR( typename MultiDofJoint::Aspect, MultiDofJointAspect ) MultiDofJoint(const MultiDofJoint&) = delete; diff --git a/dart/dynamics/PlanarJoint.cpp b/dart/dynamics/PlanarJoint.cpp index f01e5e496a8b7..41d7350321c86 100644 --- a/dart/dynamics/PlanarJoint.cpp +++ b/dart/dynamics/PlanarJoint.cpp @@ -63,14 +63,14 @@ void PlanarJoint::setProperties(const Properties& _properties) //============================================================================== void PlanarJoint::setProperties(const UniqueProperties& _properties) { - getPlanarJointAddon()->setProperties(_properties); + getPlanarJointAspect()->setProperties(_properties); } //============================================================================== PlanarJoint::Properties PlanarJoint::getPlanarJointProperties() const { return Properties(getMultiDofJointProperties(), - getPlanarJointAddon()->getProperties()); + getPlanarJointAspect()->getProperties()); } //============================================================================== @@ -120,7 +120,7 @@ bool PlanarJoint::isCyclic(size_t _index) const //============================================================================== void PlanarJoint::setXYPlane(bool _renameDofs) { - getPlanarJointAddon()->setXYPlane(); + getPlanarJointAspect()->setXYPlane(); if (_renameDofs) updateDegreeOfFreedomNames(); @@ -130,7 +130,7 @@ void PlanarJoint::setXYPlane(bool _renameDofs) //============================================================================== void PlanarJoint::setYZPlane(bool _renameDofs) { - getPlanarJointAddon()->setYZPlane(); + getPlanarJointAspect()->setYZPlane(); if (_renameDofs) updateDegreeOfFreedomNames(); @@ -140,7 +140,7 @@ void PlanarJoint::setYZPlane(bool _renameDofs) //============================================================================== void PlanarJoint::setZXPlane(bool _renameDofs) { - getPlanarJointAddon()->setZXPlane(); + getPlanarJointAspect()->setZXPlane(); if (_renameDofs) updateDegreeOfFreedomNames(); @@ -152,7 +152,7 @@ void PlanarJoint::setArbitraryPlane(const Eigen::Vector3d& _transAxis1, const Eigen::Vector3d& _transAxis2, bool _renameDofs) { - getPlanarJointAddon()->setArbitraryPlane(_transAxis1, _transAxis2); + getPlanarJointAspect()->setArbitraryPlane(_transAxis1, _transAxis2); if (_renameDofs) updateDegreeOfFreedomNames(); @@ -162,25 +162,25 @@ void PlanarJoint::setArbitraryPlane(const Eigen::Vector3d& _transAxis1, //============================================================================== PlanarJoint::PlaneType PlanarJoint::getPlaneType() const { - return getPlanarJointAddon()->getPlaneType(); + return getPlanarJointAspect()->getPlaneType(); } //============================================================================== const Eigen::Vector3d& PlanarJoint::getRotationalAxis() const { - return getPlanarJointAddon()->getRotAxis(); + return getPlanarJointAspect()->getRotAxis(); } //============================================================================== const Eigen::Vector3d& PlanarJoint::getTranslationalAxis1() const { - return getPlanarJointAddon()->getTransAxis1(); + return getPlanarJointAspect()->getTransAxis1(); } //============================================================================== const Eigen::Vector3d& PlanarJoint::getTranslationalAxis2() const { - return getPlanarJointAddon()->getTransAxis2(); + return getPlanarJointAspect()->getTransAxis2(); } //============================================================================== @@ -188,13 +188,13 @@ Eigen::Matrix PlanarJoint::getLocalJacobianStatic( const Eigen::Vector3d& _positions) const { Eigen::Matrix J = Eigen::Matrix::Zero(); - J.block<3, 1>(3, 0) = getPlanarJointAddon()->getTransAxis1(); - J.block<3, 1>(3, 1) = getPlanarJointAddon()->getTransAxis2(); - J.block<3, 1>(0, 2) = getPlanarJointAddon()->getRotAxis(); + J.block<3, 1>(3, 0) = getPlanarJointAspect()->getTransAxis1(); + J.block<3, 1>(3, 1) = getPlanarJointAspect()->getTransAxis2(); + J.block<3, 1>(0, 2) = getPlanarJointAspect()->getRotAxis(); J.leftCols<2>() = math::AdTJacFixed(mJointP.mT_ChildBodyToJoint - * math::expAngular(getPlanarJointAddon()->getRotAxis() + * math::expAngular(getPlanarJointAspect()->getRotAxis() * -_positions[2]), J.leftCols<2>()); J.col(2) = math::AdTJac(mJointP.mT_ChildBodyToJoint, J.col(2)); @@ -209,7 +209,7 @@ Eigen::Matrix PlanarJoint::getLocalJacobianStatic( PlanarJoint::PlanarJoint(const Properties& _properties) : detail::PlanarJointBase(_properties, common::NoArg) { - createPlanarJointAddon(_properties); + createPlanarJointAspect(_properties); // Inherited Joint Properties must be set in the final joint class or else we // get pure virtual function calls @@ -226,7 +226,7 @@ Joint* PlanarJoint::clone() const void PlanarJoint::updateDegreeOfFreedomNames() { std::vector affixes; - switch (getPlanarJointAddon()->getPlaneType()) + switch (getPlanarJointAspect()->getPlaneType()) { case PlaneType::XY: affixes.push_back("_x"); @@ -246,7 +246,7 @@ void PlanarJoint::updateDegreeOfFreedomNames() break; default: dterr << "Unsupported plane type in PlanarJoint named '" << mJointP.mName - << "' (" << static_cast(getPlanarJointAddon()->getPlaneType()) + << "' (" << static_cast(getPlanarJointAspect()->getPlaneType()) << ")\n"; } @@ -265,9 +265,9 @@ void PlanarJoint::updateLocalTransform() const { const Eigen::Vector3d& positions = getPositionsStatic(); mT = mJointP.mT_ParentBodyToJoint - * Eigen::Translation3d(getPlanarJointAddon()->getTransAxis1() * positions[0]) - * Eigen::Translation3d(getPlanarJointAddon()->getTransAxis2() * positions[1]) - * math::expAngular (getPlanarJointAddon()->getRotAxis() * positions[2]) + * Eigen::Translation3d(getPlanarJointAspect()->getTransAxis1() * positions[0]) + * Eigen::Translation3d(getPlanarJointAspect()->getTransAxis2() * positions[1]) + * math::expAngular (getPlanarJointAspect()->getRotAxis() * positions[2]) * mJointP.mT_ChildBodyToJoint.inverse(); // Verification @@ -284,23 +284,23 @@ void PlanarJoint::updateLocalJacobian(bool) const void PlanarJoint::updateLocalJacobianTimeDeriv() const { Eigen::Matrix J = Eigen::Matrix::Zero(); - J.block<3, 1>(3, 0) = getPlanarJointAddon()->getTransAxis1(); - J.block<3, 1>(3, 1) = getPlanarJointAddon()->getTransAxis2(); - J.block<3, 1>(0, 2) = getPlanarJointAddon()->getRotAxis(); + J.block<3, 1>(3, 0) = getPlanarJointAspect()->getTransAxis1(); + J.block<3, 1>(3, 1) = getPlanarJointAspect()->getTransAxis2(); + J.block<3, 1>(0, 2) = getPlanarJointAspect()->getRotAxis(); const Eigen::Matrix& Jacobian = getLocalJacobianStatic(); const Eigen::Vector3d& velocities = getVelocitiesStatic(); mJacobianDeriv.col(0) = -math::ad(Jacobian.col(2) * velocities[2], math::AdT(mJointP.mT_ChildBodyToJoint - * math::expAngular(getPlanarJointAddon()->getRotAxis() + * math::expAngular(getPlanarJointAspect()->getRotAxis() * -getPositionsStatic()[2]), J.col(0))); mJacobianDeriv.col(1) = -math::ad(Jacobian.col(2) * velocities[2], math::AdT(mJointP.mT_ChildBodyToJoint - * math::expAngular(getPlanarJointAddon()->getRotAxis() + * math::expAngular(getPlanarJointAspect()->getRotAxis() * -getPositionsStatic()[2]), J.col(1))); diff --git a/dart/dynamics/PlanarJoint.h b/dart/dynamics/PlanarJoint.h index cf103efc4302b..fa35b7b39c5eb 100644 --- a/dart/dynamics/PlanarJoint.h +++ b/dart/dynamics/PlanarJoint.h @@ -56,10 +56,10 @@ class PlanarJoint : public detail::PlanarJointBase using PlaneType = detail::PlaneType; using UniqueProperties = detail::PlanarJointUniqueProperties; using Properties = detail::PlanarJointProperties; - using Addon = detail::PlanarJointAddon; + using Aspect = detail::PlanarJointAspect; using Base = detail::PlanarJointBase; - DART_BAKE_SPECIALIZED_ADDON_IRREGULAR(Addon, PlanarJointAddon) + DART_BAKE_SPECIALIZED_ASPECT_IRREGULAR(Aspect, PlanarJointAspect) PlanarJoint(const PlanarJoint&) = delete; @@ -132,7 +132,7 @@ class PlanarJoint : public detail::PlanarJointBase Eigen::Matrix getLocalJacobianStatic( const Eigen::Vector3d& _positions) const override; - template friend void detail::JointPropertyUpdate(AddonType*); + template friend void detail::JointPropertyUpdate(AspectType*); protected: diff --git a/dart/dynamics/PrismaticJoint.cpp b/dart/dynamics/PrismaticJoint.cpp index 00c50c1ffe20a..b0ece9cc4ac04 100644 --- a/dart/dynamics/PrismaticJoint.cpp +++ b/dart/dynamics/PrismaticJoint.cpp @@ -69,7 +69,7 @@ void PrismaticJoint::setProperties(const UniqueProperties& _properties) PrismaticJoint::Properties PrismaticJoint::getPrismaticJointProperties() const { return Properties(getSingleDofJointProperties(), - getPrismaticJointAddon()->getProperties()); + getPrismaticJointAspect()->getProperties()); } //============================================================================== @@ -119,20 +119,20 @@ bool PrismaticJoint::isCyclic(size_t /*_index*/) const //============================================================================== void PrismaticJoint::setAxis(const Eigen::Vector3d& _axis) { - getPrismaticJointAddon()->setAxis(_axis); + getPrismaticJointAspect()->setAxis(_axis); } //============================================================================== const Eigen::Vector3d& PrismaticJoint::getAxis() const { - return getPrismaticJointAddon()->getAxis(); + return getPrismaticJointAspect()->getAxis(); } //============================================================================== PrismaticJoint::PrismaticJoint(const Properties& _properties) : detail::PrismaticJointBase(_properties, common::NoArg) { - createPrismaticJointAddon(_properties); + createPrismaticJointAspect(_properties); // Inherited Joint Properties must be set in the final joint class or else we // get pure virtual function calls diff --git a/dart/dynamics/PrismaticJoint.h b/dart/dynamics/PrismaticJoint.h index bfc82c3e90b10..b114cc5292b55 100644 --- a/dart/dynamics/PrismaticJoint.h +++ b/dart/dynamics/PrismaticJoint.h @@ -50,10 +50,10 @@ class PrismaticJoint : public detail::PrismaticJointBase friend class Skeleton; using UniqueProperties = detail::PrismaticJointUniqueProperties; using Properties = detail::PrismaticJointProperties; - using Addon = detail::PrismaticJointAddon; + using Aspect = detail::PrismaticJointAspect; using Base = detail::PrismaticJointBase; - DART_BAKE_SPECIALIZED_ADDON_IRREGULAR(Addon, PrismaticJointAddon) + DART_BAKE_SPECIALIZED_ASPECT_IRREGULAR(Aspect, PrismaticJointAspect) PrismaticJoint(const PrismaticJoint&) = delete; @@ -93,7 +93,7 @@ class PrismaticJoint : public detail::PrismaticJointBase /// const Eigen::Vector3d& getAxis() const; - template friend void detail::JointPropertyUpdate(AddonType*); + template friend void detail::JointPropertyUpdate(AspectType*); protected: diff --git a/dart/dynamics/RevoluteJoint.cpp b/dart/dynamics/RevoluteJoint.cpp index 067a1288047bf..9604c9287bcd2 100644 --- a/dart/dynamics/RevoluteJoint.cpp +++ b/dart/dynamics/RevoluteJoint.cpp @@ -70,7 +70,7 @@ void RevoluteJoint::setProperties(const UniqueProperties& _properties) RevoluteJoint::Properties RevoluteJoint::getRevoluteJointProperties() const { return Properties(getSingleDofJointProperties(), - getRevoluteJointAddon()->getProperties()); + getRevoluteJointAspect()->getProperties()); } //============================================================================== @@ -120,13 +120,13 @@ const std::string& RevoluteJoint::getStaticType() //============================================================================== void RevoluteJoint::setAxis(const Eigen::Vector3d& _axis) { - getRevoluteJointAddon()->setAxis(_axis); + getRevoluteJointAspect()->setAxis(_axis); } //============================================================================== const Eigen::Vector3d& RevoluteJoint::getAxis() const { - return getRevoluteJointAddon()->getAxis(); + return getRevoluteJointAspect()->getAxis(); } //============================================================================== @@ -134,7 +134,7 @@ RevoluteJoint::RevoluteJoint(const Properties& _properties) : detail::RevoluteJointBase(_properties, common::NoArg) // : detail::RevoluteJointBase(common::NextArgs, _properties) { - createRevoluteJointAddon(_properties); + createRevoluteJointAspect(_properties); // Inherited Joint Properties must be set in the final joint class or else we // get pure virtual function calls diff --git a/dart/dynamics/RevoluteJoint.h b/dart/dynamics/RevoluteJoint.h index 127c7174b1c35..f7460fb6afb2b 100644 --- a/dart/dynamics/RevoluteJoint.h +++ b/dart/dynamics/RevoluteJoint.h @@ -50,10 +50,10 @@ class RevoluteJoint : public detail::RevoluteJointBase friend class Skeleton; using UniqueProperties = detail::RevoluteJointUniqueProperties; using Properties = detail::RevoluteJointProperties; - using Addon = detail::RevoluteJointAddon; + using Aspect = detail::RevoluteJointAspect; using Base = detail::RevoluteJointBase; - DART_BAKE_SPECIALIZED_ADDON_IRREGULAR(Addon, RevoluteJointAddon) + DART_BAKE_SPECIALIZED_ASPECT_IRREGULAR(Aspect, RevoluteJointAspect) RevoluteJoint(const RevoluteJoint&) = delete; @@ -112,7 +112,7 @@ class RevoluteJoint : public detail::RevoluteJointBase public: - template friend void detail::JointPropertyUpdate(AddonType*); + template friend void detail::JointPropertyUpdate(AspectType*); // To get byte-aligned Eigen vectors EIGEN_MAKE_ALIGNED_OPERATOR_NEW diff --git a/dart/dynamics/ScrewJoint.cpp b/dart/dynamics/ScrewJoint.cpp index cb23c3b2b90db..dcbc1951be075 100644 --- a/dart/dynamics/ScrewJoint.cpp +++ b/dart/dynamics/ScrewJoint.cpp @@ -70,7 +70,7 @@ void ScrewJoint::setProperties(const UniqueProperties& _properties) ScrewJoint::Properties ScrewJoint::getScrewJointProperties() const { return Properties(getSingleDofJointProperties(), - getScrewJointAddon()->getProperties()); + getScrewJointAspect()->getProperties()); } //============================================================================== @@ -120,32 +120,32 @@ bool ScrewJoint::isCyclic(size_t /*_index*/) const //============================================================================== void ScrewJoint::setAxis(const Eigen::Vector3d& _axis) { - getScrewJointAddon()->setAxis(_axis); + getScrewJointAspect()->setAxis(_axis); } //============================================================================== const Eigen::Vector3d& ScrewJoint::getAxis() const { - return getScrewJointAddon()->getAxis(); + return getScrewJointAspect()->getAxis(); } //============================================================================== void ScrewJoint::setPitch(double _pitch) { - getScrewJointAddon()->setPitch(_pitch); + getScrewJointAspect()->setPitch(_pitch); } //============================================================================== double ScrewJoint::getPitch() const { - return getScrewJointAddon()->getPitch(); + return getScrewJointAspect()->getPitch(); } //============================================================================== ScrewJoint::ScrewJoint(const Properties& _properties) : detail::ScrewJointBase(_properties, common::NoArg) { - createScrewJointAddon(_properties); + createScrewJointAspect(_properties); // Inherited Joint Properties must be set in the final joint class or else we // get pure virtual function calls diff --git a/dart/dynamics/ScrewJoint.h b/dart/dynamics/ScrewJoint.h index 2ffcc1b5607c9..4d931fed07603 100644 --- a/dart/dynamics/ScrewJoint.h +++ b/dart/dynamics/ScrewJoint.h @@ -50,10 +50,10 @@ class ScrewJoint : public detail::ScrewJointBase friend class Skeleton; using UniqueProperties = detail::ScrewJointUniqueProperties; using Properties = detail::ScrewJointProperties; - using Addon = detail::ScrewJointAddon; + using Aspect = detail::ScrewJointAspect; using Base = detail::ScrewJointBase; - DART_BAKE_SPECIALIZED_ADDON_IRREGULAR(Addon, ScrewJointAddon) + DART_BAKE_SPECIALIZED_ASPECT_IRREGULAR(Aspect, ScrewJointAspect) ScrewJoint(const ScrewJoint&) = delete; @@ -99,7 +99,7 @@ class ScrewJoint : public detail::ScrewJointBase /// double getPitch() const; - template friend void detail::JointPropertyUpdate(AddonType*); + template friend void detail::JointPropertyUpdate(AspectType*); protected: diff --git a/dart/dynamics/ShapeFrame.cpp b/dart/dynamics/ShapeFrame.cpp index 5518b43bb029f..423bfddf2a24d 100644 --- a/dart/dynamics/ShapeFrame.cpp +++ b/dart/dynamics/ShapeFrame.cpp @@ -44,7 +44,7 @@ namespace dynamics { namespace detail { //============================================================================== -VisualAddonProperties::VisualAddonProperties(const Eigen::Vector4d& color, +VisualAspectProperties::VisualAspectProperties(const Eigen::Vector4d& color, const bool hidden) : mRGBA(color), mHidden(hidden) @@ -53,7 +53,7 @@ VisualAddonProperties::VisualAddonProperties(const Eigen::Vector4d& color, } //============================================================================== -CollisionAddonProperties::CollisionAddonProperties( +CollisionAspectProperties::CollisionAspectProperties( const bool collidable) : mCollidable(collidable) { @@ -61,7 +61,7 @@ CollisionAddonProperties::CollisionAddonProperties( } //============================================================================== -DynamicsAddonProperties::DynamicsAddonProperties( +DynamicsAspectProperties::DynamicsAspectProperties( const double frictionCoeff, const double restitutionCoeff) : @@ -74,15 +74,15 @@ DynamicsAddonProperties::DynamicsAddonProperties( } // namespace detail //============================================================================== -VisualAddon::VisualAddon(common::AddonManager* mgr, +VisualAspect::VisualAspect(common::Composite* mgr, const PropertiesData& properties) - : VisualAddon::BaseClass(mgr, properties) + : VisualAspect::BaseClass(mgr, properties) { // Do nothing } //============================================================================== -void VisualAddon::setRGBA(const Eigen::Vector4d& color) +void VisualAspect::setRGBA(const Eigen::Vector4d& color) { mProperties.mRGBA = color; @@ -92,19 +92,19 @@ void VisualAddon::setRGBA(const Eigen::Vector4d& color) } //============================================================================== -void VisualAddon::setColor(const Eigen::Vector3d& color) +void VisualAspect::setColor(const Eigen::Vector3d& color) { setRGB(color); } //============================================================================== -void VisualAddon::setColor(const Eigen::Vector4d& color) +void VisualAspect::setColor(const Eigen::Vector4d& color) { setRGBA(color); } //============================================================================== -void VisualAddon::setRGB(const Eigen::Vector3d& rgb) +void VisualAspect::setRGB(const Eigen::Vector3d& rgb) { Eigen::Vector4d rgba = getRGBA(); rgba.head<3>() = rgb; @@ -113,7 +113,7 @@ void VisualAddon::setRGB(const Eigen::Vector3d& rgb) } //============================================================================== -void VisualAddon::setAlpha(const double alpha) +void VisualAspect::setAlpha(const double alpha) { mProperties.mRGBA[3] = alpha; @@ -123,59 +123,59 @@ void VisualAddon::setAlpha(const double alpha) } //============================================================================== -Eigen::Vector3d VisualAddon::getColor() const +Eigen::Vector3d VisualAspect::getColor() const { return getRGB(); } //============================================================================== -Eigen::Vector3d VisualAddon::getRGB() const +Eigen::Vector3d VisualAspect::getRGB() const { return getRGBA().head<3>(); } //============================================================================== -double VisualAddon::getAlpha() const +double VisualAspect::getAlpha() const { return getRGBA()[3]; } //============================================================================== -void VisualAddon::hide() +void VisualAspect::hide() { setHidden(true); } //============================================================================== -void VisualAddon::show() +void VisualAspect::show() { setHidden(false); } //============================================================================== -bool VisualAddon::isHidden() const +bool VisualAspect::isHidden() const { return getHidden(); } //============================================================================== -CollisionAddon::CollisionAddon( - common::AddonManager* mgr, +CollisionAspect::CollisionAspect( + common::Composite* mgr, const PropertiesData& properties) - : AddonImplementation(mgr, properties) + : AspectImplementation(mgr, properties) { // mIsShapeNode = dynamic_cast(mgr); } //============================================================================== -bool CollisionAddon::isCollidable() const +bool CollisionAspect::isCollidable() const { return getCollidable(); } //============================================================================== -DynamicsAddon::DynamicsAddon( - common::AddonManager* mgr, +DynamicsAspect::DynamicsAspect( + common::Composite* mgr, const PropertiesData& properties) : BaseClass(mgr, properties) { @@ -202,18 +202,18 @@ ShapeFrame::UniqueProperties::UniqueProperties(ShapePtr&& shape) ShapeFrame::Properties::Properties( const Entity::Properties& entityProperties, const UniqueProperties& shapeFrameProperties, - const ShapeFrame::AddonProperties& addonProperties) + const ShapeFrame::AspectProperties& aspectProperties) : Entity::Properties(entityProperties), UniqueProperties(shapeFrameProperties), - mAddonProperties(addonProperties) + mAspectProperties(aspectProperties) { // Do nothing } //============================================================================== -void ShapeFrame::setProperties(const ShapeFrame::AddonProperties& properties) +void ShapeFrame::setProperties(const ShapeFrame::AspectProperties& properties) { - setAddonProperties(properties); + setAspectProperties(properties); } //============================================================================== @@ -221,7 +221,7 @@ void ShapeFrame::setProperties(const Properties& properties) { Entity::setProperties(static_cast(properties)); setProperties(static_cast(properties)); - setProperties(properties.mAddonProperties); + setProperties(properties.mAspectProperties); } //============================================================================== @@ -233,7 +233,7 @@ void ShapeFrame::setProperties(const ShapeFrame::UniqueProperties& properties) //============================================================================== const ShapeFrame::Properties ShapeFrame::getShapeFrameProperties() const { - return Properties(getEntityProperties(), mShapeFrameP, getAddonProperties()); + return Properties(getEntityProperties(), mShapeFrameP, getAspectProperties()); } //============================================================================== @@ -284,16 +284,16 @@ void ShapeFrame::draw(renderer::RenderInterface* ri, const Eigen::Vector4d& color, bool useDefaultColor) const { - auto visualAddon = getVisualAddon(); + auto visualAspect = getVisualAspect(); - if (!visualAddon || visualAddon->isHidden()) + if (!visualAspect || visualAspect->isHidden()) return; ri->pushMatrix(); ri->transform(getRelativeTransform()); if (useDefaultColor) - mShapeFrameP.mShape->draw(ri, visualAddon->getRGBA()); + mShapeFrameP.mShape->draw(ri, visualAspect->getRGBA()); else mShapeFrameP.mShape->draw(ri, color); @@ -314,7 +314,7 @@ size_t ShapeFrame::getVersion() const //============================================================================== ShapeFrame::ShapeFrame(Frame* parent, const Properties& properties) - : common::AddonManager(), + : common::Composite(), Entity(ConstructFrame), Frame(parent, ""), mShapeUpdatedSignal(), @@ -330,7 +330,7 @@ ShapeFrame::ShapeFrame(Frame* parent, const Properties& properties) ShapeFrame::ShapeFrame(Frame* parent, const std::string& name, const ShapePtr& shape) - : common::AddonManager(), + : common::Composite(), Entity(ConstructFrame), Frame(parent, name), mShapeUpdatedSignal(), diff --git a/dart/dynamics/ShapeFrame.h b/dart/dynamics/ShapeFrame.h index 52b879dbe3a3f..b962a16dd378f 100644 --- a/dart/dynamics/ShapeFrame.h +++ b/dart/dynamics/ShapeFrame.h @@ -40,8 +40,8 @@ #include #include "dart/common/Signal.h" -#include "dart/common/AddonWithVersion.h" -#include "dart/common/SpecializedForAddon.h" +#include "dart/common/AspectWithVersion.h" +#include "dart/common/SpecializedForAspect.h" #include "dart/dynamics/FixedFrame.h" #include "dart/dynamics/TemplatedJacobianNode.h" #include "dart/dynamics/EllipsoidShape.h" @@ -49,14 +49,14 @@ namespace dart { namespace dynamics { -class VisualAddon; -class CollisionAddon; -class DynamicsAddon; +class VisualAspect; +class CollisionAspect; +class DynamicsAspect; class ShapeFrame; namespace detail { -struct VisualAddonProperties +struct VisualAspectProperties { /// Color for the primitive shape Eigen::Vector4d mRGBA; @@ -67,27 +67,27 @@ struct VisualAddonProperties bool mHidden; /// Constructor - VisualAddonProperties( + VisualAspectProperties( const Eigen::Vector4d& color = Eigen::Vector4d(0.5, 0.5, 1.0, 1.0), const bool hidden = false); /// Destructor - virtual ~VisualAddonProperties() = default; + virtual ~VisualAspectProperties() = default; }; -struct CollisionAddonProperties +struct CollisionAspectProperties { /// This object is collidable if true bool mCollidable; /// Constructor - CollisionAddonProperties(const bool collidable = true); + CollisionAspectProperties(const bool collidable = true); /// Destructor - virtual ~CollisionAddonProperties() = default; + virtual ~CollisionAspectProperties() = default; }; -struct DynamicsAddonProperties +struct DynamicsAspectProperties { /// Coefficient of friction double mFrictionCoeff; @@ -96,40 +96,40 @@ struct DynamicsAddonProperties double mRestitutionCoeff; /// Constructor - DynamicsAddonProperties(const double frictionCoeff = 1.0, + DynamicsAspectProperties(const double frictionCoeff = 1.0, const double restitutionCoeff = 0.0); /// Destructor - virtual ~DynamicsAddonProperties() = default; + virtual ~DynamicsAspectProperties() = default; }; } // namespace detail -class VisualAddon final : - public common::AddonWithVersionedProperties< - VisualAddon, - detail::VisualAddonProperties, +class VisualAspect final : + public common::AspectWithVersionedProperties< + VisualAspect, + detail::VisualAspectProperties, ShapeFrame> { public: - using BaseClass = common::AddonWithVersionedProperties< - VisualAddon, detail::VisualAddonProperties, ShapeFrame>; + using BaseClass = common::AspectWithVersionedProperties< + VisualAspect, detail::VisualAspectProperties, ShapeFrame>; /// Constructor - VisualAddon(common::AddonManager* mgr, + VisualAspect(common::Composite* mgr, const PropertiesData& properties = PropertiesData()); - VisualAddon(const VisualAddon&) = delete; + VisualAspect(const VisualAspect&) = delete; /// Set RGBA color void setRGBA(const Eigen::Vector4d& color); - DART_COMMON_GET_ADDON_PROPERTY( Eigen::Vector4d, RGBA ) + DART_COMMON_GET_ASPECT_PROPERTY( Eigen::Vector4d, RGBA ) // void setRGBA(const Eigen::Vector4d& value); // const Eigen::Vector4d& getRGBA() const; - DART_COMMON_SET_GET_ADDON_PROPERTY( bool, Hidden ) + DART_COMMON_SET_GET_ASPECT_PROPERTY( bool, Hidden ) // void setHidden(const Eigen::Vector4d& value); // const Eigen::Vector4d& getHidden() const; @@ -166,19 +166,19 @@ class VisualAddon final : }; -class CollisionAddon final : - public common::AddonWithVersionedProperties< - CollisionAddon, - detail::CollisionAddonProperties, +class CollisionAspect final : + public common::AspectWithVersionedProperties< + CollisionAspect, + detail::CollisionAspectProperties, ShapeFrame> { public: - CollisionAddon(const CollisionAddon &) = delete; - CollisionAddon(dart::common::AddonManager* mgr, + CollisionAspect(const CollisionAspect &) = delete; + CollisionAspect(dart::common::Composite* mgr, const PropertiesData& properties = PropertiesData()); - DART_COMMON_SET_GET_ADDON_PROPERTY( bool, Collidable ) + DART_COMMON_SET_GET_ASPECT_PROPERTY( bool, Collidable ) // void setCollidable(const bool& value); // const bool& getCollidable() const; @@ -187,26 +187,26 @@ class CollisionAddon final : }; -class DynamicsAddon final : - public common::AddonWithVersionedProperties< - DynamicsAddon, - detail::DynamicsAddonProperties, +class DynamicsAspect final : + public common::AspectWithVersionedProperties< + DynamicsAspect, + detail::DynamicsAspectProperties, ShapeFrame> { public: - using BaseClass = common::AddonWithVersionedProperties< - DynamicsAddon, detail::DynamicsAddonProperties, ShapeFrame>; + using BaseClass = common::AspectWithVersionedProperties< + DynamicsAspect, detail::DynamicsAspectProperties, ShapeFrame>; - DynamicsAddon(const DynamicsAddon&) = delete; + DynamicsAspect(const DynamicsAspect&) = delete; - DynamicsAddon(dart::common::AddonManager* mgr, + DynamicsAspect(dart::common::Composite* mgr, const PropertiesData& properties = PropertiesData()); - DART_COMMON_SET_GET_ADDON_PROPERTY( double, FrictionCoeff ) + DART_COMMON_SET_GET_ASPECT_PROPERTY( double, FrictionCoeff ) // void setFrictionCoeff(const double& value); // const double& getFrictionCoeff() const; - DART_COMMON_SET_GET_ADDON_PROPERTY( double, RestitutionCoeff ) + DART_COMMON_SET_GET_ASPECT_PROPERTY( double, RestitutionCoeff ) // void setRestitutionCoeff(const double& value); // const double& getRestitutionCoeff() const; @@ -214,8 +214,8 @@ class DynamicsAddon final : class ShapeFrame : public virtual common::VersionCounter, - public common::SpecializedForAddon< - VisualAddon, CollisionAddon, DynamicsAddon>, + public common::SpecializedForAspect< + VisualAspect, CollisionAspect, DynamicsAspect>, public virtual Frame { public: @@ -232,7 +232,7 @@ class ShapeFrame : const Eigen::Isometry3d& oldTransform, const Eigen::Isometry3d& newTransform)>; - using AddonProperties = common::AddonManager::Properties; + using AspectProperties = common::Composite::Properties; struct UniqueProperties { @@ -258,20 +258,20 @@ class ShapeFrame : = Entity::Properties("ShapeFrame"), const UniqueProperties& shapeFrameProperties = UniqueProperties(), - const AddonProperties& addonProperties - = AddonProperties()); + const AspectProperties& aspectProperties + = AspectProperties()); virtual ~Properties() = default; - /// Properties of all the Addons attached to this ShapeFrame - AddonProperties mAddonProperties; + /// Properties of all the Aspects attached to this ShapeFrame + AspectProperties mAspectProperties; }; /// Destructor virtual ~ShapeFrame() = default; - /// Same as setAddonProperties() - void setProperties(const AddonProperties& properties); + /// Same as setAspectProperties() + void setProperties(const AspectProperties& properties); /// Set the Properties of this ShapeFrame void setProperties(const Properties& properties); @@ -297,11 +297,11 @@ class ShapeFrame : /// Return (const) shape ConstShapePtr getShape() const; - DART_BAKE_SPECIALIZED_ADDON(VisualAddon) + DART_BAKE_SPECIALIZED_ASPECT(VisualAspect) - DART_BAKE_SPECIALIZED_ADDON(CollisionAddon) + DART_BAKE_SPECIALIZED_ASPECT(CollisionAspect) - DART_BAKE_SPECIALIZED_ADDON(DynamicsAddon) + DART_BAKE_SPECIALIZED_ASPECT(DynamicsAspect) /// Render this Entity virtual void draw(renderer::RenderInterface* ri = nullptr, diff --git a/dart/dynamics/ShapeNode.cpp b/dart/dynamics/ShapeNode.cpp index 27ca918d4e9cf..edf02d889f9a0 100644 --- a/dart/dynamics/ShapeNode.cpp +++ b/dart/dynamics/ShapeNode.cpp @@ -52,10 +52,10 @@ ShapeNode::UniqueProperties::UniqueProperties( ShapeNode::Properties::Properties( const ShapeFrame::Properties& shapeFrameProperties, const ShapeNode::UniqueProperties& shapeNodeProperties, - const ShapeNode::AddonProperties& addonProperties) + const ShapeNode::AspectProperties& aspectProperties) : ShapeFrame::Properties(shapeFrameProperties), ShapeNode::UniqueProperties(shapeNodeProperties), - mAddonProperties(addonProperties) + mAspectProperties(aspectProperties) { // Do nothing } @@ -64,10 +64,10 @@ ShapeNode::Properties::Properties( ShapeNode::Properties::Properties( ShapeFrame::Properties&& shapeFrameProperties, ShapeNode::UniqueProperties&& shapeNodeProperties, - ShapeNode::AddonProperties&& addonProperties) + ShapeNode::AspectProperties&& aspectProperties) : ShapeFrame::Properties(std::move(shapeFrameProperties)), ShapeNode::UniqueProperties(std::move(shapeNodeProperties)), - mAddonProperties(std::move(addonProperties)) + mAspectProperties(std::move(aspectProperties)) { // Do nothing } @@ -78,7 +78,7 @@ void ShapeNode::setProperties(const Properties& properties) ShapeFrame::setProperties( static_cast(properties)); setProperties(static_cast(properties)); - setAddonProperties(properties.mAddonProperties); + setAspectProperties(properties.mAspectProperties); } //============================================================================== @@ -91,7 +91,7 @@ void ShapeNode::setProperties(const ShapeNode::UniqueProperties& properties) const ShapeNode::Properties ShapeNode::getShapeNodeProperties() const { return Properties(getShapeFrameProperties(), mShapeNodeP, - getAddonProperties()); + getAspectProperties()); } //============================================================================== @@ -353,7 +353,7 @@ ShapeNode::ShapeNode(BodyNode* bodyNode, Node* ShapeNode::cloneNode(BodyNode* parent) const { ShapeNode* shapeNode = new ShapeNode(parent, Properties()); - shapeNode->duplicateAddons(this); + shapeNode->duplicateAspects(this); shapeNode->copy(this); diff --git a/dart/dynamics/ShapeNode.h b/dart/dynamics/ShapeNode.h index a6d88aaed7334..0e49a0c948a5f 100644 --- a/dart/dynamics/ShapeNode.h +++ b/dart/dynamics/ShapeNode.h @@ -48,9 +48,9 @@ namespace dart { namespace dynamics { -class VisualAddon; -class CollisionAddon; -class DynamicsAddon; +class VisualAspect; +class CollisionAspect; +class DynamicsAspect; class ShapeFrame; class ShapeNode : public virtual FixedFrame, @@ -72,7 +72,7 @@ class ShapeNode : public virtual FixedFrame, const Eigen::Isometry3d& oldTransform, const Eigen::Isometry3d& newTransform)>; - using AddonProperties = common::AddonManager::Properties; + using AspectProperties = common::Composite::Properties; struct UniqueProperties { @@ -96,16 +96,16 @@ class ShapeNode : public virtual FixedFrame, = ShapeFrame::Properties(), const ShapeNode::UniqueProperties& shapeNodeProperties = ShapeNode::UniqueProperties(), - const AddonProperties& addonProperties = AddonProperties()); + const AspectProperties& aspectProperties = AspectProperties()); /// Composed move constructor Properties( ShapeFrame::Properties&& shapeFrameProperties, ShapeNode::UniqueProperties&& shapeNodeProperties, - AddonProperties&& addonProperties); + AspectProperties&& aspectProperties); - /// The properties of the ShapeNode's Addons - AddonProperties mAddonProperties; + /// The properties of the ShapeNode's Aspects + AspectProperties mAspectProperties; }; /// Destructor diff --git a/dart/dynamics/SimpleFrame.cpp b/dart/dynamics/SimpleFrame.cpp index 403823fc1d16f..1b6ef98a1740a 100644 --- a/dart/dynamics/SimpleFrame.cpp +++ b/dart/dynamics/SimpleFrame.cpp @@ -59,7 +59,7 @@ SimpleFrame::SimpleFrame(Frame* _refFrame, const std::string& _name, //============================================================================== SimpleFrame::SimpleFrame(const SimpleFrame& _otherFrame, Frame* _refFrame) : Entity(ConstructFrame), - common::AddonManager(), + common::Composite(), Frame(_refFrame, ""), Detachable(), ShapeFrame(_refFrame, ""), @@ -69,7 +69,7 @@ SimpleFrame::SimpleFrame(const SimpleFrame& _otherFrame, Frame* _refFrame) mPartialAcceleration(Eigen::Vector6d::Zero()) { copy(_otherFrame, _refFrame); - duplicateAddons(&_otherFrame); + duplicateAspects(&_otherFrame); } //============================================================================== diff --git a/dart/dynamics/SingleDofJoint.cpp b/dart/dynamics/SingleDofJoint.cpp index 6d159d9cafcd2..defe4960b7ead 100644 --- a/dart/dynamics/SingleDofJoint.cpp +++ b/dart/dynamics/SingleDofJoint.cpp @@ -77,13 +77,13 @@ void SingleDofJoint::setProperties(const Properties& _properties) //============================================================================== void SingleDofJoint::setProperties(const UniqueProperties& _properties) { - getSingleDofJointAddon()->setProperties(_properties); + getSingleDofJointAspect()->setProperties(_properties); } //============================================================================== SingleDofJoint::Properties SingleDofJoint::getSingleDofJointProperties() const { - return Properties(mJointP, getSingleDofJointAddon()->getProperties()); + return Properties(mJointP, getSingleDofJointAspect()->getProperties()); } //============================================================================== @@ -177,20 +177,20 @@ const std::string& SingleDofJoint::setDofName(size_t _index, preserveDofName(0, _preserveName); - if(_name == getSingleDofJointAddon()->mProperties.mDofName) - return getSingleDofJointAddon()->mProperties.mDofName; + if(_name == getSingleDofJointAspect()->mProperties.mDofName) + return getSingleDofJointAspect()->mProperties.mDofName; const SkeletonPtr& skel = getSkeleton(); if(skel) { - getSingleDofJointAddon()->mProperties.mDofName = + getSingleDofJointAspect()->mProperties.mDofName = skel->mNameMgrForDofs.changeObjectName(mDof, _name); skel->incrementVersion(); } else - getSingleDofJointAddon()->mProperties.mDofName = _name; + getSingleDofJointAspect()->mProperties.mDofName = _name; - return getSingleDofJointAddon()->mProperties.mDofName; + return getSingleDofJointAspect()->mProperties.mDofName; } //============================================================================== @@ -202,7 +202,7 @@ void SingleDofJoint::preserveDofName(size_t _index, bool _preserve) return; } - getSingleDofJointAddon()->setPreserveDofName(_preserve); + getSingleDofJointAspect()->setPreserveDofName(_preserve); } //============================================================================== @@ -213,7 +213,7 @@ bool SingleDofJoint::isDofNamePreserved(size_t _index) const SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( isDofNamePreserved, _index ); } - return getSingleDofJointAddon()->getPreserveDofName(); + return getSingleDofJointAspect()->getPreserveDofName(); } //============================================================================== @@ -227,7 +227,7 @@ const std::string& SingleDofJoint::getDofName(size_t _index) const assert(false); } - return getSingleDofJointAddon()->getDofName(); + return getSingleDofJointAspect()->getDofName(); } //============================================================================== @@ -243,8 +243,8 @@ void SingleDofJoint::setCommand(size_t _index, double _command) { case FORCE: mCommand = math::clip(_command, - getSingleDofJointAddon()->getForceLowerLimit(), - getSingleDofJointAddon()->getForceUpperLimit()); + getSingleDofJointAspect()->getForceLowerLimit(), + getSingleDofJointAspect()->getForceUpperLimit()); break; case PASSIVE: if(_command != 0.0) @@ -257,18 +257,18 @@ void SingleDofJoint::setCommand(size_t _index, double _command) break; case SERVO: mCommand = math::clip(_command, - getSingleDofJointAddon()->getVelocityLowerLimit(), - getSingleDofJointAddon()->getVelocityUpperLimit()); + getSingleDofJointAspect()->getVelocityLowerLimit(), + getSingleDofJointAspect()->getVelocityUpperLimit()); break; case ACCELERATION: mCommand = math::clip(_command, - getSingleDofJointAddon()->getAccelerationLowerLimit(), - getSingleDofJointAddon()->getAccelerationUpperLimit()); + getSingleDofJointAspect()->getAccelerationLowerLimit(), + getSingleDofJointAspect()->getAccelerationUpperLimit()); break; case VELOCITY: mCommand = math::clip(_command, - getSingleDofJointAddon()->getVelocityLowerLimit(), - getSingleDofJointAddon()->getVelocityUpperLimit()); + getSingleDofJointAspect()->getVelocityLowerLimit(), + getSingleDofJointAspect()->getVelocityUpperLimit()); // TODO: This possibly makes the acceleration to exceed the limits. break; case LOCKED: @@ -373,7 +373,7 @@ void SingleDofJoint::setPositionLowerLimit(size_t _index, double _position) return; } - getSingleDofJointAddon()->setPositionLowerLimit(_position); + getSingleDofJointAspect()->setPositionLowerLimit(_position); } //============================================================================== @@ -385,7 +385,7 @@ double SingleDofJoint::getPositionLowerLimit(size_t _index) const return 0.0; } - return getSingleDofJointAddon()->getPositionLowerLimit(); + return getSingleDofJointAspect()->getPositionLowerLimit(); } //============================================================================== @@ -397,7 +397,7 @@ void SingleDofJoint::setPositionUpperLimit(size_t _index, double _position) return; } - getSingleDofJointAddon()->setPositionUpperLimit(_position); + getSingleDofJointAspect()->setPositionUpperLimit(_position); } //============================================================================== @@ -409,7 +409,7 @@ double SingleDofJoint::getPositionUpperLimit(size_t _index) const return 0.0; } - return getSingleDofJointAddon()->getPositionUpperLimit(); + return getSingleDofJointAspect()->getPositionUpperLimit(); } //============================================================================== @@ -421,8 +421,8 @@ bool SingleDofJoint::hasPositionLimit(size_t _index) const return true; } - return std::isfinite(getSingleDofJointAddon()->getPositionLowerLimit()) - || std::isfinite(getSingleDofJointAddon()->getPositionUpperLimit()); + return std::isfinite(getSingleDofJointAspect()->getPositionLowerLimit()) + || std::isfinite(getSingleDofJointAspect()->getPositionUpperLimit()); } //============================================================================== @@ -434,13 +434,13 @@ void SingleDofJoint::resetPosition(size_t _index) return; } - setPositionStatic(getSingleDofJointAddon()->getInitialPosition()); + setPositionStatic(getSingleDofJointAspect()->getInitialPosition()); } //============================================================================== void SingleDofJoint::resetPositions() { - setPositionStatic(getSingleDofJointAddon()->getInitialPosition()); + setPositionStatic(getSingleDofJointAspect()->getInitialPosition()); } //============================================================================== @@ -452,7 +452,7 @@ void SingleDofJoint::setInitialPosition(size_t _index, double _initial) return; } - getSingleDofJointAddon()->setInitialPosition(_initial); + getSingleDofJointAspect()->setInitialPosition(_initial); } //============================================================================== @@ -464,7 +464,7 @@ double SingleDofJoint::getInitialPosition(size_t _index) const return 0.0; } - return getSingleDofJointAddon()->getInitialPosition(); + return getSingleDofJointAspect()->getInitialPosition(); } //============================================================================== @@ -483,7 +483,7 @@ void SingleDofJoint::setInitialPositions(const Eigen::VectorXd& _initial) Eigen::VectorXd SingleDofJoint::getInitialPositions() const { return Eigen::Matrix::Constant( - getSingleDofJointAddon()->getInitialPosition()); + getSingleDofJointAspect()->getInitialPosition()); } //============================================================================== @@ -549,7 +549,7 @@ void SingleDofJoint::setVelocityLowerLimit(size_t _index, double _velocity) return; } - getSingleDofJointAddon()->setVelocityLowerLimit(_velocity); + getSingleDofJointAspect()->setVelocityLowerLimit(_velocity); } //============================================================================== @@ -561,7 +561,7 @@ double SingleDofJoint::getVelocityLowerLimit(size_t _index) const return 0.0; } - return getSingleDofJointAddon()->getVelocityLowerLimit(); + return getSingleDofJointAspect()->getVelocityLowerLimit(); } //============================================================================== @@ -573,7 +573,7 @@ void SingleDofJoint::setVelocityUpperLimit(size_t _index, double _velocity) return; } - getSingleDofJointAddon()->setVelocityUpperLimit(_velocity); + getSingleDofJointAspect()->setVelocityUpperLimit(_velocity); } //============================================================================== @@ -585,7 +585,7 @@ double SingleDofJoint::getVelocityUpperLimit(size_t _index) const return 0.0; } - return getSingleDofJointAddon()->getVelocityUpperLimit(); + return getSingleDofJointAspect()->getVelocityUpperLimit(); } //============================================================================== @@ -597,13 +597,13 @@ void SingleDofJoint::resetVelocity(size_t _index) return; } - setVelocityStatic(getSingleDofJointAddon()->getInitialVelocity()); + setVelocityStatic(getSingleDofJointAspect()->getInitialVelocity()); } //============================================================================== void SingleDofJoint::resetVelocities() { - setVelocityStatic(getSingleDofJointAddon()->getInitialVelocity()); + setVelocityStatic(getSingleDofJointAspect()->getInitialVelocity()); } //============================================================================== @@ -615,7 +615,7 @@ void SingleDofJoint::setInitialVelocity(size_t _index, double _initial) return; } - getSingleDofJointAddon()->setInitialVelocity(_initial); + getSingleDofJointAspect()->setInitialVelocity(_initial); } //============================================================================== @@ -627,7 +627,7 @@ double SingleDofJoint::getInitialVelocity(size_t _index) const return 0.0; } - return getSingleDofJointAddon()->getInitialVelocity(); + return getSingleDofJointAspect()->getInitialVelocity(); } //============================================================================== @@ -646,7 +646,7 @@ void SingleDofJoint::setInitialVelocities(const Eigen::VectorXd& _initial) Eigen::VectorXd SingleDofJoint::getInitialVelocities() const { return Eigen::Matrix::Constant( - getSingleDofJointAddon()->getInitialVelocity()); + getSingleDofJointAspect()->getInitialVelocity()); } //============================================================================== @@ -719,7 +719,7 @@ void SingleDofJoint::setAccelerationLowerLimit(size_t _index, return; } - getSingleDofJointAddon()->setAccelerationLowerLimit(_acceleration); + getSingleDofJointAspect()->setAccelerationLowerLimit(_acceleration); } //============================================================================== @@ -731,7 +731,7 @@ double SingleDofJoint::getAccelerationLowerLimit(size_t _index) const return 0.0; } - return getSingleDofJointAddon()->getAccelerationLowerLimit(); + return getSingleDofJointAspect()->getAccelerationLowerLimit(); } //============================================================================== @@ -744,7 +744,7 @@ void SingleDofJoint::setAccelerationUpperLimit(size_t _index, return; } - getSingleDofJointAddon()->setAccelerationUpperLimit(_acceleration); + getSingleDofJointAspect()->setAccelerationUpperLimit(_acceleration); } //============================================================================== @@ -756,7 +756,7 @@ double SingleDofJoint::getAccelerationUpperLimit(size_t _index) const return 0.0; } - return getSingleDofJointAddon()->getAccelerationUpperLimit(); + return getSingleDofJointAspect()->getAccelerationUpperLimit(); } //============================================================================== @@ -882,7 +882,7 @@ void SingleDofJoint::setForceLowerLimit(size_t _index, double _force) return; } - getSingleDofJointAddon()->setForceLowerLimit(_force); + getSingleDofJointAspect()->setForceLowerLimit(_force); } //============================================================================== @@ -894,7 +894,7 @@ double SingleDofJoint::getForceLowerLimit(size_t _index) const return 0.0; } - return getSingleDofJointAddon()->getForceLowerLimit(); + return getSingleDofJointAspect()->getForceLowerLimit(); } //============================================================================== @@ -906,7 +906,7 @@ void SingleDofJoint::setForceUpperLimit(size_t _index, double _force) return; } - getSingleDofJointAddon()->setForceUpperLimit(_force); + getSingleDofJointAspect()->setForceUpperLimit(_force); } //============================================================================== @@ -918,7 +918,7 @@ double SingleDofJoint::getForceUpperLimit(size_t _index) const return 0.0; } - return getSingleDofJointAddon()->getForceUpperLimit(); + return getSingleDofJointAspect()->getForceUpperLimit(); } //============================================================================== @@ -1029,7 +1029,7 @@ void SingleDofJoint::setSpringStiffness(size_t _index, double _k) assert(_k >= 0.0); - getSingleDofJointAddon()->setSpringStiffness(_k); + getSingleDofJointAspect()->setSpringStiffness(_k); } //============================================================================== @@ -1041,7 +1041,7 @@ double SingleDofJoint::getSpringStiffness(size_t _index) const return 0.0; } - return getSingleDofJointAddon()->getSpringStiffness(); + return getSingleDofJointAspect()->getSpringStiffness(); } //============================================================================== @@ -1053,19 +1053,19 @@ void SingleDofJoint::setRestPosition(size_t _index, double _q0) return; } - if (getSingleDofJointAddon()->getPositionLowerLimit() > _q0 - || getSingleDofJointAddon()->getPositionUpperLimit() < _q0) + if (getSingleDofJointAspect()->getPositionLowerLimit() > _q0 + || getSingleDofJointAspect()->getPositionUpperLimit() < _q0) { dtwarn << "[SingleDofJoint::setRestPosition] Value of _q0 [" << _q0 << "] is out of the limit range [" - << getSingleDofJointAddon()->getPositionLowerLimit() << ", " - << getSingleDofJointAddon()->getPositionUpperLimit() + << getSingleDofJointAspect()->getPositionLowerLimit() << ", " + << getSingleDofJointAspect()->getPositionUpperLimit() << "] for index [" << _index << "] of Joint [" << getName() << "].\n"; return; } - getSingleDofJointAddon()->setRestPosition(_q0); + getSingleDofJointAspect()->setRestPosition(_q0); } //============================================================================== @@ -1077,7 +1077,7 @@ double SingleDofJoint::getRestPosition(size_t _index) const return 0.0; } - return getSingleDofJointAddon()->getRestPosition(); + return getSingleDofJointAspect()->getRestPosition(); } //============================================================================== @@ -1091,7 +1091,7 @@ void SingleDofJoint::setDampingCoefficient(size_t _index, double _d) assert(_d >= 0.0); - getSingleDofJointAddon()->setDampingCoefficient(_d); + getSingleDofJointAspect()->setDampingCoefficient(_d); } //============================================================================== @@ -1103,7 +1103,7 @@ double SingleDofJoint::getDampingCoefficient(size_t _index) const return 0.0; } - return getSingleDofJointAddon()->getDampingCoefficient(); + return getSingleDofJointAspect()->getDampingCoefficient(); } //============================================================================== @@ -1117,7 +1117,7 @@ void SingleDofJoint::setCoulombFriction(size_t _index, double _friction) assert(_friction >= 0.0); - getSingleDofJointAddon()->setFriction(_friction); + getSingleDofJointAspect()->setFriction(_friction); } //============================================================================== @@ -1129,16 +1129,16 @@ double SingleDofJoint::getCoulombFriction(size_t _index) const return 0.0; } - return getSingleDofJointAddon()->getFriction(); + return getSingleDofJointAspect()->getFriction(); } //============================================================================== double SingleDofJoint::getPotentialEnergy() const { // Spring energy - double pe = 0.5 * getSingleDofJointAddon()->getSpringStiffness() - * (getPositionStatic() - getSingleDofJointAddon()->getRestPosition()) - * (getPositionStatic() - getSingleDofJointAddon()->getRestPosition()); + double pe = 0.5 * getSingleDofJointAspect()->getSpringStiffness() + * (getPositionStatic() - getSingleDofJointAspect()->getRestPosition()) + * (getPositionStatic() - getSingleDofJointAspect()->getRestPosition()); return pe; } @@ -1168,7 +1168,7 @@ SingleDofJoint::SingleDofJoint(const Properties& _properties) mInvM_a(0.0), mInvMassMatrixSegment(0.0) { - createSingleDofJointAddon(_properties); + createSingleDofJointAspect(_properties); } //============================================================================== @@ -1176,7 +1176,7 @@ void SingleDofJoint::registerDofs() { SkeletonPtr skel = getSkeleton(); if(skel) - getSingleDofJointAddon()->mProperties.mDofName = + getSingleDofJointAspect()->mProperties.mDofName = skel->mNameMgrForDofs.issueNewNameAndAdd(mDof->getName(), mDof); } @@ -1488,8 +1488,8 @@ void SingleDofJoint::updateInvProjArtInertiaImplicitDynamic( double projAI = Jacobian.dot(_artInertia * Jacobian); // Add additional inertia for implicit damping and spring force - projAI += _timeStep * getSingleDofJointAddon()->getDampingCoefficient() - + _timeStep * _timeStep * getSingleDofJointAddon()->getSpringStiffness(); + projAI += _timeStep * getSingleDofJointAspect()->getDampingCoefficient() + + _timeStep * _timeStep * getSingleDofJointAspect()->getSpringStiffness(); // Inversion of the projected articulated inertia for implicit damping and // spring force @@ -1681,12 +1681,12 @@ void SingleDofJoint::updateTotalForceDynamic( const double nextPosition = getPositionStatic() + _timeStep*getVelocityStatic(); const double springForce = - -getSingleDofJointAddon()->getSpringStiffness() - * (nextPosition - getSingleDofJointAddon()->getRestPosition()); + -getSingleDofJointAspect()->getSpringStiffness() + * (nextPosition - getSingleDofJointAspect()->getRestPosition()); // Damping force const double dampingForce = - -getSingleDofJointAddon()->getDampingCoefficient() * getVelocityStatic(); + -getSingleDofJointAspect()->getDampingCoefficient() * getVelocityStatic(); // Compute alpha mTotalForce = mForce + springForce + dampingForce @@ -1842,7 +1842,7 @@ void SingleDofJoint::updateForceID(const Eigen::Vector6d& _bodyForce, if (_withDampingForces) { const double dampingForce = - -getSingleDofJointAddon()->getDampingCoefficient() * getVelocityStatic(); + -getSingleDofJointAspect()->getDampingCoefficient() * getVelocityStatic(); mForce -= dampingForce; } @@ -1852,8 +1852,8 @@ void SingleDofJoint::updateForceID(const Eigen::Vector6d& _bodyForce, const double nextPosition = getPositionStatic() + _timeStep*getVelocityStatic(); const double springForce = - -getSingleDofJointAddon()->getSpringStiffness() - *(nextPosition - getSingleDofJointAddon()->getRestPosition()); + -getSingleDofJointAspect()->getSpringStiffness() + *(nextPosition - getSingleDofJointAspect()->getRestPosition()); mForce -= springForce; } } diff --git a/dart/dynamics/SingleDofJoint.h b/dart/dynamics/SingleDofJoint.h index af14fe6c4c811..f643a8f85ca92 100644 --- a/dart/dynamics/SingleDofJoint.h +++ b/dart/dynamics/SingleDofJoint.h @@ -56,9 +56,9 @@ class SingleDofJoint : public detail::SingleDofJointBase using UniqueProperties = detail::SingleDofJointUniqueProperties; using Properties = detail::SingleDofJointProperties; - using Addon = detail::SingleDofJointAddon; + using Aspect = detail::SingleDofJointAspect; - DART_BAKE_SPECIALIZED_ADDON_IRREGULAR(Addon, SingleDofJointAddon) + DART_BAKE_SPECIALIZED_ASPECT_IRREGULAR(Aspect, SingleDofJointAspect) /// Destructor virtual ~SingleDofJoint(); @@ -397,7 +397,7 @@ class SingleDofJoint : public detail::SingleDofJointBase // Documentation inherited virtual Eigen::Vector6d getBodyConstraintWrench() const override; - template friend void detail::JointPropertyUpdate(AddonType*); + template friend void detail::JointPropertyUpdate(AspectType*); protected: diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index 659efc54d6a7e..5e2bdbf8cef0d 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -188,11 +188,11 @@ Skeleton::ExtendedProperties::ExtendedProperties( const BodyNodeExtendedProperties& bodyNodeProperties, const JointExtendedProperties& jointProperties, const std::vector& parentNames, - const AddonProperties& addonProperties) + const AspectProperties& aspectProperties) : mBodyNodeProperties(bodyNodeProperties), mJointProperties(jointProperties), mParentBodyNodeNames(parentNames), - mAddonProperties(addonProperties) + mAspectProperties(aspectProperties) { // Do nothing } diff --git a/dart/dynamics/Skeleton.h b/dart/dynamics/Skeleton.h index a0f868ded9395..4582d07d79270 100644 --- a/dart/dynamics/Skeleton.h +++ b/dart/dynamics/Skeleton.h @@ -62,7 +62,7 @@ namespace dynamics { /// class Skeleton class Skeleton : public virtual common::VersionCounter, - public virtual common::AddonManager, + public virtual common::Composite, public MetaSkeleton, public virtual SkeletonSpecializedFor { @@ -136,7 +136,7 @@ class Skeleton : /// The Properties of this Skeleton which are independent of the components /// within the Skeleton, such as its BodyNodes and Joints. This does not - /// include any Properties of the Skeleton's Addons. + /// include any Properties of the Skeleton's Aspects. struct Properties { /// Name of the Skeleton @@ -164,7 +164,7 @@ class Skeleton : /// Version number of the Skeleton. This will get incremented each time any /// Property of the Skeleton or a component within the Skeleton is changed. - /// If you create a custom Addon or Node, you must increment the Skeleton + /// If you create a custom Aspect or Node, you must increment the Skeleton /// version number each time one of its Properties is changed, or else the /// machinery used to record Skeletons and Worlds might fail to capture its /// Property changes. @@ -183,10 +183,10 @@ class Skeleton : using BodyNodeExtendedProperties = std::vector; using JointExtendedProperties = std::vector; - using AddonProperties = common::AddonManager::Properties; + using AspectProperties = common::Composite::Properties; /// The Properties of this Skeleton and everything within the Skeleton, - /// including Addons and Nodes that are attached to the + /// including Aspects and Nodes that are attached to the struct ExtendedProperties : Properties { /// Properties of all the BodyNodes in this Skeleton @@ -199,17 +199,17 @@ class Skeleton : /// allows the layout of the Skeleton to be reconstructed. std::vector mParentBodyNodeNames; - /// Properties of any Addons that are attached directly to this Skeleton - /// object (does NOT include Addons that are attached to BodyNodes or Joints + /// Properties of any Aspects that are attached directly to this Skeleton + /// object (does NOT include Aspects that are attached to BodyNodes or Joints /// within this Skeleton). - AddonProperties mAddonProperties; + AspectProperties mAspectProperties; /// Default constructor ExtendedProperties( const BodyNodeExtendedProperties& bodyNodeProperties = BodyNodeExtendedProperties(), const JointExtendedProperties& jointProperties = JointExtendedProperties(), const std::vector& parentNames = std::vector(), - const AddonProperties& addonProperties = AddonProperties()); + const AspectProperties& aspectProperties = AspectProperties()); }; //---------------------------------------------------------------------------- diff --git a/dart/dynamics/SoftBodyNode.cpp b/dart/dynamics/SoftBodyNode.cpp index eda8fec5e5f84..151c66ffe904f 100644 --- a/dart/dynamics/SoftBodyNode.cpp +++ b/dart/dynamics/SoftBodyNode.cpp @@ -123,19 +123,19 @@ SoftBodyNodeProperties::SoftBodyNodeProperties( } //============================================================================== -void SoftBodyNodeStateUpdate(SoftBodyAddon* addon) +void SoftBodyNodeStateUpdate(SoftBodyAspect* aspect) { - if(SoftBodyNode* sbn = addon->getManager()) + if(SoftBodyNode* sbn = aspect->getManager()) sbn->mNotifier->notifyTransformUpdate(); } //============================================================================== -void SoftBodyNodePropertiesUpdate(SoftBodyAddon* addon) +void SoftBodyNodePropertiesUpdate(SoftBodyAspect* aspect) { - if(SoftBodyNode* sbn = addon->getManager()) + if(SoftBodyNode* sbn = aspect->getManager()) { sbn->configurePointMasses(sbn->mSoftShapeNode.lock()); - SoftBodyNodeStateUpdate(addon); + SoftBodyNodeStateUpdate(aspect); } } @@ -162,7 +162,7 @@ void SoftBodyNode::setProperties(const Properties& _properties) //============================================================================== void SoftBodyNode::setProperties(const UniqueProperties& _properties) { - getSoftBodyAddon()->setProperties(_properties); + getSoftBodyAspect()->setProperties(_properties); } //============================================================================== @@ -228,18 +228,18 @@ SoftBodyNode::SoftBodyNode(BodyNode* _parentBodyNode, common::NoArg), mSoftShapeNode(nullptr) { - createSoftBodyAddon(); + createSoftBodyAspect(); mNotifier = new PointMassNotifier(this, "PointMassNotifier"); std::cout << "Creating and assigning mSoftShapeNode" << std::endl; ShapeNode* softNode = createShapeNodeWith< - VisualAddon, CollisionAddon, DynamicsAddon>( + VisualAspect, CollisionAspect, DynamicsAspect>( std::make_shared(this), getName()+"_SoftMeshShape"); mSoftShapeNode = softNode; // Dev's Note: We do this workaround (instead of just using setProperties(~)) // because mSoftShapeNode cannot be used until init(SkeletonPtr) has been // called on this BodyNode, but that happens after construction is finished. - getSoftBodyAddon()->mProperties = _properties; + getSoftBodyAspect()->mProperties = _properties; configurePointMasses(softNode); mNotifier->notifyTransformUpdate(); } @@ -251,7 +251,7 @@ BodyNode* SoftBodyNode::clone(BodyNode* _parentBodyNode, SoftBodyNode* clonedBn = new SoftBodyNode( _parentBodyNode, _parentJoint, getSoftBodyNodeProperties()); - clonedBn->matchAddons(this); + clonedBn->matchAspects(this); if(cloneNodes) clonedBn->matchNodes(this); @@ -262,7 +262,7 @@ BodyNode* SoftBodyNode::clone(BodyNode* _parentBodyNode, //============================================================================== void SoftBodyNode::configurePointMasses(ShapeNode* softNode) { - const UniqueProperties& softProperties = getSoftBodyAddon()->getProperties(); + const UniqueProperties& softProperties = getSoftBodyAspect()->getProperties(); size_t newCount = softProperties.mPointProps.size(); size_t oldCount = mPointMasses.size(); @@ -288,8 +288,8 @@ void SoftBodyNode::configurePointMasses(ShapeNode* softNode) } } - // Resize the number of States in the Addon - getSoftBodyAddon()->_getState().mPointStates.resize( + // Resize the number of States in the Aspect + getSoftBodyAspect()->_getState().mPointStates.resize( softProperties.mPointProps.size(), PointMass::State()); // Access the SoftMeshShape and reallocate its meshes @@ -1189,7 +1189,7 @@ void SoftBodyNode::draw(renderer::RenderInterface* _ri, _ri->pushName((unsigned)mID); - auto shapeNodes = getShapeNodesWith(); + auto shapeNodes = getShapeNodesWith(); for (auto shapeNode : shapeNodes) shapeNode->draw(_ri, _color, _useDefaultColor); @@ -1210,7 +1210,7 @@ void SoftBodyNode::draw(renderer::RenderInterface* _ri, // _ri->setPenColor(fleshColor); // if (_showMeshs) { - _ri->setPenColor(mSoftShapeNode.lock()->get()->getRGBA()); + _ri->setPenColor(mSoftShapeNode.lock()->get()->getRGBA()); glEnable(GL_LIGHTING); glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); @@ -1253,20 +1253,20 @@ void SoftBodyNode::draw(renderer::RenderInterface* _ri, //============================================================================== PointMass::State& SoftBodyNode::getPointState(size_t index) { - return getSoftBodyAddon()->_getState().mPointStates.at(index); + return getSoftBodyAspect()->_getState().mPointStates.at(index); } //============================================================================== SoftBodyNode::UniqueProperties& SoftBodyNode::getSoftPropertiesAndInc() { - getSoftBodyAddon()->incrementVersion(); - return getSoftBodyAddon()->_getProperties(); + getSoftBodyAspect()->incrementVersion(); + return getSoftBodyAspect()->_getProperties(); } //============================================================================== const SoftBodyNode::UniqueProperties& SoftBodyNode::getSoftProperties() const { - return getSoftBodyAddon()->_getProperties(); + return getSoftBodyAspect()->_getProperties(); } //============================================================================== diff --git a/dart/dynamics/SoftBodyNode.h b/dart/dynamics/SoftBodyNode.h index d4f1a61b7cd65..139a316029f05 100644 --- a/dart/dynamics/SoftBodyNode.h +++ b/dart/dynamics/SoftBodyNode.h @@ -53,15 +53,15 @@ class SoftBodyNode : public detail::SoftBodyNodeBase friend class Skeleton; friend class PointMass; friend class PointMassNotifier; - friend void detail::SoftBodyNodeStateUpdate(SoftBodyAddon* addon); - friend void detail::SoftBodyNodePropertiesUpdate(SoftBodyAddon* addon); + friend void detail::SoftBodyNodeStateUpdate(SoftBodyAspect* aspect); + friend void detail::SoftBodyNodePropertiesUpdate(SoftBodyAspect* aspect); using UniqueProperties = detail::SoftBodyNodeUniqueProperties; using Properties = detail::SoftBodyNodeProperties; - using Addon = detail::SoftBodyAddon; + using Aspect = detail::SoftBodyAspect; using Base = detail::SoftBodyNodeBase; - DART_BAKE_SPECIALIZED_ADDON_IRREGULAR(detail::SoftBodyAddon, SoftBodyAddon) + DART_BAKE_SPECIALIZED_ASPECT_IRREGULAR(detail::SoftBodyAspect, SoftBodyAspect) /// \brief virtual ~SoftBodyNode(); @@ -154,7 +154,7 @@ class SoftBodyNode : public detail::SoftBodyNodeBase virtual BodyNode* clone(BodyNode* _parentBodyNode, Joint* _parentJoint, bool cloneNodes) const override; - /// Used by SoftBodyAddon to have this SoftBodyNode reconstruct its + /// Used by SoftBodyAspect to have this SoftBodyNode reconstruct its /// SoftMeshShape void configurePointMasses(ShapeNode* softNode); diff --git a/dart/dynamics/UniversalJoint.cpp b/dart/dynamics/UniversalJoint.cpp index 64f3a38627b1f..396b1fd476208 100644 --- a/dart/dynamics/UniversalJoint.cpp +++ b/dart/dynamics/UniversalJoint.cpp @@ -69,7 +69,7 @@ void UniversalJoint::setProperties(const UniqueProperties& _properties) UniversalJoint::Properties UniversalJoint::getUniversalJointProperties() const { return Properties(getMultiDofJointProperties(), - getUniversalJointAddon()->getProperties()); + getUniversalJointAspect()->getProperties()); } //============================================================================== @@ -119,25 +119,25 @@ bool UniversalJoint::isCyclic(size_t _index) const //============================================================================== void UniversalJoint::setAxis1(const Eigen::Vector3d& _axis) { - getUniversalJointAddon()->setAxis1(_axis); + getUniversalJointAspect()->setAxis1(_axis); } //============================================================================== void UniversalJoint::setAxis2(const Eigen::Vector3d& _axis) { - getUniversalJointAddon()->setAxis2(_axis); + getUniversalJointAspect()->setAxis2(_axis); } //============================================================================== const Eigen::Vector3d& UniversalJoint::getAxis1() const { - return getUniversalJointAddon()->getAxis1(); + return getUniversalJointAspect()->getAxis1(); } //============================================================================== const Eigen::Vector3d& UniversalJoint::getAxis2() const { - return getUniversalJointAddon()->getAxis2(); + return getUniversalJointAspect()->getAxis2(); } //============================================================================== @@ -157,7 +157,7 @@ Eigen::Matrix UniversalJoint::getLocalJacobianStatic( UniversalJoint::UniversalJoint(const Properties& _properties) : detail::UniversalJointBase(_properties, common::NoArg) { - createUniversalJointAddon(_properties); + createUniversalJointAspect(_properties); // Inherited Joint Properties must be set in the final joint class or else we // get pure virtual function calls diff --git a/dart/dynamics/UniversalJoint.h b/dart/dynamics/UniversalJoint.h index 465d2720be20a..d6d13083fe747 100644 --- a/dart/dynamics/UniversalJoint.h +++ b/dart/dynamics/UniversalJoint.h @@ -50,10 +50,10 @@ class UniversalJoint : public detail::UniversalJointBase friend class Skeleton; using UniqueProperties = detail::UniversalJointUniqueProperties; using Properties = detail::UniversalJointProperties; - using Addon = detail::UniversalJointAddon; + using Aspect = detail::UniversalJointAspect; using Base = detail::UniversalJointBase; - DART_BAKE_SPECIALIZED_ADDON_IRREGULAR(Addon, UniversalJointAddon) + DART_BAKE_SPECIALIZED_ASPECT_IRREGULAR(Aspect, UniversalJointAspect) UniversalJoint(const UniversalJoint&) = delete; @@ -103,7 +103,7 @@ class UniversalJoint : public detail::UniversalJointBase Eigen::Matrix getLocalJacobianStatic( const Eigen::Vector2d& _positions) const override; - template friend void detail::JointPropertyUpdate(AddonType*); + template friend void detail::JointPropertyUpdate(AspectType*); protected: diff --git a/dart/dynamics/detail/BasicNodeManager.h b/dart/dynamics/detail/BasicNodeManager.h index 1517c1ccb90af..51ded235e451b 100644 --- a/dart/dynamics/detail/BasicNodeManager.h +++ b/dart/dynamics/detail/BasicNodeManager.h @@ -269,92 +269,92 @@ const NodeType* BasicNodeManagerForSkeleton::getNode( } //============================================================================== -#define DART_BAKE_SPECIALIZED_NODE_IRREGULAR( TypeName, AddonName, PluralAddonName )\ - inline size_t getNum ## PluralAddonName () const\ +#define DART_BAKE_SPECIALIZED_NODE_IRREGULAR( TypeName, AspectName, PluralAspectName )\ + inline size_t getNum ## PluralAspectName () const\ { return getNumNodes< TypeName >(); }\ - inline TypeName * get ## AddonName (size_t index)\ + inline TypeName * get ## AspectName (size_t index)\ { return getNode< TypeName >(index); }\ - inline const TypeName * get ## AddonName (size_t index) const\ + inline const TypeName * get ## AspectName (size_t index) const\ { return getNode< TypeName >(index); } //============================================================================== -#define DART_BAKE_SPECIALIZED_NODE( AddonName )\ - DART_BAKE_SPECIALIZED_NODE_IRREGULAR( AddonName, AddonName, AddonName ## s ) +#define DART_BAKE_SPECIALIZED_NODE( AspectName )\ + DART_BAKE_SPECIALIZED_NODE_IRREGULAR( AspectName, AspectName, AspectName ## s ) //============================================================================== -#define DART_BAKE_SPECIALIZED_NODE_SKEL_IRREGULAR( TypeName, AddonName, PluralAddonName )\ - DART_BAKE_SPECIALIZED_NODE_IRREGULAR( TypeName, AddonName, PluralAddonName )\ - inline size_t getNum ## PluralAddonName (size_t treeIndex) const\ +#define DART_BAKE_SPECIALIZED_NODE_SKEL_IRREGULAR( TypeName, AspectName, PluralAspectName )\ + DART_BAKE_SPECIALIZED_NODE_IRREGULAR( TypeName, AspectName, PluralAspectName )\ + inline size_t getNum ## PluralAspectName (size_t treeIndex) const\ { return getNumNodes< TypeName >(treeIndex); }\ - inline TypeName * get ## AddonName (size_t treeIndex, size_t nodeIndex)\ + inline TypeName * get ## AspectName (size_t treeIndex, size_t nodeIndex)\ { return getNode< TypeName >(treeIndex, nodeIndex); }\ - inline const TypeName * get ## AddonName (size_t treeIndex, size_t nodeIndex) const\ + inline const TypeName * get ## AspectName (size_t treeIndex, size_t nodeIndex) const\ { return getNode< TypeName >(treeIndex, nodeIndex); }\ \ - inline TypeName * get ## AddonName (const std::string& name)\ + inline TypeName * get ## AspectName (const std::string& name)\ { return getNode< TypeName >(name); }\ - inline const TypeName * get ## AddonName (const std::string& name) const\ + inline const TypeName * get ## AspectName (const std::string& name) const\ { return getNode< TypeName >(name); } //============================================================================== -#define DART_BAKE_SPECIALIZED_NODE_SKEL( AddonName )\ - DART_BAKE_SPECIALIZED_NODE_SKEL_IRREGULAR( AddonName, AddonName, AddonName ## s) +#define DART_BAKE_SPECIALIZED_NODE_SKEL( AspectName )\ + DART_BAKE_SPECIALIZED_NODE_SKEL_IRREGULAR( AspectName, AspectName, AspectName ## s) //============================================================================== -#define DART_BAKE_SPECIALIZED_NODE_IRREGULAR_DECLARATIONS( TypeName, AddonName, PluralAddonName )\ - size_t getNum ## PluralAddonName () const;\ - TypeName * get ## AddonName (size_t index);\ - const TypeName * get ## AddonName (size_t index) const; +#define DART_BAKE_SPECIALIZED_NODE_IRREGULAR_DECLARATIONS( TypeName, AspectName, PluralAspectName )\ + size_t getNum ## PluralAspectName () const;\ + TypeName * get ## AspectName (size_t index);\ + const TypeName * get ## AspectName (size_t index) const; //============================================================================== -#define DART_BAKE_SPECIALIZED_NODE_DECLARATIONS( AddonName )\ - DART_BAKE_SPECIALIZED_NODE_IRREGULAR_DECLARATIONS( AddonName, AddonName, AddonName ## s ) +#define DART_BAKE_SPECIALIZED_NODE_DECLARATIONS( AspectName )\ + DART_BAKE_SPECIALIZED_NODE_IRREGULAR_DECLARATIONS( AspectName, AspectName, AspectName ## s ) //============================================================================== -#define DART_BAKE_SPECIALIZED_NODE_SKEL_IRREGULAR_DECLARATIONS( TypeName, AddonName, PluralAddonName )\ - DART_BAKE_SPECIALIZED_NODE_IRREGULAR_DECLARATIONS( TypeName, AddonName, PluralAddonName )\ - size_t getNum ## PluralAddonName (size_t treeIndex) const;\ - TypeName * get ## AddonName (size_t treeIndex, size_t nodeIndex);\ - const TypeName * get ## AddonName (size_t treeIndex, size_t nodeIndex) const;\ +#define DART_BAKE_SPECIALIZED_NODE_SKEL_IRREGULAR_DECLARATIONS( TypeName, AspectName, PluralAspectName )\ + DART_BAKE_SPECIALIZED_NODE_IRREGULAR_DECLARATIONS( TypeName, AspectName, PluralAspectName )\ + size_t getNum ## PluralAspectName (size_t treeIndex) const;\ + TypeName * get ## AspectName (size_t treeIndex, size_t nodeIndex);\ + const TypeName * get ## AspectName (size_t treeIndex, size_t nodeIndex) const;\ \ - TypeName * get ## AddonName (const std::string& name);\ - const TypeName * get ## AddonName (const std::string& name) const; + TypeName * get ## AspectName (const std::string& name);\ + const TypeName * get ## AspectName (const std::string& name) const; //============================================================================== -#define DART_BAKE_SPECIALIZED_NODE_SKEL_DECLARATIONS( AddonName )\ - DART_BAKE_SPECIALIZED_NODE_SKEL_IRREGULAR_DECLARATIONS( AddonName, AddonName, AddonName ## s ) +#define DART_BAKE_SPECIALIZED_NODE_SKEL_DECLARATIONS( AspectName )\ + DART_BAKE_SPECIALIZED_NODE_SKEL_IRREGULAR_DECLARATIONS( AspectName, AspectName, AspectName ## s ) //============================================================================== -#define DART_BAKE_SPECIALIZED_NODE_IRREGULAR_DEFINITIONS( ClassName, TypeName, AddonName, PluralAddonName )\ - size_t ClassName :: getNum ## PluralAddonName () const\ +#define DART_BAKE_SPECIALIZED_NODE_IRREGULAR_DEFINITIONS( ClassName, TypeName, AspectName, PluralAspectName )\ + size_t ClassName :: getNum ## PluralAspectName () const\ { return getNumNodes< TypeName >(); }\ - TypeName * ClassName :: get ## AddonName (size_t index)\ + TypeName * ClassName :: get ## AspectName (size_t index)\ { return getNode< TypeName >(index); }\ - const TypeName * ClassName :: get ## AddonName (size_t index) const\ + const TypeName * ClassName :: get ## AspectName (size_t index) const\ { return getNode< TypeName >(index); } //============================================================================== -#define DART_BAKE_SPECIALIZED_NODE_DEFINITIONS( ClassName, AddonName )\ - DART_BAKE_SPECIALIZED_NODE_IRREGULAR_DEFINITIONS( ClassName, AddonName, AddonName, AddonName ## s ) +#define DART_BAKE_SPECIALIZED_NODE_DEFINITIONS( ClassName, AspectName )\ + DART_BAKE_SPECIALIZED_NODE_IRREGULAR_DEFINITIONS( ClassName, AspectName, AspectName, AspectName ## s ) //============================================================================== -#define DART_BAKE_SPECIALIZED_NODE_SKEL_IRREGULAR_DEFINITIONS( ClassName, TypeName, AddonName, PluralAddonName )\ - DART_BAKE_SPECIALIZED_NODE_IRREGULAR_DEFINITIONS( ClassName, TypeName, AddonName, PluralAddonName )\ - size_t ClassName :: getNum ## PluralAddonName (size_t treeIndex) const\ +#define DART_BAKE_SPECIALIZED_NODE_SKEL_IRREGULAR_DEFINITIONS( ClassName, TypeName, AspectName, PluralAspectName )\ + DART_BAKE_SPECIALIZED_NODE_IRREGULAR_DEFINITIONS( ClassName, TypeName, AspectName, PluralAspectName )\ + size_t ClassName :: getNum ## PluralAspectName (size_t treeIndex) const\ { return getNumNodes< TypeName >(treeIndex); }\ - TypeName * ClassName :: get ## AddonName (size_t treeIndex, size_t nodeIndex)\ + TypeName * ClassName :: get ## AspectName (size_t treeIndex, size_t nodeIndex)\ { return getNode< TypeName >(treeIndex, nodeIndex); }\ - const TypeName * ClassName :: get ## AddonName (size_t treeIndex, size_t nodeIndex) const\ + const TypeName * ClassName :: get ## AspectName (size_t treeIndex, size_t nodeIndex) const\ { return getNode< TypeName >(treeIndex, nodeIndex); }\ \ - TypeName * ClassName :: get ## AddonName (const std::string& name)\ + TypeName * ClassName :: get ## AspectName (const std::string& name)\ { return getNode< TypeName >(name); }\ - const TypeName * ClassName :: get ## AddonName (const std::string& name) const\ + const TypeName * ClassName :: get ## AspectName (const std::string& name) const\ { return getNode< TypeName >(name); } //============================================================================== -#define DART_BAKE_SPECIALIZED_NODE_SKEL_DEFINITIONS( ClassName, AddonName )\ - DART_BAKE_SPECIALIZED_NODE_SKEL_IRREGULAR_DEFINITIONS( ClassName, AddonName, AddonName, AddonName ## s ) +#define DART_BAKE_SPECIALIZED_NODE_SKEL_DEFINITIONS( ClassName, AspectName )\ + DART_BAKE_SPECIALIZED_NODE_SKEL_IRREGULAR_DEFINITIONS( ClassName, AspectName, AspectName, AspectName ## s ) } // namespace detail } // namespace dynamics diff --git a/dart/dynamics/detail/BodyNode.h b/dart/dynamics/detail/BodyNode.h index 5c8e70a2c1304..a625c6033d804 100644 --- a/dart/dynamics/detail/BodyNode.h +++ b/dart/dynamics/detail/BodyNode.h @@ -158,27 +158,27 @@ ShapeNode* BodyNode::createShapeNode(ShapeNodeProperties properties, } //============================================================================== -template +template ShapeNode* BodyNode::createShapeNodeWith(const ShapePtr& shape) { - return createShapeNodeWith(shape, getName()+"_ShapeNode_" + return createShapeNodeWith(shape, getName()+"_ShapeNode_" +std::to_string(getNumShapeNodes())); } //============================================================================== -template +template ShapeNode* BodyNode::createShapeNodeWith( const ShapePtr& shape, const std::string& name) { auto shapeNode = createShapeNode(shape, name); - common::createAddons(shapeNode); + common::createAspects(shapeNode); return shapeNode; } //============================================================================== -template +template size_t BodyNode::getNumShapeNodesWith() const { auto count = 0u; @@ -186,7 +186,7 @@ size_t BodyNode::getNumShapeNodesWith() const for (auto i = 0u; i < numShapeNode; ++i) { - if (getShapeNode(i)->has()) + if (getShapeNode(i)->has()) ++count; } @@ -194,7 +194,7 @@ size_t BodyNode::getNumShapeNodesWith() const } //============================================================================== -template +template const std::vector BodyNode::getShapeNodesWith() { std::vector shapeNodes; @@ -205,7 +205,7 @@ const std::vector BodyNode::getShapeNodesWith() { auto shapeNode = getShapeNode(i); - if (shapeNode->has()) + if (shapeNode->has()) shapeNodes.push_back(shapeNode); } @@ -213,7 +213,7 @@ const std::vector BodyNode::getShapeNodesWith() } //============================================================================== -template +template const std::vector BodyNode::getShapeNodesWith() const { std::vector shapeNodes; @@ -224,7 +224,7 @@ const std::vector BodyNode::getShapeNodesWith() const { const auto shapeNode = getShapeNode(i); - if (shapeNode->has()) + if (shapeNode->has()) shapeNodes.push_back(shapeNode); } @@ -232,10 +232,10 @@ const std::vector BodyNode::getShapeNodesWith() const } //============================================================================== -template +template void BodyNode::removeAllShapeNodesWith() { - auto shapeNodes = getShapeNodesWith(); + auto shapeNodes = getShapeNodesWith(); for (auto shapeNode : shapeNodes) shapeNode->remove(); } diff --git a/dart/dynamics/detail/BodyNodeProperties.h b/dart/dynamics/detail/BodyNodeProperties.h index 41c6545dcbfd1..34867cbc06569 100644 --- a/dart/dynamics/detail/BodyNodeProperties.h +++ b/dart/dynamics/detail/BodyNodeProperties.h @@ -104,25 +104,25 @@ struct BodyNodeExtendedProperties : BodyNodeProperties using NodePropertiesVector = common::ExtensibleVector< std::unique_ptr >; using NodePropertiesMap = std::map< std::type_index, std::unique_ptr >; using NodeProperties = common::ExtensibleMapHolder; - using AddonProperties = common::AddonManager::Properties; + using AspectProperties = common::Composite::Properties; /// Composed constructor BodyNodeExtendedProperties( const BodyNodeProperties& standardProperties = Properties(), const NodeProperties& nodeProperties = NodeProperties(), - const AddonProperties& addonProperties = AddonProperties()); + const AspectProperties& aspectProperties = AspectProperties()); /// Composed move constructor BodyNodeExtendedProperties( BodyNodeProperties&& standardProperties, NodeProperties&& nodeProperties, - AddonProperties&& addonProperties); + AspectProperties&& aspectProperties); /// Properties of all the Nodes attached to this BodyNode NodeProperties mNodeProperties; - /// Properties of all the Addons attached to this BodyNode - AddonProperties mAddonProperties; + /// Properties of all the Aspects attached to this BodyNode + AspectProperties mAspectProperties; }; } // namespace detail diff --git a/dart/dynamics/detail/EulerJointProperties.h b/dart/dynamics/detail/EulerJointProperties.h index 06c2f9dbabdda..8587a99edb14d 100644 --- a/dart/dynamics/detail/EulerJointProperties.h +++ b/dart/dynamics/detail/EulerJointProperties.h @@ -84,19 +84,19 @@ struct EulerJointProperties : }; //============================================================================== -class EulerJointAddon final : - public common::AddonWithVersionedProperties< - EulerJointAddon, EulerJointUniqueProperties, EulerJoint, - detail::JointPropertyUpdate > +class EulerJointAspect final : + public common::AspectWithVersionedProperties< + EulerJointAspect, EulerJointUniqueProperties, EulerJoint, + detail::JointPropertyUpdate > { public: - DART_COMMON_JOINT_ADDON_CONSTRUCTOR( EulerJointAddon ) - DART_COMMON_SET_GET_ADDON_PROPERTY( AxisOrder, AxisOrder ) + DART_COMMON_JOINT_ASPECT_CONSTRUCTOR( EulerJointAspect ) + DART_COMMON_SET_GET_ASPECT_PROPERTY( AxisOrder, AxisOrder ) }; //============================================================================== -using EulerJointBase = common::AddonManagerJoiner< - MultiDofJoint<3>, common::RequiresAddon >; +using EulerJointBase = common::CompositeJoiner< + MultiDofJoint<3>, common::RequiresAspect >; } // namespace detail } // namespace dynamics diff --git a/dart/dynamics/detail/MultiDofJoint.h b/dart/dynamics/detail/MultiDofJoint.h index 874a9d32e1f15..66d7d920c7056 100644 --- a/dart/dynamics/detail/MultiDofJoint.h +++ b/dart/dynamics/detail/MultiDofJoint.h @@ -116,7 +116,7 @@ typename MultiDofJoint::Properties MultiDofJoint::getMultiDofJointProperties() const { return MultiDofJoint::Properties( - mJointP, getMultiDofJointAddon()->getProperties()); + mJointP, getMultiDofJointAspect()->getProperties()); } //============================================================================== @@ -188,7 +188,7 @@ const std::string& MultiDofJoint::setDofName(size_t _index, } preserveDofName(_index, _preserveName); - std::string& dofName = getMultiDofJointAddon()->_getDofNameReference(_index); + std::string& dofName = getMultiDofJointAspect()->_getDofNameReference(_index); if(_name == dofName) return dofName; @@ -215,7 +215,7 @@ void MultiDofJoint::preserveDofName(size_t _index, bool _preserve) return; } - getMultiDofJointAddon()->setPreserveDofName(_index, _preserve); + getMultiDofJointAspect()->setPreserveDofName(_index, _preserve); } //============================================================================== @@ -228,7 +228,7 @@ bool MultiDofJoint::isDofNamePreserved(size_t _index) const _index = 0; } - return getMultiDofJointAddon()->getPreserveDofName(_index); + return getMultiDofJointAspect()->getPreserveDofName(_index); } //============================================================================== @@ -241,10 +241,10 @@ const std::string& MultiDofJoint::getDofName(size_t _index) const << _index << "] in Joint [" << getName() << "], but that is out of " << "bounds (max " << DOF-1 << "). Returning name of DOF 0.\n"; assert(false); - return getMultiDofJointAddon()->getDofName(0); + return getMultiDofJointAspect()->getDofName(0); } - return getMultiDofJointAddon()->getDofName(_index); + return getMultiDofJointAspect()->getDofName(_index); } //============================================================================== @@ -293,8 +293,8 @@ void MultiDofJoint::setCommand(size_t _index, double _command) { case FORCE: mCommands[_index] = math::clip(_command, - getMultiDofJointAddon()->getForceLowerLimit(_index), - getMultiDofJointAddon()->getForceUpperLimit(_index)); + getMultiDofJointAspect()->getForceLowerLimit(_index), + getMultiDofJointAspect()->getForceUpperLimit(_index)); break; case PASSIVE: if(0.0 != _command) @@ -307,18 +307,18 @@ void MultiDofJoint::setCommand(size_t _index, double _command) break; case SERVO: mCommands[_index] = math::clip(_command, - getMultiDofJointAddon()->getVelocityLowerLimit(_index), - getMultiDofJointAddon()->getVelocityUpperLimit(_index)); + getMultiDofJointAspect()->getVelocityLowerLimit(_index), + getMultiDofJointAspect()->getVelocityUpperLimit(_index)); break; case ACCELERATION: mCommands[_index] = math::clip(_command, - getMultiDofJointAddon()->getAccelerationLowerLimit(_index), - getMultiDofJointAddon()->getAccelerationUpperLimit(_index)); + getMultiDofJointAspect()->getAccelerationLowerLimit(_index), + getMultiDofJointAspect()->getAccelerationUpperLimit(_index)); break; case VELOCITY: mCommands[_index] = math::clip(_command, - getMultiDofJointAddon()->getVelocityLowerLimit(_index), - getMultiDofJointAddon()->getVelocityUpperLimit(_index)); + getMultiDofJointAspect()->getVelocityLowerLimit(_index), + getMultiDofJointAspect()->getVelocityUpperLimit(_index)); // TODO: This possibly makes the acceleration to exceed the limits. break; case LOCKED: @@ -363,8 +363,8 @@ void MultiDofJoint::setCommands(const Eigen::VectorXd& _commands) { case FORCE: mCommands = math::clip(_commands, - getMultiDofJointAddon()->getForceLowerLimits(), - getMultiDofJointAddon()->getForceUpperLimits()); + getMultiDofJointAspect()->getForceLowerLimits(), + getMultiDofJointAspect()->getForceUpperLimits()); break; case PASSIVE: if(Vector::Zero() != _commands) @@ -377,18 +377,18 @@ void MultiDofJoint::setCommands(const Eigen::VectorXd& _commands) break; case SERVO: mCommands = math::clip(_commands, - getMultiDofJointAddon()->getVelocityLowerLimits(), - getMultiDofJointAddon()->getVelocityUpperLimits()); + getMultiDofJointAspect()->getVelocityLowerLimits(), + getMultiDofJointAspect()->getVelocityUpperLimits()); break; case ACCELERATION: mCommands = math::clip(_commands, - getMultiDofJointAddon()->getAccelerationLowerLimits(), - getMultiDofJointAddon()->getAccelerationUpperLimits()); + getMultiDofJointAspect()->getAccelerationLowerLimits(), + getMultiDofJointAspect()->getAccelerationUpperLimits()); break; case VELOCITY: mCommands = math::clip(_commands, - getMultiDofJointAddon()->getVelocityLowerLimits(), - getMultiDofJointAddon()->getVelocityUpperLimits()); + getMultiDofJointAspect()->getVelocityLowerLimits(), + getMultiDofJointAspect()->getVelocityUpperLimits()); // TODO: This possibly makes the acceleration to exceed the limits. break; case LOCKED: @@ -481,7 +481,7 @@ void MultiDofJoint::setPositionLowerLimit(size_t _index, double _position) return; } - getMultiDofJointAddon()->setPositionLowerLimit(_index, _position); + getMultiDofJointAspect()->setPositionLowerLimit(_index, _position); } //============================================================================== @@ -494,7 +494,7 @@ double MultiDofJoint::getPositionLowerLimit(size_t _index) const return 0.0; } - return getMultiDofJointAddon()->getPositionLowerLimit(_index); + return getMultiDofJointAspect()->getPositionLowerLimit(_index); } //============================================================================== @@ -507,7 +507,7 @@ void MultiDofJoint::setPositionUpperLimit(size_t _index, double _position) return; } - getMultiDofJointAddon()->setPositionUpperLimit(_index, _position); + getMultiDofJointAspect()->setPositionUpperLimit(_index, _position); } //============================================================================== @@ -520,14 +520,14 @@ void MultiDofJoint::resetPosition(size_t _index) return; } - setPosition(_index, getMultiDofJointAddon()->getInitialPosition(_index)); + setPosition(_index, getMultiDofJointAspect()->getInitialPosition(_index)); } //============================================================================== template void MultiDofJoint::resetPositions() { - setPositionsStatic(getMultiDofJointAddon()->getInitialPositions()); + setPositionsStatic(getMultiDofJointAspect()->getInitialPositions()); } //============================================================================== @@ -540,7 +540,7 @@ void MultiDofJoint::setInitialPosition(size_t _index, double _initial) return; } - getMultiDofJointAddon()->setInitialPosition(_index, _initial); + getMultiDofJointAspect()->setInitialPosition(_index, _initial); } //============================================================================== @@ -553,7 +553,7 @@ double MultiDofJoint::getInitialPosition(size_t _index) const return 0.0; } - return getMultiDofJointAddon()->getInitialPosition(_index); + return getMultiDofJointAspect()->getInitialPosition(_index); } //============================================================================== @@ -566,14 +566,14 @@ void MultiDofJoint::setInitialPositions(const Eigen::VectorXd& _initial) return; } - getMultiDofJointAddon()->setInitialPositions(_initial); + getMultiDofJointAspect()->setInitialPositions(_initial); } //============================================================================== template Eigen::VectorXd MultiDofJoint::getInitialPositions() const { - return getMultiDofJointAddon()->getInitialPositions(); + return getMultiDofJointAspect()->getInitialPositions(); } //============================================================================== @@ -586,7 +586,7 @@ double MultiDofJoint::getPositionUpperLimit(size_t _index) const return 0.0; } - return getMultiDofJointAddon()->getPositionUpperLimit(_index); + return getMultiDofJointAspect()->getPositionUpperLimit(_index); } //============================================================================== @@ -599,8 +599,8 @@ bool MultiDofJoint::hasPositionLimit(size_t _index) const return true; } - return std::isfinite(getMultiDofJointAddon()->getPositionUpperLimit(_index)) - || std::isfinite(getMultiDofJointAddon()->getPositionLowerLimit(_index)); + return std::isfinite(getMultiDofJointAspect()->getPositionUpperLimit(_index)) + || std::isfinite(getMultiDofJointAspect()->getPositionLowerLimit(_index)); } //============================================================================== @@ -676,7 +676,7 @@ void MultiDofJoint::setVelocityLowerLimit(size_t _index, double _velocity) return; } - getMultiDofJointAddon()->setVelocityLowerLimit(_index, _velocity); + getMultiDofJointAspect()->setVelocityLowerLimit(_index, _velocity); } //============================================================================== @@ -689,7 +689,7 @@ double MultiDofJoint::getVelocityLowerLimit(size_t _index) const return 0.0; } - return getMultiDofJointAddon()->getVelocityLowerLimit(_index); + return getMultiDofJointAspect()->getVelocityLowerLimit(_index); } //============================================================================== @@ -702,7 +702,7 @@ void MultiDofJoint::setVelocityUpperLimit(size_t _index, double _velocity) return; } - getMultiDofJointAddon()->setVelocityUpperLimit(_index, _velocity); + getMultiDofJointAspect()->setVelocityUpperLimit(_index, _velocity); } //============================================================================== @@ -715,7 +715,7 @@ double MultiDofJoint::getVelocityUpperLimit(size_t _index) const return 0.0; } - return getMultiDofJointAddon()->getVelocityUpperLimit(_index); + return getMultiDofJointAspect()->getVelocityUpperLimit(_index); } //============================================================================== @@ -728,14 +728,14 @@ void MultiDofJoint::resetVelocity(size_t _index) return; } - setVelocity(_index, getMultiDofJointAddon()->getInitialVelocity(_index)); + setVelocity(_index, getMultiDofJointAspect()->getInitialVelocity(_index)); } //============================================================================== template void MultiDofJoint::resetVelocities() { - setVelocitiesStatic(getMultiDofJointAddon()->getInitialVelocities()); + setVelocitiesStatic(getMultiDofJointAspect()->getInitialVelocities()); } //============================================================================== @@ -748,7 +748,7 @@ void MultiDofJoint::setInitialVelocity(size_t _index, double _initial) return; } - getMultiDofJointAddon()->setInitialVelocity(_index, _initial); + getMultiDofJointAspect()->setInitialVelocity(_index, _initial); } //============================================================================== @@ -761,7 +761,7 @@ double MultiDofJoint::getInitialVelocity(size_t _index) const return 0.0; } - return getMultiDofJointAddon()->getInitialVelocity(_index); + return getMultiDofJointAspect()->getInitialVelocity(_index); } //============================================================================== @@ -774,14 +774,14 @@ void MultiDofJoint::setInitialVelocities(const Eigen::VectorXd& _initial) return; } - getMultiDofJointAddon()->setInitialVelocities(_initial); + getMultiDofJointAspect()->setInitialVelocities(_initial); } //============================================================================== template Eigen::VectorXd MultiDofJoint::getInitialVelocities() const { - return getMultiDofJointAddon()->getInitialVelocities(); + return getMultiDofJointAspect()->getInitialVelocities(); } //============================================================================== @@ -865,7 +865,7 @@ void MultiDofJoint::setAccelerationLowerLimit(size_t _index, return; } - getMultiDofJointAddon()->setAccelerationLowerLimit(_index, _acceleration); + getMultiDofJointAspect()->setAccelerationLowerLimit(_index, _acceleration); } //============================================================================== @@ -878,7 +878,7 @@ double MultiDofJoint::getAccelerationLowerLimit(size_t _index) const return 0.0; } - return getMultiDofJointAddon()->getAccelerationLowerLimit(_index); + return getMultiDofJointAspect()->getAccelerationLowerLimit(_index); } //============================================================================== @@ -892,7 +892,7 @@ void MultiDofJoint::setAccelerationUpperLimit(size_t _index, return; } - getMultiDofJointAddon()->setAccelerationUpperLimit(_index, _acceleration); + getMultiDofJointAspect()->setAccelerationUpperLimit(_index, _acceleration); } //============================================================================== @@ -905,7 +905,7 @@ double MultiDofJoint::getAccelerationUpperLimit(size_t _index) const return 0.0; } - return getMultiDofJointAddon()->getAccelerationUpperLimit(_index); + return getMultiDofJointAspect()->getAccelerationUpperLimit(_index); } //============================================================================== @@ -1046,7 +1046,7 @@ void MultiDofJoint::setForceLowerLimit(size_t _index, double _force) return; } - getMultiDofJointAddon()->setForceLowerLimit(_index, _force); + getMultiDofJointAspect()->setForceLowerLimit(_index, _force); } //============================================================================== @@ -1059,7 +1059,7 @@ double MultiDofJoint::getForceLowerLimit(size_t _index) const return 0.0; } - return getMultiDofJointAddon()->getForceLowerLimit(_index); + return getMultiDofJointAspect()->getForceLowerLimit(_index); } //============================================================================== @@ -1072,7 +1072,7 @@ void MultiDofJoint::setForceUpperLimit(size_t _index, double _force) return; } - getMultiDofJointAddon()->setForceUpperLimit(_index, _force); + getMultiDofJointAspect()->setForceUpperLimit(_index, _force); } //============================================================================== @@ -1085,7 +1085,7 @@ double MultiDofJoint::getForceUpperLimit(size_t _index) const return 0.0; } - return getMultiDofJointAddon()->getForceUpperLimit(_index); + return getMultiDofJointAspect()->getForceUpperLimit(_index); } //============================================================================== @@ -1207,7 +1207,7 @@ void MultiDofJoint::setSpringStiffness(size_t _index, double _k) assert(_k >= 0.0); - getMultiDofJointAddon()->setSpringStiffness(_index, _k); + getMultiDofJointAspect()->setSpringStiffness(_index, _k); } //============================================================================== @@ -1220,7 +1220,7 @@ double MultiDofJoint::getSpringStiffness(size_t _index) const return 0.0; } - return getMultiDofJointAddon()->getSpringStiffness(_index); + return getMultiDofJointAspect()->getSpringStiffness(_index); } //============================================================================== @@ -1233,19 +1233,19 @@ void MultiDofJoint::setRestPosition(size_t _index, double _q0) return; } - if (getMultiDofJointAddon()->getPositionLowerLimit(_index) > _q0 - || getMultiDofJointAddon()->getPositionUpperLimit(_index) < _q0) + if (getMultiDofJointAspect()->getPositionLowerLimit(_index) > _q0 + || getMultiDofJointAspect()->getPositionUpperLimit(_index) < _q0) { dtwarn << "[MultiDofJoint::setRestPosition] Value of _q0 [" << _q0 << "], is out of the limit range [" - << getMultiDofJointAddon()->getPositionLowerLimit(_index) << ", " - << getMultiDofJointAddon()->getPositionUpperLimit(_index) + << getMultiDofJointAspect()->getPositionLowerLimit(_index) << ", " + << getMultiDofJointAspect()->getPositionUpperLimit(_index) << "] for index [" << _index << "] of Joint [" << getName() << "].\n"; return; } - getMultiDofJointAddon()->setRestPosition(_index, _q0); + getMultiDofJointAspect()->setRestPosition(_index, _q0); } //============================================================================== @@ -1258,7 +1258,7 @@ double MultiDofJoint::getRestPosition(size_t _index) const return 0.0; } - return getMultiDofJointAddon()->getRestPosition(_index); + return getMultiDofJointAspect()->getRestPosition(_index); } //============================================================================== @@ -1273,7 +1273,7 @@ void MultiDofJoint::setDampingCoefficient(size_t _index, double _d) assert(_d >= 0.0); - getMultiDofJointAddon()->setDampingCoefficient(_index, _d); + getMultiDofJointAspect()->setDampingCoefficient(_index, _d); } //============================================================================== @@ -1286,7 +1286,7 @@ double MultiDofJoint::getDampingCoefficient(size_t _index) const return 0.0; } - return getMultiDofJointAddon()->getDampingCoefficient(_index); + return getMultiDofJointAspect()->getDampingCoefficient(_index); } //============================================================================== @@ -1301,7 +1301,7 @@ void MultiDofJoint::setCoulombFriction(size_t _index, double _friction) assert(_friction >= 0.0); - getMultiDofJointAddon()->setFriction(_index, _friction); + getMultiDofJointAspect()->setFriction(_index, _friction); } //============================================================================== @@ -1314,7 +1314,7 @@ double MultiDofJoint::getCoulombFriction(size_t _index) const return 0.0; } - return getMultiDofJointAddon()->getFriction(_index); + return getMultiDofJointAspect()->getFriction(_index); } //============================================================================== @@ -1323,9 +1323,9 @@ double MultiDofJoint::getPotentialEnergy() const { // Spring energy Eigen::VectorXd displacement = - getPositionsStatic() - getMultiDofJointAddon()->getRestPositions(); + getPositionsStatic() - getMultiDofJointAspect()->getRestPositions(); double pe = 0.5 * displacement.dot( - getMultiDofJointAddon()->getSpringStiffnesses().asDiagonal() + getMultiDofJointAspect()->getSpringStiffnesses().asDiagonal() * displacement); return pe; @@ -1362,7 +1362,7 @@ MultiDofJoint::MultiDofJoint(const Properties& _properties) mTotalForce(Vector::Zero()), mTotalImpulse(Vector::Zero()) { - createMultiDofJointAddon(_properties); + createMultiDofJointAspect(_properties); for (size_t i = 0; i < DOF; ++i) mDofs[i] = createDofPointer(i); @@ -1375,7 +1375,7 @@ void MultiDofJoint::registerDofs() const SkeletonPtr& skel = mChildBodyNode->getSkeleton(); for (size_t i = 0; i < DOF; ++i) { - getMultiDofJointAddon()->mProperties.mDofNames[i] = + getMultiDofJointAspect()->mProperties.mDofNames[i] = skel->mNameMgrForDofs.issueNewNameAndAdd(mDofs[i]->getName(), mDofs[i]); } } @@ -1719,8 +1719,8 @@ void MultiDofJoint::updateInvProjArtInertiaImplicitDynamic( // Add additional inertia for implicit damping and spring force for (size_t i = 0; i < DOF; ++i) { - projAI(i, i) += _timeStep * getMultiDofJointAddon()->getDampingCoefficient(i) - + _timeStep * _timeStep * getMultiDofJointAddon()->getSpringStiffness(i); + projAI(i, i) += _timeStep * getMultiDofJointAspect()->getDampingCoefficient(i) + + _timeStep * _timeStep * getMultiDofJointAspect()->getSpringStiffness(i); } // Inversion of projected articulated inertia @@ -1937,13 +1937,13 @@ void MultiDofJoint::updateTotalForceDynamic( { // Spring force const Eigen::Matrix springForce - = (-getMultiDofJointAddon()->getSpringStiffnesses()).asDiagonal() - *(getPositionsStatic() - getMultiDofJointAddon()->getRestPositions() + = (-getMultiDofJointAspect()->getSpringStiffnesses()).asDiagonal() + *(getPositionsStatic() - getMultiDofJointAspect()->getRestPositions() + getVelocitiesStatic()*_timeStep); // Damping force const Eigen::Matrix dampingForce - = (-getMultiDofJointAddon()->getDampingCoefficients()).asDiagonal()* + = (-getMultiDofJointAspect()->getDampingCoefficients()).asDiagonal()* getVelocitiesStatic(); // @@ -2118,7 +2118,7 @@ void MultiDofJoint::updateForceID(const Eigen::Vector6d& _bodyForce, if (_withDampingForces) { const Eigen::Matrix dampingForces - = (-getMultiDofJointAddon()->getDampingCoefficients()).asDiagonal() + = (-getMultiDofJointAspect()->getDampingCoefficients()).asDiagonal() *getVelocitiesStatic(); mForces -= dampingForces; } @@ -2127,8 +2127,8 @@ void MultiDofJoint::updateForceID(const Eigen::Vector6d& _bodyForce, if (_withSpringForces) { const Eigen::Matrix springForces - = (-getMultiDofJointAddon()->getSpringStiffnesses()).asDiagonal() - *(getPositionsStatic() - getMultiDofJointAddon()->getRestPositions() + = (-getMultiDofJointAspect()->getSpringStiffnesses()).asDiagonal() + *(getPositionsStatic() - getMultiDofJointAspect()->getRestPositions() + getVelocitiesStatic()*_timeStep); mForces -= springForces; } diff --git a/dart/dynamics/detail/MultiDofJointProperties.h b/dart/dynamics/detail/MultiDofJointProperties.h index daff08dc6309b..92b319e93b753 100644 --- a/dart/dynamics/detail/MultiDofJointProperties.h +++ b/dart/dynamics/detail/MultiDofJointProperties.h @@ -38,7 +38,7 @@ #define DART_DYNAMICS_DETAIL_MULTIDOFJOINTPROPERTIES_H_ #include "dart/dynamics/Joint.h" -#include "dart/common/AddonWithVersion.h" +#include "dart/common/AspectWithVersion.h" namespace dart { namespace dynamics { @@ -156,40 +156,40 @@ struct MultiDofJointProperties : //============================================================================== template -class MultiDofJointAddon final : - public common::AddonWithVersionedProperties< - MultiDofJointAddon, MultiDofJointUniqueProperties, MultiDofJoint, - common::detail::NoOp*> > +class MultiDofJointAspect final : + public common::AspectWithVersionedProperties< + MultiDofJointAspect, MultiDofJointUniqueProperties, MultiDofJoint, + common::detail::NoOp*> > { public: - MultiDofJointAddon(const MultiDofJointAddon&) = delete; + MultiDofJointAspect(const MultiDofJointAspect&) = delete; - MultiDofJointAddon(common::AddonManager* mgr, - const typename MultiDofJointAddon::PropertiesData& properties); + MultiDofJointAspect(common::Composite* mgr, + const typename MultiDofJointAspect::PropertiesData& properties); constexpr static size_t NumDofs = DOF; using Vector = Eigen::Matrix; using BoolArray = std::array; using StringArray = std::array; - DART_COMMON_SET_GET_MULTIDOF_ADDON(double, Vector, PositionLowerLimit) - DART_COMMON_SET_GET_MULTIDOF_ADDON(double, Vector, PositionUpperLimit) - DART_COMMON_SET_GET_MULTIDOF_ADDON(double, Vector, InitialPosition) - DART_COMMON_SET_GET_MULTIDOF_ADDON(double, Vector, VelocityLowerLimit) - DART_COMMON_SET_GET_MULTIDOF_ADDON(double, Vector, VelocityUpperLimit) - DART_COMMON_IRREGULAR_SET_GET_MULTIDOF_ADDON(double, Vector, InitialVelocity, InitialVelocities) - DART_COMMON_SET_GET_MULTIDOF_ADDON(double, Vector, AccelerationLowerLimit) - DART_COMMON_SET_GET_MULTIDOF_ADDON(double, Vector, AccelerationUpperLimit) - DART_COMMON_SET_GET_MULTIDOF_ADDON(double, Vector, ForceLowerLimit) - DART_COMMON_SET_GET_MULTIDOF_ADDON(double, Vector, ForceUpperLimit) - DART_COMMON_IRREGULAR_SET_GET_MULTIDOF_ADDON(double, Vector, SpringStiffness, SpringStiffnesses) - DART_COMMON_SET_GET_MULTIDOF_ADDON(double, Vector, RestPosition) - DART_COMMON_SET_GET_MULTIDOF_ADDON(double, Vector, DampingCoefficient) - DART_COMMON_SET_GET_MULTIDOF_ADDON(double, Vector, Friction) - DART_COMMON_SET_GET_MULTIDOF_ADDON(bool, BoolArray, PreserveDofName) + DART_COMMON_SET_GET_MULTIDOF_ASPECT(double, Vector, PositionLowerLimit) + DART_COMMON_SET_GET_MULTIDOF_ASPECT(double, Vector, PositionUpperLimit) + DART_COMMON_SET_GET_MULTIDOF_ASPECT(double, Vector, InitialPosition) + DART_COMMON_SET_GET_MULTIDOF_ASPECT(double, Vector, VelocityLowerLimit) + DART_COMMON_SET_GET_MULTIDOF_ASPECT(double, Vector, VelocityUpperLimit) + DART_COMMON_IRREGULAR_SET_GET_MULTIDOF_ASPECT(double, Vector, InitialVelocity, InitialVelocities) + DART_COMMON_SET_GET_MULTIDOF_ASPECT(double, Vector, AccelerationLowerLimit) + DART_COMMON_SET_GET_MULTIDOF_ASPECT(double, Vector, AccelerationUpperLimit) + DART_COMMON_SET_GET_MULTIDOF_ASPECT(double, Vector, ForceLowerLimit) + DART_COMMON_SET_GET_MULTIDOF_ASPECT(double, Vector, ForceUpperLimit) + DART_COMMON_IRREGULAR_SET_GET_MULTIDOF_ASPECT(double, Vector, SpringStiffness, SpringStiffnesses) + DART_COMMON_SET_GET_MULTIDOF_ASPECT(double, Vector, RestPosition) + DART_COMMON_SET_GET_MULTIDOF_ASPECT(double, Vector, DampingCoefficient) + DART_COMMON_SET_GET_MULTIDOF_ASPECT(double, Vector, Friction) + DART_COMMON_SET_GET_MULTIDOF_ASPECT(bool, BoolArray, PreserveDofName) const std::string& setDofName(size_t index, const std::string& name, bool preserveName); - DART_COMMON_GET_ADDON_PROPERTY_ARRAY(MultiDofJointAddon, std::string, StringArray, DofName, DofNames, DOF) + DART_COMMON_GET_ASPECT_PROPERTY_ARRAY(MultiDofJointAspect, std::string, StringArray, DofName, DofNames, DOF) friend class MultiDofJoint; @@ -291,19 +291,19 @@ MultiDofJointProperties::MultiDofJointProperties( // See this StackOverflow answer: http://stackoverflow.com/a/14396189/111426 // template -constexpr size_t MultiDofJointAddon::NumDofs; +constexpr size_t MultiDofJointAspect::NumDofs; //============================================================================== template -MultiDofJointAddon::MultiDofJointAddon( - common::AddonManager* mgr, - const typename MultiDofJointAddon::PropertiesData& properties = - typename MultiDofJointAddon::PropertiesData()) - : common::AddonWithVersionedProperties< - typename MultiDofJointAddon::Derived, - typename MultiDofJointAddon::PropertiesData, - typename MultiDofJointAddon::ManagerType, - &common::detail::NoOp::Derived*> >( +MultiDofJointAspect::MultiDofJointAspect( + common::Composite* mgr, + const typename MultiDofJointAspect::PropertiesData& properties = + typename MultiDofJointAspect::PropertiesData()) + : common::AspectWithVersionedProperties< + typename MultiDofJointAspect::Derived, + typename MultiDofJointAspect::PropertiesData, + typename MultiDofJointAspect::ManagerType, + &common::detail::NoOp::Derived*> >( mgr, properties) { // Do nothing @@ -311,7 +311,7 @@ MultiDofJointAddon::MultiDofJointAddon( //============================================================================== template -const std::string& MultiDofJointAddon::setDofName( +const std::string& MultiDofJointAspect::setDofName( size_t index, const std::string& name, bool preserveName) { return this->getManager()->setDofName(index, name, preserveName); @@ -319,7 +319,7 @@ const std::string& MultiDofJointAddon::setDofName( //============================================================================== template -std::string& MultiDofJointAddon::_getDofNameReference(size_t index) +std::string& MultiDofJointAspect::_getDofNameReference(size_t index) { return this->mProperties.mDofNames[index]; } diff --git a/dart/dynamics/detail/PlanarJointProperties.cpp b/dart/dynamics/detail/PlanarJointProperties.cpp index c7b3e80ccdf9e..f97d903e0dac6 100644 --- a/dart/dynamics/detail/PlanarJointProperties.cpp +++ b/dart/dynamics/detail/PlanarJointProperties.cpp @@ -150,28 +150,28 @@ PlanarJointProperties::PlanarJointProperties( } //============================================================================== -void PlanarJointAddon::setXYPlane() +void PlanarJointAspect::setXYPlane() { mProperties.setXYPlane(); notifyPropertiesUpdate(); } //============================================================================== -void PlanarJointAddon::setYZPlane() +void PlanarJointAspect::setYZPlane() { mProperties.setYZPlane(); notifyPropertiesUpdate(); } //============================================================================== -void PlanarJointAddon::setZXPlane() +void PlanarJointAspect::setZXPlane() { mProperties.setZXPlane(); notifyPropertiesUpdate(); } //============================================================================== -void PlanarJointAddon::setArbitraryPlane(const Eigen::Vector3d& _axis1, +void PlanarJointAspect::setArbitraryPlane(const Eigen::Vector3d& _axis1, const Eigen::Vector3d& _axis2) { mProperties.setArbitraryPlane(_axis1, _axis2); diff --git a/dart/dynamics/detail/PlanarJointProperties.h b/dart/dynamics/detail/PlanarJointProperties.h index 1fd44376d9043..abad427fcf814 100644 --- a/dart/dynamics/detail/PlanarJointProperties.h +++ b/dart/dynamics/detail/PlanarJointProperties.h @@ -123,13 +123,13 @@ struct PlanarJointProperties : }; //============================================================================== -class PlanarJointAddon final : - public common::AddonWithVersionedProperties< - PlanarJointAddon, PlanarJointUniqueProperties, PlanarJoint, - detail::JointPropertyUpdate > +class PlanarJointAspect final : + public common::AspectWithVersionedProperties< + PlanarJointAspect, PlanarJointUniqueProperties, PlanarJoint, + detail::JointPropertyUpdate > { public: - DART_COMMON_JOINT_ADDON_CONSTRUCTOR( PlanarJointAddon ) + DART_COMMON_JOINT_ASPECT_CONSTRUCTOR( PlanarJointAspect ) void setXYPlane(); void setYZPlane(); @@ -137,15 +137,15 @@ class PlanarJointAddon final : void setArbitraryPlane(const Eigen::Vector3d& _axis1, const Eigen::Vector3d& _axis2); - DART_COMMON_GET_ADDON_PROPERTY( PlaneType, PlaneType ) - DART_COMMON_GET_ADDON_PROPERTY( Eigen::Vector3d, TransAxis1 ) - DART_COMMON_GET_ADDON_PROPERTY( Eigen::Vector3d, TransAxis2 ) - DART_COMMON_GET_ADDON_PROPERTY( Eigen::Vector3d, RotAxis ) + DART_COMMON_GET_ASPECT_PROPERTY( PlaneType, PlaneType ) + DART_COMMON_GET_ASPECT_PROPERTY( Eigen::Vector3d, TransAxis1 ) + DART_COMMON_GET_ASPECT_PROPERTY( Eigen::Vector3d, TransAxis2 ) + DART_COMMON_GET_ASPECT_PROPERTY( Eigen::Vector3d, RotAxis ) }; //============================================================================== -using PlanarJointBase = common::AddonManagerJoiner< - MultiDofJoint<3>, common::RequiresAddon >; +using PlanarJointBase = common::CompositeJoiner< + MultiDofJoint<3>, common::RequiresAspect >; } // namespace detail } // namespace dynamics diff --git a/dart/dynamics/detail/PrismaticJointProperties.cpp b/dart/dynamics/detail/PrismaticJointProperties.cpp index 69c48ad5bfd26..162d8da26e291 100644 --- a/dart/dynamics/detail/PrismaticJointProperties.cpp +++ b/dart/dynamics/detail/PrismaticJointProperties.cpp @@ -59,14 +59,14 @@ PrismaticJointProperties::PrismaticJointProperties( } //============================================================================== -void PrismaticJointAddon::setAxis(const Eigen::Vector3d& _axis) +void PrismaticJointAspect::setAxis(const Eigen::Vector3d& _axis) { mProperties.mAxis = _axis.normalized(); notifyPropertiesUpdate(); } //============================================================================== -const Eigen::Vector3d& PrismaticJointAddon::getAxis() const +const Eigen::Vector3d& PrismaticJointAspect::getAxis() const { return mProperties.mAxis; } diff --git a/dart/dynamics/detail/PrismaticJointProperties.h b/dart/dynamics/detail/PrismaticJointProperties.h index 52001860ae092..01b20eba67be3 100644 --- a/dart/dynamics/detail/PrismaticJointProperties.h +++ b/dart/dynamics/detail/PrismaticJointProperties.h @@ -76,21 +76,21 @@ struct PrismaticJointProperties : }; //============================================================================== -class PrismaticJointAddon final : - public common::AddonWithVersionedProperties< - PrismaticJointAddon, PrismaticJointUniqueProperties, PrismaticJoint, - detail::JointPropertyUpdate > +class PrismaticJointAspect final : + public common::AspectWithVersionedProperties< + PrismaticJointAspect, PrismaticJointUniqueProperties, PrismaticJoint, + detail::JointPropertyUpdate > { public: - DART_COMMON_JOINT_ADDON_CONSTRUCTOR( PrismaticJointAddon ) + DART_COMMON_JOINT_ASPECT_CONSTRUCTOR( PrismaticJointAspect ) void setAxis(const Eigen::Vector3d& _axis); const Eigen::Vector3d& getAxis() const; }; //============================================================================== -using PrismaticJointBase = common::AddonManagerJoiner< - SingleDofJoint, common::RequiresAddon >; +using PrismaticJointBase = common::CompositeJoiner< + SingleDofJoint, common::RequiresAspect >; } // namespace detail } // namespace dynamics diff --git a/dart/dynamics/detail/RevoluteJointProperties.cpp b/dart/dynamics/detail/RevoluteJointProperties.cpp index 1fba6b7b2a061..90a31ae2b3742 100644 --- a/dart/dynamics/detail/RevoluteJointProperties.cpp +++ b/dart/dynamics/detail/RevoluteJointProperties.cpp @@ -59,14 +59,14 @@ RevoluteJointProperties::RevoluteJointProperties( } //============================================================================== -void RevoluteJointAddon::setAxis(const Eigen::Vector3d& _axis) +void RevoluteJointAspect::setAxis(const Eigen::Vector3d& _axis) { mProperties.mAxis = _axis.normalized(); notifyPropertiesUpdate(); } //============================================================================== -const Eigen::Vector3d& RevoluteJointAddon::getAxis() const +const Eigen::Vector3d& RevoluteJointAspect::getAxis() const { return mProperties.mAxis; } diff --git a/dart/dynamics/detail/RevoluteJointProperties.h b/dart/dynamics/detail/RevoluteJointProperties.h index e660199bc32df..f4d8ba924b9e5 100644 --- a/dart/dynamics/detail/RevoluteJointProperties.h +++ b/dart/dynamics/detail/RevoluteJointProperties.h @@ -76,21 +76,21 @@ struct RevoluteJointProperties : }; //============================================================================== -class RevoluteJointAddon final : - public common::AddonWithVersionedProperties< - RevoluteJointAddon, RevoluteJointUniqueProperties, RevoluteJoint, - detail::JointPropertyUpdate > +class RevoluteJointAspect final : + public common::AspectWithVersionedProperties< + RevoluteJointAspect, RevoluteJointUniqueProperties, RevoluteJoint, + detail::JointPropertyUpdate > { public: - DART_COMMON_JOINT_ADDON_CONSTRUCTOR( RevoluteJointAddon ) + DART_COMMON_JOINT_ASPECT_CONSTRUCTOR( RevoluteJointAspect ) void setAxis(const Eigen::Vector3d& _axis); const Eigen::Vector3d& getAxis() const; }; //============================================================================== -using RevoluteJointBase = common::AddonManagerJoiner< - SingleDofJoint, common::RequiresAddon >; +using RevoluteJointBase = common::CompositeJoiner< + SingleDofJoint, common::RequiresAspect >; } // namespace detail diff --git a/dart/dynamics/detail/ScrewJointProperties.cpp b/dart/dynamics/detail/ScrewJointProperties.cpp index ef01305bf4c7c..c9b422d56bfa2 100644 --- a/dart/dynamics/detail/ScrewJointProperties.cpp +++ b/dart/dynamics/detail/ScrewJointProperties.cpp @@ -60,14 +60,14 @@ ScrewJointProperties::ScrewJointProperties( } //============================================================================== -void ScrewJointAddon::setAxis(const Eigen::Vector3d& _axis) +void ScrewJointAspect::setAxis(const Eigen::Vector3d& _axis) { mProperties.mAxis = _axis.normalized(); notifyPropertiesUpdate(); } //============================================================================== -const Eigen::Vector3d& ScrewJointAddon::getAxis() const +const Eigen::Vector3d& ScrewJointAspect::getAxis() const { return mProperties.mAxis; } diff --git a/dart/dynamics/detail/ScrewJointProperties.h b/dart/dynamics/detail/ScrewJointProperties.h index de011ade87667..460c46fd180f9 100644 --- a/dart/dynamics/detail/ScrewJointProperties.h +++ b/dart/dynamics/detail/ScrewJointProperties.h @@ -80,23 +80,23 @@ struct ScrewJointProperties : SingleDofJoint::Properties, }; //============================================================================== -class ScrewJointAddon final : - public common::AddonWithVersionedProperties< - ScrewJointAddon, ScrewJointUniqueProperties, ScrewJoint, - detail::JointPropertyUpdate > +class ScrewJointAspect final : + public common::AspectWithVersionedProperties< + ScrewJointAspect, ScrewJointUniqueProperties, ScrewJoint, + detail::JointPropertyUpdate > { public: - DART_COMMON_JOINT_ADDON_CONSTRUCTOR( ScrewJointAddon ) + DART_COMMON_JOINT_ASPECT_CONSTRUCTOR( ScrewJointAspect ) void setAxis(const Eigen::Vector3d& _axis); const Eigen::Vector3d& getAxis() const; - DART_COMMON_SET_GET_ADDON_PROPERTY(double, Pitch) + DART_COMMON_SET_GET_ASPECT_PROPERTY(double, Pitch) }; //============================================================================== -using ScrewJointBase = common::AddonManagerJoiner< - SingleDofJoint, common::RequiresAddon >; +using ScrewJointBase = common::CompositeJoiner< + SingleDofJoint, common::RequiresAspect >; } // namespace detail } // namespace dynamics diff --git a/dart/dynamics/detail/SingleDofJointProperties.cpp b/dart/dynamics/detail/SingleDofJointProperties.cpp index 948c3449a62da..c83e5d5e342ca 100644 --- a/dart/dynamics/detail/SingleDofJointProperties.cpp +++ b/dart/dynamics/detail/SingleDofJointProperties.cpp @@ -87,7 +87,7 @@ SingleDofJointProperties::SingleDofJointProperties( } //============================================================================== -const std::string& SingleDofJointAddon::setDofName( +const std::string& SingleDofJointAspect::setDofName( const std::string& name, bool preserveName) { return getManager()->setDofName(0, name, preserveName); diff --git a/dart/dynamics/detail/SingleDofJointProperties.h b/dart/dynamics/detail/SingleDofJointProperties.h index 47ce71ab12bd3..52cc985f2109e 100644 --- a/dart/dynamics/detail/SingleDofJointProperties.h +++ b/dart/dynamics/detail/SingleDofJointProperties.h @@ -37,9 +37,9 @@ #ifndef DART_DYNAMICS_DETAIL_SINGLEDOFJOINTPROPERTIES_H_ #define DART_DYNAMICS_DETAIL_SINGLEDOFJOINTPROPERTIES_H_ -#include "dart/common/RequiresAddon.h" +#include "dart/common/RequiresAspect.h" -#include "dart/common/AddonWithVersion.h" +#include "dart/common/AspectWithVersion.h" #include "dart/dynamics/Joint.h" namespace dart { @@ -136,40 +136,40 @@ struct SingleDofJointProperties : }; //============================================================================== -class SingleDofJointAddon final : - public common::AddonWithVersionedProperties< - SingleDofJointAddon, SingleDofJointUniqueProperties, +class SingleDofJointAspect final : + public common::AspectWithVersionedProperties< + SingleDofJointAspect, SingleDofJointUniqueProperties, SingleDofJoint, common::detail::NoOp> { public: - DART_COMMON_ADDON_PROPERTY_CONSTRUCTOR( SingleDofJointAddon, &common::detail::NoOp ) - - DART_COMMON_SET_GET_ADDON_PROPERTY(double, PositionLowerLimit) - DART_COMMON_SET_GET_ADDON_PROPERTY(double, PositionUpperLimit) - DART_COMMON_SET_GET_ADDON_PROPERTY(double, InitialPosition) - DART_COMMON_SET_GET_ADDON_PROPERTY(double, VelocityLowerLimit) - DART_COMMON_SET_GET_ADDON_PROPERTY(double, VelocityUpperLimit) - DART_COMMON_SET_GET_ADDON_PROPERTY(double, InitialVelocity) - DART_COMMON_SET_GET_ADDON_PROPERTY(double, AccelerationLowerLimit) - DART_COMMON_SET_GET_ADDON_PROPERTY(double, AccelerationUpperLimit) - DART_COMMON_SET_GET_ADDON_PROPERTY(double, ForceLowerLimit) - DART_COMMON_SET_GET_ADDON_PROPERTY(double, ForceUpperLimit) - DART_COMMON_SET_GET_ADDON_PROPERTY(double, SpringStiffness) - DART_COMMON_SET_GET_ADDON_PROPERTY(double, RestPosition) - DART_COMMON_SET_GET_ADDON_PROPERTY(double, DampingCoefficient) - DART_COMMON_SET_GET_ADDON_PROPERTY(double, Friction) - DART_COMMON_SET_GET_ADDON_PROPERTY(bool, PreserveDofName) + DART_COMMON_ASPECT_PROPERTY_CONSTRUCTOR( SingleDofJointAspect, &common::detail::NoOp ) + + DART_COMMON_SET_GET_ASPECT_PROPERTY(double, PositionLowerLimit) + DART_COMMON_SET_GET_ASPECT_PROPERTY(double, PositionUpperLimit) + DART_COMMON_SET_GET_ASPECT_PROPERTY(double, InitialPosition) + DART_COMMON_SET_GET_ASPECT_PROPERTY(double, VelocityLowerLimit) + DART_COMMON_SET_GET_ASPECT_PROPERTY(double, VelocityUpperLimit) + DART_COMMON_SET_GET_ASPECT_PROPERTY(double, InitialVelocity) + DART_COMMON_SET_GET_ASPECT_PROPERTY(double, AccelerationLowerLimit) + DART_COMMON_SET_GET_ASPECT_PROPERTY(double, AccelerationUpperLimit) + DART_COMMON_SET_GET_ASPECT_PROPERTY(double, ForceLowerLimit) + DART_COMMON_SET_GET_ASPECT_PROPERTY(double, ForceUpperLimit) + DART_COMMON_SET_GET_ASPECT_PROPERTY(double, SpringStiffness) + DART_COMMON_SET_GET_ASPECT_PROPERTY(double, RestPosition) + DART_COMMON_SET_GET_ASPECT_PROPERTY(double, DampingCoefficient) + DART_COMMON_SET_GET_ASPECT_PROPERTY(double, Friction) + DART_COMMON_SET_GET_ASPECT_PROPERTY(bool, PreserveDofName) const std::string& setDofName(const std::string& name, bool preserveName = true); - DART_COMMON_GET_ADDON_PROPERTY(std::string, DofName) + DART_COMMON_GET_ASPECT_PROPERTY(std::string, DofName) friend class dart::dynamics::SingleDofJoint; }; //============================================================================== -using SingleDofJointBase = common::AddonManagerJoiner< - Joint, common::RequiresAddon >; +using SingleDofJointBase = common::CompositeJoiner< + Joint, common::RequiresAspect >; } // namespace detail } // namespace dynamics diff --git a/dart/dynamics/detail/SoftBodyNodeProperties.h b/dart/dynamics/detail/SoftBodyNodeProperties.h index 4262339841c51..c577ed8a37912 100644 --- a/dart/dynamics/detail/SoftBodyNodeProperties.h +++ b/dart/dynamics/detail/SoftBodyNodeProperties.h @@ -39,7 +39,7 @@ #include "dart/dynamics/BodyNode.h" #include "dart/dynamics/PointMass.h" -#include "dart/common/RequiresAddon.h" +#include "dart/common/RequiresAspect.h" namespace dart { namespace dynamics { @@ -54,7 +54,7 @@ class SoftMeshShape; namespace detail { //============================================================================== -class SoftBodyAddon; +class SoftBodyAspect; //============================================================================== struct SoftBodyNodeUniqueState @@ -118,46 +118,46 @@ struct SoftBodyNodeProperties }; //============================================================================== -void SoftBodyNodeStateUpdate(SoftBodyAddon* addon); +void SoftBodyNodeStateUpdate(SoftBodyAspect* aspect); //============================================================================== -void SoftBodyNodePropertiesUpdate(SoftBodyAddon* addon); +void SoftBodyNodePropertiesUpdate(SoftBodyAspect* aspect); //============================================================================== -class SoftBodyAddon final : - public common::AddonWithStateAndVersionedProperties< - SoftBodyAddon, SoftBodyNodeUniqueState, SoftBodyNodeUniqueProperties, +class SoftBodyAspect final : + public common::AspectWithStateAndVersionedProperties< + SoftBodyAspect, SoftBodyNodeUniqueState, SoftBodyNodeUniqueProperties, SoftBodyNode, &SoftBodyNodeStateUpdate, &SoftBodyNodePropertiesUpdate > { public: friend class dart::dynamics::SoftBodyNode; - DART_COMMON_ADDON_STATE_PROPERTY_CONSTRUCTORS(SoftBodyAddon) + DART_COMMON_ASPECT_STATE_PROPERTY_CONSTRUCTORS(SoftBodyAspect) - DART_COMMON_SET_GET_ADDON_PROPERTY(double, Kv) - DART_COMMON_SET_GET_ADDON_PROPERTY(double, Ke) - DART_COMMON_SET_GET_ADDON_PROPERTY(double, DampCoeff) + DART_COMMON_SET_GET_ASPECT_PROPERTY(double, Kv) + DART_COMMON_SET_GET_ASPECT_PROPERTY(double, Ke) + DART_COMMON_SET_GET_ASPECT_PROPERTY(double, DampCoeff) protected: - /// Get a direct reference to the State of this Addon + /// Get a direct reference to the State of this Aspect inline StateData& _getState() { return mState; } - /// Get a direct reference to the State of this Addon + /// Get a direct reference to the State of this Aspect inline const StateData& _getState() const { return mState; } - /// Get a direct reference to the Properties of this Addon + /// Get a direct reference to the Properties of this Aspect inline PropertiesData& _getProperties() { return mProperties; } - /// Get a direct const reference to the Properties of this Addon + /// Get a direct const reference to the Properties of this Aspect inline const PropertiesData& _getProperties() const { return mProperties; } }; //============================================================================== -using SoftBodyNodeBase = common::AddonManagerJoiner< - BodyNode, common::RequiresAddon >; +using SoftBodyNodeBase = common::CompositeJoiner< + BodyNode, common::RequiresAspect >; } // namespace detail } // namespace dynamics diff --git a/dart/dynamics/detail/SpecializedNodeManager.h b/dart/dynamics/detail/SpecializedNodeManager.h index 0e89d4b989280..3445e9d7dd2e4 100644 --- a/dart/dynamics/detail/SpecializedNodeManager.h +++ b/dart/dynamics/detail/SpecializedNodeManager.h @@ -44,7 +44,7 @@ namespace dynamics { // This preprocessor token should only be used by the unittest that is // responsible for checking that the specialized routines are being used to -// access specialized Addons +// access specialized Aspects #ifdef DART_UNITTEST_SPECIALIZED_NODE_ACCESS bool usedSpecializedNodeAccess; #endif // DART_UNITTEST_SPECIALIZED_NODE_ACCESS diff --git a/dart/dynamics/detail/UniversalJointProperties.cpp b/dart/dynamics/detail/UniversalJointProperties.cpp index dbaf5a96cd548..a80698efba582 100644 --- a/dart/dynamics/detail/UniversalJointProperties.cpp +++ b/dart/dynamics/detail/UniversalJointProperties.cpp @@ -59,27 +59,27 @@ UniversalJointProperties::UniversalJointProperties( } //============================================================================== -void UniversalJointAddon::setAxis1(const Eigen::Vector3d& _axis) +void UniversalJointAspect::setAxis1(const Eigen::Vector3d& _axis) { mProperties.mAxis[0] = _axis.normalized(); notifyPropertiesUpdate(); } //============================================================================== -const Eigen::Vector3d& UniversalJointAddon::getAxis1() const +const Eigen::Vector3d& UniversalJointAspect::getAxis1() const { return mProperties.mAxis[0]; } //============================================================================== -void UniversalJointAddon::setAxis2(const Eigen::Vector3d& _axis) +void UniversalJointAspect::setAxis2(const Eigen::Vector3d& _axis) { mProperties.mAxis[1] = _axis.normalized(); notifyPropertiesUpdate(); } //============================================================================== -const Eigen::Vector3d& UniversalJointAddon::getAxis2() const +const Eigen::Vector3d& UniversalJointAspect::getAxis2() const { return mProperties.mAxis[1]; } diff --git a/dart/dynamics/detail/UniversalJointProperties.h b/dart/dynamics/detail/UniversalJointProperties.h index 969af0260183f..25b300646eb3f 100644 --- a/dart/dynamics/detail/UniversalJointProperties.h +++ b/dart/dynamics/detail/UniversalJointProperties.h @@ -77,13 +77,13 @@ struct UniversalJointProperties : }; //============================================================================== -class UniversalJointAddon final : - public common::AddonWithVersionedProperties< - UniversalJointAddon, UniversalJointUniqueProperties, UniversalJoint, - detail::JointPropertyUpdate > +class UniversalJointAspect final : + public common::AspectWithVersionedProperties< + UniversalJointAspect, UniversalJointUniqueProperties, UniversalJoint, + detail::JointPropertyUpdate > { public: - DART_COMMON_JOINT_ADDON_CONSTRUCTOR( UniversalJointAddon ) + DART_COMMON_JOINT_ASPECT_CONSTRUCTOR( UniversalJointAspect ) void setAxis1(const Eigen::Vector3d& _axis); const Eigen::Vector3d& getAxis1() const; void setAxis2(const Eigen::Vector3d& _axis); @@ -91,8 +91,8 @@ class UniversalJointAddon final : }; //============================================================================== -using UniversalJointBase = common::AddonManagerJoiner< - MultiDofJoint<2>, common::RequiresAddon >; +using UniversalJointBase = common::CompositeJoiner< + MultiDofJoint<2>, common::RequiresAspect >; } // namespace detail } // namespace dynamics diff --git a/dart/renderer/OpenGLRenderInterface.cpp b/dart/renderer/OpenGLRenderInterface.cpp index f2e1e5ba9abd7..b3bd89bc737c5 100644 --- a/dart/renderer/OpenGLRenderInterface.cpp +++ b/dart/renderer/OpenGLRenderInterface.cpp @@ -508,18 +508,18 @@ void OpenGLRenderInterface::draw(dynamics::BodyNode* node, if (!shapeFrame) continue; - if (shapeFrame->hasVisualAddon()) + if (shapeFrame->hasVisualAspect()) draw(shapeFrame->getShape().get()); - else if (colMesh && shapeFrame->hasCollisionAddon()) + else if (colMesh && shapeFrame->hasCollisionAspect()) draw(shapeFrame->getShape().get()); } for (auto i = 0u; i < node->getNumNodes(); ++i) { auto shapeNode = node->getNode(i); - if (shapeNode->hasVisualAddon()) + if (shapeNode->hasVisualAspect()) draw(shapeNode->getShape().get()); - else if (colMesh && shapeNode->hasCollisionAddon()) + else if (colMesh && shapeNode->hasCollisionAspect()) draw(shapeNode->getShape().get()); } @@ -612,11 +612,11 @@ void OpenGLRenderInterface::draw(dynamics::ShapeFrame* shapeFrame) auto shape = shapeFrame->getShape().get(); Eigen::Isometry3d pose = shapeFrame->getRelativeTransform(); - auto visualAddon = shapeFrame->get(); + auto visualAspect = shapeFrame->get(); Eigen::Vector3d color = Eigen::Vector3d::Random(); - if (visualAddon) - color = visualAddon->getColor(); + if (visualAspect) + color = visualAspect->getColor(); glPushMatrix(); diff --git a/dart/utils/SkelParser.cpp b/dart/utils/SkelParser.cpp index aefe1abd4f62c..ce828cbab7a73 100644 --- a/dart/utils/SkelParser.cpp +++ b/dart/utils/SkelParser.cpp @@ -261,7 +261,7 @@ void readCollisionShapeNode( const common::Uri& baseUri, const common::ResourceRetrieverPtr& retriever); -void readAddons( +void readAspects( const dynamics::SkeletonPtr& skeleton, tinyxml2::XMLElement* skeletonElement, const common::Uri& baseUri, @@ -562,13 +562,13 @@ void readVisualizationShapeNode( bodyNode->getName() + " - visual shape", baseUri, retriever); - auto visualAddon = newShapeNode->getVisualAddon(true); + auto visualAspect = newShapeNode->getVisualAspect(true); // color if (hasElement(vizShapeNodeEle, "color")) { Eigen::Vector3d color = getValueVector3d(vizShapeNodeEle, "color"); - visualAddon->setColor(color); + visualAspect->setColor(color); } } @@ -584,19 +584,19 @@ void readCollisionShapeNode( bodyNode->getName() + " - collision shape", baseUri, retriever); - auto collisionAddon = newShapeNode->getCollisionAddon(true); - newShapeNode->createDynamicsAddon(); + auto collisionAspect = newShapeNode->getCollisionAspect(true); + newShapeNode->createDynamicsAspect(); // collidable if (hasElement(collShapeNodeEle, "collidable")) { const bool collidable = getValueDouble(collShapeNodeEle, "collidable"); - collisionAddon->setCollidable(collidable); + collisionAspect->setCollidable(collidable); } } //============================================================================== -void readAddons( +void readAspects( const dynamics::SkeletonPtr& skeleton, tinyxml2::XMLElement* skeletonElement, const common::Uri& baseUri, @@ -1010,9 +1010,9 @@ dynamics::SkeletonPtr readSkeleton( it = joints.find(nextJoint->second); } - // Read addons here since addons cannot be added if the BodyNodes haven't + // Read aspects here since aspects cannot be added if the BodyNodes haven't // created yet. - readAddons(newSkeleton, _skeletonElement, _baseUri, _retriever); + readAspects(newSkeleton, _skeletonElement, _baseUri, _retriever); newSkeleton->resetPositions(); newSkeleton->resetVelocities(); diff --git a/dart/utils/VskParser.cpp b/dart/utils/VskParser.cpp index cc52a77ae008d..7f3f3535e46a8 100644 --- a/dart/utils/VskParser.cpp +++ b/dart/utils/VskParser.cpp @@ -527,14 +527,14 @@ bool readShape(const tinyxml2::XMLElement* shapeEle, } dynamics::ShapeNode* node = parent->createShapeNodeWith< - dynamics::VisualAddon, - dynamics::CollisionAddon, - dynamics::DynamicsAddon>( + dynamics::VisualAspect, + dynamics::CollisionAspect, + dynamics::DynamicsAspect>( shape, parent->getName()+"_shape_"+std::to_string(parent->getNumShapeNodes())); node->setRelativeTransform(localTransform); - node->getVisualAddon()->setColor(vskData.bodyNodeColorMap[parent]); + node->getVisualAspect()->setColor(vskData.bodyNodeColorMap[parent]); return true; } @@ -915,15 +915,15 @@ void generateShapes(const dynamics::SkeletonPtr& skel, VskData& vskData) localTransform.translation() = 0.5 * tf.translation(); dynamics::ShapeNode* shapeNode = parent->createShapeNodeWith< - dynamics::VisualAddon, - dynamics::CollisionAddon, - dynamics::DynamicsAddon>( + dynamics::VisualAspect, + dynamics::CollisionAspect, + dynamics::DynamicsAspect>( dynamics::ShapePtr(new dynamics::EllipsoidShape(size)), parent->getName()+"_shape_"+ std::to_string(parent->getNumShapeNodes())); shapeNode->setRelativeTransform(localTransform); - shapeNode->getVisualAddon()->setColor(vskData.bodyNodeColorMap[parent]); + shapeNode->getVisualAspect()->setColor(vskData.bodyNodeColorMap[parent]); } // Remove redundant leaf body nodes with no shape diff --git a/dart/utils/sdf/SdfParser.cpp b/dart/utils/sdf/SdfParser.cpp index 6380cdb69db3f..4b981a37d2f50 100644 --- a/dart/utils/sdf/SdfParser.cpp +++ b/dart/utils/sdf/SdfParser.cpp @@ -186,7 +186,7 @@ void readCollisionShapeNode( const std::string& skelPath, const common::ResourceRetrieverPtr& retriever); -void readAddons( +void readAspects( const dynamics::SkeletonPtr& skeleton, tinyxml2::XMLElement* skeletonElement, const std::string& skelPath, @@ -497,9 +497,9 @@ dynamics::SkeletonPtr readSkeleton( body = sdfBodyNodes.begin(); } - // Read addons here since addons cannot be added if the BodyNodes haven't + // Read aspects here since aspects cannot be added if the BodyNodes haven't // created yet. - readAddons(newSkeleton, skeletonElement, skelPath, retriever); + readAspects(newSkeleton, skeletonElement, skelPath, retriever); // Set positions to their initial values newSkeleton->resetPositions(); @@ -997,7 +997,7 @@ void readVisualizationShapeNode( bodyNode->getName() + " - visual shape", skelPath, retriever); - newShapeNode->createVisualAddon(); + newShapeNode->createVisualAspect(); } //============================================================================== @@ -1012,11 +1012,11 @@ void readCollisionShapeNode( bodyNode->getName() + " - collision shape", skelPath, retriever); - newShapeNode->createCollisionAddon(); + newShapeNode->createCollisionAspect(); } //============================================================================== -void readAddons( +void readAspects( const dynamics::SkeletonPtr& skeleton, tinyxml2::XMLElement* skeletonElement, const std::string& skelPath, diff --git a/dart/utils/urdf/DartLoader.cpp b/dart/utils/urdf/DartLoader.cpp index 944137fe60ae7..bb24e2eb2e606 100644 --- a/dart/utils/urdf/DartLoader.cpp +++ b/dart/utils/urdf/DartLoader.cpp @@ -424,11 +424,11 @@ bool DartLoader::createDartNodeProperties( return true; } -void setMaterial(dynamics::VisualAddon* visualAddon, const urdf::Visual* viz) +void setMaterial(dynamics::VisualAspect* visualAspect, const urdf::Visual* viz) { if(viz->material) { - visualAddon->setColor(Eigen::Vector3d(viz->material->color.r, + visualAspect->setColor(Eigen::Vector3d(viz->material->color.r, viz->material->color.g, viz->material->color.b)); } @@ -455,9 +455,9 @@ bool DartLoader::createShapeNodes( if(shape) { - auto shapeNode = bodyNode->createShapeNodeWith(shape); + auto shapeNode = bodyNode->createShapeNodeWith(shape); shapeNode->setRelativeTransform(toEigen(visual->origin)); - setMaterial(shapeNode->getVisualAddon(), visual.get()); + setMaterial(shapeNode->getVisualAspect(), visual.get()); } else { @@ -474,7 +474,7 @@ bool DartLoader::createShapeNodes( if (shape) { auto shapeNode = bodyNode->createShapeNodeWith< - dynamics::CollisionAddon, dynamics::DynamicsAddon>(shape); + dynamics::CollisionAspect, dynamics::DynamicsAspect>(shape); shapeNode->setRelativeTransform(toEigen(collision->origin)); } else diff --git a/osgDart/DragAndDrop.cpp b/osgDart/DragAndDrop.cpp index 97b770465c624..1cc5975c66bfc 100644 --- a/osgDart/DragAndDrop.cpp +++ b/osgDart/DragAndDrop.cpp @@ -628,7 +628,7 @@ void InteractiveFrameDnD::update() { const auto shapeFrames = tool->getShapeFrames(); for(size_t s=0; sgetVisualAddon(true)->setHidden(true); + shapeFrames[s]->getVisualAspect(true)->setHidden(true); } } } @@ -645,7 +645,7 @@ void InteractiveFrameDnD::update() { const auto shapeFrames = tool->getShapeFrames(); for(size_t s=0; sgetVisualAddon(true)->setHidden(false); + shapeFrames[s]->getVisualAspect(true)->setHidden(false); } } } diff --git a/osgDart/InteractiveFrame.cpp b/osgDart/InteractiveFrame.cpp index 35fbddb87a620..3e59432c2aa88 100644 --- a/osgDart/InteractiveFrame.cpp +++ b/osgDart/InteractiveFrame.cpp @@ -60,7 +60,7 @@ void InteractiveTool::setEnabled(bool enabled) { mEnabled = enabled; for(auto& frame : mSimpleFrames) - frame->getVisualAddon(true)->setHidden(!enabled); + frame->getVisualAspect(true)->setHidden(!enabled); } //============================================================================== @@ -73,7 +73,7 @@ bool InteractiveTool::getEnabled() const void InteractiveTool::setAlpha(double alpha) { for(auto& frame : mSimpleFrames) - frame->getVisualAddon(true)->setAlpha(alpha); + frame->getVisualAspect(true)->setAlpha(alpha); } //============================================================================== @@ -117,7 +117,7 @@ dart::dynamics::SimpleFrame* InteractiveTool::addShapeFrame( auto shapeFrame = mSimpleFrames.back().get(); shapeFrame->setShape(shape); - shapeFrame->createVisualAddon(); + shapeFrame->createVisualAspect(); return shapeFrame; } @@ -231,7 +231,7 @@ dart::dynamics::SimpleFrame* InteractiveFrame::addShapeFrame( auto shapeFrame = mSimpleFrames.back().get(); shapeFrame->setShape(shape); - shapeFrame->createVisualAddon(); + shapeFrame->createVisualAspect(); return shapeFrame; } @@ -580,7 +580,7 @@ void InteractiveFrame::createStandardVisualizationShapes(double size, Eigen::Vector3d c(Eigen::Vector3d::Zero()); c[i] = 1.0; auto shapeFrame = addShapeFrame(line); - shapeFrame->getVisualAddon(true)->setColor(c); + shapeFrame->getVisualAspect(true)->setColor(c); } } diff --git a/osgDart/SupportPolygonVisual.cpp b/osgDart/SupportPolygonVisual.cpp index a1cb60875687c..4f18b3bc18688 100644 --- a/osgDart/SupportPolygonVisual.cpp +++ b/osgDart/SupportPolygonVisual.cpp @@ -329,9 +329,9 @@ void SupportPolygonVisual::refresh() mCom->setTransform(tf); if(dart::math::isInsideSupportPolygon(Cproj, poly)) - mCom->getVisualAddon(true)->setColor(mValidColor); + mCom->getVisualAspect(true)->setColor(mValidColor); else - mCom->getVisualAddon(true)->setColor(mInvalidColor); + mCom->getVisualAspect(true)->setColor(mInvalidColor); mComNode->refresh(); @@ -376,7 +376,7 @@ void SupportPolygonVisual::initialize() std::make_shared( mCentroidRadius/2.0*Eigen::Vector3d::Ones())); - mCentroid->getVisualAddon(true)->setColor( + mCentroid->getVisualAspect(true)->setColor( Eigen::Vector4d(color[0], color[1], color[2], color[3])); mCentroidNode = new ShapeFrameNode(mCentroid.get(), nullptr); diff --git a/osgDart/examples/osgAtlasPuppet.cpp b/osgDart/examples/osgAtlasPuppet.cpp index c86298bee9e10..ce4e2a697a284 100644 --- a/osgDart/examples/osgAtlasPuppet.cpp +++ b/osgDart/examples/osgAtlasPuppet.cpp @@ -505,8 +505,8 @@ SkeletonPtr createGround() std::make_shared(Eigen::Vector3d(10,10,thickness)); auto shapeNode = ground->getBodyNode(0)->createShapeNodeWith< - VisualAddon, CollisionAddon, DynamicsAddon>(groundShape); - shapeNode->getVisualAddon()->setColor(dart::Color::Blue(0.2)); + VisualAspect, CollisionAspect, DynamicsAspect>(groundShape); + shapeNode->getVisualAspect()->setColor(dart::Color::Blue(0.2)); return ground; } @@ -527,8 +527,8 @@ SkeletonPtr createAtlas() tf.translation() = Eigen::Vector3d(0.1*Eigen::Vector3d(0.0, 0.0, 1.0)); auto shapeNode = - atlas->getBodyNode(0)->createShapeNodeWith(boxShape); - shapeNode->getVisualAddon()->setColor(dart::Color::Black()); + atlas->getBodyNode(0)->createShapeNodeWith(boxShape); + shapeNode->getVisualAspect()->setColor(dart::Color::Black()); shapeNode->setRelativeTransform(tf); return atlas; diff --git a/osgDart/examples/osgDragAndDrop.cpp b/osgDart/examples/osgDragAndDrop.cpp index d001620fa005e..ec1b9e4f4d732 100644 --- a/osgDart/examples/osgDragAndDrop.cpp +++ b/osgDart/examples/osgDragAndDrop.cpp @@ -61,7 +61,7 @@ int main() std::shared_ptr x_shape( new BoxShape(Eigen::Vector3d(0.2, 0.2, 0.2))); x_marker->setShape(x_shape); - x_marker->getVisualAddon(true)->setColor(Eigen::Vector3d(0.9, 0.0, 0.0)); + x_marker->getVisualAspect(true)->setColor(Eigen::Vector3d(0.9, 0.0, 0.0)); world->addSimpleFrame(x_marker); tf.translation() = Eigen::Vector3d(0.0, 8.0, 0.0); @@ -69,7 +69,7 @@ int main() std::shared_ptr y_shape( new BoxShape(Eigen::Vector3d(0.2, 0.2, 0.2))); y_marker->setShape(y_shape); - y_marker->getVisualAddon(true)->setColor(Eigen::Vector3d(0.0, 0.9, 0.0)); + y_marker->getVisualAspect(true)->setColor(Eigen::Vector3d(0.0, 0.9, 0.0)); world->addSimpleFrame(y_marker); tf.translation() = Eigen::Vector3d(0.0, 0.0, 8.0); @@ -77,7 +77,7 @@ int main() std::shared_ptr z_shape( new BoxShape(Eigen::Vector3d(0.2, 0.2, 0.2))); z_marker->setShape(z_shape); - z_marker->getVisualAddon(true)->setColor(Eigen::Vector3d(0.0, 0.0, 0.9)); + z_marker->getVisualAspect(true)->setColor(Eigen::Vector3d(0.0, 0.0, 0.9)); world->addSimpleFrame(z_marker); diff --git a/osgDart/examples/osgHuboPuppet.cpp b/osgDart/examples/osgHuboPuppet.cpp index be4bc5626208d..e5306abe06ab5 100644 --- a/osgDart/examples/osgHuboPuppet.cpp +++ b/osgDart/examples/osgHuboPuppet.cpp @@ -1089,8 +1089,8 @@ SkeletonPtr createGround() std::make_shared(Eigen::Vector3d(10,10,thickness)); auto shapeNode = ground->getBodyNode(0)->createShapeNodeWith< - VisualAddon, CollisionAddon, DynamicsAddon>(groundShape); - shapeNode->getVisualAddon()->setColor(dart::Color::Blue(0.2)); + VisualAspect, CollisionAspect, DynamicsAspect>(groundShape); + shapeNode->getVisualAspect()->setColor(dart::Color::Blue(0.2)); return ground; } diff --git a/osgDart/examples/osgOperationalSpaceControl.cpp b/osgDart/examples/osgOperationalSpaceControl.cpp index 4c711e69f8c0e..59f5be167c3ac 100644 --- a/osgDart/examples/osgOperationalSpaceControl.cpp +++ b/osgDart/examples/osgOperationalSpaceControl.cpp @@ -79,7 +79,7 @@ class OperationalSpaceControlWorld : public osgDart::WorldNode mTarget = std::make_shared(Frame::World(), "target", tf); ShapePtr ball(new EllipsoidShape(Eigen::Vector3d(0.05,0.05,0.05))); mTarget->setShape(ball); - mTarget->getVisualAddon(true)->setColor(Eigen::Vector3d(0.9,0,0)); + mTarget->getVisualAspect(true)->setColor(Eigen::Vector3d(0.9,0,0)); mWorld->addSimpleFrame(mTarget); mOffset = mEndEffector->getWorldTransform().rotation().transpose() * mOffset; @@ -266,7 +266,7 @@ int main() for(size_t i=0; igetNumBodyNodes(); ++i) { BodyNode* bn = robot->getBodyNode(i); - auto shapeNodes = bn->getShapeNodesWith(); + auto shapeNodes = bn->getShapeNodesWith(); for(auto shapeNode : shapeNodes) { std::shared_ptr mesh = diff --git a/osgDart/examples/osgSoftBodies.cpp b/osgDart/examples/osgSoftBodies.cpp index d39d38cc6196a..44ee31891d029 100644 --- a/osgDart/examples/osgSoftBodies.cpp +++ b/osgDart/examples/osgSoftBodies.cpp @@ -64,12 +64,12 @@ class RecordingWorld : public osgDart::WorldNode const SkeletonPtr& skeleton = mWorld->getSkeleton(i); State state; state.mConfig = skeleton->getConfiguration(); - state.mAddonStates.reserve(skeleton->getNumBodyNodes()); + state.mAspectStates.reserve(skeleton->getNumBodyNodes()); for(size_t j=0; j < skeleton->getNumBodyNodes(); ++j) { BodyNode* bn = skeleton->getBodyNode(j); - state.mAddonStates.push_back(bn->getAddonStates()); + state.mAspectStates.push_back(bn->getAspectStates()); } slice.push_back(state); @@ -110,7 +110,7 @@ class RecordingWorld : public osgDart::WorldNode for(size_t j=0; j < skeleton->getNumBodyNodes(); ++j) { BodyNode* bn = skeleton->getBodyNode(j); - bn->setAddonStates(state.mAddonStates[j]); + bn->setAspectStates(state.mAspectStates[j]); } } @@ -141,7 +141,7 @@ class RecordingWorld : public osgDart::WorldNode struct State { Skeleton::Configuration mConfig; - std::vector mAddonStates; + std::vector mAspectStates; }; using TimeSlice = std::vector; diff --git a/osgDart/render/BoxShapeNode.cpp b/osgDart/render/BoxShapeNode.cpp index 7e2d9084199d7..1547be94cce16 100644 --- a/osgDart/render/BoxShapeNode.cpp +++ b/osgDart/render/BoxShapeNode.cpp @@ -71,7 +71,7 @@ class BoxShapeDrawable : public osg::ShapeDrawable public: BoxShapeDrawable(dart::dynamics::BoxShape* shape, - dart::dynamics::VisualAddon* visualAddon); + dart::dynamics::VisualAspect* visualAspect); void refresh(bool firstTime); @@ -80,7 +80,7 @@ class BoxShapeDrawable : public osg::ShapeDrawable virtual ~BoxShapeDrawable(); dart::dynamics::BoxShape* mBoxShape; - dart::dynamics::VisualAddon* mVisualAddon; + dart::dynamics::VisualAspect* mVisualAspect; }; @@ -92,7 +92,7 @@ BoxShapeNode::BoxShapeNode(std::shared_ptr shape, mGeode(nullptr) { extractData(true); - setNodeMask(mVisualAddon->isHidden()? 0x0 : ~0x0); + setNodeMask(mVisualAspect->isHidden()? 0x0 : ~0x0); } //============================================================================== @@ -100,7 +100,7 @@ void BoxShapeNode::refresh() { mUtilized = true; - setNodeMask(mVisualAddon->isHidden()? 0x0 : ~0x0); + setNodeMask(mVisualAspect->isHidden()? 0x0 : ~0x0); if(mShape->getDataVariance() == dart::dynamics::Shape::STATIC) return; @@ -152,7 +152,7 @@ void BoxShapeGeode::extractData() { if(nullptr == mDrawable) { - mDrawable = new BoxShapeDrawable(mBoxShape.get(), mVisualAddon); + mDrawable = new BoxShapeDrawable(mBoxShape.get(), mVisualAspect); addDrawable(mDrawable); return; } @@ -168,9 +168,9 @@ BoxShapeGeode::~BoxShapeGeode() //============================================================================== BoxShapeDrawable::BoxShapeDrawable(dart::dynamics::BoxShape* shape, - dart::dynamics::VisualAddon* visualAddon) + dart::dynamics::VisualAspect* visualAspect) : mBoxShape(shape), - mVisualAddon(visualAddon) + mVisualAspect(visualAspect) { refresh(true); } @@ -196,7 +196,7 @@ void BoxShapeDrawable::refresh(bool firstTime) if(mBoxShape->checkDataVariance(dart::dynamics::Shape::DYNAMIC_COLOR) || firstTime) { - setColor(eigToOsgVec4(mVisualAddon->getRGBA())); + setColor(eigToOsgVec4(mVisualAspect->getRGBA())); } } diff --git a/osgDart/render/CylinderShapeNode.cpp b/osgDart/render/CylinderShapeNode.cpp index 526d487c068fc..f0607e8fd0075 100644 --- a/osgDart/render/CylinderShapeNode.cpp +++ b/osgDart/render/CylinderShapeNode.cpp @@ -72,7 +72,7 @@ class CylinderShapeDrawable : public osg::ShapeDrawable public: CylinderShapeDrawable(dart::dynamics::CylinderShape* shape, - dart::dynamics::VisualAddon* visualAddon, + dart::dynamics::VisualAspect* visualAspect, CylinderShapeGeode* parent); void refresh(bool firstTime); @@ -82,7 +82,7 @@ class CylinderShapeDrawable : public osg::ShapeDrawable virtual ~CylinderShapeDrawable(); dart::dynamics::CylinderShape* mCylinderShape; - dart::dynamics::VisualAddon* mVisualAddon; + dart::dynamics::VisualAspect* mVisualAspect; CylinderShapeGeode* mParent; @@ -97,7 +97,7 @@ CylinderShapeNode::CylinderShapeNode( mGeode(nullptr) { extractData(true); - setNodeMask(mVisualAddon->isHidden()? 0x0 : ~0x0); + setNodeMask(mVisualAspect->isHidden()? 0x0 : ~0x0); } //============================================================================== @@ -105,7 +105,7 @@ void CylinderShapeNode::refresh() { mUtilized = true; - setNodeMask(mVisualAddon->isHidden()? 0x0 : ~0x0); + setNodeMask(mVisualAspect->isHidden()? 0x0 : ~0x0); if(mShape->getDataVariance() == dart::dynamics::Shape::STATIC) return; @@ -158,7 +158,7 @@ void CylinderShapeGeode::extractData() { if(nullptr == mDrawable) { - mDrawable = new CylinderShapeDrawable(mCylinderShape, mVisualAddon, this); + mDrawable = new CylinderShapeDrawable(mCylinderShape, mVisualAspect, this); addDrawable(mDrawable); return; } @@ -175,10 +175,10 @@ CylinderShapeGeode::~CylinderShapeGeode() //============================================================================== CylinderShapeDrawable::CylinderShapeDrawable( dart::dynamics::CylinderShape* shape, - dart::dynamics::VisualAddon* visualAddon, + dart::dynamics::VisualAspect* visualAspect, CylinderShapeGeode* parent) : mCylinderShape(shape), - mVisualAddon(visualAddon), + mVisualAspect(visualAspect), mParent(parent) { refresh(true); @@ -206,7 +206,7 @@ void CylinderShapeDrawable::refresh(bool firstTime) if(mCylinderShape->checkDataVariance(dart::dynamics::Shape::DYNAMIC_COLOR) || firstTime) { - setColor(eigToOsgVec4(mVisualAddon->getRGBA())); + setColor(eigToOsgVec4(mVisualAspect->getRGBA())); } } diff --git a/osgDart/render/EllipsoidShapeNode.cpp b/osgDart/render/EllipsoidShapeNode.cpp index 9ca4630446ee1..9ced6e5d38dee 100644 --- a/osgDart/render/EllipsoidShapeNode.cpp +++ b/osgDart/render/EllipsoidShapeNode.cpp @@ -75,7 +75,7 @@ class EllipsoidShapeDrawable : public osg::ShapeDrawable public: EllipsoidShapeDrawable(dart::dynamics::EllipsoidShape* shape, - dart::dynamics::VisualAddon* visualAddon, + dart::dynamics::VisualAspect* visualAspect, EllipsoidShapeGeode* parent); void refresh(bool firstTime); @@ -85,7 +85,7 @@ class EllipsoidShapeDrawable : public osg::ShapeDrawable virtual ~EllipsoidShapeDrawable(); dart::dynamics::EllipsoidShape* mEllipsoidShape; - dart::dynamics::VisualAddon* mVisualAddon; + dart::dynamics::VisualAspect* mVisualAspect; EllipsoidShapeGeode* mParent; }; @@ -99,7 +99,7 @@ EllipsoidShapeNode::EllipsoidShapeNode( mGeode(nullptr) { extractData(true); - setNodeMask(mVisualAddon->isHidden()? 0x0 : ~0x0); + setNodeMask(mVisualAspect->isHidden()? 0x0 : ~0x0); } //============================================================================== @@ -107,7 +107,7 @@ void EllipsoidShapeNode::refresh() { mUtilized = true; - setNodeMask(mVisualAddon->isHidden()? 0x0 : ~0x0); + setNodeMask(mVisualAspect->isHidden()? 0x0 : ~0x0); if(mShape->getDataVariance() == dart::dynamics::Shape::STATIC) return; @@ -177,7 +177,7 @@ void EllipsoidShapeGeode::extractData() { if(nullptr == mDrawable) { - mDrawable = new EllipsoidShapeDrawable(mEllipsoidShape, mVisualAddon, this); + mDrawable = new EllipsoidShapeDrawable(mEllipsoidShape, mVisualAspect, this); addDrawable(mDrawable); return; } @@ -194,10 +194,10 @@ EllipsoidShapeGeode::~EllipsoidShapeGeode() //============================================================================== EllipsoidShapeDrawable::EllipsoidShapeDrawable( dart::dynamics::EllipsoidShape* shape, - dart::dynamics::VisualAddon* visualAddon, + dart::dynamics::VisualAspect* visualAspect, EllipsoidShapeGeode* parent) : mEllipsoidShape(shape), - mVisualAddon(visualAddon), + mVisualAspect(visualAspect), mParent(parent) { refresh(true); @@ -225,7 +225,7 @@ void EllipsoidShapeDrawable::refresh(bool firstTime) if(mEllipsoidShape->checkDataVariance(dart::dynamics::Shape::DYNAMIC_COLOR) || firstTime) { - setColor(eigToOsgVec4(mVisualAddon->getRGBA())); + setColor(eigToOsgVec4(mVisualAspect->getRGBA())); } } diff --git a/osgDart/render/LineSegmentShapeNode.cpp b/osgDart/render/LineSegmentShapeNode.cpp index 64f8c315b38f4..aae9225542b01 100644 --- a/osgDart/render/LineSegmentShapeNode.cpp +++ b/osgDart/render/LineSegmentShapeNode.cpp @@ -76,7 +76,7 @@ class LineSegmentShapeDrawable : public osg::Geometry public: LineSegmentShapeDrawable(dart::dynamics::LineSegmentShape* shape, - dart::dynamics::VisualAddon* visualAddon); + dart::dynamics::VisualAspect* visualAspect); void refresh(bool firstTime); @@ -85,7 +85,7 @@ class LineSegmentShapeDrawable : public osg::Geometry virtual ~LineSegmentShapeDrawable(); dart::dynamics::LineSegmentShape* mLineSegmentShape; - dart::dynamics::VisualAddon* mVisualAddon; + dart::dynamics::VisualAspect* mVisualAspect; osg::ref_ptr mVertices; osg::ref_ptr mColors; @@ -101,7 +101,7 @@ LineSegmentShapeNode::LineSegmentShapeNode( { mNode = this; extractData(true); - setNodeMask(mVisualAddon->isHidden()? 0x0 : ~0x0); + setNodeMask(mVisualAspect->isHidden()? 0x0 : ~0x0); } //============================================================================== @@ -109,7 +109,7 @@ void LineSegmentShapeNode::refresh() { mUtilized = true; - setNodeMask(mVisualAddon->isHidden()? 0x0 : ~0x0); + setNodeMask(mVisualAspect->isHidden()? 0x0 : ~0x0); if(mShape->getDataVariance() == dart::dynamics::Shape::STATIC) return; @@ -170,7 +170,7 @@ void LineSegmentShapeGeode::extractData(bool firstTime) if(nullptr == mDrawable) { - mDrawable = new LineSegmentShapeDrawable(mLineSegmentShape.get(), mVisualAddon); + mDrawable = new LineSegmentShapeDrawable(mLineSegmentShape.get(), mVisualAspect); addDrawable(mDrawable); return; } @@ -187,9 +187,9 @@ LineSegmentShapeGeode::~LineSegmentShapeGeode() //============================================================================== LineSegmentShapeDrawable::LineSegmentShapeDrawable( dart::dynamics::LineSegmentShape* shape, - dart::dynamics::VisualAddon* visualAddon) + dart::dynamics::VisualAspect* visualAspect) : mLineSegmentShape(shape), - mVisualAddon(visualAddon), + mVisualAspect(visualAspect), mVertices(new osg::Vec3Array), mColors(new osg::Vec4Array) { @@ -246,7 +246,7 @@ void LineSegmentShapeDrawable::refresh(bool firstTime) if(mColors->size() != 1) mColors->resize(1); - (*mColors)[0] = eigToOsgVec4(mVisualAddon->getRGBA()); + (*mColors)[0] = eigToOsgVec4(mVisualAspect->getRGBA()); setColorArray(mColors, osg::Array::BIND_OVERALL); } diff --git a/osgDart/render/MeshShapeNode.cpp b/osgDart/render/MeshShapeNode.cpp index 850cf87a3502e..985ebee0df14b 100644 --- a/osgDart/render/MeshShapeNode.cpp +++ b/osgDart/render/MeshShapeNode.cpp @@ -144,7 +144,7 @@ MeshShapeNode::MeshShapeNode(std::shared_ptr shape, mRootAiNode(nullptr) { extractData(true); - setNodeMask(mVisualAddon->isHidden()? 0x0 : ~0x0); + setNodeMask(mVisualAspect->isHidden()? 0x0 : ~0x0); } //============================================================================== @@ -152,7 +152,7 @@ void MeshShapeNode::refresh() { mUtilized = true; - setNodeMask(mVisualAddon->isHidden()? 0x0 : ~0x0); + setNodeMask(mVisualAspect->isHidden()? 0x0 : ~0x0); if(mShape->getDataVariance() == dart::dynamics::Shape::STATIC) return; @@ -624,7 +624,7 @@ void MeshShapeGeometry::extractData(bool firstTime) if(!isColored || mMeshShape->getColorMode() == dart::dynamics::MeshShape::SHAPE_COLOR) { - const Eigen::Vector4d& c = mVisualAddon->getRGBA(); + const Eigen::Vector4d& c = mVisualAspect->getRGBA(); if(mColors->size() != 1) mColors->resize(1); diff --git a/osgDart/render/PlaneShapeNode.cpp b/osgDart/render/PlaneShapeNode.cpp index 0a17fccb6072f..7d7737b3449e2 100644 --- a/osgDart/render/PlaneShapeNode.cpp +++ b/osgDart/render/PlaneShapeNode.cpp @@ -72,7 +72,7 @@ class PlaneShapeDrawable : public osg::ShapeDrawable public: PlaneShapeDrawable(dart::dynamics::PlaneShape* shape, - dart::dynamics::VisualAddon* visualAddon, + dart::dynamics::VisualAspect* visualAspect, PlaneShapeGeode* parent); void refresh(bool firstTime); @@ -82,7 +82,7 @@ class PlaneShapeDrawable : public osg::ShapeDrawable virtual ~PlaneShapeDrawable(); dart::dynamics::PlaneShape* mPlaneShape; - dart::dynamics::VisualAddon* mVisualAddon; + dart::dynamics::VisualAspect* mVisualAspect; PlaneShapeGeode* mParent; }; @@ -96,7 +96,7 @@ PlaneShapeNode::PlaneShapeNode( mGeode(nullptr) { extractData(true); - setNodeMask(mVisualAddon->isHidden()? 0x0 : ~0x0); + setNodeMask(mVisualAspect->isHidden()? 0x0 : ~0x0); } //============================================================================== @@ -104,7 +104,7 @@ void PlaneShapeNode::refresh() { mUtilized = true; - setNodeMask(mVisualAddon->isHidden()? 0x0 : ~0x0); + setNodeMask(mVisualAspect->isHidden()? 0x0 : ~0x0); if(mShape->getDataVariance() == dart::dynamics::Shape::STATIC) return; @@ -156,7 +156,7 @@ void PlaneShapeGeode::extractData() { if(nullptr == mDrawable) { - mDrawable = new PlaneShapeDrawable(mPlaneShape, mVisualAddon, this); + mDrawable = new PlaneShapeDrawable(mPlaneShape, mVisualAspect, this); addDrawable(mDrawable); return; } @@ -172,10 +172,10 @@ PlaneShapeGeode::~PlaneShapeGeode() //============================================================================== PlaneShapeDrawable::PlaneShapeDrawable(dart::dynamics::PlaneShape* shape, - dart::dynamics::VisualAddon* visualAddon, + dart::dynamics::VisualAspect* visualAspect, PlaneShapeGeode* parent) : mPlaneShape(shape), - mVisualAddon(visualAddon), + mVisualAspect(visualAspect), mParent(parent) { refresh(true); @@ -205,7 +205,7 @@ void PlaneShapeDrawable::refresh(bool firstTime) if(mPlaneShape->checkDataVariance(dart::dynamics::Shape::DYNAMIC_COLOR) || firstTime) { - setColor(eigToOsgVec4(mVisualAddon->getRGBA())); + setColor(eigToOsgVec4(mVisualAspect->getRGBA())); } } diff --git a/osgDart/render/ShapeNode.cpp b/osgDart/render/ShapeNode.cpp index bc3c09151d81d..90d8a9826814e 100644 --- a/osgDart/render/ShapeNode.cpp +++ b/osgDart/render/ShapeNode.cpp @@ -50,7 +50,7 @@ ShapeNode::ShapeNode(std::shared_ptr shape, mUtilized(true) { mShapeFrame = mParentShapeFrameNode->getShapeFrame(); - mVisualAddon = mShapeFrame->getVisualAddon(true); + mVisualAspect = mShapeFrame->getVisualAspect(true); } //============================================================================== @@ -72,15 +72,15 @@ const dart::dynamics::ShapeFrame* ShapeNode::getShapeFrame() const } //============================================================================== -dart::dynamics::VisualAddon* ShapeNode::getVisualAddon() +dart::dynamics::VisualAspect* ShapeNode::getVisualAspect() { - return mVisualAddon; + return mVisualAspect; } //============================================================================== -const dart::dynamics::VisualAddon* ShapeNode::getVisualAddon() const +const dart::dynamics::VisualAspect* ShapeNode::getVisualAspect() const { - return mVisualAddon; + return mVisualAspect; } //============================================================================== diff --git a/osgDart/render/ShapeNode.h b/osgDart/render/ShapeNode.h index da7ce55f02466..211e62ef5a734 100644 --- a/osgDart/render/ShapeNode.h +++ b/osgDart/render/ShapeNode.h @@ -49,7 +49,7 @@ namespace dynamics { class Shape; class ShapeFrame; class SimpleFrame; -class VisualAddon; +class VisualAspect; } // namespace dynamics } // namespace dart @@ -75,9 +75,9 @@ class ShapeNode const dart::dynamics::ShapeFrame* getShapeFrame() const; - dart::dynamics::VisualAddon* getVisualAddon(); + dart::dynamics::VisualAspect* getVisualAspect(); - const dart::dynamics::VisualAddon* getVisualAddon() const; + const dart::dynamics::VisualAspect* getVisualAspect() const; /// Cast this ShapeNode into an osg::Node osg::Node* getNode(); @@ -107,8 +107,8 @@ class ShapeNode /// Pointer to the SimpleFrame associated with this ShapeNode dart::dynamics::ShapeFrame* mShapeFrame; - /// Pointer to the VisualAddon associated with this ShapeNode - dart::dynamics::VisualAddon* mVisualAddon; + /// Pointer to the VisualAspect associated with this ShapeNode + dart::dynamics::VisualAspect* mVisualAspect; /// Pointer to the parent ShapeFrameNode of this ShapeNode ShapeFrameNode* mParentShapeFrameNode; diff --git a/osgDart/render/SoftMeshShapeNode.cpp b/osgDart/render/SoftMeshShapeNode.cpp index 2a552c8e1e87a..0c2a24ca6d093 100644 --- a/osgDart/render/SoftMeshShapeNode.cpp +++ b/osgDart/render/SoftMeshShapeNode.cpp @@ -64,7 +64,7 @@ class SoftMeshShapeGeode : public ShapeNode, public osg::Geode virtual ~SoftMeshShapeGeode(); dart::dynamics::SoftMeshShape* mSoftMeshShape; - dart::dynamics::VisualAddon* mVisualAddon; + dart::dynamics::VisualAspect* mVisualAspect; SoftMeshShapeDrawable* mDrawable; }; @@ -75,7 +75,7 @@ class SoftMeshShapeDrawable : public osg::Geometry public: SoftMeshShapeDrawable(dart::dynamics::SoftMeshShape* shape, - dart::dynamics::VisualAddon* visualAddon); + dart::dynamics::VisualAspect* visualAspect); void refresh(bool firstTime); @@ -90,7 +90,7 @@ class SoftMeshShapeDrawable : public osg::Geometry std::vector mEigNormals; dart::dynamics::SoftMeshShape* mSoftMeshShape; - dart::dynamics::VisualAddon* mVisualAddon; + dart::dynamics::VisualAspect* mVisualAspect; }; @@ -103,7 +103,7 @@ SoftMeshShapeNode::SoftMeshShapeNode( mGeode(nullptr) { extractData(true); - setNodeMask(mVisualAddon->isHidden()? 0x0 : ~0x0); + setNodeMask(mVisualAspect->isHidden()? 0x0 : ~0x0); } //============================================================================== @@ -111,7 +111,7 @@ void SoftMeshShapeNode::refresh() { mUtilized = true; - setNodeMask(mVisualAddon->isHidden()? 0x0 : ~0x0); + setNodeMask(mVisualAspect->isHidden()? 0x0 : ~0x0); if(mShape->getDataVariance() == dart::dynamics::Shape::STATIC) return; @@ -145,7 +145,7 @@ SoftMeshShapeGeode::SoftMeshShapeGeode( SoftMeshShapeNode* parentNode) : ShapeNode(parentNode->getShape(), parentShapeFrame, this), mSoftMeshShape(shape), - mVisualAddon(parentNode->getVisualAddon()), + mVisualAspect(parentNode->getVisualAspect()), mDrawable(nullptr) { getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON); @@ -165,7 +165,7 @@ void SoftMeshShapeGeode::extractData() { if(nullptr == mDrawable) { - mDrawable = new SoftMeshShapeDrawable(mSoftMeshShape, mVisualAddon); + mDrawable = new SoftMeshShapeDrawable(mSoftMeshShape, mVisualAspect); addDrawable(mDrawable); return; } @@ -182,12 +182,12 @@ SoftMeshShapeGeode::~SoftMeshShapeGeode() //============================================================================== SoftMeshShapeDrawable::SoftMeshShapeDrawable( dart::dynamics::SoftMeshShape* shape, - dart::dynamics::VisualAddon* visualAddon) + dart::dynamics::VisualAspect* visualAspect) : mVertices(new osg::Vec3Array), mNormals(new osg::Vec3Array), mColors(new osg::Vec4Array), mSoftMeshShape(shape), - mVisualAddon(visualAddon) + mVisualAspect(visualAspect) { refresh(true); } @@ -284,7 +284,7 @@ void SoftMeshShapeDrawable::refresh(bool firstTime) if(mColors->size() != 1) mColors->resize(1); - (*mColors)[0] = eigToOsgVec4(mVisualAddon->getRGBA()); + (*mColors)[0] = eigToOsgVec4(mVisualAspect->getRGBA()); setColorArray(mColors, osg::Array::BIND_OVERALL); } diff --git a/tutorials/tutorialBiped-Finished.cpp b/tutorials/tutorialBiped-Finished.cpp index 2ddb7c1f29d65..8cc048694b399 100644 --- a/tutorials/tutorialBiped-Finished.cpp +++ b/tutorials/tutorialBiped-Finished.cpp @@ -268,8 +268,8 @@ class MyWindow : public SimWindow if(mForceCountDown > 0) { BodyNode* bn = mWorld->getSkeleton("biped")->getBodyNode("h_abdomen"); - auto shapeNodes = bn->getShapeNodesWith(); - shapeNodes[0]->getVisualAddon()->setColor(dart::Color::Red()); + auto shapeNodes = bn->getShapeNodesWith(); + shapeNodes[0]->getVisualAspect()->setColor(dart::Color::Red()); if(mPositiveSign) bn->addExtForce(default_force * Eigen::Vector3d::UnitX(), @@ -437,8 +437,8 @@ SkeletonPtr createFloor() std::shared_ptr box( new BoxShape(Eigen::Vector3d(floor_width, floor_height, floor_width))); auto shapeNode - = body->createShapeNodeWith(box); - shapeNode->getVisualAddon()->setColor(dart::Color::Black()); + = body->createShapeNodeWith(box); + shapeNode->getVisualAspect()->setColor(dart::Color::Black()); // Put the body into position Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); diff --git a/tutorials/tutorialBiped.cpp b/tutorials/tutorialBiped.cpp index aa9df83813390..fa585412bd755 100644 --- a/tutorials/tutorialBiped.cpp +++ b/tutorials/tutorialBiped.cpp @@ -202,8 +202,8 @@ class MyWindow : public SimWindow if(mForceCountDown > 0) { BodyNode* bn = mWorld->getSkeleton("biped")->getBodyNode("h_abdomen"); - auto shapeNodes = bn->getShapeNodesWith(); - shapeNodes[0]->getVisualAddon()->setColor(dart::Color::Red()); + auto shapeNodes = bn->getShapeNodesWith(); + shapeNodes[0]->getVisualAspect()->setColor(dart::Color::Red()); if(mPositiveSign) bn->addExtForce(default_force * Eigen::Vector3d::UnitX(), @@ -278,8 +278,8 @@ SkeletonPtr createFloor() std::shared_ptr box( new BoxShape(Eigen::Vector3d(floor_width, floor_height, floor_width))); auto shapeNode - = body->createShapeNodeWith(box); - shapeNode->getVisualAddon()->setColor(dart::Color::Black()); + = body->createShapeNodeWith(box); + shapeNode->getVisualAspect()->setColor(dart::Color::Black()); // Put the body into position Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); diff --git a/tutorials/tutorialCollisions-Finished.cpp b/tutorials/tutorialCollisions-Finished.cpp index 765775cb18a35..e799c149e132a 100644 --- a/tutorials/tutorialCollisions-Finished.cpp +++ b/tutorials/tutorialCollisions-Finished.cpp @@ -394,7 +394,7 @@ BodyNode* addRigidBody(const SkeletonPtr& chain, const std::string& name, default_shape_height*Eigen::Vector3d::Ones()); } - bn->createShapeNodeWith(shape); + bn->createShapeNodeWith(shape); // Setup the inertia for the body Inertia inertia; @@ -496,10 +496,10 @@ BodyNode* addSoftBody(const SkeletonPtr& chain, const std::string& name, bn->setInertia(inertia); // Make the shape transparent - auto visualAddon = bn->getShapeNodesWith()[0]->getVisualAddon(); - Eigen::Vector4d color = visualAddon->getRGBA(); + auto visualAspect = bn->getShapeNodesWith()[0]->getVisualAspect(); + Eigen::Vector4d color = visualAspect->getRGBA(); color[3] = 0.4; - visualAddon->setRGBA(color); + visualAspect->setRGBA(color); return bn; } @@ -510,9 +510,9 @@ void setAllColors(const SkeletonPtr& object, const Eigen::Vector3d& color) for(size_t i=0; i < object->getNumBodyNodes(); ++i) { BodyNode* bn = object->getBodyNode(i); - auto visualShapeNodes = bn->getShapeNodesWith(); + auto visualShapeNodes = bn->getShapeNodesWith(); for(auto visualShapeNode : visualShapeNodes) - visualShapeNode->getVisualAddon()->setColor(color); + visualShapeNode->getVisualAspect()->setColor(color); } } @@ -571,7 +571,7 @@ SkeletonPtr createSoftBody() Eigen::Vector3d dims(width, width, height); dims *= 0.6; std::shared_ptr box = std::make_shared(dims); - bn->createShapeNodeWith(box); + bn->createShapeNodeWith(box); Inertia inertia; inertia.setMass(default_shape_density * box->getVolume()); @@ -597,7 +597,7 @@ SkeletonPtr createHybridBody() double box_shape_height = default_shape_height; std::shared_ptr box = std::make_shared( box_shape_height*Eigen::Vector3d::Ones()); - bn->createShapeNodeWith(box); + bn->createShapeNodeWith(box); Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); tf.translation() = Eigen::Vector3d(box_shape_height/2.0, 0, 0); @@ -623,8 +623,8 @@ SkeletonPtr createGround() Eigen::Vector3d(default_ground_width, default_ground_width, default_wall_thickness)); auto shapeNode - = bn->createShapeNodeWith(shape); - shapeNode->getVisualAddon()->setColor(Eigen::Vector3d(1.0, 1.0, 1.0)); + = bn->createShapeNodeWith(shape); + shapeNode->getVisualAspect()->setColor(Eigen::Vector3d(1.0, 1.0, 1.0)); return ground; } @@ -639,8 +639,8 @@ SkeletonPtr createWall() Eigen::Vector3d(default_wall_thickness, default_ground_width, default_wall_height)); auto shapeNode - = bn->createShapeNodeWith(shape); - shapeNode->getVisualAddon()->setColor(Eigen::Vector3d(0.8, 0.8, 0.8)); + = bn->createShapeNodeWith(shape); + shapeNode->getVisualAspect()->setColor(Eigen::Vector3d(0.8, 0.8, 0.8)); Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); tf.translation() = Eigen::Vector3d( diff --git a/tutorials/tutorialCollisions.cpp b/tutorials/tutorialCollisions.cpp index 4c33c8e8428df..4a926e696c9ba 100644 --- a/tutorials/tutorialCollisions.cpp +++ b/tutorials/tutorialCollisions.cpp @@ -335,9 +335,9 @@ void setAllColors(const SkeletonPtr& object, const Eigen::Vector3d& color) for(size_t i=0; i < object->getNumBodyNodes(); ++i) { BodyNode* bn = object->getBodyNode(i); - auto visualShapeNodes = bn->getShapeNodesWith(); + auto visualShapeNodes = bn->getShapeNodesWith(); for(auto visualShapeNode : visualShapeNodes) - visualShapeNode->getVisualAddon()->setColor(color); + visualShapeNode->getVisualAspect()->setColor(color); } } @@ -424,8 +424,8 @@ SkeletonPtr createGround() Eigen::Vector3d(default_ground_width, default_ground_width, default_wall_thickness)); auto shapeNode - = bn->createShapeNodeWith(shape); - shapeNode->getVisualAddon()->setColor(Eigen::Vector3d(1.0, 1.0, 1.0)); + = bn->createShapeNodeWith(shape); + shapeNode->getVisualAspect()->setColor(Eigen::Vector3d(1.0, 1.0, 1.0)); return ground; } @@ -440,8 +440,8 @@ SkeletonPtr createWall() Eigen::Vector3d(default_wall_thickness, default_ground_width, default_wall_height)); auto shapeNode - = bn->createShapeNodeWith(shape); - shapeNode->getVisualAddon()->setColor(Eigen::Vector3d(0.8, 0.8, 0.8)); + = bn->createShapeNodeWith(shape); + shapeNode->getVisualAspect()->setColor(Eigen::Vector3d(0.8, 0.8, 0.8)); Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); tf.translation() = Eigen::Vector3d( diff --git a/tutorials/tutorialDominoes-Finished.cpp b/tutorials/tutorialDominoes-Finished.cpp index 38ac5ee84f8e5..e80dae68a2053 100644 --- a/tutorials/tutorialDominoes-Finished.cpp +++ b/tutorials/tutorialDominoes-Finished.cpp @@ -429,7 +429,7 @@ SkeletonPtr createDomino() new BoxShape(Eigen::Vector3d(default_domino_depth, default_domino_width, default_domino_height))); - body->createShapeNodeWith(box); + body->createShapeNodeWith(box); // Set up inertia for the domino dart::dynamics::Inertia inertia; @@ -456,8 +456,8 @@ SkeletonPtr createFloor() std::shared_ptr box( new BoxShape(Eigen::Vector3d(floor_width, floor_width, floor_height))); auto shapeNode - = body->createShapeNodeWith(box); - shapeNode->getVisualAddon()->setColor(dart::Color::Black()); + = body->createShapeNodeWith(box); + shapeNode->getVisualAspect()->setColor(dart::Color::Black()); // Put the body into position Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); diff --git a/tutorials/tutorialDominoes.cpp b/tutorials/tutorialDominoes.cpp index c229a4fefbce4..5103aa6cf404c 100644 --- a/tutorials/tutorialDominoes.cpp +++ b/tutorials/tutorialDominoes.cpp @@ -282,7 +282,7 @@ SkeletonPtr createDomino() new BoxShape(Eigen::Vector3d(default_domino_depth, default_domino_width, default_domino_height))); - body->createShapeNodeWith(box); + body->createShapeNodeWith(box); // Set up inertia for the domino dart::dynamics::Inertia inertia; @@ -309,8 +309,8 @@ SkeletonPtr createFloor() std::shared_ptr box( new BoxShape(Eigen::Vector3d(floor_width, floor_width, floor_height))); auto shapeNode - = body->createShapeNodeWith(box); - shapeNode->getVisualAddon()->setColor(dart::Color::Black()); + = body->createShapeNodeWith(box); + shapeNode->getVisualAspect()->setColor(dart::Color::Black()); // Put the body into position Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); diff --git a/tutorials/tutorialMultiPendulum-Finished.cpp b/tutorials/tutorialMultiPendulum-Finished.cpp index 36dae47af0607..46ccad4545700 100644 --- a/tutorials/tutorialMultiPendulum-Finished.cpp +++ b/tutorials/tutorialMultiPendulum-Finished.cpp @@ -257,9 +257,9 @@ class MyWindow : public dart::gui::SimWindow for(size_t i = 0; i < mPendulum->getNumBodyNodes(); ++i) { BodyNode* bn = mPendulum->getBodyNode(i); - auto visualShapeNodes = bn->getShapeNodesWith(); + auto visualShapeNodes = bn->getShapeNodesWith(); for(size_t j = 0; j < 2; ++j) - visualShapeNodes[j]->getVisualAddon()->setColor(dart::Color::Blue()); + visualShapeNodes[j]->getVisualAspect()->setColor(dart::Color::Blue()); // If we have three visualization shapes, that means the arrow is // attached. We should remove it in case this body is no longer @@ -282,8 +282,8 @@ class MyWindow : public dart::gui::SimWindow dof->setForce( mPositiveSign? default_torque : -default_torque ); BodyNode* bn = dof->getChildBodyNode(); - auto visualShapeNodes = bn->getShapeNodesWith(); - visualShapeNodes[0]->getVisualAddon()->setColor(dart::Color::Red()); + auto visualShapeNodes = bn->getShapeNodesWith(); + visualShapeNodes[0]->getVisualAspect()->setColor(dart::Color::Red()); --mForceCountDown[i]; } @@ -307,9 +307,9 @@ class MyWindow : public dart::gui::SimWindow } bn->addExtForce(force, location, true, true); - auto shapeNodes = bn->getShapeNodesWith(); - shapeNodes[1]->getVisualAddon()->setColor(dart::Color::Red()); - bn->createShapeNodeWith(mArrow); + auto shapeNodes = bn->getShapeNodesWith(); + shapeNodes[1]->getVisualAspect()->setColor(dart::Color::Red()); + bn->createShapeNodeWith(mArrow); --mForceCountDown[i]; } @@ -350,8 +350,8 @@ void setGeometry(const BodyNodePtr& bn) // Create a shpae node for visualization and collision checking auto shapeNode - = bn->createShapeNodeWith(box); - shapeNode->getVisualAddon()->setColor(dart::Color::Blue()); + = bn->createShapeNodeWith(box); + shapeNode->getVisualAspect()->setColor(dart::Color::Blue()); // Set the location of the shape node Eigen::Isometry3d box_tf(Eigen::Isometry3d::Identity()); @@ -378,8 +378,8 @@ BodyNode* makeRootBody(const SkeletonPtr& pendulum, const std::string& name) const double& R = default_width; std::shared_ptr ball( new EllipsoidShape(sqrt(2) * Eigen::Vector3d(R, R, R))); - auto shapeNode = bn->createShapeNodeWith(ball); - shapeNode->getVisualAddon()->setColor(dart::Color::Blue()); + auto shapeNode = bn->createShapeNodeWith(ball); + shapeNode->getVisualAspect()->setColor(dart::Color::Blue()); // Set the geometry of the Body setGeometry(bn); @@ -414,8 +414,8 @@ BodyNode* addBody(const SkeletonPtr& pendulum, BodyNode* parent, tf.linear() = dart::math::eulerXYZToMatrix( Eigen::Vector3d(90.0 * M_PI / 180.0, 0, 0)); - auto shapeNode = bn->createShapeNodeWith(cyl); - shapeNode->getVisualAddon()->setColor(dart::Color::Blue()); + auto shapeNode = bn->createShapeNodeWith(cyl); + shapeNode->getVisualAspect()->setColor(dart::Color::Blue()); shapeNode->setRelativeTransform(tf); // Set the geometry of the Body diff --git a/tutorials/tutorialMultiPendulum.cpp b/tutorials/tutorialMultiPendulum.cpp index ec46f8bbf95ee..e4a35c69d2621 100644 --- a/tutorials/tutorialMultiPendulum.cpp +++ b/tutorials/tutorialMultiPendulum.cpp @@ -279,8 +279,8 @@ void setGeometry(const BodyNodePtr& bn) // Create a shpae node for visualization and collision checking auto shapeNode - = bn->createShapeNodeWith(box); - shapeNode->getVisualAddon()->setColor(dart::Color::Blue()); + = bn->createShapeNodeWith(box); + shapeNode->getVisualAspect()->setColor(dart::Color::Blue()); // Set the location of the shape node Eigen::Isometry3d box_tf(Eigen::Isometry3d::Identity()); @@ -307,8 +307,8 @@ BodyNode* makeRootBody(const SkeletonPtr& pendulum, const std::string& name) const double& R = default_width; std::shared_ptr ball( new EllipsoidShape(sqrt(2) * Eigen::Vector3d(R, R, R))); - auto shapeNode = bn->createShapeNodeWith(ball); - shapeNode->getVisualAddon()->setColor(dart::Color::Blue()); + auto shapeNode = bn->createShapeNodeWith(ball); + shapeNode->getVisualAspect()->setColor(dart::Color::Blue()); // Set the geometry of the Body setGeometry(bn); @@ -343,8 +343,8 @@ BodyNode* addBody(const SkeletonPtr& pendulum, BodyNode* parent, tf.linear() = dart::math::eulerXYZToMatrix( Eigen::Vector3d(90.0 * M_PI / 180.0, 0, 0)); - auto shapeNode = bn->createShapeNodeWith(cyl); - shapeNode->getVisualAddon()->setColor(dart::Color::Blue()); + auto shapeNode = bn->createShapeNodeWith(cyl); + shapeNode->getVisualAspect()->setColor(dart::Color::Blue()); shapeNode->setRelativeTransform(tf); // Set the geometry of the Body diff --git a/unittests/TestHelpers.h b/unittests/TestHelpers.h index 0c89e63ab09db..708d2fda9df5e 100644 --- a/unittests/TestHelpers.h +++ b/unittests/TestHelpers.h @@ -118,7 +118,7 @@ void addEndEffector(SkeletonPtr robot, BodyNode* parent_node, Vector3d dim) parent_node, joint, node); auto bodyNode = pair.second; bodyNode->createShapeNodeWith< - VisualAddon, CollisionAddon, DynamicsAddon>(shape); + VisualAspect, CollisionAspect, DynamicsAspect>(shape); } //============================================================================== @@ -174,11 +174,11 @@ SkeletonPtr createThreeLinkRobot(Vector3d dim1, TypeOfDOF type1, std::pair pair1 = add1DofJoint( robot, nullptr, node, "joint1", 0.0, -DART_PI, DART_PI, type1); auto current_node = pair1.second; - auto shapeNode = current_node->createShapeNodeWith(shape); + auto shapeNode = current_node->createShapeNodeWith(shape); if(collisionShape) { - shapeNode->createCollisionAddon(); - shapeNode->createDynamicsAddon(); + shapeNode->createCollisionAspect(); + shapeNode->createDynamicsAspect(); } BodyNode* parent_node = current_node; @@ -198,11 +198,11 @@ SkeletonPtr createThreeLinkRobot(Vector3d dim1, TypeOfDOF type1, joint->setTransformFromParentBodyNode(T); auto current_node = pair2.second; - auto shapeNode = current_node->createShapeNodeWith(shape); + auto shapeNode = current_node->createShapeNodeWith(shape); if(collisionShape) { - shapeNode->createCollisionAddon(); - shapeNode->createDynamicsAddon(); + shapeNode->createCollisionAspect(); + shapeNode->createDynamicsAspect(); } parent_node = pair2.second; @@ -224,11 +224,11 @@ SkeletonPtr createThreeLinkRobot(Vector3d dim1, TypeOfDOF type1, joint->setTransformFromParentBodyNode(T); auto current_node = pair3.second; - auto shapeNode = current_node->createShapeNodeWith(shape); + auto shapeNode = current_node->createShapeNodeWith(shape); if(collisionShape) { - shapeNode->createCollisionAddon(); - shapeNode->createDynamicsAddon(); + shapeNode->createCollisionAspect(); + shapeNode->createDynamicsAspect(); } parent_node = pair3.second; @@ -276,7 +276,7 @@ SkeletonPtr createNLinkRobot(int _n, Vector3d dim, TypeOfDOF type, joint->setDampingCoefficient(0, 0.01); auto current_node = pair1.second; - current_node->createShapeNodeWith( + current_node->createShapeNodeWith( shape); BodyNode* parent_node = current_node; @@ -303,7 +303,7 @@ SkeletonPtr createNLinkRobot(int _n, Vector3d dim, TypeOfDOF type, joint->setDampingCoefficient(0, 0.01); auto current_node = newPair.second; - current_node->createShapeNodeWith( + current_node->createShapeNodeWith( shape); parent_node = current_node; @@ -346,7 +346,7 @@ SkeletonPtr createNLinkPendulum(size_t numBodyNodes, joint->setDampingCoefficient(0, 0.01); auto current_node = pair1.second; - current_node->createShapeNodeWith( + current_node->createShapeNodeWith( shape); BodyNode* parent_node = current_node; @@ -373,7 +373,7 @@ SkeletonPtr createNLinkPendulum(size_t numBodyNodes, joint->setDampingCoefficient(0, 0.01); auto current_node = newPair.second; - current_node->createShapeNodeWith( + current_node->createShapeNodeWith( shape); parent_node = current_node; @@ -408,7 +408,7 @@ SkeletonPtr createGround( nullptr, joint, node); auto body_node = pair.second; - body_node->createShapeNodeWith( + body_node->createShapeNodeWith( shape); return skeleton; @@ -447,7 +447,7 @@ SkeletonPtr createSphere( BodyNode* bn = sphere->getBodyNode(0); std::shared_ptr ellipShape( new EllipsoidShape(Vector3d::Constant(_radius * 2.0))); - bn->createShapeNodeWith(ellipShape); + bn->createShapeNodeWith(ellipShape); return sphere; } @@ -462,7 +462,7 @@ SkeletonPtr createBox( BodyNode* bn = box->getBodyNode(0); std::shared_ptr boxShape(new BoxShape(_size)); - bn->createShapeNodeWith(boxShape); + bn->createShapeNodeWith(boxShape); return box; } diff --git a/unittests/testAddon.cpp b/unittests/testAddon.cpp index 32955427cc537..419483bfa7d8d 100644 --- a/unittests/testAddon.cpp +++ b/unittests/testAddon.cpp @@ -35,9 +35,9 @@ */ // Use this preprocessor token to allow us to test that the specialized versions -// of Addon functions are being used appropriately. This preprocessor token -// should NOT be used anywhere outside of this file (testAddon.cpp). -#define DART_UNITTEST_SPECIALIZED_ADDON_ACCESS +// of Aspect functions are being used appropriately. This preprocessor token +// should NOT be used anywhere outside of this file (testAspect.cpp). +#define DART_UNITTEST_SPECIALIZED_ASPECT_ACCESS #include @@ -45,72 +45,72 @@ #include "dart/common/Subject.h" #include "dart/common/sub_ptr.h" -#include "dart/common/AddonManager.h" -#include "dart/common/SpecializedForAddon.h" +#include "dart/common/Composite.h" +#include "dart/common/SpecializedForAspect.h" #include "dart/dynamics/EulerJoint.h" using namespace dart::common; -// Testing the creation of an Addon using the AddonWithState template class -class StateAddonTest : public dart::common::AddonWithState< - StateAddonTest, dart::common::Empty> +// Testing the creation of an Aspect using the AspectWithState template class +class StateAspectTest : public dart::common::AspectWithState< + StateAspectTest, dart::common::Empty> { public: - StateAddonTest(AddonManager* mgr, const StateData& state = StateData()) - : dart::common::AddonWithState(mgr, state) + StateAspectTest(Composite* mgr, const StateData& state = StateData()) + : dart::common::AspectWithState(mgr, state) { } }; -class StateAndPropertiesAddonTest : public dart::common::AddonWithVersionedProperties< - StateAndPropertiesAddonTest, dart::common::Empty> +class StateAndPropertiesAspectTest : public dart::common::AspectWithVersionedProperties< + StateAndPropertiesAspectTest, dart::common::Empty> { }; -class GenericAddon : public Addon, public Subject +class GenericAspect : public Aspect, public Subject { public: - GenericAddon(AddonManager* manager) - : Addon(manager) + GenericAspect(Composite* manager) + : Aspect(manager) { // Do nothing } - std::unique_ptr cloneAddon(AddonManager* newManager) const override final + std::unique_ptr cloneAspect(Composite* newManager) const override final { - return dart::common::make_unique(newManager); + return dart::common::make_unique(newManager); } }; -class SpecializedAddon : public Addon, public Subject +class SpecializedAspect : public Aspect, public Subject { public: - SpecializedAddon(AddonManager* manager) - : Addon(manager) + SpecializedAspect(Composite* manager) + : Aspect(manager) { // Do nothing } - std::unique_ptr cloneAddon(AddonManager* newManager) const override final + std::unique_ptr cloneAspect(Composite* newManager) const override final { - return dart::common::make_unique(newManager); + return dart::common::make_unique(newManager); } }; template -class StatefulAddon : public Addon, public Subject +class StatefulAspect : public Aspect, public Subject { public: - class State : public Addon::State + class State : public Aspect::State { public: @@ -122,18 +122,18 @@ class StatefulAddon : public Addon, public Subject T val; - std::unique_ptr clone() const override + std::unique_ptr clone() const override { return dart::common::make_unique(*this); } - void copy(const Addon::State& anotherState) override + void copy(const Aspect::State& anotherState) override { val = static_cast(anotherState).val; } }; - class Properties : public Addon::Properties + class Properties : public Aspect::Properties { public: @@ -145,64 +145,64 @@ class StatefulAddon : public Addon, public Subject T val; - std::unique_ptr clone() const override + std::unique_ptr clone() const override { return dart::common::make_unique(*this); } - void copy(const Addon::Properties& otherProperties) override + void copy(const Aspect::Properties& otherProperties) override { val = static_cast(otherProperties).val; } }; - StatefulAddon(AddonManager* mgr) - : Addon(mgr) + StatefulAspect(Composite* mgr) + : Aspect(mgr) { // Do nothing } - StatefulAddon(AddonManager* mgr, const StatefulAddon& other) - : Addon(mgr), + StatefulAspect(Composite* mgr, const StatefulAspect& other) + : Aspect(mgr), mState(other.mState), mProperties(other.mProperties) { // Do nothing } - StatefulAddon(AddonManager* mgr, const T& state) - : Addon(mgr), mState(state) + StatefulAspect(Composite* mgr, const T& state) + : Aspect(mgr), mState(state) { // Do nothing } - StatefulAddon(AddonManager* mgr, const T& state, const T& properties) - : Addon(mgr), + StatefulAspect(Composite* mgr, const T& state, const T& properties) + : Aspect(mgr), mState(state), mProperties(properties) { // Do nothing } - std::unique_ptr cloneAddon(AddonManager* newManager) const override final + std::unique_ptr cloneAspect(Composite* newManager) const override final { - return dart::common::make_unique(newManager, *this); + return dart::common::make_unique(newManager, *this); } - void setAddonState(const Addon::State& otherState) override + void setAspectState(const Aspect::State& otherState) override { mState.copy(otherState); } - const Addon::State* getAddonState() const override + const Aspect::State* getAspectState() const override { return &mState; } - void setAddonProperties(const Addon::Properties& someProperties) override + void setAspectProperties(const Aspect::Properties& someProperties) override { mProperties.copy(someProperties); } - const Addon::Properties* getAddonProperties() const override + const Aspect::Properties* getAspectProperties() const override { return &mProperties; } @@ -219,382 +219,382 @@ class StatefulAddon : public Addon, public Subject }; -typedef StatefulAddon DoubleAddon; -typedef StatefulAddon FloatAddon; -typedef StatefulAddon CharAddon; -typedef StatefulAddon IntAddon; +typedef StatefulAspect DoubleAspect; +typedef StatefulAspect FloatAspect; +typedef StatefulAspect CharAspect; +typedef StatefulAspect IntAspect; -class CustomSpecializedManager : public SpecializedForAddon { }; +class CustomSpecializedManager : public SpecializedForAspect { }; -TEST(Addon, Generic) +TEST(Aspect, Generic) { - AddonManager mgr; + Composite mgr; - EXPECT_TRUE( mgr.get() == nullptr ); + EXPECT_TRUE( mgr.get() == nullptr ); - sub_ptr addon = mgr.create(); - GenericAddon* rawAddon = addon; - EXPECT_FALSE( nullptr == addon ); - EXPECT_TRUE( mgr.get() == addon ); + sub_ptr aspect = mgr.create(); + GenericAspect* rawAspect = aspect; + EXPECT_FALSE( nullptr == aspect ); + EXPECT_TRUE( mgr.get() == aspect ); - GenericAddon* newAddon = mgr.create(); - EXPECT_FALSE( nullptr == newAddon ); - EXPECT_TRUE( nullptr == addon ); - EXPECT_FALSE( rawAddon == newAddon ); + GenericAspect* newAspect = mgr.create(); + EXPECT_FALSE( nullptr == newAspect ); + EXPECT_TRUE( nullptr == aspect ); + EXPECT_FALSE( rawAspect == newAspect ); } -TEST(Addon, Specialized) +TEST(Aspect, Specialized) { - usedSpecializedAddonAccess = false; + usedSpecializedAspectAccess = false; CustomSpecializedManager mgr; - EXPECT_TRUE( mgr.get() == nullptr ); - EXPECT_TRUE( usedSpecializedAddonAccess ); usedSpecializedAddonAccess = false; -// EXPECT_TRUE( mgr.getSpecializedAddon() == nullptr ); - EXPECT_TRUE( mgr.get() == nullptr ); - EXPECT_FALSE( usedSpecializedAddonAccess ); usedSpecializedAddonAccess = false; + EXPECT_TRUE( mgr.get() == nullptr ); + EXPECT_TRUE( usedSpecializedAspectAccess ); usedSpecializedAspectAccess = false; +// EXPECT_TRUE( mgr.getSpecializedAspect() == nullptr ); + EXPECT_TRUE( mgr.get() == nullptr ); + EXPECT_FALSE( usedSpecializedAspectAccess ); usedSpecializedAspectAccess = false; - sub_ptr spec = mgr.create(); - EXPECT_TRUE( usedSpecializedAddonAccess ); usedSpecializedAddonAccess = false; - SpecializedAddon* rawSpec = spec; + sub_ptr spec = mgr.create(); + EXPECT_TRUE( usedSpecializedAspectAccess ); usedSpecializedAspectAccess = false; + SpecializedAspect* rawSpec = spec; - sub_ptr generic = mgr.create(); - EXPECT_FALSE( usedSpecializedAddonAccess ); usedSpecializedAddonAccess = false; - GenericAddon* rawGeneric = generic; + sub_ptr generic = mgr.create(); + EXPECT_FALSE( usedSpecializedAspectAccess ); usedSpecializedAspectAccess = false; + GenericAspect* rawGeneric = generic; - EXPECT_TRUE( mgr.get() == spec ); - EXPECT_TRUE( usedSpecializedAddonAccess ); usedSpecializedAddonAccess = false; -// EXPECT_TRUE( mgr.getSpecializedAddon() == spec ); + EXPECT_TRUE( mgr.get() == spec ); + EXPECT_TRUE( usedSpecializedAspectAccess ); usedSpecializedAspectAccess = false; +// EXPECT_TRUE( mgr.getSpecializedAspect() == spec ); - EXPECT_TRUE( mgr.get() == generic ); - EXPECT_FALSE( usedSpecializedAddonAccess ); usedSpecializedAddonAccess = false; + EXPECT_TRUE( mgr.get() == generic ); + EXPECT_FALSE( usedSpecializedAspectAccess ); usedSpecializedAspectAccess = false; -// SpecializedAddon* newSpec = mgr.createSpecializedAddon(); - SpecializedAddon* newSpec = mgr.create(); - EXPECT_TRUE( usedSpecializedAddonAccess ); usedSpecializedAddonAccess = false; +// SpecializedAspect* newSpec = mgr.createSpecializedAspect(); + SpecializedAspect* newSpec = mgr.create(); + EXPECT_TRUE( usedSpecializedAspectAccess ); usedSpecializedAspectAccess = false; - GenericAddon* newGeneric = mgr.create(); - EXPECT_FALSE( usedSpecializedAddonAccess ); usedSpecializedAddonAccess = false; + GenericAspect* newGeneric = mgr.create(); + EXPECT_FALSE( usedSpecializedAspectAccess ); usedSpecializedAspectAccess = false; EXPECT_TRUE( nullptr == spec ); EXPECT_TRUE( nullptr == generic ); - EXPECT_FALSE( mgr.get() == rawSpec ); - EXPECT_TRUE( usedSpecializedAddonAccess ); usedSpecializedAddonAccess = false; -// EXPECT_FALSE( mgr.getSpecializedAddon() == rawSpec ); + EXPECT_FALSE( mgr.get() == rawSpec ); + EXPECT_TRUE( usedSpecializedAspectAccess ); usedSpecializedAspectAccess = false; +// EXPECT_FALSE( mgr.getSpecializedAspect() == rawSpec ); - EXPECT_FALSE( mgr.get() == rawGeneric ); - EXPECT_FALSE( usedSpecializedAddonAccess ); usedSpecializedAddonAccess = false; + EXPECT_FALSE( mgr.get() == rawGeneric ); + EXPECT_FALSE( usedSpecializedAspectAccess ); usedSpecializedAspectAccess = false; - EXPECT_TRUE( mgr.get() == newSpec ); - EXPECT_TRUE( usedSpecializedAddonAccess ); usedSpecializedAddonAccess = false; -// EXPECT_TRUE( mgr.getSpecializedAddon() == newSpec ); + EXPECT_TRUE( mgr.get() == newSpec ); + EXPECT_TRUE( usedSpecializedAspectAccess ); usedSpecializedAspectAccess = false; +// EXPECT_TRUE( mgr.getSpecializedAspect() == newSpec ); - EXPECT_TRUE( mgr.get() == newGeneric ); - EXPECT_FALSE( usedSpecializedAddonAccess ); usedSpecializedAddonAccess = false; + EXPECT_TRUE( mgr.get() == newGeneric ); + EXPECT_FALSE( usedSpecializedAspectAccess ); usedSpecializedAspectAccess = false; } -TEST(Addon, Releasing) +TEST(Aspect, Releasing) { - AddonManager sender; + Composite sender; CustomSpecializedManager receiver; // ---- Test generic releases ---- { - EXPECT_TRUE( sender.get() == nullptr ); - EXPECT_TRUE( receiver.get() == nullptr ); + EXPECT_TRUE( sender.get() == nullptr ); + EXPECT_TRUE( receiver.get() == nullptr ); - sub_ptr addon = sender.create(); + sub_ptr aspect = sender.create(); - EXPECT_TRUE( sender.get() == addon ); - EXPECT_TRUE( receiver.get() == nullptr ); + EXPECT_TRUE( sender.get() == aspect ); + EXPECT_TRUE( receiver.get() == nullptr ); - receiver.set(sender.release()); + receiver.set(sender.release()); - EXPECT_FALSE( nullptr == addon ); + EXPECT_FALSE( nullptr == aspect ); - EXPECT_TRUE( sender.get() == nullptr ); - EXPECT_TRUE( receiver.get() == addon ); + EXPECT_TRUE( sender.get() == nullptr ); + EXPECT_TRUE( receiver.get() == aspect ); - sender.set(receiver.release()); + sender.set(receiver.release()); - EXPECT_FALSE( nullptr == addon ); + EXPECT_FALSE( nullptr == aspect ); - EXPECT_TRUE( sender.get() == addon ); - EXPECT_TRUE( receiver.get() == nullptr ); + EXPECT_TRUE( sender.get() == aspect ); + EXPECT_TRUE( receiver.get() == nullptr ); - sender.release(); + sender.release(); - EXPECT_TRUE( nullptr == addon ); - EXPECT_TRUE( sender.get() == nullptr ); - EXPECT_TRUE( receiver.get() == nullptr ); + EXPECT_TRUE( nullptr == aspect ); + EXPECT_TRUE( sender.get() == nullptr ); + EXPECT_TRUE( receiver.get() == nullptr ); } // ---- Test specialized releases ---- { - EXPECT_TRUE( sender.get() == nullptr ); -// EXPECT_TRUE( receiver.getSpecializedAddon() == nullptr ); + EXPECT_TRUE( sender.get() == nullptr ); +// EXPECT_TRUE( receiver.getSpecializedAspect() == nullptr ); - sub_ptr spec = sender.create(); + sub_ptr spec = sender.create(); - EXPECT_TRUE( sender.get() == spec ); -// EXPECT_TRUE( receiver.getSpecializedAddon() == nullptr ); + EXPECT_TRUE( sender.get() == spec ); +// EXPECT_TRUE( receiver.getSpecializedAspect() == nullptr ); -// receiver.setSpecializedAddon(sender.release()); - receiver.set(sender.release()); +// receiver.setSpecializedAspect(sender.release()); + receiver.set(sender.release()); EXPECT_FALSE( nullptr == spec ); - EXPECT_TRUE( sender.get() == nullptr ); -// EXPECT_TRUE( receiver.getSpecializedAddon() == spec ); + EXPECT_TRUE( sender.get() == nullptr ); +// EXPECT_TRUE( receiver.getSpecializedAspect() == spec ); -// sender.set(receiver.releaseSpecializedAddon()); - sender.set(receiver.release()); +// sender.set(receiver.releaseSpecializedAspect()); + sender.set(receiver.release()); EXPECT_FALSE( nullptr == spec ); - EXPECT_TRUE( sender.get() == spec ); -// EXPECT_TRUE( receiver.getSpecializedAddon() == nullptr ); + EXPECT_TRUE( sender.get() == spec ); +// EXPECT_TRUE( receiver.getSpecializedAspect() == nullptr ); - sender.release(); + sender.release(); EXPECT_TRUE( nullptr == spec ); - EXPECT_TRUE( sender.get() == nullptr ); -// EXPECT_TRUE( receiver.getSpecializedAddon() == nullptr ); + EXPECT_TRUE( sender.get() == nullptr ); +// EXPECT_TRUE( receiver.getSpecializedAspect() == nullptr ); } // ---- Test non-moving set method ---- { // The set() methods being used in this block of code will make clones of - // the addons that are being passed in instead of transferring their + // the aspects that are being passed in instead of transferring their // ownership like the previous blocks of code were. - sub_ptr addon = sender.create(); + sub_ptr aspect = sender.create(); - // This should create a copy of the GenericAddon without taking the addon + // This should create a copy of the GenericAspect without taking the aspect // away from 'sender' - receiver.set(sender.get()); + receiver.set(sender.get()); - EXPECT_FALSE( nullptr == addon ); - EXPECT_FALSE( receiver.get() == addon ); - EXPECT_FALSE( receiver.get() == nullptr ); - EXPECT_TRUE( sender.get() == addon ); + EXPECT_FALSE( nullptr == aspect ); + EXPECT_FALSE( receiver.get() == aspect ); + EXPECT_FALSE( receiver.get() == nullptr ); + EXPECT_TRUE( sender.get() == aspect ); - sub_ptr rec_addon = receiver.get(); - EXPECT_FALSE( nullptr == rec_addon ); + sub_ptr rec_aspect = receiver.get(); + EXPECT_FALSE( nullptr == rec_aspect ); - // This should replace the first GenericAddon that was created in 'sender' - sender.set(receiver.get()); + // This should replace the first GenericAspect that was created in 'sender' + sender.set(receiver.get()); - EXPECT_TRUE( nullptr == addon ); - EXPECT_FALSE( nullptr == rec_addon ); - EXPECT_FALSE( sender.get() == receiver.get() ); + EXPECT_TRUE( nullptr == aspect ); + EXPECT_FALSE( nullptr == rec_aspect ); + EXPECT_FALSE( sender.get() == receiver.get() ); - sub_ptr addon2 = sender.get(); - EXPECT_FALSE( nullptr == addon2 ); + sub_ptr aspect2 = sender.get(); + EXPECT_FALSE( nullptr == aspect2 ); - sender.set(receiver.release()); - EXPECT_TRUE( nullptr == addon2 ); - EXPECT_FALSE( nullptr == rec_addon ); - EXPECT_TRUE( receiver.get() == nullptr ); - EXPECT_TRUE( sender.get() == rec_addon ); + sender.set(receiver.release()); + EXPECT_TRUE( nullptr == aspect2 ); + EXPECT_FALSE( nullptr == rec_aspect ); + EXPECT_TRUE( receiver.get() == nullptr ); + EXPECT_TRUE( sender.get() == rec_aspect ); } } -template -void makeStatesDifferent(AddonT* addon, const AddonT* differentFrom) +template +void makeStatesDifferent(AspectT* aspect, const AspectT* differentFrom) { size_t counter = 0; - while( addon->mState.val == differentFrom->mState.val ) + while( aspect->mState.val == differentFrom->mState.val ) { - addon->randomize(); + aspect->randomize(); ++counter; if(counter > 10) { - dtwarn << "[testAddon::makeStatesDifferent] Randomization failed to make " + dtwarn << "[testAspect::makeStatesDifferent] Randomization failed to make " << "the states different after " << counter << " attempts!\n"; break; } } } -template -void makePropertiesDifferent(AddonT* addon, const AddonT* differentFrom) +template +void makePropertiesDifferent(AspectT* aspect, const AspectT* differentFrom) { size_t counter = 0; - while( addon->mProperties.val == differentFrom->mProperties.val ) + while( aspect->mProperties.val == differentFrom->mProperties.val ) { - addon->randomize(); + aspect->randomize(); ++counter; if(counter > 10) { - dtwarn << "[testAddon::makePropertiesDifferent] Randomization failed to " + dtwarn << "[testAspect::makePropertiesDifferent] Randomization failed to " << "make the states different after " << counter << " attempts!\n"; break; } } } -TEST(Addon, StateAndProperties) +TEST(Aspect, StateAndProperties) { - AddonManager mgr1; - mgr1.create(); - mgr1.create(); - mgr1.create(); - mgr1.create(); + Composite mgr1; + mgr1.create(); + mgr1.create(); + mgr1.create(); + mgr1.create(); - AddonManager mgr2; - mgr2.create(); - mgr2.create(); + Composite mgr2; + mgr2.create(); + mgr2.create(); - mgr1.create(); + mgr1.create(); // ---- Test state transfer ---- - mgr2.setAddonStates(mgr1.getAddonStates()); + mgr2.setAspectStates(mgr1.getAspectStates()); - EXPECT_EQ( mgr1.get()->mState.val, - mgr2.get()->mState.val ); + EXPECT_EQ( mgr1.get()->mState.val, + mgr2.get()->mState.val ); - EXPECT_EQ( mgr1.get()->mState.val, - mgr2.get()->mState.val ); + EXPECT_EQ( mgr1.get()->mState.val, + mgr2.get()->mState.val ); - makeStatesDifferent( mgr2.get(), mgr1.get() ); - makeStatesDifferent( mgr2.get(), mgr1.get() ); + makeStatesDifferent( mgr2.get(), mgr1.get() ); + makeStatesDifferent( mgr2.get(), mgr1.get() ); - EXPECT_NE( mgr1.get()->mState.val, - mgr2.get()->mState.val ); + EXPECT_NE( mgr1.get()->mState.val, + mgr2.get()->mState.val ); - EXPECT_NE( mgr1.get()->mState.val, - mgr2.get()->mState.val ); + EXPECT_NE( mgr1.get()->mState.val, + mgr2.get()->mState.val ); - mgr1.setAddonStates( mgr2.getAddonStates() ); + mgr1.setAspectStates( mgr2.getAspectStates() ); - EXPECT_EQ( mgr1.get()->mState.val, - mgr2.get()->mState.val ); + EXPECT_EQ( mgr1.get()->mState.val, + mgr2.get()->mState.val ); - EXPECT_EQ( mgr1.get()->mState.val, - mgr2.get()->mState.val ); + EXPECT_EQ( mgr1.get()->mState.val, + mgr2.get()->mState.val ); - EXPECT_TRUE( nullptr == mgr2.get() ); - EXPECT_TRUE( nullptr == mgr2.get() ); + EXPECT_TRUE( nullptr == mgr2.get() ); + EXPECT_TRUE( nullptr == mgr2.get() ); // ---- Test property transfer ---- - mgr2.setAddonProperties(mgr1.getAddonProperties()); + mgr2.setAspectProperties(mgr1.getAspectProperties()); - EXPECT_EQ( mgr1.get()->mProperties.val, - mgr2.get()->mProperties.val ); + EXPECT_EQ( mgr1.get()->mProperties.val, + mgr2.get()->mProperties.val ); - EXPECT_EQ( mgr1.get()->mProperties.val, - mgr2.get()->mProperties.val ); + EXPECT_EQ( mgr1.get()->mProperties.val, + mgr2.get()->mProperties.val ); - makePropertiesDifferent( mgr2.get(), mgr1.get() ); - makePropertiesDifferent( mgr2.get(), mgr1.get() ); + makePropertiesDifferent( mgr2.get(), mgr1.get() ); + makePropertiesDifferent( mgr2.get(), mgr1.get() ); - EXPECT_NE( mgr1.get()->mProperties.val, - mgr2.get()->mProperties.val ); + EXPECT_NE( mgr1.get()->mProperties.val, + mgr2.get()->mProperties.val ); - EXPECT_NE( mgr1.get()->mProperties.val, - mgr2.get()->mProperties.val ); + EXPECT_NE( mgr1.get()->mProperties.val, + mgr2.get()->mProperties.val ); - mgr1.setAddonProperties( mgr2.getAddonProperties() ); + mgr1.setAspectProperties( mgr2.getAspectProperties() ); - EXPECT_EQ( mgr1.get()->mProperties.val, - mgr2.get()->mProperties.val ); + EXPECT_EQ( mgr1.get()->mProperties.val, + mgr2.get()->mProperties.val ); - EXPECT_EQ( mgr1.get()->mProperties.val, - mgr2.get()->mProperties.val ); + EXPECT_EQ( mgr1.get()->mProperties.val, + mgr2.get()->mProperties.val ); - EXPECT_TRUE( nullptr == mgr2.get() ); - EXPECT_TRUE( nullptr == mgr2.get() ); + EXPECT_TRUE( nullptr == mgr2.get() ); + EXPECT_TRUE( nullptr == mgr2.get() ); } -TEST(Addon, Construction) +TEST(Aspect, Construction) { - AddonManager mgr; + Composite mgr; - mgr.create(); + mgr.create(); double s = dart::math::random(0, 100); - mgr.create(s); - EXPECT_EQ(mgr.get()->mState.val, s); + mgr.create(s); + EXPECT_EQ(mgr.get()->mState.val, s); double p = dart::math::random(0, 100); - mgr.create(dart::math::random(0, 100), p); - EXPECT_EQ(mgr.get()->mProperties.val, p); + mgr.create(dart::math::random(0, 100), p); + EXPECT_EQ(mgr.get()->mProperties.val, p); } -TEST(Addon, Joints) +TEST(Aspect, Joints) { - usedSpecializedAddonAccess = false; + usedSpecializedAspectAccess = false; dart::dynamics::SkeletonPtr skel = Skeleton::create(); dart::dynamics::EulerJoint* euler = skel->createJointAndBodyNodePair().first; - euler->getMultiDofJointAddon(); - EXPECT_TRUE(usedSpecializedAddonAccess); usedSpecializedAddonAccess = false; - euler->getEulerJointAddon(); - EXPECT_TRUE(usedSpecializedAddonAccess); usedSpecializedAddonAccess = false; + euler->getMultiDofJointAspect(); + EXPECT_TRUE(usedSpecializedAspectAccess); usedSpecializedAspectAccess = false; + euler->getEulerJointAspect(); + EXPECT_TRUE(usedSpecializedAspectAccess); usedSpecializedAspectAccess = false; dart::dynamics::PlanarJoint* planar = skel->createJointAndBodyNodePair().first; - planar->getMultiDofJointAddon(); - EXPECT_TRUE(usedSpecializedAddonAccess); usedSpecializedAddonAccess = false; - planar->getPlanarJointAddon(); - EXPECT_TRUE(usedSpecializedAddonAccess); usedSpecializedAddonAccess = false; + planar->getMultiDofJointAspect(); + EXPECT_TRUE(usedSpecializedAspectAccess); usedSpecializedAspectAccess = false; + planar->getPlanarJointAspect(); + EXPECT_TRUE(usedSpecializedAspectAccess); usedSpecializedAspectAccess = false; dart::dynamics::PrismaticJoint* prismatic = skel->createJointAndBodyNodePair().first; - prismatic->getSingleDofJointAddon(); - EXPECT_TRUE(usedSpecializedAddonAccess); usedSpecializedAddonAccess = false; - prismatic->getPrismaticJointAddon(); - EXPECT_TRUE(usedSpecializedAddonAccess); usedSpecializedAddonAccess = false; + prismatic->getSingleDofJointAspect(); + EXPECT_TRUE(usedSpecializedAspectAccess); usedSpecializedAspectAccess = false; + prismatic->getPrismaticJointAspect(); + EXPECT_TRUE(usedSpecializedAspectAccess); usedSpecializedAspectAccess = false; dart::dynamics::RevoluteJoint* revolute = skel->createJointAndBodyNodePair().first; - revolute->getSingleDofJointAddon(); - EXPECT_TRUE(usedSpecializedAddonAccess); usedSpecializedAddonAccess = false; - revolute->getRevoluteJointAddon(); - EXPECT_TRUE(usedSpecializedAddonAccess); usedSpecializedAddonAccess = false; + revolute->getSingleDofJointAspect(); + EXPECT_TRUE(usedSpecializedAspectAccess); usedSpecializedAspectAccess = false; + revolute->getRevoluteJointAspect(); + EXPECT_TRUE(usedSpecializedAspectAccess); usedSpecializedAspectAccess = false; dart::dynamics::ScrewJoint* screw = skel->createJointAndBodyNodePair().first; - screw->getSingleDofJointAddon(); - EXPECT_TRUE(usedSpecializedAddonAccess); usedSpecializedAddonAccess = false; - screw->getScrewJointAddon(); - EXPECT_TRUE(usedSpecializedAddonAccess); usedSpecializedAddonAccess = false; + screw->getSingleDofJointAspect(); + EXPECT_TRUE(usedSpecializedAspectAccess); usedSpecializedAspectAccess = false; + screw->getScrewJointAspect(); + EXPECT_TRUE(usedSpecializedAspectAccess); usedSpecializedAspectAccess = false; dart::dynamics::UniversalJoint* universal = skel->createJointAndBodyNodePair().first; - universal->getMultiDofJointAddon(); - EXPECT_TRUE(usedSpecializedAddonAccess); usedSpecializedAddonAccess = false; - universal->getUniversalJointAddon(); - EXPECT_TRUE(usedSpecializedAddonAccess); usedSpecializedAddonAccess = false; + universal->getMultiDofJointAspect(); + EXPECT_TRUE(usedSpecializedAspectAccess); usedSpecializedAspectAccess = false; + universal->getUniversalJointAspect(); + EXPECT_TRUE(usedSpecializedAspectAccess); usedSpecializedAspectAccess = false; // Regression test for issue #645 - universal->getMultiDofJointAddon(true); + universal->getMultiDofJointAspect(true); } -TEST(Addon, Duplication) +TEST(Aspect, Duplication) { - AddonManager mgr1, mgr2; + Composite mgr1, mgr2; - mgr1.create(); - mgr1.create(); - mgr1.create(); - mgr1.create(); + mgr1.create(); + mgr1.create(); + mgr1.create(); + mgr1.create(); - mgr2.create(); + mgr2.create(); - mgr2.duplicateAddons(&mgr1); + mgr2.duplicateAspects(&mgr1); - EXPECT_FALSE(mgr2.get() == nullptr); - EXPECT_FALSE(mgr2.get() == nullptr); - EXPECT_FALSE(mgr2.get() == nullptr); - EXPECT_FALSE(mgr2.get() == nullptr); + EXPECT_FALSE(mgr2.get() == nullptr); + EXPECT_FALSE(mgr2.get() == nullptr); + EXPECT_FALSE(mgr2.get() == nullptr); + EXPECT_FALSE(mgr2.get() == nullptr); } int main(int argc, char* argv[]) diff --git a/unittests/testInverseKinematics.cpp b/unittests/testInverseKinematics.cpp index 26ffd50057530..21a647f5906f5 100644 --- a/unittests/testInverseKinematics.cpp +++ b/unittests/testInverseKinematics.cpp @@ -58,7 +58,7 @@ SkeletonPtr createFreeFloatingTwoLinkRobot(Vector3d dim1, BodyNode* parent_node = robot->createJointAndBodyNodePair(nullptr, MultiDofJoint<6>::Properties(std::string("joint1")), node).second; - parent_node->createShapeNodeWith( + parent_node->createShapeNodeWith( shape); // Create the second link @@ -68,7 +68,7 @@ SkeletonPtr createFreeFloatingTwoLinkRobot(Vector3d dim1, std::pair pair2 = add1DofJoint( robot, parent_node, node, "joint2", 0.0, -DART_PI, DART_PI, type2); - pair2.second->createShapeNodeWith( + pair2.second->createShapeNodeWith( shape); Joint* joint = pair2.first; diff --git a/unittests/testJoints.cpp b/unittests/testJoints.cpp index 3a12435e68afd..226e8324729b8 100644 --- a/unittests/testJoints.cpp +++ b/unittests/testJoints.cpp @@ -638,7 +638,7 @@ SkeletonPtr createPendulum(Joint::ActuatorType actType) for (size_t i = 0; i < pendulum->getNumBodyNodes(); ++i) { auto bodyNode = pendulum->getBodyNode(i); - bodyNode->removeAllShapeNodesWith(); + bodyNode->removeAllShapeNodesWith(); } // Joint common setting diff --git a/unittests/testSdfParser.cpp b/unittests/testSdfParser.cpp index e46558139a7b7..57222a61fc3ad 100644 --- a/unittests/testSdfParser.cpp +++ b/unittests/testSdfParser.cpp @@ -67,8 +67,8 @@ TEST(SdfParser, SDFSingleBodyWithoutJoint) BodyNodePtr bodyNode = skel->getBodyNode(0); EXPECT_TRUE(bodyNode != nullptr); - EXPECT_EQ(bodyNode->getNumShapeNodesWith(), 1u); - EXPECT_EQ(bodyNode->getNumShapeNodesWith(), 1u); + EXPECT_EQ(bodyNode->getNumShapeNodesWith(), 1u); + EXPECT_EQ(bodyNode->getNumShapeNodesWith(), 1u); JointPtr joint = skel->getJoint(0); EXPECT_TRUE(joint != nullptr); From 7b5abbb866de54551f946936648024e841292f94 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Mon, 28 Mar 2016 14:24:01 -0400 Subject: [PATCH 06/62] more renaming --- dart/common/Aspect.cpp | 8 +- dart/common/Aspect.h | 37 +++--- dart/common/AspectWithVersion.h | 30 ++--- dart/common/Composite.cpp | 33 +++--- dart/common/Composite.h | 24 ++-- dart/common/SpecializedForAspect.h | 2 +- dart/common/detail/Aspect.h | 33 +++--- dart/common/detail/AspectWithVersion.h | 92 +++++++-------- dart/common/detail/Composite.h | 108 +++++++++--------- dart/common/detail/SpecializedForAspect.h | 10 +- dart/dynamics/EndEffector.cpp | 2 +- dart/dynamics/Joint.h | 4 +- dart/dynamics/ShapeFrame.cpp | 4 +- dart/dynamics/SoftBodyNode.cpp | 4 +- .../dynamics/detail/MultiDofJointProperties.h | 2 +- .../detail/SingleDofJointProperties.cpp | 2 +- 16 files changed, 213 insertions(+), 182 deletions(-) diff --git a/dart/common/Aspect.cpp b/dart/common/Aspect.cpp index ce51eb0b5b3d4..53419e00ef170 100644 --- a/dart/common/Aspect.cpp +++ b/dart/common/Aspect.cpp @@ -80,7 +80,13 @@ Aspect::Aspect(Composite* manager) } //============================================================================== -void Aspect::setManager(Composite* /*newManager*/, bool /*transfer*/) +void Aspect::setComposite(Composite* /*newComposite*/) +{ + // Do nothing +} + +//============================================================================== +void Aspect::loseComposite(Composite* /*oldComposite*/) { // Do nothing } diff --git a/dart/common/Aspect.h b/dart/common/Aspect.h index 0885be17a1890..8c0cb9242e2ee 100644 --- a/dart/common/Aspect.h +++ b/dart/common/Aspect.h @@ -53,7 +53,7 @@ class Aspect friend class Composite; - /// If your Aspect has a State class, then that State class should inherit this + /// If your Aspect has a State, then that State class should inherit this /// Aspect::State class. This allows us to safely serialize, store, and clone /// the states of arbitrary Aspect extensions. If your Aspect is stateless, then /// you do not have to worry about extending this class, because @@ -72,7 +72,7 @@ class Aspect template using StateMixer = ExtensibleMixer; - /// If your Aspect has a Properties class, then it should inherit this + /// If your Aspect has Properties, then that Properties class should inherit this /// Aspect::Properties class. This allows us to safely serialize, store, and /// clone the properties of arbitrary Aspect extensions. If your Aspect has no /// properties, then you do not have to worry about extending this class, @@ -95,7 +95,7 @@ class Aspect virtual ~Aspect() = default; /// Clone this Aspect into a new manager - virtual std::unique_ptr cloneAspect(Composite* newManager) const = 0; + virtual std::unique_ptr cloneAspect(Composite* newComposite) const = 0; /// Set the State of this Aspect. By default, this does nothing. virtual void setAspectState(const State& otherState); @@ -116,7 +116,7 @@ class Aspect /// Constructor /// /// We require the Composite argument in this constructor to make it clear - /// to extensions that they must have an Composite argument in their + /// to extensions that they must have a Composite argument in their /// constructors. Aspect(Composite* manager); @@ -125,32 +125,37 @@ class Aspect /// to a new Composite [transfer will be true]. You should override this /// function if your Aspect requires special handling in either of those cases. /// By default, this function does nothing. - virtual void setManager(Composite* newManager, bool transfer); + virtual void setComposite(Composite* newComposite); + /// This function will be triggered if your Aspect is about to be removed from + /// its Composite. While this function is being called, the Aspect is still a + /// valid part of the Composite; it will be removed immediately after this + /// function call. By default, this function does nothing. + virtual void loseComposite(Composite* oldComposite); }; //============================================================================== -template -class ManagerTrackingAspect : public Aspect +template +class CompositeTrackingAspect : public Aspect { public: /// Default constructor - ManagerTrackingAspect(Composite* mgr); + CompositeTrackingAspect(Composite* mgr); - /// Get the Manager of this Aspect - ManagerType* getManager(); + /// Get the Composite of this Aspect + CompositeType* getComposite(); - /// Get the Manager of this Aspect - const ManagerType* getManager() const; + /// Get the Composite of this Aspect + const CompositeType* getComposite() const; protected: /// Grab the new manager - void setManager(Composite* newManager, bool transfer); + void setComposite(Composite* newComposite); - /// Pointer to the current Manager of this Aspect - ManagerType* mManager; + /// Pointer to the current Composite of this Aspect + CompositeType* mComposite; }; @@ -161,7 +166,7 @@ class ManagerTrackingAspect : public Aspect #define DART_COMMON_ASPECT_PROPERTY_CONSTRUCTOR( ClassName, UpdatePropertiesMacro )\ ClassName (const ClassName &) = delete;\ inline ClassName (dart::common::Composite* mgr, const PropertiesData& properties = PropertiesData())\ - : AspectWithVersionedProperties< Base, Derived, PropertiesData, ManagerType, UpdatePropertiesMacro>(mgr, properties) { } + : AspectWithVersionedProperties< Base, Derived, PropertiesData, CompositeType, UpdatePropertiesMacro>(mgr, properties) { } //============================================================================== #define DART_COMMON_JOINT_ASPECT_CONSTRUCTOR( ClassName )\ diff --git a/dart/common/AspectWithVersion.h b/dart/common/AspectWithVersion.h index 11d25b68e8ec2..835756b1cda79 100644 --- a/dart/common/AspectWithVersion.h +++ b/dart/common/AspectWithVersion.h @@ -45,51 +45,51 @@ namespace common { //============================================================================== template > using AspectWithState = - detail::AspectWithState, DerivedT, StateDataT, ManagerT, updateState>; + detail::AspectWithState, DerivedT, StateDataT, CompositeT, updateState>; //============================================================================== template > using AspectWithVersionedProperties = - detail::AspectWithVersionedProperties, DerivedT, PropertiesDataT, ManagerT, updateProperties>; + detail::AspectWithVersionedProperties, DerivedT, PropertiesDataT, CompositeT, updateProperties>; //============================================================================== template , void (*updateProperties)(DerivedT*) = updateState> class AspectWithStateAndVersionedProperties : public detail::AspectWithVersionedProperties< - AspectWithState, - DerivedT, PropertiesDataT, ManagerT, updateProperties> + AspectWithState, + DerivedT, PropertiesDataT, CompositeT, updateProperties> { public: using Derived = DerivedT; using StateData = StateDataT; using PropertiesData = PropertiesDataT; - using ManagerType = ManagerT; + using CompositeType = CompositeT; using State = common::Aspect::StateMixer; using Properties = common::Aspect::PropertiesMixer; constexpr static void (*UpdateState)(Derived*) = updateState; constexpr static void (*UpdateProperties)(Derived*) = updateProperties; using AspectStateImplementation = AspectWithState< - Derived, StateData, ManagerType, updateState>; + Derived, StateData, CompositeType, updateState>; using AspectPropertiesImplementation = detail::AspectWithVersionedProperties< AspectStateImplementation, - Derived, PropertiesData, ManagerType, updateProperties>; + Derived, PropertiesData, CompositeType, updateProperties>; using AspectImplementation = AspectWithStateAndVersionedProperties< - DerivedT, StateDataT, PropertiesDataT, ManagerT, + DerivedT, StateDataT, PropertiesDataT, CompositeT, updateState, updateProperties>; AspectWithStateAndVersionedProperties() = delete; @@ -128,22 +128,22 @@ class AspectWithStateAndVersionedProperties : template constexpr void (*AspectWithStateAndVersionedProperties::UpdateState) + PropertiesDataT, CompositeT, updateState, updateProperties>::UpdateState) (DerivedT*); //============================================================================== template constexpr void (*AspectWithStateAndVersionedProperties::UpdateProperties) + PropertiesDataT, CompositeT, updateState, updateProperties>::UpdateProperties) (DerivedT*); } // namespace common diff --git a/dart/common/Composite.cpp b/dart/common/Composite.cpp index 672e0f4f4d16d..edefca4643143 100644 --- a/dart/common/Composite.cpp +++ b/dart/common/Composite.cpp @@ -188,9 +188,9 @@ void Composite::copyAspectPropertiesTo( } //============================================================================== -void Composite::duplicateAspects(const Composite* fromManager) +void Composite::duplicateAspects(const Composite* fromComposite) { - if(nullptr == fromManager) + if(nullptr == fromComposite) { dterr << "[Composite::duplicateAspects] You have asked to duplicate the " << "Aspects of a nullptr, which is not allowed!\n"; @@ -198,10 +198,10 @@ void Composite::duplicateAspects(const Composite* fromManager) return; } - if(this == fromManager) + if(this == fromComposite) return; - const AspectMap& otherMap = fromManager->mAspectMap; + const AspectMap& otherMap = fromComposite->mAspectMap; AspectMap::iterator receiving = mAspectMap.begin(); AspectMap::const_iterator incoming = otherMap.begin(); @@ -210,7 +210,7 @@ void Composite::duplicateAspects(const Composite* fromManager) { if( mAspectMap.end() == receiving ) { - // If we've reached the end of this Manager's AspectMap, then we should + // If we've reached the end of this Composite's AspectMap, then we should // just add each entry _set(incoming->first, incoming->second.get()); ++incoming; @@ -229,7 +229,7 @@ void Composite::duplicateAspects(const Composite* fromManager) } else { - // If this Manager does not have an entry corresponding to the incoming + // If this Composite does not have an entry corresponding to the incoming // Aspect, then we must create it _set(incoming->first, incoming->second.get()); ++incoming; @@ -238,9 +238,9 @@ void Composite::duplicateAspects(const Composite* fromManager) } //============================================================================== -void Composite::matchAspects(const Composite* otherManager) +void Composite::matchAspects(const Composite* otherComposite) { - if(nullptr == otherManager) + if(nullptr == otherComposite) { dterr << "[Composite::matchAspects] You have asked to match the Aspects " << "of a nullptr, which is not allowed!\n"; @@ -251,14 +251,21 @@ void Composite::matchAspects(const Composite* otherManager) for(auto& aspect : mAspectMap) aspect.second = nullptr; - duplicateAspects(otherManager); + duplicateAspects(otherComposite); } //============================================================================== -void Composite::becomeManager(Aspect* aspect, bool transfer) +void Composite::addToComposite(Aspect* aspect) { if(aspect) - aspect->setManager(this, transfer); + aspect->setComposite(this); +} + +//============================================================================== +void Composite::removeFromComposite(Aspect* aspect) +{ + if(aspect) + aspect->loseComposite(this); } //============================================================================== @@ -267,7 +274,7 @@ void Composite::_set(std::type_index type_idx, const Aspect* aspect) if(aspect) { mAspectMap[type_idx] = aspect->cloneAspect(this); - becomeManager(mAspectMap[type_idx].get(), false); + addToComposite(mAspectMap[type_idx].get()); } else { @@ -279,7 +286,7 @@ void Composite::_set(std::type_index type_idx, const Aspect* aspect) void Composite::_set(std::type_index type_idx, std::unique_ptr aspect) { mAspectMap[type_idx] = std::move(aspect); - becomeManager(mAspectMap[type_idx].get(), true); + addToComposite(mAspectMap[type_idx].get()); } } // namespace common diff --git a/dart/common/Composite.h b/dart/common/Composite.h index 3aedca5c46a54..c236745244dc5 100644 --- a/dart/common/Composite.h +++ b/dart/common/Composite.h @@ -127,13 +127,13 @@ class Composite template std::unique_ptr release(); - /// Check if this Manager is specialized for a specific type of Aspect. + /// Check if this Composite is specialized for a specific type of Aspect. /// /// By default, this simply returns false. template static constexpr bool isSpecializedFor(); - /// Check if this Manager requires this specific type of Aspect + /// Check if this Composite requires this specific type of Aspect template bool requires() const; @@ -160,21 +160,25 @@ class Composite /// Composite void copyAspectPropertiesTo(Properties& outgoingProperties) const; - /// Give this Composite a copy of each Aspect from otherManager - void duplicateAspects(const Composite* fromManager); + /// Give this Composite a copy of each Aspect from otherComposite + void duplicateAspects(const Composite* fromComposite); - /// Make the Aspects of this Composite match the Aspects of otherManager. Any - /// Aspects in this Composite which do not exist in otherManager will be + /// Make the Aspects of this Composite match the Aspects of otherComposite. Any + /// Aspects in this Composite which do not exist in otherComposite will be /// erased. - void matchAspects(const Composite* otherManager); + void matchAspects(const Composite* otherComposite); protected: template struct type { }; - /// Become the Composite of the given Aspect. This allows derived - /// Composite types to call the protected Aspect::setManager function. - void becomeManager(Aspect* aspect, bool transfer); + /// Add this Aspect to the Composite. This allows derived Composite types to + /// call the protected Aspect::setComposite function. + void addToComposite(Aspect* aspect); + + /// Remove this Aspect from the Composite. This allows derived Composite types + /// to call the protected Aspect::loseComposite function. + void removeFromComposite(Aspect* aspect); /// Non-templated version of set(const T*) void _set(std::type_index type_idx, const Aspect* aspect); diff --git a/dart/common/SpecializedForAspect.h b/dart/common/SpecializedForAspect.h index b315f2abb3bfd..a48d901cecc84 100644 --- a/dart/common/SpecializedForAspect.h +++ b/dart/common/SpecializedForAspect.h @@ -97,7 +97,7 @@ class SpecializedForAspect : public virtual Composite template std::unique_ptr release(); - /// Check if this Manager is specialized for a specific type of Aspect + /// Check if this Composite is specialized for a specific type of Aspect template static constexpr bool isSpecializedFor(); diff --git a/dart/common/detail/Aspect.h b/dart/common/detail/Aspect.h index a8fa53cb8bb6e..79042124b959e 100644 --- a/dart/common/detail/Aspect.h +++ b/dart/common/detail/Aspect.h @@ -46,41 +46,40 @@ namespace dart { namespace common { //============================================================================== -template -ManagerTrackingAspect::ManagerTrackingAspect( +template +CompositeTrackingAspect::CompositeTrackingAspect( Composite* mgr) : Aspect(mgr), - mManager(nullptr) // This will be set later when the Manager calls setManager + mComposite(nullptr) // This will be set later when the Composite calls setComposite { // Do nothing } //============================================================================== -template -ManagerType* ManagerTrackingAspect::getManager() +template +CompositeType* CompositeTrackingAspect::getComposite() { - return mManager; + return mComposite; } //============================================================================== -template -const ManagerType* ManagerTrackingAspect::getManager() const +template +const CompositeType* CompositeTrackingAspect::getComposite() const { - return mManager; + return mComposite; } //============================================================================== -template -void ManagerTrackingAspect::setManager( - Composite* newManager, bool) +template +void CompositeTrackingAspect::setComposite(Composite* newComposite) { - mManager = dynamic_cast(newManager); - if(nullptr == mManager) + mComposite = dynamic_cast(newComposite); + if(nullptr == mComposite) { - dterr << "[" << typeid(*this).name() << "::setManager] Attempting to use a " - << "[" << typeid(newManager).name() << "] type manager, but this " + dterr << "[" << typeid(*this).name() << "::setComposite] Attempting to use a " + << "[" << typeid(newComposite).name() << "] type manager, but this " << "Aspect is only designed to be attached to a [" - << typeid(ManagerType).name() << "] type manager. This may cause " + << typeid(CompositeType).name() << "] type manager. This may cause " << "undefined behavior!\n"; assert(false); } diff --git a/dart/common/detail/AspectWithVersion.h b/dart/common/detail/AspectWithVersion.h index 746152a80570f..59d030ed5323c 100644 --- a/dart/common/detail/AspectWithVersion.h +++ b/dart/common/detail/AspectWithVersion.h @@ -48,7 +48,7 @@ namespace detail { /// AspectWithProtectedState generates implementations of the State managing /// functions for an Aspect class. template > class AspectWithState : public BaseT { @@ -57,12 +57,12 @@ class AspectWithState : public BaseT using Base = BaseT; using Derived = DerivedT; using StateData = StateDataT; - using ManagerType = ManagerT; + using CompositeType = CompositeT; using State = Aspect::StateMixer; constexpr static void (*UpdateState)(Derived*) = updateState; using AspectImplementation = AspectWithState< - Base, Derived, StateData, ManagerT, updateState>; + Base, Derived, StateData, CompositeT, updateState>; AspectWithState() = delete; AspectWithState(const AspectWithState&) = delete; @@ -94,7 +94,7 @@ class AspectWithState : public BaseT // Documentation inherited std::unique_ptr cloneAspect( - Composite* newManager) const override; + Composite* newComposite) const override; protected: @@ -106,7 +106,7 @@ class AspectWithState : public BaseT /// AspectWithProtectedProperties generates implementations of the Property /// managing functions for an Aspect class. template > class AspectWithVersionedProperties : public BaseT { @@ -115,12 +115,12 @@ class AspectWithVersionedProperties : public BaseT using Base = BaseT; using Derived = DerivedT; using PropertiesData = PropertiesDataT; - using ManagerType = ManagerT; + using CompositeType = CompositeT; using Properties = Aspect::PropertiesMixer; constexpr static void (*UpdateProperties)(Derived*) = updateProperties; using AspectImplementation = AspectWithVersionedProperties< - Base, Derived, PropertiesData, ManagerT, updateProperties>; + Base, Derived, PropertiesData, CompositeT, updateProperties>; AspectWithVersionedProperties() = delete; AspectWithVersionedProperties(const AspectWithVersionedProperties&) = delete; @@ -153,9 +153,9 @@ class AspectWithVersionedProperties : public BaseT // Documentation inherited std::unique_ptr cloneAspect( - Composite* newManager) const override; + Composite* newComposite) const override; - /// Increment the version of this Aspect and its Manager + /// Increment the version of this Aspect and its Composite size_t incrementVersion(); /// Call UpdateProperties(this) and incrementVersion() @@ -176,15 +176,15 @@ class AspectWithVersionedProperties : public BaseT // See this StackOverflow answer: http://stackoverflow.com/a/14396189/111426 // template + class CompositeT, void (*updateState)(DerivedT*)> constexpr void (*AspectWithState< - BaseT, DerivedT, StateDataT, ManagerT, updateState>::UpdateState)( + BaseT, DerivedT, StateDataT, CompositeT, updateState>::UpdateState)( DerivedT*); //============================================================================== template -AspectWithState:: + class CompositeT, void (*updateState)(DerivedT*)> +AspectWithState:: AspectWithState(Composite* mgr, const StateDataT& state) : BaseT(mgr), mState(state) @@ -194,8 +194,8 @@ AspectWithState(Composite* mgr, const StateDataT& state) //============================================================================== template -void AspectWithState:: + class CompositeT, void (*updateState)(DerivedT*)> +void AspectWithState:: setAspectState(const Aspect::State& otherState) { setState(static_cast(otherState)); @@ -203,9 +203,9 @@ setAspectState(const Aspect::State& otherState) //============================================================================== template + class CompositeT, void (*updateState)(DerivedT*)> const Aspect::State* -AspectWithState:: +AspectWithState:: getAspectState() const { return &mState; @@ -213,8 +213,8 @@ getAspectState() const //============================================================================== template -void AspectWithState:: + class CompositeT, void (*updateState)(DerivedT*)> +void AspectWithState:: setState(const StateData& state) { static_cast(mState) = state; @@ -223,8 +223,8 @@ setState(const StateData& state) //============================================================================== template -auto AspectWithState:: + class CompositeT, void (*updateState)(DerivedT*)> +auto AspectWithState:: getState() const -> const State& { return mState; @@ -232,12 +232,12 @@ getState() const -> const State& //============================================================================== template + class CompositeT, void (*updateState)(DerivedT*)> std::unique_ptr -AspectWithState:: - cloneAspect(Composite* newManager) const +AspectWithState:: + cloneAspect(Composite* newComposite) const { - return common::make_unique(newManager, mState); + return common::make_unique(newComposite, mState); } //============================================================================== @@ -248,16 +248,16 @@ AspectWithState:: // See this StackOverflow answer: http://stackoverflow.com/a/14396189/111426 // template + class CompositeT, void (*updateProperties)(DerivedT*)> constexpr void (*AspectWithVersionedProperties:: + CompositeT, updateProperties>:: UpdateProperties)(DerivedT*); //============================================================================== template + class CompositeT, void (*updateProperties)(DerivedT*)> AspectWithVersionedProperties:: + CompositeT, updateProperties>:: AspectWithVersionedProperties( Composite* mgr, const PropertiesData& properties) : BaseT(mgr), @@ -268,9 +268,9 @@ AspectWithVersionedProperties( //============================================================================== template + class CompositeT, void (*updateProperties)(DerivedT*)> void AspectWithVersionedProperties:: + CompositeT, updateProperties>:: setAspectProperties(const Aspect::Properties& someProperties) { setProperties(static_cast(someProperties)); @@ -278,10 +278,10 @@ setAspectProperties(const Aspect::Properties& someProperties) //============================================================================== template + class CompositeT, void (*updateProperties)(DerivedT*)> const Aspect::Properties* AspectWithVersionedProperties:: + CompositeT, updateProperties>:: getAspectProperties() const { return &mProperties; @@ -289,9 +289,9 @@ getAspectProperties() const //============================================================================== template + class CompositeT, void (*updateProperties)(DerivedT*)> void AspectWithVersionedProperties:: + CompositeT, updateProperties>:: setProperties(const PropertiesData& properties) { static_cast(mProperties) = properties; @@ -300,9 +300,9 @@ setProperties(const PropertiesData& properties) //============================================================================== template + class CompositeT, void (*updateProperties)(DerivedT*)> auto AspectWithVersionedProperties:: + CompositeT, updateProperties>:: getProperties() const -> const Properties& { return mProperties; @@ -310,22 +310,22 @@ getProperties() const -> const Properties& //============================================================================== template + class CompositeT, void (*updateProperties)(DerivedT*)> std::unique_ptr AspectWithVersionedProperties:: -cloneAspect(Composite* newManager) const + CompositeT, updateProperties>:: +cloneAspect(Composite* newComposite) const { - return common::make_unique(newManager, mProperties); + return common::make_unique(newComposite, mProperties); } //============================================================================== template + class CompositeT, void (*updateProperties)(DerivedT*)> size_t AspectWithVersionedProperties::incrementVersion() + CompositeT, updateProperties>::incrementVersion() { - if(ManagerType* mgr = this->getManager()) + if(CompositeType* mgr = this->getComposite()) return mgr->incrementVersion(); return 0; @@ -333,10 +333,10 @@ size_t AspectWithVersionedProperties + class CompositeT, void (*updateProperties)(DerivedT*)> void AspectWithVersionedProperties< BaseT, DerivedT, PropertiesData, - ManagerT, updateProperties>::notifyPropertiesUpdate() + CompositeT, updateProperties>::notifyPropertiesUpdate() { UpdateProperties(static_cast(this)); this->incrementVersion(); diff --git a/dart/common/detail/Composite.h b/dart/common/detail/Composite.h index a4c1e7c89c93c..17fd003ecedad 100644 --- a/dart/common/detail/Composite.h +++ b/dart/common/detail/Composite.h @@ -96,7 +96,7 @@ T* Composite::create(Args&&... args) { T* aspect = new T(this, std::forward(args)...); mAspectMap[typeid(T)] = std::unique_ptr(aspect); - becomeManager(aspect, false); + addToComposite(aspect); return aspect; } @@ -108,7 +108,10 @@ void Composite::erase() AspectMap::iterator it = mAspectMap.find( typeid(T) ); DART_COMMON_CHECK_ILLEGAL_ASPECT_ERASE(erase, T, DART_BLANK) if(mAspectMap.end() != it) + { + removeFromComposite(it->second.get()); it->second = nullptr; + } } //============================================================================== @@ -119,7 +122,10 @@ std::unique_ptr Composite::release() AspectMap::iterator it = mAspectMap.find( typeid(T) ); DART_COMMON_CHECK_ILLEGAL_ASPECT_ERASE(release, T, nullptr) if(mAspectMap.end() != it) + { + removeFromComposite(it->second.get()); extraction = std::unique_ptr(static_cast(it->second.release())); + } return extraction; } @@ -159,56 +165,56 @@ void createAspects(T* mgr) //============================================================================== // Create non-template alternatives to Composite functions -#define DART_BAKE_SPECIALIZED_ASPECT_IRREGULAR( TypeName, AspectName ) \ - inline bool has ## AspectName () const \ - { \ - return this->template has(); \ - } \ - \ - inline TypeName * get ## AspectName () \ - { \ - return this->template get(); \ - } \ - \ - inline const TypeName* get ## AspectName () const \ - { \ - return this->template get(); \ - } \ - \ - inline TypeName * get ## AspectName (const bool createIfNull) \ - { \ - TypeName* aspect = get ## AspectName(); \ - \ - if (createIfNull && nullptr == aspect) \ - return create ## AspectName(); \ - \ - return aspect; \ - } \ - \ - inline void set ## AspectName (const TypeName * aspect) \ - { \ - this->template set(aspect); \ - } \ - \ - inline void set ## AspectName (std::unique_ptr< TypeName >&& aspect) \ - { \ - this->template set(std::move(aspect)); \ - } \ - \ - template \ - inline TypeName * create ## AspectName (Args&&... args) \ - { \ - return this->template create(std::forward(args)...); \ - } \ - \ - inline void erase ## AspectName () \ - { \ - this->template erase(); \ - } \ - \ - inline std::unique_ptr< TypeName > release ## AspectName () \ - { \ - return this->template release(); \ +#define DART_BAKE_SPECIALIZED_ASPECT_IRREGULAR( TypeName, AspectName )\ + inline bool has ## AspectName () const\ + {\ + return this->template has();\ + }\ +\ + inline TypeName * get ## AspectName ()\ + {\ + return this->template get();\ + }\ +\ + inline const TypeName* get ## AspectName () const\ + {\ + return this->template get();\ + }\ +\ + inline TypeName * get ## AspectName (const bool createIfNull)\ + {\ + TypeName* aspect = get ## AspectName();\ +\ + if (createIfNull && nullptr == aspect)\ + return create ## AspectName();\ +\ + return aspect;\ + }\ +\ + inline void set ## AspectName (const TypeName * aspect)\ + {\ + this->template set(aspect);\ + }\ +\ + inline void set ## AspectName (std::unique_ptr< TypeName >&& aspect)\ + {\ + this->template set(std::move(aspect));\ + }\ +\ + template \ + inline TypeName * create ## AspectName (Args&&... args)\ + {\ + return this->template create(std::forward(args)...);\ + }\ +\ + inline void erase ## AspectName ()\ + {\ + this->template erase();\ + }\ +\ + inline std::unique_ptr< TypeName > release ## AspectName ()\ + {\ + return this->template release();\ } //============================================================================== diff --git a/dart/common/detail/SpecializedForAspect.h b/dart/common/detail/SpecializedForAspect.h index a6c9697cba536..5f4d8036abe7c 100644 --- a/dart/common/detail/SpecializedForAspect.h +++ b/dart/common/detail/SpecializedForAspect.h @@ -210,7 +210,7 @@ void SpecializedForAspect::_set( if(aspect) { mSpecAspectIterator->second = aspect->cloneAspect(this); - becomeManager(mSpecAspectIterator->second.get(), false); + addToComposite(mSpecAspectIterator->second.get()); } else { @@ -236,7 +236,7 @@ void SpecializedForAspect::_set( #endif // DART_UNITTEST_SPECIALIZED_ASPECT_ACCESS mSpecAspectIterator->second = std::move(aspect); - becomeManager(mSpecAspectIterator->second.get(), true); + addToComposite(mSpecAspectIterator->second.get()); } //============================================================================== @@ -259,7 +259,7 @@ SpecAspect* SpecializedForAspect::_create( SpecAspect* aspect = new SpecAspect(this, std::forward(args)...); mSpecAspectIterator->second = std::unique_ptr(aspect); - becomeManager(aspect, false); + addToComposite(aspect); return aspect; } @@ -281,6 +281,8 @@ void SpecializedForAspect::_erase(type) #endif // DART_UNITTEST_SPECIALIZED_ASPECT_ACCESS DART_COMMON_CHECK_ILLEGAL_ASPECT_ERASE(erase, SpecAspect, DART_BLANK); + + removeFromComposite(mSpecAspectIterator->second.get()); mSpecAspectIterator->second = nullptr; } @@ -302,6 +304,8 @@ std::unique_ptr SpecializedForAspect::_release( #endif // DART_UNITTEST_SPECIALIZED_ASPECT_ACCESS DART_COMMON_CHECK_ILLEGAL_ASPECT_ERASE(release, SpecAspect, nullptr); + + removeFromComposite(mSpecAspectIterator->second.get()); std::unique_ptr extraction( static_cast(mSpecAspectIterator->second.release())); diff --git a/dart/dynamics/EndEffector.cpp b/dart/dynamics/EndEffector.cpp index 58cfa9b59a6ef..ee0592b632700 100644 --- a/dart/dynamics/EndEffector.cpp +++ b/dart/dynamics/EndEffector.cpp @@ -45,7 +45,7 @@ namespace detail { void SupportUpdate(Support* support) { - if(EndEffector* ee = support->getManager()) + if(EndEffector* ee = support->getComposite()) ee->getSkeleton()->notifySupportUpdate(ee->getTreeIndex()); } diff --git a/dart/dynamics/Joint.h b/dart/dynamics/Joint.h index 48bb7d9662e09..05f5760ff65ae 100644 --- a/dart/dynamics/Joint.h +++ b/dart/dynamics/Joint.h @@ -1001,8 +1001,8 @@ namespace detail { template void JointPropertyUpdate(AspectType* aspect) { - aspect->getManager()->notifyPositionUpdate(); - aspect->getManager()->updateLocalJacobian(); + aspect->getComposite()->notifyPositionUpdate(); + aspect->getComposite()->updateLocalJacobian(); } } // namespace detail diff --git a/dart/dynamics/ShapeFrame.cpp b/dart/dynamics/ShapeFrame.cpp index 423bfddf2a24d..7626049fb812c 100644 --- a/dart/dynamics/ShapeFrame.cpp +++ b/dart/dynamics/ShapeFrame.cpp @@ -88,7 +88,7 @@ void VisualAspect::setRGBA(const Eigen::Vector4d& color) notifyPropertiesUpdate(); - mManager->getShape()->notifyColorUpdate(color); + mComposite->getShape()->notifyColorUpdate(color); } //============================================================================== @@ -119,7 +119,7 @@ void VisualAspect::setAlpha(const double alpha) notifyPropertiesUpdate(); - mManager->getShape()->notifyAlphaUpdate(alpha); + mComposite->getShape()->notifyAlphaUpdate(alpha); } //============================================================================== diff --git a/dart/dynamics/SoftBodyNode.cpp b/dart/dynamics/SoftBodyNode.cpp index 151c66ffe904f..48772b17e8972 100644 --- a/dart/dynamics/SoftBodyNode.cpp +++ b/dart/dynamics/SoftBodyNode.cpp @@ -125,14 +125,14 @@ SoftBodyNodeProperties::SoftBodyNodeProperties( //============================================================================== void SoftBodyNodeStateUpdate(SoftBodyAspect* aspect) { - if(SoftBodyNode* sbn = aspect->getManager()) + if(SoftBodyNode* sbn = aspect->getComposite()) sbn->mNotifier->notifyTransformUpdate(); } //============================================================================== void SoftBodyNodePropertiesUpdate(SoftBodyAspect* aspect) { - if(SoftBodyNode* sbn = aspect->getManager()) + if(SoftBodyNode* sbn = aspect->getComposite()) { sbn->configurePointMasses(sbn->mSoftShapeNode.lock()); SoftBodyNodeStateUpdate(aspect); diff --git a/dart/dynamics/detail/MultiDofJointProperties.h b/dart/dynamics/detail/MultiDofJointProperties.h index 92b319e93b753..af901e654e931 100644 --- a/dart/dynamics/detail/MultiDofJointProperties.h +++ b/dart/dynamics/detail/MultiDofJointProperties.h @@ -302,7 +302,7 @@ MultiDofJointAspect::MultiDofJointAspect( : common::AspectWithVersionedProperties< typename MultiDofJointAspect::Derived, typename MultiDofJointAspect::PropertiesData, - typename MultiDofJointAspect::ManagerType, + typename MultiDofJointAspect::CompositeType, &common::detail::NoOp::Derived*> >( mgr, properties) { diff --git a/dart/dynamics/detail/SingleDofJointProperties.cpp b/dart/dynamics/detail/SingleDofJointProperties.cpp index c83e5d5e342ca..65519904eefd0 100644 --- a/dart/dynamics/detail/SingleDofJointProperties.cpp +++ b/dart/dynamics/detail/SingleDofJointProperties.cpp @@ -90,7 +90,7 @@ SingleDofJointProperties::SingleDofJointProperties( const std::string& SingleDofJointAspect::setDofName( const std::string& name, bool preserveName) { - return getManager()->setDofName(0, name, preserveName); + return getComposite()->setDofName(0, name, preserveName); } } // namespace detail From 96cf10feb8ccdded34479a93c39190eac8aa8b6a Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Mon, 28 Mar 2016 14:44:25 -0400 Subject: [PATCH 07/62] renamed testAddon.cpp to testAspect.cpp --- unittests/{testAddon.cpp => testAspect.cpp} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename unittests/{testAddon.cpp => testAspect.cpp} (100%) diff --git a/unittests/testAddon.cpp b/unittests/testAspect.cpp similarity index 100% rename from unittests/testAddon.cpp rename to unittests/testAspect.cpp From c7bf610ae4f4332c5488d73cedb690e373bb71b2 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Mon, 28 Mar 2016 15:17:08 -0400 Subject: [PATCH 08/62] renamed Composite member function names --- dart/common/Composite.cpp | 16 ++++++------- dart/common/Composite.h | 12 +++++----- dart/dynamics/BodyNode.cpp | 4 ++-- dart/dynamics/BodyNode.h | 2 +- dart/dynamics/EndEffector.cpp | 14 +++++------ dart/dynamics/ShapeFrame.cpp | 4 ++-- dart/dynamics/ShapeNode.cpp | 4 ++-- osgDart/examples/osgSoftBodies.cpp | 4 ++-- unittests/testAspect.cpp | 38 ++++++++++++++++++++++++++---- 9 files changed, 64 insertions(+), 34 deletions(-) diff --git a/dart/common/Composite.cpp b/dart/common/Composite.cpp index edefca4643143..bbdd17be15d90 100644 --- a/dart/common/Composite.cpp +++ b/dart/common/Composite.cpp @@ -44,7 +44,7 @@ namespace dart { namespace common { //============================================================================== -void Composite::setAspectStates(const State& newStates) +void Composite::setCompositeState(const State& newStates) { const StateMap& stateMap = newStates.getMap(); @@ -124,23 +124,23 @@ void extractMapData(MapType& outgoingMap, const Composite::AspectMap& aspectMap) } //============================================================================== -Composite::State Composite::getAspectStates() const +Composite::State Composite::getCompositeState() const { State states; - copyAspectStatesTo(states); + copyCompositeStateTo(states); return states; } //============================================================================== -void Composite::copyAspectStatesTo(State& outgoingStates) const +void Composite::copyCompositeStateTo(State& outgoingStates) const { StateMap& states = outgoingStates.getMap(); extractMapData(states, mAspectMap); } //============================================================================== -void Composite::setAspectProperties(const Properties& newProperties) +void Composite::setCompositeProperties(const Properties& newProperties) { const PropertiesMap& propertiesMap = newProperties.getMap(); @@ -170,16 +170,16 @@ void Composite::setAspectProperties(const Properties& newProperties) } //============================================================================== -Composite::Properties Composite::getAspectProperties() const +Composite::Properties Composite::getCompositeProperties() const { Properties properties; - copyAspectPropertiesTo(properties); + copyCompositePropertiesTo(properties); return properties; } //============================================================================== -void Composite::copyAspectPropertiesTo( +void Composite::copyCompositePropertiesTo( Properties& outgoingProperties) const { PropertiesMap& properties = outgoingProperties.getMap(); diff --git a/dart/common/Composite.h b/dart/common/Composite.h index c236745244dc5..4d746da47962b 100644 --- a/dart/common/Composite.h +++ b/dart/common/Composite.h @@ -140,25 +140,25 @@ class Composite /// Set the states of the aspects in this Composite based on the given /// Composite::State. The states of any Aspect types that do not exist /// within this manager will be ignored. - void setAspectStates(const State& newStates); + void setCompositeState(const State& newStates); /// Get the states of the aspects inside of this Composite - State getAspectStates() const; + State getCompositeState() const; /// Fill outgoingStates with the states of the aspects inside this Composite - void copyAspectStatesTo(State& outgoingStates) const; + void copyCompositeStateTo(State& outgoingStates) const; /// Set the properties of the aspects in this Composite based on the given /// Composite::Properties. The properties of any Aspect types that do not /// exist within this manager will be ignored. - void setAspectProperties(const Properties& newProperties); + void setCompositeProperties(const Properties& newProperties); /// Get the properties of the aspects inside of this Composite - Properties getAspectProperties() const; + Properties getCompositeProperties() const; /// Fill outgoingProperties with the properties of the aspects inside this /// Composite - void copyAspectPropertiesTo(Properties& outgoingProperties) const; + void copyCompositePropertiesTo(Properties& outgoingProperties) const; /// Give this Composite a copy of each Aspect from otherComposite void duplicateAspects(const Composite* fromComposite); diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index ac01c0bb70dfc..eaa6bf857643f 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -233,7 +233,7 @@ void BodyNode::setProperties(const NodeProperties& _properties) //============================================================================== void BodyNode::setProperties(const AspectProperties& _properties) { - setAspectProperties(_properties); + setCompositeProperties(_properties); } //============================================================================== @@ -298,7 +298,7 @@ BodyNode::ExtendedProperties BodyNode::getExtendedProperties() const { return ExtendedProperties(getBodyNodeProperties(), getAttachedNodeProperties(), - getAspectProperties()); + getCompositeProperties()); } //============================================================================== diff --git a/dart/dynamics/BodyNode.h b/dart/dynamics/BodyNode.h index d875cc187778b..211bfa7e0d01a 100644 --- a/dart/dynamics/BodyNode.h +++ b/dart/dynamics/BodyNode.h @@ -115,7 +115,7 @@ class BodyNode : /// Set the Properties of the attached Nodes void setProperties(const NodeProperties& _properties); - /// Same as setAspectProperties() + /// Same as setCompositeProperties() void setProperties(const AspectProperties& _properties); /// Set the Properties of this BodyNode diff --git a/dart/dynamics/EndEffector.cpp b/dart/dynamics/EndEffector.cpp index ee0592b632700..e5b9128b2bfdf 100644 --- a/dart/dynamics/EndEffector.cpp +++ b/dart/dynamics/EndEffector.cpp @@ -101,20 +101,20 @@ EndEffector::PropertiesData::PropertiesData( void EndEffector::setState(const StateData& _state) { setRelativeTransform(_state.mRelativeTransform); - setAspectStates(_state.mAspectStates); + setCompositeState(_state.mAspectStates); } //============================================================================== void EndEffector::setState(StateData&& _state) { setRelativeTransform(_state.mRelativeTransform); - setAspectStates(std::move(_state.mAspectStates)); + setCompositeState(std::move(_state.mAspectStates)); } //============================================================================== EndEffector::StateData EndEffector::getEndEffectorState() const { - return StateData(mRelativeTf, getAspectStates()); + return StateData(mRelativeTf, getCompositeState()); } //============================================================================== @@ -122,7 +122,7 @@ void EndEffector::setProperties(const PropertiesData& _properties, bool _useNow) { Entity::setProperties(_properties); setProperties(static_cast(_properties), _useNow); - setAspectProperties(_properties.mAspectProperties); + setCompositeProperties(_properties.mAspectProperties); } //============================================================================== @@ -136,7 +136,7 @@ void EndEffector::setProperties(const UniqueProperties& _properties, EndEffector::PropertiesData EndEffector::getEndEffectorProperties() const { return PropertiesData(getEntityProperties(), mEndEffectorP, - getAspectProperties()); + getCompositeProperties()); } //============================================================================== @@ -199,7 +199,7 @@ void EndEffector::copyNodeStateTo(std::unique_ptr& outputState) con static_cast(outputState.get()); state->mRelativeTransform = mRelativeTf; - copyAspectStatesTo(state->mAspectStates); + copyCompositeStateTo(state->mAspectStates); } else { @@ -231,7 +231,7 @@ void EndEffector::copyNodePropertiesTo( static_cast(*properties) = getEntityProperties(); static_cast(*properties) = mEndEffectorP; - copyAspectPropertiesTo(properties->mAspectProperties); + copyCompositePropertiesTo(properties->mAspectProperties); } else { diff --git a/dart/dynamics/ShapeFrame.cpp b/dart/dynamics/ShapeFrame.cpp index 7626049fb812c..fa4fb174a8bc4 100644 --- a/dart/dynamics/ShapeFrame.cpp +++ b/dart/dynamics/ShapeFrame.cpp @@ -213,7 +213,7 @@ ShapeFrame::Properties::Properties( //============================================================================== void ShapeFrame::setProperties(const ShapeFrame::AspectProperties& properties) { - setAspectProperties(properties); + setCompositeProperties(properties); } //============================================================================== @@ -233,7 +233,7 @@ void ShapeFrame::setProperties(const ShapeFrame::UniqueProperties& properties) //============================================================================== const ShapeFrame::Properties ShapeFrame::getShapeFrameProperties() const { - return Properties(getEntityProperties(), mShapeFrameP, getAspectProperties()); + return Properties(getEntityProperties(), mShapeFrameP, getCompositeProperties()); } //============================================================================== diff --git a/dart/dynamics/ShapeNode.cpp b/dart/dynamics/ShapeNode.cpp index edf02d889f9a0..21ee361852950 100644 --- a/dart/dynamics/ShapeNode.cpp +++ b/dart/dynamics/ShapeNode.cpp @@ -78,7 +78,7 @@ void ShapeNode::setProperties(const Properties& properties) ShapeFrame::setProperties( static_cast(properties)); setProperties(static_cast(properties)); - setAspectProperties(properties.mAspectProperties); + setCompositeProperties(properties.mAspectProperties); } //============================================================================== @@ -91,7 +91,7 @@ void ShapeNode::setProperties(const ShapeNode::UniqueProperties& properties) const ShapeNode::Properties ShapeNode::getShapeNodeProperties() const { return Properties(getShapeFrameProperties(), mShapeNodeP, - getAspectProperties()); + getCompositeProperties()); } //============================================================================== diff --git a/osgDart/examples/osgSoftBodies.cpp b/osgDart/examples/osgSoftBodies.cpp index 44ee31891d029..22f1552179ac0 100644 --- a/osgDart/examples/osgSoftBodies.cpp +++ b/osgDart/examples/osgSoftBodies.cpp @@ -69,7 +69,7 @@ class RecordingWorld : public osgDart::WorldNode for(size_t j=0; j < skeleton->getNumBodyNodes(); ++j) { BodyNode* bn = skeleton->getBodyNode(j); - state.mAspectStates.push_back(bn->getAspectStates()); + state.mAspectStates.push_back(bn->getCompositeState()); } slice.push_back(state); @@ -110,7 +110,7 @@ class RecordingWorld : public osgDart::WorldNode for(size_t j=0; j < skeleton->getNumBodyNodes(); ++j) { BodyNode* bn = skeleton->getBodyNode(j); - bn->setAspectStates(state.mAspectStates[j]); + bn->setCompositeState(state.mAspectStates[j]); } } diff --git a/unittests/testAspect.cpp b/unittests/testAspect.cpp index 419483bfa7d8d..ff109f3ba7cf1 100644 --- a/unittests/testAspect.cpp +++ b/unittests/testAspect.cpp @@ -47,11 +47,41 @@ #include "dart/common/sub_ptr.h" #include "dart/common/Composite.h" #include "dart/common/SpecializedForAspect.h" +#include "dart/common/EmbeddedAspect.h" #include "dart/dynamics/EulerJoint.h" using namespace dart::common; +struct SomeState { }; + +class SomeStateComposite : public virtual dart::common::Composite +{ +public: + + void setAspectState(const SomeState& state) { mState = state; } + + const SomeState& getAspectState() const { return mState; } + +protected: + + SomeState mState; + +}; + +class SomePropertiesComposite : public virtual dart::common::Composite +{ +public: + + +}; + +class SomeEmbeddedAspect : public dart::common::detail::EmbeddedStateAspect< + dart::common::CompositeTrackingAspect, SomeEmbeddedAspect, SomeState> +{ + +}; + // Testing the creation of an Aspect using the AspectWithState template class class StateAspectTest : public dart::common::AspectWithState< StateAspectTest, dart::common::Empty> @@ -451,7 +481,7 @@ TEST(Aspect, StateAndProperties) // ---- Test state transfer ---- - mgr2.setAspectStates(mgr1.getAspectStates()); + mgr2.setCompositeState(mgr1.getCompositeState()); EXPECT_EQ( mgr1.get()->mState.val, mgr2.get()->mState.val ); @@ -468,7 +498,7 @@ TEST(Aspect, StateAndProperties) EXPECT_NE( mgr1.get()->mState.val, mgr2.get()->mState.val ); - mgr1.setAspectStates( mgr2.getAspectStates() ); + mgr1.setCompositeState( mgr2.getCompositeState() ); EXPECT_EQ( mgr1.get()->mState.val, mgr2.get()->mState.val ); @@ -482,7 +512,7 @@ TEST(Aspect, StateAndProperties) // ---- Test property transfer ---- - mgr2.setAspectProperties(mgr1.getAspectProperties()); + mgr2.setCompositeProperties(mgr1.getCompositeProperties()); EXPECT_EQ( mgr1.get()->mProperties.val, mgr2.get()->mProperties.val ); @@ -499,7 +529,7 @@ TEST(Aspect, StateAndProperties) EXPECT_NE( mgr1.get()->mProperties.val, mgr2.get()->mProperties.val ); - mgr1.setAspectProperties( mgr2.getAspectProperties() ); + mgr1.setCompositeProperties( mgr2.getCompositeProperties() ); EXPECT_EQ( mgr1.get()->mProperties.val, mgr2.get()->mProperties.val ); From 29c7d682a96fab76a8c918dd6c173b97673c448c Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Mon, 28 Mar 2016 16:24:39 -0400 Subject: [PATCH 09/62] renamed mgr to comp to reflect change of class names --- dart/common/Aspect.h | 21 +++++++------ dart/common/AspectWithVersion.h | 8 ++--- dart/common/detail/Aspect.h | 12 +++++-- dart/common/detail/AspectWithVersion.h | 24 +++++++------- dart/dynamics/ShapeFrame.cpp | 14 ++++----- dart/dynamics/ShapeFrame.h | 6 ++-- .../dynamics/detail/MultiDofJointProperties.h | 7 ++--- unittests/testAspect.cpp | 31 +++++++++++++++---- 8 files changed, 75 insertions(+), 48 deletions(-) diff --git a/dart/common/Aspect.h b/dart/common/Aspect.h index 8c0cb9242e2ee..453670b34721d 100644 --- a/dart/common/Aspect.h +++ b/dart/common/Aspect.h @@ -141,7 +141,7 @@ class CompositeTrackingAspect : public Aspect public: /// Default constructor - CompositeTrackingAspect(Composite* mgr); + CompositeTrackingAspect(Composite* comp); /// Get the Composite of this Aspect CompositeType* getComposite(); @@ -151,8 +151,11 @@ class CompositeTrackingAspect : public Aspect protected: - /// Grab the new manager - void setComposite(Composite* newComposite); + /// Grab the new Composite + void setComposite(Composite* newComposite) override; + + /// Clear the old Composite + void loseComposite(Composite* oldComposite) override; /// Pointer to the current Composite of this Aspect CompositeType* mComposite; @@ -165,8 +168,8 @@ class CompositeTrackingAspect : public Aspect //============================================================================== #define DART_COMMON_ASPECT_PROPERTY_CONSTRUCTOR( ClassName, UpdatePropertiesMacro )\ ClassName (const ClassName &) = delete;\ - inline ClassName (dart::common::Composite* mgr, const PropertiesData& properties = PropertiesData())\ - : AspectWithVersionedProperties< Base, Derived, PropertiesData, CompositeType, UpdatePropertiesMacro>(mgr, properties) { } + inline ClassName (dart::common::Composite* comp, const PropertiesData& properties = PropertiesData())\ + : AspectWithVersionedProperties< Base, Derived, PropertiesData, CompositeType, UpdatePropertiesMacro>(comp, properties) { } //============================================================================== #define DART_COMMON_JOINT_ASPECT_CONSTRUCTOR( ClassName )\ @@ -175,10 +178,10 @@ class CompositeTrackingAspect : public Aspect //============================================================================== #define DART_COMMON_ASPECT_STATE_PROPERTY_CONSTRUCTORS(ClassName)\ ClassName (const ClassName &) = delete;\ - inline ClassName (dart::common::Composite* mgr, const StateData& state = StateData(), const PropertiesData& properties = PropertiesData())\ - : AspectImplementation(mgr, state, properties) { }\ - inline ClassName (dart::common::Composite* mgr, const PropertiesData& properties, const StateData state = StateData())\ - : AspectImplementation(mgr, properties, state) { } + inline ClassName (dart::common::Composite* comp, const StateData& state = StateData(), const PropertiesData& properties = PropertiesData())\ + : AspectImplementation(comp, state, properties) { }\ + inline ClassName (dart::common::Composite* comp, const PropertiesData& properties, const StateData state = StateData())\ + : AspectImplementation(comp, properties, state) { } //============================================================================== #define DART_COMMON_SET_ASPECT_PROPERTY_CUSTOM( Type, Name, Update )\ diff --git a/dart/common/AspectWithVersion.h b/dart/common/AspectWithVersion.h index 835756b1cda79..2827d5667ae0b 100644 --- a/dart/common/AspectWithVersion.h +++ b/dart/common/AspectWithVersion.h @@ -98,20 +98,20 @@ class AspectWithStateAndVersionedProperties : /// Construct using a StateData and a PropertiesData instance AspectWithStateAndVersionedProperties( - common::Composite* mgr, + common::Composite* comp, const StateData& state = StateData(), const PropertiesData& properties = PropertiesData()) - : AspectPropertiesImplementation(mgr, properties, state) + : AspectPropertiesImplementation(comp, properties, state) { // Do nothing } /// Construct using a PropertiesData and a StateData instance AspectWithStateAndVersionedProperties( - common::Composite* mgr, + common::Composite* comp, const PropertiesData& properties, const StateData& state = StateData()) - : AspectPropertiesImplementation(mgr, properties, state) + : AspectPropertiesImplementation(comp, properties, state) { // Do nothing } diff --git a/dart/common/detail/Aspect.h b/dart/common/detail/Aspect.h index 79042124b959e..30aa5ae996f7d 100644 --- a/dart/common/detail/Aspect.h +++ b/dart/common/detail/Aspect.h @@ -47,9 +47,8 @@ namespace common { //============================================================================== template -CompositeTrackingAspect::CompositeTrackingAspect( - Composite* mgr) - : Aspect(mgr), +CompositeTrackingAspect::CompositeTrackingAspect(Composite* comp) + : Aspect(comp), mComposite(nullptr) // This will be set later when the Composite calls setComposite { // Do nothing @@ -85,6 +84,13 @@ void CompositeTrackingAspect::setComposite(Composite* newComposit } } +//============================================================================== +template +void CompositeTrackingAspect::loseComposite(Composite* /*oldComposite*/) +{ + mComposite = nullptr; +} + } // namespace common } // namespace dart diff --git a/dart/common/detail/AspectWithVersion.h b/dart/common/detail/AspectWithVersion.h index 59d030ed5323c..9c69c71a06527 100644 --- a/dart/common/detail/AspectWithVersion.h +++ b/dart/common/detail/AspectWithVersion.h @@ -68,13 +68,13 @@ class AspectWithState : public BaseT AspectWithState(const AspectWithState&) = delete; /// Construct using a StateData instance - AspectWithState(Composite* mgr, const StateData& state = StateData()); + AspectWithState(Composite* comp, const StateData& state = StateData()); /// Construct this Aspect and pass args into the constructor of the Base class template - AspectWithState(Composite* mgr, const StateData& state, + AspectWithState(Composite* comp, const StateData& state, BaseArgs&&... args) - : Base(mgr, std::forward(args)...), + : Base(comp, std::forward(args)...), mState(state) { // Do nothing @@ -127,13 +127,13 @@ class AspectWithVersionedProperties : public BaseT /// Construct using a PropertiesData instance AspectWithVersionedProperties( - Composite* mgr, const PropertiesData& properties = PropertiesData()); + Composite* comp, const PropertiesData& properties = PropertiesData()); /// Construct this Aspect and pass args into the constructor of the Base class template AspectWithVersionedProperties( - Composite* mgr, const PropertiesData& properties, BaseArgs&&... args) - : Base(mgr, std::forward(args)...), + Composite* comp, const PropertiesData& properties, BaseArgs&&... args) + : Base(comp, std::forward(args)...), mProperties(properties) { // Do nothing @@ -185,8 +185,8 @@ constexpr void (*AspectWithState< template AspectWithState:: -AspectWithState(Composite* mgr, const StateDataT& state) - : BaseT(mgr), +AspectWithState(Composite* comp, const StateDataT& state) + : BaseT(comp), mState(state) { // Do nothing @@ -259,8 +259,8 @@ template :: AspectWithVersionedProperties( - Composite* mgr, const PropertiesData& properties) - : BaseT(mgr), + Composite* comp, const PropertiesData& properties) + : BaseT(comp), mProperties(properties) { // Do nothing @@ -325,8 +325,8 @@ template ::incrementVersion() { - if(CompositeType* mgr = this->getComposite()) - return mgr->incrementVersion(); + if(CompositeType* comp = this->getComposite()) + return comp->incrementVersion(); return 0; } diff --git a/dart/dynamics/ShapeFrame.cpp b/dart/dynamics/ShapeFrame.cpp index fa4fb174a8bc4..c84c286027725 100644 --- a/dart/dynamics/ShapeFrame.cpp +++ b/dart/dynamics/ShapeFrame.cpp @@ -74,9 +74,9 @@ DynamicsAspectProperties::DynamicsAspectProperties( } // namespace detail //============================================================================== -VisualAspect::VisualAspect(common::Composite* mgr, +VisualAspect::VisualAspect(common::Composite* comp, const PropertiesData& properties) - : VisualAspect::BaseClass(mgr, properties) + : VisualAspect::BaseClass(comp, properties) { // Do nothing } @@ -160,11 +160,11 @@ bool VisualAspect::isHidden() const //============================================================================== CollisionAspect::CollisionAspect( - common::Composite* mgr, + common::Composite* comp, const PropertiesData& properties) - : AspectImplementation(mgr, properties) + : AspectImplementation(comp, properties) { -// mIsShapeNode = dynamic_cast(mgr); +// mIsShapeNode = dynamic_cast(comp); } //============================================================================== @@ -175,9 +175,9 @@ bool CollisionAspect::isCollidable() const //============================================================================== DynamicsAspect::DynamicsAspect( - common::Composite* mgr, + common::Composite* comp, const PropertiesData& properties) - : BaseClass(mgr, properties) + : BaseClass(comp, properties) { // Do nothing } diff --git a/dart/dynamics/ShapeFrame.h b/dart/dynamics/ShapeFrame.h index b962a16dd378f..1287b04a88809 100644 --- a/dart/dynamics/ShapeFrame.h +++ b/dart/dynamics/ShapeFrame.h @@ -117,7 +117,7 @@ class VisualAspect final : VisualAspect, detail::VisualAspectProperties, ShapeFrame>; /// Constructor - VisualAspect(common::Composite* mgr, + VisualAspect(common::Composite* comp, const PropertiesData& properties = PropertiesData()); VisualAspect(const VisualAspect&) = delete; @@ -175,7 +175,7 @@ class CollisionAspect final : public: CollisionAspect(const CollisionAspect &) = delete; - CollisionAspect(dart::common::Composite* mgr, + CollisionAspect(dart::common::Composite* comp, const PropertiesData& properties = PropertiesData()); DART_COMMON_SET_GET_ASPECT_PROPERTY( bool, Collidable ) @@ -200,7 +200,7 @@ class DynamicsAspect final : DynamicsAspect(const DynamicsAspect&) = delete; - DynamicsAspect(dart::common::Composite* mgr, + DynamicsAspect(dart::common::Composite* comp, const PropertiesData& properties = PropertiesData()); DART_COMMON_SET_GET_ASPECT_PROPERTY( double, FrictionCoeff ) diff --git a/dart/dynamics/detail/MultiDofJointProperties.h b/dart/dynamics/detail/MultiDofJointProperties.h index af901e654e931..a8f6fdcda728f 100644 --- a/dart/dynamics/detail/MultiDofJointProperties.h +++ b/dart/dynamics/detail/MultiDofJointProperties.h @@ -164,7 +164,7 @@ class MultiDofJointAspect final : public: MultiDofJointAspect(const MultiDofJointAspect&) = delete; - MultiDofJointAspect(common::Composite* mgr, + MultiDofJointAspect(common::Composite* comp, const typename MultiDofJointAspect::PropertiesData& properties); constexpr static size_t NumDofs = DOF; @@ -295,8 +295,7 @@ constexpr size_t MultiDofJointAspect::NumDofs; //============================================================================== template -MultiDofJointAspect::MultiDofJointAspect( - common::Composite* mgr, +MultiDofJointAspect::MultiDofJointAspect(common::Composite* comp, const typename MultiDofJointAspect::PropertiesData& properties = typename MultiDofJointAspect::PropertiesData()) : common::AspectWithVersionedProperties< @@ -304,7 +303,7 @@ MultiDofJointAspect::MultiDofJointAspect( typename MultiDofJointAspect::PropertiesData, typename MultiDofJointAspect::CompositeType, &common::detail::NoOp::Derived*> >( - mgr, properties) + comp, properties) { // Do nothing } diff --git a/unittests/testAspect.cpp b/unittests/testAspect.cpp index ff109f3ba7cf1..dd1419fbcfc70 100644 --- a/unittests/testAspect.cpp +++ b/unittests/testAspect.cpp @@ -53,9 +53,19 @@ using namespace dart::common; -struct SomeState { }; +struct SomeStateData { }; +using SomeState = dart::common::Aspect::StateMixer; +struct SomePropertiesData { }; +using SomeProperties = dart::common::Aspect::PropertiesMixer; + +class SomeStateComposite; +class SomeEmbeddedStateAspect : public dart::common::EmbeddedStateAspect< + SomeEmbeddedStateAspect, SomeState, SomeStateComposite> +{ + +}; -class SomeStateComposite : public virtual dart::common::Composite +class SomeStateComposite : public virtual dart::common::RequiresAspect { public: @@ -73,15 +83,19 @@ class SomePropertiesComposite : public virtual dart::common::Composite { public: + void setAspectProperties(const SomeProperties& properties) { mProperties = properties; } -}; + const SomeProperties& getAspectProperties() const { return mProperties; } -class SomeEmbeddedAspect : public dart::common::detail::EmbeddedStateAspect< - dart::common::CompositeTrackingAspect, SomeEmbeddedAspect, SomeState> -{ +protected: + + SomeProperties mProperties; }; + + + // Testing the creation of an Aspect using the AspectWithState template class class StateAspectTest : public dart::common::AspectWithState< StateAspectTest, dart::common::Empty> @@ -627,6 +641,11 @@ TEST(Aspect, Duplication) EXPECT_FALSE(mgr2.get() == nullptr); } +TEST(Aspect, Embedded) +{ + SomeStateComposite comp; +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv); From 8e7cd2d54e9efcf5932599ab163f13917234c9a4 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Mon, 28 Mar 2016 16:25:04 -0400 Subject: [PATCH 10/62] beginning implementation of EmbeddedAspect --- dart/common/EmbeddedAspect.h | 75 +++++++++++ dart/common/detail/EmbeddedAspect.h | 190 ++++++++++++++++++++++++++++ 2 files changed, 265 insertions(+) create mode 100644 dart/common/EmbeddedAspect.h create mode 100644 dart/common/detail/EmbeddedAspect.h diff --git a/dart/common/EmbeddedAspect.h b/dart/common/EmbeddedAspect.h new file mode 100644 index 0000000000000..72606efb9027d --- /dev/null +++ b/dart/common/EmbeddedAspect.h @@ -0,0 +1,75 @@ +/* + * 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_COMMON_EMBEDDEDASPECT_H_ +#define DART_COMMON_EMBEDDEDASPECT_H_ + +#include "dart/common/detail/EmbeddedAspect.h" + +namespace dart { +namespace common { + +//============================================================================== +template , + const StateT& (*getState)(const DerivedT*) = + &detail::DefaultGetEmbeddedState > +using EmbeddedStateAspect = + detail::EmbeddedStateAspect, DerivedT, + StateT, setState, getState>; + +//============================================================================== +template , + const PropertiesT& (*getProperties)(const DerivedT*) = + &detail::DefaultGetEmbeddedProperties > +using EmbeddedPropertiesAspect = + detail::EmbeddedPropertiesAspect, DerivedT, + PropertiesT, setProperties, getProperties>; + +//============================================================================== + + +} // namespace common +} // namespace dart + +#endif // DART_COMMON_EMBEDDEDASPECT_H_ diff --git a/dart/common/detail/EmbeddedAspect.h b/dart/common/detail/EmbeddedAspect.h new file mode 100644 index 0000000000000..79b3890a2050f --- /dev/null +++ b/dart/common/detail/EmbeddedAspect.h @@ -0,0 +1,190 @@ +/* + * 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_COMMON_DETAIL_EMBEDDEDASPECT_H_ +#define DART_COMMON_DETAIL_EMBEDDEDASPECT_H_ + +#include "dart/common/Aspect.h" +#include "dart/common/StlHelpers.h" + +namespace dart { +namespace common { +namespace detail { + +//============================================================================== +template +void DefaultSetEmbeddedState(AspectT* aspect, const StateT& state) +{ + aspect->getComposite()->setAspectState(state); +} + +//============================================================================== +template +const StateT& DefaultGetEmbeddedState(const AspectT* aspect) +{ + return aspect->getComposite()->getAspectState(); +} + +//============================================================================== +template +void DefaultSetEmbeddedProperties(AspectT* aspect, const PropertiesT& properties) +{ + aspect->getComposite()->setAspectProperties(properties); +} + +//============================================================================== +template +const PropertiesT& DefaultGetEmbeddedProperties(const AspectT* aspect) +{ + return aspect->getComposite()->getAspectProperties(); +} + +//============================================================================== +template +class EmbeddedStateAspect : public BaseT +{ +public: + + using Base = BaseT; + using Derived = DerivedT; + using State = StateT; + constexpr static void (*SetState)(Derived*, const State&) = setState; + constexpr static const State& (*GetState)(const Derived*) = getState; + + EmbeddedStateAspect() = delete; + EmbeddedStateAspect(const EmbeddedStateAspect&) = delete; + + /// Construct using a State instance + EmbeddedStateAspect(Composite* comp, const State& state = State()) + : BaseT(comp), + mTemporaryState(make_unique(state)) + { + // Do nothing + } + + /// Construct this Aspect and pass args into the constructor of the Base class + template + EmbeddedStateAspect( + Composite* comp, const State& state, BaseArgs&&... args) + : Base(comp, std::forward(args)...), + mTemporaryState(make_unique(state)) + { + // Do nothing + } + + void setAspectState(const Aspect::State& state) override final + { + SetState(this, static_cast(state)); + } + + const Aspect::State* getAspectState() const override final + { + return &GetState(this); + } + + +protected: + + /// During transitions between Composite objects, this will hold the State of + /// the Aspect. Once the Aspect has been moved into a new Composite, this + /// State will be pushed into the Composite and cleared. + std::unique_ptr mTemporaryState; + +}; + +//============================================================================== +template , + const PropertiesT& (*getProperties)(const DerivedT*) = + &DefaultGetEmbeddedProperties > +class EmbeddedPropertiesAspect : public BaseT +{ +public: + + using Base = BaseT; + using Derived = DerivedT; + using Properties = PropertiesT; + constexpr static void (*SetProperties)(Derived*, const Properties&) = setProperties; + constexpr static const Properties& (*GetProperties)(const Derived*) = getProperties; + +}; + + +//============================================================================== +// +// These namespace-level definitions are required to enable ODR-use of static +// constexpr member variables. +// +// See this StackOverflow answer: http://stackoverflow.com/a/14396189/111426 +// +template +constexpr void (*EmbeddedStateAspect< + BaseT, DerivedT, StateT, setState, getState>::SetState)( + DerivedT*, const StateT&); + +//============================================================================== +template +constexpr const StateT& (*EmbeddedStateAspect< + BaseT, DerivedT, StateT, setState, getState>::GetState)(const DerivedT*); + +//============================================================================== +template +constexpr void (*EmbeddedPropertiesAspect< + BaseT, DerivedT, PropertiesT, setProperties, getProperties>::SetProperties)( + DerivedT*, const PropertiesT&); + +//============================================================================== +template +constexpr const PropertiesT& (*EmbeddedPropertiesAspect< + BaseT, DerivedT, PropertiesT, setProperties, getProperties>::GetProperties)( + const DerivedT*); + +} // namespace detail +} // namespace common +} // namespace dart + +#endif // DART_COMMON_DETAIL_EMBEDDEDASPECT_H_ From fb05138b67b7f3631935210ff6326c0813ef6bc2 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Mon, 28 Mar 2016 18:06:28 -0400 Subject: [PATCH 11/62] update name of CollisionAddon --- dart/collision/bullet/BulletCollisionNode.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dart/collision/bullet/BulletCollisionNode.cpp b/dart/collision/bullet/BulletCollisionNode.cpp index 10031008fad2f..115e3554c5610 100644 --- a/dart/collision/bullet/BulletCollisionNode.cpp +++ b/dart/collision/bullet/BulletCollisionNode.cpp @@ -55,7 +55,7 @@ namespace collision { BulletCollisionNode::BulletCollisionNode(dynamics::BodyNode* bodyNode) : CollisionNode(bodyNode) { - auto collShapeNodes = bodyNode->getShapeNodesWith(); + auto collShapeNodes = bodyNode->getShapeNodesWith(); for (auto shapeNode : collShapeNodes) { auto shape = shapeNode->getShape(); From aa068c009375374806777ebaa9fac1c9337bdb5d Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Mon, 28 Mar 2016 18:46:23 -0400 Subject: [PATCH 12/62] continuing EmbeddedAspect implementation --- dart/common/EmbeddedAspect.h | 13 +++--- dart/common/detail/EmbeddedAspect.h | 64 +++++++++++++++++++++++------ 2 files changed, 59 insertions(+), 18 deletions(-) diff --git a/dart/common/EmbeddedAspect.h b/dart/common/EmbeddedAspect.h index 72606efb9027d..ee48a9190e934 100644 --- a/dart/common/EmbeddedAspect.h +++ b/dart/common/EmbeddedAspect.h @@ -46,25 +46,26 @@ namespace common { template , - const StateT& (*getState)(const DerivedT*) = + const StateT& (*getEmbeddedState)(const DerivedT*) = &detail::DefaultGetEmbeddedState > using EmbeddedStateAspect = detail::EmbeddedStateAspect, DerivedT, - StateT, setState, getState>; + StateT, setEmbeddedState, getEmbeddedState>; //============================================================================== template , const PropertiesT& (*getProperties)(const DerivedT*) = &detail::DefaultGetEmbeddedProperties > using EmbeddedPropertiesAspect = - detail::EmbeddedPropertiesAspect, DerivedT, - PropertiesT, setProperties, getProperties>; + detail::EmbeddedPropertiesAspect< + CompositeTrackingAspect, DerivedT, + PropertiesT, setEmbeddedProperties, getProperties>; //============================================================================== diff --git a/dart/common/detail/EmbeddedAspect.h b/dart/common/detail/EmbeddedAspect.h index 79b3890a2050f..86f2e66fe8ac6 100644 --- a/dart/common/detail/EmbeddedAspect.h +++ b/dart/common/detail/EmbeddedAspect.h @@ -74,8 +74,8 @@ const PropertiesT& DefaultGetEmbeddedProperties(const AspectT* aspect) //============================================================================== template + void (*setEmbeddedState)(DerivedT*, const StateT&), + const StateT& (*getEmbeddedState)(const DerivedT*)> class EmbeddedStateAspect : public BaseT { public: @@ -83,8 +83,8 @@ class EmbeddedStateAspect : public BaseT using Base = BaseT; using Derived = DerivedT; using State = StateT; - constexpr static void (*SetState)(Derived*, const State&) = setState; - constexpr static const State& (*GetState)(const Derived*) = getState; + constexpr static void (*SetEmbeddedState)(Derived*, const State&) = setEmbeddedState; + constexpr static const State& (*GetEmbeddedState)(const Derived*) = getEmbeddedState; EmbeddedStateAspect() = delete; EmbeddedStateAspect(const EmbeddedStateAspect&) = delete; @@ -109,17 +109,57 @@ class EmbeddedStateAspect : public BaseT void setAspectState(const Aspect::State& state) override final { - SetState(this, static_cast(state)); + setState(static_cast(state)); + } + + void setState(const State& state) + { + if(this->getComposite()) + { + SetEmbeddedState(this, static_cast(state)); + return; + } + + mTemporaryState = make_unique(state); } const Aspect::State* getAspectState() const override final { - return &GetState(this); + return &getState(); } + const State& getState() const + { + if(this->getComposite()) + { + return GetEmbeddedState(this); + } + + if(!mTemporaryState) + mTemporaryState = make_unique(); + + return *mTemporaryState; + } protected: + void setComposite(Composite* newComposite) override + { + Base::setComposite(newComposite); + if(mTemporaryState) + SetEmbeddedState(this, *mTemporaryState); + else + SetEmbeddedState(this, State()); + + mTemporaryState = nullptr; + } + + void loseComposite(Composite* oldComposite) override + { + mTemporaryState = make_unique(GetEmbeddedState(this)); + Base::loseComposite(oldComposite); + } + /// During transitions between Composite objects, this will hold the State of /// the Aspect. Once the Aspect has been moved into a new Composite, this /// State will be pushed into the Composite and cleared. @@ -154,18 +194,18 @@ class EmbeddedPropertiesAspect : public BaseT // See this StackOverflow answer: http://stackoverflow.com/a/14396189/111426 // template + void (*setEmbeddedState)(DerivedT*, const StateT&), + const StateT& (*getEmbeddedState)(const DerivedT*)> constexpr void (*EmbeddedStateAspect< - BaseT, DerivedT, StateT, setState, getState>::SetState)( + BaseT, DerivedT, StateT, setEmbeddedState, getEmbeddedState>::SetEmbeddedState)( DerivedT*, const StateT&); //============================================================================== template + void (*setEmbeddedState)(DerivedT*, const StateT&), + const StateT& (*getEmbeddedState)(const DerivedT*)> constexpr const StateT& (*EmbeddedStateAspect< - BaseT, DerivedT, StateT, setState, getState>::GetState)(const DerivedT*); + BaseT, DerivedT, StateT, setEmbeddedState, getEmbeddedState>::GetEmbeddedState)(const DerivedT*); //============================================================================== template Date: Mon, 28 Mar 2016 21:13:29 -0400 Subject: [PATCH 13/62] implementing EmbeddedAspects -- need to fix template error --- dart/common/Aspect.h | 4 +- dart/common/AspectWithVersion.h | 19 ++- dart/common/EmbeddedAspect.h | 70 +++++++++- dart/common/detail/EmbeddedAspect.h | 129 +++++++++++++++--- .../dynamics/detail/MultiDofJointProperties.h | 2 +- unittests/testAspect.cpp | 17 ++- 6 files changed, 214 insertions(+), 27 deletions(-) diff --git a/dart/common/Aspect.h b/dart/common/Aspect.h index 453670b34721d..a54b8716a4995 100644 --- a/dart/common/Aspect.h +++ b/dart/common/Aspect.h @@ -179,9 +179,9 @@ class CompositeTrackingAspect : public Aspect #define DART_COMMON_ASPECT_STATE_PROPERTY_CONSTRUCTORS(ClassName)\ ClassName (const ClassName &) = delete;\ inline ClassName (dart::common::Composite* comp, const StateData& state = StateData(), const PropertiesData& properties = PropertiesData())\ - : AspectImplementation(comp, state, properties) { }\ + : AspectImpl(comp, state, properties) { }\ inline ClassName (dart::common::Composite* comp, const PropertiesData& properties, const StateData state = StateData())\ - : AspectImplementation(comp, properties, state) { } + : AspectImpl(comp, properties, state) { } //============================================================================== #define DART_COMMON_SET_ASPECT_PROPERTY_CUSTOM( Type, Name, Update )\ diff --git a/dart/common/AspectWithVersion.h b/dart/common/AspectWithVersion.h index 2827d5667ae0b..76751c1c7cfd9 100644 --- a/dart/common/AspectWithVersion.h +++ b/dart/common/AspectWithVersion.h @@ -81,14 +81,14 @@ class AspectWithStateAndVersionedProperties : constexpr static void (*UpdateState)(Derived*) = updateState; constexpr static void (*UpdateProperties)(Derived*) = updateProperties; - using AspectStateImplementation = AspectWithState< + using AspectStateImpl = AspectWithState< Derived, StateData, CompositeType, updateState>; - using AspectPropertiesImplementation = detail::AspectWithVersionedProperties< - AspectStateImplementation, + using AspectPropertiesImpl = detail::AspectWithVersionedProperties< + AspectStateImpl, Derived, PropertiesData, CompositeType, updateProperties>; - using AspectImplementation = AspectWithStateAndVersionedProperties< + using AspectImpl = AspectWithStateAndVersionedProperties< DerivedT, StateDataT, PropertiesDataT, CompositeT, updateState, updateProperties>; @@ -101,7 +101,7 @@ class AspectWithStateAndVersionedProperties : common::Composite* comp, const StateData& state = StateData(), const PropertiesData& properties = PropertiesData()) - : AspectPropertiesImplementation(comp, properties, state) + : AspectPropertiesImpl(comp, properties, state) { // Do nothing } @@ -111,11 +111,18 @@ class AspectWithStateAndVersionedProperties : common::Composite* comp, const PropertiesData& properties, const StateData& state = StateData()) - : AspectPropertiesImplementation(comp, properties, state) + : AspectPropertiesImpl(comp, properties, state) { // Do nothing } + // Documentation inherited + std::unique_ptr cloneAspect(Composite* newComposite) const override + { + return make_unique( + newComposite, this->getState(), this->getProperties()); + } + }; //============================================================================== diff --git a/dart/common/EmbeddedAspect.h b/dart/common/EmbeddedAspect.h index ee48a9190e934..a15126f1ce2e1 100644 --- a/dart/common/EmbeddedAspect.h +++ b/dart/common/EmbeddedAspect.h @@ -58,7 +58,7 @@ using EmbeddedStateAspect = template , const PropertiesT& (*getProperties)(const DerivedT*) = &detail::DefaultGetEmbeddedProperties > @@ -68,7 +68,75 @@ using EmbeddedPropertiesAspect = PropertiesT, setEmbeddedProperties, getProperties>; //============================================================================== +template , + const StateT& (*getEmbeddedState)(const DerivedT*) = + &detail::DefaultGetEmbeddedState, + void (*setEmbeddedProperties)(DerivedT*, const PropertiesT&) = + &detail::DefaultSetEmbeddedProperties, + const PropertiesT& (*getEmbeddedProperties)(const DerivedT*) = + &detail::DefaultGetEmbeddedProperties > +class EmbeddedStateAndPropertiesAspect : + public detail::EmbeddedPropertiesAspect< + EmbeddedStateAspect, + DerivedT, PropertiesT, setEmbeddedProperties, getEmbeddedProperties> +{ +public: + + using Derived = DerivedT; + using State = StateT; + using Properties = PropertiesT; + using CompositeType = CompositeT; + + using AspectStateImpl = EmbeddedStateAspect< + Derived, State, CompositeType, setEmbeddedState, getEmbeddedState>; + + using AspectPropertiesImpl = detail::EmbeddedPropertiesAspect< + AspectStateImpl, Derived, Properties, + setEmbeddedProperties, getEmbeddedProperties>; + + using AspectImpl = EmbeddedStateAndPropertiesAspect< + Derived, State, Properties, CompositeType, + setEmbeddedState, getEmbeddedState, + setEmbeddedProperties, getEmbeddedProperties>; + + EmbeddedStateAndPropertiesAspect() = delete; + EmbeddedStateAndPropertiesAspect( + const EmbeddedStateAndPropertiesAspect&) = delete; + + /// Construct using a State and Properties instance + EmbeddedStateAndPropertiesAspect( + common::Composite* comp, + const State& state = State(), + const Properties& properties = Properties()) + : AspectPropertiesImpl(comp, properties, state) + { + // Do nothing + } + + /// Construct using a Properties and State instance + EmbeddedStateAndPropertiesAspect( + common::Composite* comp, + const Properties& properties, + const State& state = State()) + : AspectPropertiesImpl(comp, properties, state) + { + // Do nothing + } + + // Documentation inherited + std::unique_ptr cloneAspect(Composite* newComposite) const override + { + return make_unique( + newComposite, this->getState(), this->getProperties()); + } +}; } // namespace common } // namespace dart diff --git a/dart/common/detail/EmbeddedAspect.h b/dart/common/detail/EmbeddedAspect.h index 86f2e66fe8ac6..2040e519e6053 100644 --- a/dart/common/detail/EmbeddedAspect.h +++ b/dart/common/detail/EmbeddedAspect.h @@ -107,27 +107,33 @@ class EmbeddedStateAspect : public BaseT // Do nothing } + // Documentation inherited void setAspectState(const Aspect::State& state) override final { setState(static_cast(state)); } + // Documentation inherited void setState(const State& state) { if(this->getComposite()) { - SetEmbeddedState(this, static_cast(state)); + SetEmbeddedState(this, state); return; } + // If the correct type of Composite is not available, we store this on the + // heap until this Aspect is moved. mTemporaryState = make_unique(state); } + // Documentation inherited const Aspect::State* getAspectState() const override final { return &getState(); } + // Documentation inherited const State& getState() const { if(this->getComposite()) @@ -143,6 +149,7 @@ class EmbeddedStateAspect : public BaseT protected: + /// Pass the temporary State of this Aspect into the new Composite void setComposite(Composite* newComposite) override { Base::setComposite(newComposite); @@ -154,6 +161,7 @@ class EmbeddedStateAspect : public BaseT mTemporaryState = nullptr; } + /// Save the embedded State of this Composite before we remove the Aspect void loseComposite(Composite* oldComposite) override { mTemporaryState = make_unique(GetEmbeddedState(this)); @@ -169,9 +177,9 @@ class EmbeddedStateAspect : public BaseT //============================================================================== template , - const PropertiesT& (*getProperties)(const DerivedT*) = + const PropertiesT& (*getEmbeddedProperties)(const DerivedT*) = &DefaultGetEmbeddedProperties > class EmbeddedPropertiesAspect : public BaseT { @@ -180,8 +188,96 @@ class EmbeddedPropertiesAspect : public BaseT using Base = BaseT; using Derived = DerivedT; using Properties = PropertiesT; - constexpr static void (*SetProperties)(Derived*, const Properties&) = setProperties; - constexpr static const Properties& (*GetProperties)(const Derived*) = getProperties; + constexpr static void (*SetEmbeddedProperties)(Derived*, const Properties&) = setEmbeddedProperties; + constexpr static const Properties& (*GetEmbeddedProperties)(const Derived*) = getEmbeddedProperties; + + EmbeddedPropertiesAspect() = delete; + EmbeddedPropertiesAspect(const EmbeddedPropertiesAspect&) = delete; + + /// Construct using a Properties instance + EmbeddedPropertiesAspect( + Composite* comp, const Properties& properties = Properties()) + : BaseT(comp), + mTemporaryProperties(make_unique(properties)) + { + // Do nothing + } + + /// Construct this Aspect and pass args into the constructor of the Base class + template + EmbeddedPropertiesAspect( + Composite* comp, const Properties& properties, BaseArgs&&... args) + : Base(comp, std::forward(args)...), + mTemporaryProperties(make_unique(properties)) + { + // Do nothing + } + + // Documentation inherited + void setAspectProperties(const Aspect::Properties& properties) override final + { + setProperties(static_cast(properties)); + } + + // Documentation inherited + void setProperties(const Properties& properties) + { + if(this->getComposite()) + { + SetEmbeddedProperties(this, properties); + return; + } + + // If the correct type of Composite is not available, we store this on the + // heap until this Aspect is moved. + mTemporaryProperties = make_unique(properties); + } + + // Documentation inherited + const Aspect::Properties* getAspectProperties() const override final + { + return &getProperties(); + } + + // Documentation inherited + const Properties& getProperties() const + { + if(this->getComposite()) + { + return GetEmbeddedProperties(this); + } + + if(!mTemporaryProperties) + mTemporaryProperties = make_unique(); + + return *mTemporaryProperties; + } + +protected: + + /// Pass the temporary Properties of this Aspect into the new Composite + void setComposite(Composite* newComposite) override + { + Base::setComposite(newComposite); + if(mTemporaryProperties) + SetEmbeddedProperties(this, *mTemporaryProperties); + else + SetEmbeddedProperties(this, Properties()); + + mTemporaryProperties = nullptr; + } + + /// Save the embedded Properties of this Composite before we remove the Aspect + void loseComposite(Composite* oldComposite) + { + mTemporaryProperties = make_unique(GetEmbeddedProperties(this)); + Base::loseComposite(oldComposite); + } + + /// During transitions between Composite objects, this will hold the Properties + /// of the Aspect. Once the Aspect has been moved into a new Composite, these + /// Properties will be pushed into the Composite and cleared. + std::unique_ptr mTemporaryProperties; }; @@ -197,31 +293,32 @@ template constexpr void (*EmbeddedStateAspect< - BaseT, DerivedT, StateT, setEmbeddedState, getEmbeddedState>::SetEmbeddedState)( - DerivedT*, const StateT&); + BaseT, DerivedT, StateT, setEmbeddedState, + getEmbeddedState>::SetEmbeddedState)(DerivedT*, const StateT&); //============================================================================== template constexpr const StateT& (*EmbeddedStateAspect< - BaseT, DerivedT, StateT, setEmbeddedState, getEmbeddedState>::GetEmbeddedState)(const DerivedT*); + BaseT, DerivedT, StateT, setEmbeddedState, + getEmbeddedState>::GetEmbeddedState)(const DerivedT*); //============================================================================== template + void (*setEmbeddedProperties)(DerivedT*, const PropertiesT&), + const PropertiesT& (*getEmbeddedProperties)(const DerivedT*)> constexpr void (*EmbeddedPropertiesAspect< - BaseT, DerivedT, PropertiesT, setProperties, getProperties>::SetProperties)( - DerivedT*, const PropertiesT&); + BaseT, DerivedT, PropertiesT, setEmbeddedProperties, + getEmbeddedProperties>::SetEmbeddedProperties)(DerivedT*, const PropertiesT&); //============================================================================== template + void (*setEmbeddedProperties)(DerivedT*, const PropertiesT&), + const PropertiesT& (*getEmbeddedProperties)(const DerivedT*)> constexpr const PropertiesT& (*EmbeddedPropertiesAspect< - BaseT, DerivedT, PropertiesT, setProperties, getProperties>::GetProperties)( - const DerivedT*); + BaseT, DerivedT, PropertiesT, setEmbeddedProperties, + getEmbeddedProperties>::GetEmbeddedProperties)(const DerivedT*); } // namespace detail } // namespace common diff --git a/dart/dynamics/detail/MultiDofJointProperties.h b/dart/dynamics/detail/MultiDofJointProperties.h index c83a7ea061366..006165b4fd90e 100644 --- a/dart/dynamics/detail/MultiDofJointProperties.h +++ b/dart/dynamics/detail/MultiDofJointProperties.h @@ -166,7 +166,7 @@ class MultiDofJointAspect final : MultiDofJointAspect(common::Composite* comp, const typename MultiDofJointAspect::PropertiesData& properties = - typename MultiDofJointAddon::PropertiesData()); + typename MultiDofJointAspect::PropertiesData()); constexpr static size_t NumDofs = DOF; using Vector = Eigen::Matrix; diff --git a/unittests/testAspect.cpp b/unittests/testAspect.cpp index dd1419fbcfc70..1675e76eae0d2 100644 --- a/unittests/testAspect.cpp +++ b/unittests/testAspect.cpp @@ -79,7 +79,15 @@ class SomeStateComposite : public virtual dart::common::RequiresAspect +{ +public: + +}; + +class SomePropertiesComposite : public dart::common::RequiresAspect { public: @@ -93,6 +101,13 @@ class SomePropertiesComposite : public virtual dart::common::Composite }; +class SomeStateAndPropertiesComposite; +class SomeEmbeddedStateAndPropertiesAspect : public dart::common::EmbeddedStateAndPropertiesAspect< + SomeEmbeddedStateAndPropertiesAspect, SomeState, SomeProperties, SomeStateAndPropertiesComposite> +{ +public: + +}; From 0c11c9bf685672450c580267c3aad8c55887c3b8 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Tue, 29 Mar 2016 12:38:52 -0400 Subject: [PATCH 14/62] finished implementation of embedded aspects -- needs testing --- dart/common/EmbeddedAspect.h | 206 ++++++++++++++++++++++------ dart/common/detail/EmbeddedAspect.h | 7 +- unittests/testAspect.cpp | 61 ++++---- 3 files changed, 190 insertions(+), 84 deletions(-) diff --git a/dart/common/EmbeddedAspect.h b/dart/common/EmbeddedAspect.h index a15126f1ce2e1..95c64e999e176 100644 --- a/dart/common/EmbeddedAspect.h +++ b/dart/common/EmbeddedAspect.h @@ -38,72 +38,141 @@ #define DART_COMMON_EMBEDDEDASPECT_H_ #include "dart/common/detail/EmbeddedAspect.h" +#include "dart/common/RequiresAspect.h" namespace dart { namespace common { //============================================================================== -template , - const StateT& (*getEmbeddedState)(const DerivedT*) = - &detail::DefaultGetEmbeddedState > -using EmbeddedStateAspect = - detail::EmbeddedStateAspect, DerivedT, - StateT, setEmbeddedState, getEmbeddedState>; +/// This is the implementation of a standard embedded-state Aspect. Inherit +/// the EmbedState class (next class down in the header) to use this Aspect +/// implementation. +/// +/// For more control over how your embedded-state Aspect is implemented, you can +/// use the detail::EmbeddedStateAspect class. +template +class EmbeddedStateAspect : public detail::EmbeddedStateAspect< + CompositeTrackingAspect, + EmbeddedStateAspect, StateT> +{ + // Do nothing +}; + +//============================================================================== +/// Inherit this class to embed a State into your Composite object. DerivedT is +/// the name of your class and StateDataT is a "plain-old data" structure that +/// holds your state information. +/// +/// Your derived class must implement the following functions: +/// +/// \code{.cpp} +/// void setAspectState(const AspectState& state); +/// \endcode +/// +/// To embed both state and properties information, use EmbedStateAndProperties. +template +class EmbedState : public virtual common::RequiresAspect< + common::EmbeddedStateAspect> > +{ +public: + + using Derived = DerivedT; + using AspectStateData = StateDataT; + using AspectState = common::Aspect::StateMixer; + using Aspect = common::EmbeddedStateAspect; + + const AspectState& getAspectState() const + { + return mAspectState; + } + +protected: + + /// Aspect::State data, directly accessible to your derived class + AspectState mAspectState; + +}; + +//============================================================================== +/// This is the implementation of a standard embedded-properties Aspect. Inherit +/// the EmbedProperties (next class down in the header) to use this Aspect +/// implementation. +/// +/// For more control over how your embedded-properties Aspect is implemented, +/// you can use the detail::EmbeddedPropertiesAspect class. +template +class EmbeddedPropertiesAspect : public detail::EmbeddedPropertiesAspect< + CompositeTrackingAspect, + EmbeddedPropertiesAspect, PropertiesT> +{ + // Do nothing +}; //============================================================================== -template , - const PropertiesT& (*getProperties)(const DerivedT*) = - &detail::DefaultGetEmbeddedProperties > -using EmbeddedPropertiesAspect = - detail::EmbeddedPropertiesAspect< - CompositeTrackingAspect, DerivedT, - PropertiesT, setEmbeddedProperties, getProperties>; +/// Inherit this class to embed Properties into your Composite object. DerivedT +/// is the name of your class and PropertiesDataT is a "plain-old data" +/// structure that holds your properties information. +/// +/// Your derived class must implement the following function: +/// +/// \code{.cpp} +/// void setAspectProperties(const AspectProperties& state); +/// \endcode +/// +/// To embed both state and properties information, use EmbedStateAndProperties. +template +class EmbedProperties : public virtual common::RequiresAspect< + common::EmbeddedPropertiesAspect< + DerivedT, common::Aspect::PropertiesMixer> > +{ +public: + + using Derived = DerivedT; + using AspectPropertiesData = PropertiesDataT; + using AspectProperties = common::Aspect::PropertiesMixer; + using Aspect = common::EmbeddedPropertiesAspect; + + const AspectProperties& getAspectProperties() const + { + return mAspectProperties; + } + +protected: + + /// Aspect::Properties data, directly accessible to your derived class + AspectProperties mAspectProperties; + +}; //============================================================================== -template , - const StateT& (*getEmbeddedState)(const DerivedT*) = - &detail::DefaultGetEmbeddedState, - void (*setEmbeddedProperties)(DerivedT*, const PropertiesT&) = - &detail::DefaultSetEmbeddedProperties, - const PropertiesT& (*getEmbeddedProperties)(const DerivedT*) = - &detail::DefaultGetEmbeddedProperties > +/// This is the implementation of a standard combination of embedded-state and +/// embedded-properties Aspect. Inherit the EmbedStateAndProperties (next class +/// down in the header) to use this Aspect implementation. +template class EmbeddedStateAndPropertiesAspect : public detail::EmbeddedPropertiesAspect< - EmbeddedStateAspect, - DerivedT, PropertiesT, setEmbeddedProperties, getEmbeddedProperties> + detail::EmbeddedStateAspect< + CompositeTrackingAspect, + EmbeddedStateAndPropertiesAspect, + StateT>, + EmbeddedStateAndPropertiesAspect, + PropertiesT> { public: - using Derived = DerivedT; + using Derived = EmbeddedStateAndPropertiesAspect; using State = StateT; using Properties = PropertiesT; using CompositeType = CompositeT; - using AspectStateImpl = EmbeddedStateAspect< - Derived, State, CompositeType, setEmbeddedState, getEmbeddedState>; + using AspectStateImpl = detail::EmbeddedStateAspect< + CompositeTrackingAspect, Derived, State>; using AspectPropertiesImpl = detail::EmbeddedPropertiesAspect< - AspectStateImpl, Derived, Properties, - setEmbeddedProperties, getEmbeddedProperties>; + AspectStateImpl, Derived, Properties>; using AspectImpl = EmbeddedStateAndPropertiesAspect< - Derived, State, Properties, CompositeType, - setEmbeddedState, getEmbeddedState, - setEmbeddedProperties, getEmbeddedProperties>; + CompositeType, State, Properties>; EmbeddedStateAndPropertiesAspect() = delete; EmbeddedStateAndPropertiesAspect( @@ -138,6 +207,55 @@ class EmbeddedStateAndPropertiesAspect : }; +//============================================================================== +/// Inherit this class to embed both State and Properties into your Composite +/// object. DerivedT is the name of your class, StateDataT is a "plain-old data" +/// structure that holds your state information, and PropertiesDataT is a +/// "plain-old data" structure that holds your properties information. +/// +/// Your derived class must implement the following functions: +/// +/// \code{.cpp} +/// void setAspectState(const AspectState& state); +/// void setAspectProperties(const AspectProperties& state); +/// \endcode +template +class EmbedStateAndProperties : public virtual common::RequiresAspect< + common::EmbeddedStateAndPropertiesAspect< + DerivedT, + common::Aspect::StateMixer, + common::Aspect::PropertiesMixer> > +{ +public: + + using Derived = DerivedT; + using AspectStateData = StateDataT; + using AspectState = common::Aspect::StateMixer; + using AspectPropertiesData = PropertiesDataT; + using AspectProperties = common::Aspect::PropertiesMixer; + using Aspect = common::EmbeddedStateAndPropertiesAspect< + Derived, AspectState, AspectProperties>; + + const AspectState& getAspectState() const + { + return mAspectState; + } + + const AspectProperties& getAspectProperties() const + { + return mAspectProperties; + } + +protected: + + /// Aspect::State data, directly accessible to your derived class + AspectState mAspectState; + + /// Aspect::Properties data, directly accessible to your derived class + AspectProperties mAspectProperties; + +}; + } // namespace common } // namespace dart diff --git a/dart/common/detail/EmbeddedAspect.h b/dart/common/detail/EmbeddedAspect.h index 2040e519e6053..2a15144304449 100644 --- a/dart/common/detail/EmbeddedAspect.h +++ b/dart/common/detail/EmbeddedAspect.h @@ -74,8 +74,10 @@ const PropertiesT& DefaultGetEmbeddedProperties(const AspectT* aspect) //============================================================================== template + void (*setEmbeddedState)(DerivedT*, const StateT&) = + &DefaultSetEmbeddedState, + const StateT& (*getEmbeddedState)(const DerivedT*) = + &DefaultGetEmbeddedState > class EmbeddedStateAspect : public BaseT { public: @@ -264,6 +266,7 @@ class EmbeddedPropertiesAspect : public BaseT else SetEmbeddedProperties(this, Properties()); + // Release the temporary memory mTemporaryProperties = nullptr; } diff --git a/unittests/testAspect.cpp b/unittests/testAspect.cpp index 1675e76eae0d2..ff037f8fb3ed4 100644 --- a/unittests/testAspect.cpp +++ b/unittests/testAspect.cpp @@ -54,62 +54,45 @@ using namespace dart::common; struct SomeStateData { }; -using SomeState = dart::common::Aspect::StateMixer; struct SomePropertiesData { }; -using SomeProperties = dart::common::Aspect::PropertiesMixer; -class SomeStateComposite; -class SomeEmbeddedStateAspect : public dart::common::EmbeddedStateAspect< - SomeEmbeddedStateAspect, SomeState, SomeStateComposite> -{ - -}; - -class SomeStateComposite : public virtual dart::common::RequiresAspect +class EmbeddedStateComposite : + public EmbedState { public: - void setAspectState(const SomeState& state) { mState = state; } - - const SomeState& getAspectState() const { return mState; } - -protected: - - SomeState mState; - -}; - -class SomePropertiesComposite; -class SomeEmbeddedPropertiesAspect : public dart::common::EmbeddedPropertiesAspect< - SomeEmbeddedPropertiesAspect, SomeProperties, SomePropertiesComposite> -{ -public: + void setAspectState(const AspectState& state) { mAspectState = state; } }; -class SomePropertiesComposite : public dart::common::RequiresAspect +class EmbeddedPropertiesComposite : + public EmbedProperties { public: - void setAspectProperties(const SomeProperties& properties) { mProperties = properties; } - - const SomeProperties& getAspectProperties() const { return mProperties; } - -protected: - - SomeProperties mProperties; + void setAspectProperties(const AspectProperties& properties) + { + mAspectProperties = properties; + } }; -class SomeStateAndPropertiesComposite; -class SomeEmbeddedStateAndPropertiesAspect : public dart::common::EmbeddedStateAndPropertiesAspect< - SomeEmbeddedStateAndPropertiesAspect, SomeState, SomeProperties, SomeStateAndPropertiesComposite> + +class EmbeddedStateAndPropertiesComposite : + public EmbedStateAndProperties< + EmbeddedStateAndPropertiesComposite, + SomeStateData, SomePropertiesData> { public: -}; + void setAspectState(const AspectState& state) { mAspectState = state; } + void setAspectProperties(const AspectProperties& properties) + { + mAspectProperties = properties; + } +}; // Testing the creation of an Aspect using the AspectWithState template class class StateAspectTest : public dart::common::AspectWithState< @@ -658,7 +641,9 @@ TEST(Aspect, Duplication) TEST(Aspect, Embedded) { - SomeStateComposite comp; + EmbeddedStateComposite s_comp; + EmbeddedPropertiesComposite p_comp; + EmbeddedStateAndPropertiesComposite sp_comp; } int main(int argc, char* argv[]) From 233454ee6dcddc4c450d5096e8dad71ffe563c47 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Tue, 29 Mar 2016 13:53:08 -0400 Subject: [PATCH 15/62] finished implementation of embedded aspects and created tests --- dart/common/EmbeddedAspect.h | 30 +++++- dart/common/detail/EmbeddedAspect.h | 60 +++++++---- unittests/testAspect.cpp | 162 ++++++++++++++++++++++++++-- 3 files changed, 223 insertions(+), 29 deletions(-) diff --git a/dart/common/EmbeddedAspect.h b/dart/common/EmbeddedAspect.h index 95c64e999e176..3ac13fc31cff1 100644 --- a/dart/common/EmbeddedAspect.h +++ b/dart/common/EmbeddedAspect.h @@ -55,7 +55,20 @@ class EmbeddedStateAspect : public detail::EmbeddedStateAspect< CompositeTrackingAspect, EmbeddedStateAspect, StateT> { - // Do nothing +public: + + using State = StateT; + using Base = CompositeTrackingAspect; + using Derived = EmbeddedStateAspect; + using Impl = detail::EmbeddedStateAspect; + + template + EmbeddedStateAspect(Args&&... args) + : Impl(std::forward(args)...) + { + // Do nothing + } + }; //============================================================================== @@ -105,7 +118,20 @@ class EmbeddedPropertiesAspect : public detail::EmbeddedPropertiesAspect< CompositeTrackingAspect, EmbeddedPropertiesAspect, PropertiesT> { - // Do nothing +public: + + using Properties = PropertiesT; + using Base = CompositeTrackingAspect; + using Derived = EmbeddedPropertiesAspect; + using Impl = detail::EmbeddedPropertiesAspect; + + template + EmbeddedPropertiesAspect(Args&&... args) + : Impl(std::forward(args)...) + { + // Do nothing + } + }; //============================================================================== diff --git a/dart/common/detail/EmbeddedAspect.h b/dart/common/detail/EmbeddedAspect.h index 2a15144304449..94744660febfc 100644 --- a/dart/common/detail/EmbeddedAspect.h +++ b/dart/common/detail/EmbeddedAspect.h @@ -94,7 +94,7 @@ class EmbeddedStateAspect : public BaseT /// Construct using a State instance EmbeddedStateAspect(Composite* comp, const State& state = State()) : BaseT(comp), - mTemporaryState(make_unique(state)) + mTemporaryState(make_unique(state)) { // Do nothing } @@ -104,7 +104,7 @@ class EmbeddedStateAspect : public BaseT EmbeddedStateAspect( Composite* comp, const State& state, BaseArgs&&... args) : Base(comp, std::forward(args)...), - mTemporaryState(make_unique(state)) + mTemporaryState(make_unique(state)) { // Do nothing } @@ -120,13 +120,13 @@ class EmbeddedStateAspect : public BaseT { if(this->getComposite()) { - SetEmbeddedState(this, state); + SetEmbeddedState(static_cast(this), state); return; } // If the correct type of Composite is not available, we store this on the // heap until this Aspect is moved. - mTemporaryState = make_unique(state); + mTemporaryState = make_unique(state); } // Documentation inherited @@ -140,15 +140,26 @@ class EmbeddedStateAspect : public BaseT { if(this->getComposite()) { - return GetEmbeddedState(this); + return GetEmbeddedState(static_cast(this)); } if(!mTemporaryState) - mTemporaryState = make_unique(); + { + dterr << "[detail::EmbeddedStateAspect::getState] This Aspect is not in " + << "a Composite, but it also does not have a temporary State " + << "available. This should not happen! Please report this as a " + << "bug!\n"; + assert(false); + } return *mTemporaryState; } + std::unique_ptr cloneAspect(Composite* newComposite) const override + { + return make_unique(newComposite, this->getState()); + } + protected: /// Pass the temporary State of this Aspect into the new Composite @@ -156,9 +167,9 @@ class EmbeddedStateAspect : public BaseT { Base::setComposite(newComposite); if(mTemporaryState) - SetEmbeddedState(this, *mTemporaryState); + SetEmbeddedState(static_cast(this), *mTemporaryState); else - SetEmbeddedState(this, State()); + SetEmbeddedState(static_cast(this), State()); mTemporaryState = nullptr; } @@ -166,7 +177,8 @@ class EmbeddedStateAspect : public BaseT /// Save the embedded State of this Composite before we remove the Aspect void loseComposite(Composite* oldComposite) override { - mTemporaryState = make_unique(GetEmbeddedState(this)); + mTemporaryState = make_unique( + GetEmbeddedState(static_cast(this))); Base::loseComposite(oldComposite); } @@ -200,7 +212,7 @@ class EmbeddedPropertiesAspect : public BaseT EmbeddedPropertiesAspect( Composite* comp, const Properties& properties = Properties()) : BaseT(comp), - mTemporaryProperties(make_unique(properties)) + mTemporaryProperties(make_unique(properties)) { // Do nothing } @@ -210,7 +222,7 @@ class EmbeddedPropertiesAspect : public BaseT EmbeddedPropertiesAspect( Composite* comp, const Properties& properties, BaseArgs&&... args) : Base(comp, std::forward(args)...), - mTemporaryProperties(make_unique(properties)) + mTemporaryProperties(make_unique(properties)) { // Do nothing } @@ -226,13 +238,13 @@ class EmbeddedPropertiesAspect : public BaseT { if(this->getComposite()) { - SetEmbeddedProperties(this, properties); + SetEmbeddedProperties(static_cast(this), properties); return; } // If the correct type of Composite is not available, we store this on the // heap until this Aspect is moved. - mTemporaryProperties = make_unique(properties); + mTemporaryProperties = make_unique(properties); } // Documentation inherited @@ -246,15 +258,26 @@ class EmbeddedPropertiesAspect : public BaseT { if(this->getComposite()) { - return GetEmbeddedProperties(this); + return GetEmbeddedProperties(static_cast(this)); } if(!mTemporaryProperties) - mTemporaryProperties = make_unique(); + { + dterr << "[detail::EmbeddedPropertiesAspect::getProperties] This Aspect " + << "is not in a Composite, but it also does not have temporary " + << "Properties available. This should not happen! Please report " + << "this as a bug!\n"; + assert(false); + } return *mTemporaryProperties; } + std::unique_ptr cloneAspect(Composite* newComposite) const override + { + return make_unique(newComposite, this->getProperties()); + } + protected: /// Pass the temporary Properties of this Aspect into the new Composite @@ -262,9 +285,9 @@ class EmbeddedPropertiesAspect : public BaseT { Base::setComposite(newComposite); if(mTemporaryProperties) - SetEmbeddedProperties(this, *mTemporaryProperties); + SetEmbeddedProperties(static_cast(this), *mTemporaryProperties); else - SetEmbeddedProperties(this, Properties()); + SetEmbeddedProperties(static_cast(this), Properties()); // Release the temporary memory mTemporaryProperties = nullptr; @@ -273,7 +296,8 @@ class EmbeddedPropertiesAspect : public BaseT /// Save the embedded Properties of this Composite before we remove the Aspect void loseComposite(Composite* oldComposite) { - mTemporaryProperties = make_unique(GetEmbeddedProperties(this)); + mTemporaryProperties = make_unique( + GetEmbeddedProperties(static_cast(this))); Base::loseComposite(oldComposite); } diff --git a/unittests/testAspect.cpp b/unittests/testAspect.cpp index ff037f8fb3ed4..3c9c93a42534d 100644 --- a/unittests/testAspect.cpp +++ b/unittests/testAspect.cpp @@ -53,23 +53,84 @@ using namespace dart::common; -struct SomeStateData { }; -struct SomePropertiesData { }; +struct EmbeddedStateData +{ + double d; + int i; + + EmbeddedStateData() : d(0.0), i(0) + { + // Do nothing + } + + bool operator==(const EmbeddedStateData& other) const + { + if(other.d != d) + return false; + + if(other.i != i) + return false; + + return true; + } + + bool operator!=(const EmbeddedStateData& other) const + { + return !(*this == other); + } +}; + +struct EmbeddedPropertiesData +{ + bool b; + float f; + + EmbeddedPropertiesData() : b(false), f(0.0) + { + // Do nothing + } + + bool operator==(const EmbeddedPropertiesData& other) const + { + if(other.b != b) + return false; + + if(other.f != f) + return false; + + return true; + } + + bool operator!=(const EmbeddedPropertiesData& other) const + { + return !(*this == other); + } +}; class EmbeddedStateComposite : - public EmbedState + public EmbedState { public: + EmbeddedStateComposite() + { + create(); + } + void setAspectState(const AspectState& state) { mAspectState = state; } }; class EmbeddedPropertiesComposite : - public EmbedProperties + public EmbedProperties { public: + EmbeddedPropertiesComposite() + { + create(); + } + void setAspectProperties(const AspectProperties& properties) { mAspectProperties = properties; @@ -77,14 +138,18 @@ class EmbeddedPropertiesComposite : }; - class EmbeddedStateAndPropertiesComposite : public EmbedStateAndProperties< EmbeddedStateAndPropertiesComposite, - SomeStateData, SomePropertiesData> + EmbeddedStateData, EmbeddedPropertiesData> { public: + EmbeddedStateAndPropertiesComposite() + { + create(); + } + void setAspectState(const AspectState& state) { mAspectState = state; } void setAspectProperties(const AspectProperties& properties) @@ -641,9 +706,88 @@ TEST(Aspect, Duplication) TEST(Aspect, Embedded) { - EmbeddedStateComposite s_comp; - EmbeddedPropertiesComposite p_comp; - EmbeddedStateAndPropertiesComposite sp_comp; + EmbeddedStateComposite s; + EmbeddedPropertiesComposite p; + EmbeddedStateAndPropertiesComposite sp; + + // --------- Test Embedded State ----------- + EmbeddedStateComposite::AspectState state = s.getAspectState(); + EmbeddedStateComposite::AspectState a_state = + s.get()->getState(); + + EXPECT_TRUE(state == a_state); + + state.d = 3.5; + state.i = 750; + s.setAspectState(state); + + state = s.getAspectState(); + a_state = s.get()->getState(); + EXPECT_EQ(3.5, state.d); + EXPECT_EQ(750, state.i); + EXPECT_TRUE(state == a_state); + + EXPECT_EQ(&s.get()->getState(), + s.get()->getAspectState()); + EXPECT_EQ(&s.getAspectState(), + s.get()->getAspectState()); + + state.d = -4e-3; + state.i = -18; + s.get()->setAspectState(state); + + state = s.getAspectState(); + a_state = s.get()->getState(); + EXPECT_EQ(-4e-3, state.d); + EXPECT_EQ(-18, state.i); + EXPECT_TRUE(state == a_state); + + + // --------- Test Embedded Properties ----------- + EmbeddedPropertiesComposite::AspectProperties prop = p.getAspectProperties(); + EmbeddedPropertiesComposite::AspectProperties a_prop = + p.get()->getProperties(); + + EXPECT_TRUE(prop == a_prop); + + prop.f = 7.5; + prop.b = true; + p.setAspectProperties(prop); + + prop = p.getAspectProperties(); + a_prop = p.get()->getProperties(); + EXPECT_EQ(7.5, prop.f); + EXPECT_EQ(true, prop.b); + EXPECT_TRUE(prop == a_prop); + + // Make sure the pointers are consistent + EXPECT_EQ(&p.get()->getProperties(), + p.get()->getAspectProperties()); + EXPECT_EQ(&p.getAspectProperties(), + p.get()->getAspectProperties()); + + prop.f = -7e5; + prop.b = false; + p.get()->setAspectProperties(prop); + + prop = p.getAspectProperties(); + a_prop = p.get()->getProperties(); + EXPECT_EQ(-7e5, prop.f); + EXPECT_EQ(false, prop.b); + EXPECT_TRUE(prop == a_prop); + + + // --------- Test Embedded State and Properties Combination ----------- + // Make sure the pointers are consistent + EXPECT_EQ(&sp.get()->getState(), + sp.get()->getAspectState()); + EXPECT_EQ(&sp.getAspectState(), + sp.get()->getAspectState()); + + EXPECT_EQ(&sp.get()->getProperties(), + sp.get()->getAspectProperties()); + EXPECT_EQ(&sp.getAspectProperties(), + sp.get()->getAspectProperties()); } int main(int argc, char* argv[]) From b7c20dc97b5c0f7e97e9ba2f9ba57a79d129e0ea Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Tue, 29 Mar 2016 18:13:50 -0400 Subject: [PATCH 16/62] implemented embedded aspect inheritance and created tests for it --- dart/common/EmbeddedAspect.h | 77 +++++++++++++++++++++++++++++ unittests/testAspect.cpp | 96 ++++++++++++++++++++++++++++++++++++ 2 files changed, 173 insertions(+) diff --git a/dart/common/EmbeddedAspect.h b/dart/common/EmbeddedAspect.h index 3ac13fc31cff1..4a764d4682719 100644 --- a/dart/common/EmbeddedAspect.h +++ b/dart/common/EmbeddedAspect.h @@ -39,6 +39,7 @@ #include "dart/common/detail/EmbeddedAspect.h" #include "dart/common/RequiresAspect.h" +#include "dart/common/CompositeJoiner.h" namespace dart { namespace common { @@ -106,6 +107,29 @@ class EmbedState : public virtual common::RequiresAspect< }; +//============================================================================== +/// This is an alternative to EmbedState which allows your class to also inherit +/// other Composite objects by listing them as the third (and later) template +/// arguments. +template +class EmbedStateOnTopOf : public CompositeJoiner< + EmbedState, BaseComposites...> +{ +public: + + using Impl = EmbedState; + using Derived = typename Impl::Derived; + using AspectStateData = typename Impl::AspectStateData; + using AspectState = typename Impl::AspectState; + using Aspect = typename Impl::Aspect; + using Impl::getAspectState; + +protected: + + using Impl::mAspectState; + +}; + //============================================================================== /// This is the implementation of a standard embedded-properties Aspect. Inherit /// the EmbedProperties (next class down in the header) to use this Aspect @@ -170,6 +194,29 @@ class EmbedProperties : public virtual common::RequiresAspect< }; +//============================================================================== +/// This is an alternative to EmbedProperties which allows your class to also +/// inherit other Composite objects by listing them as the third (and later) +/// template arguments. +template +class EmbedPropertiesOnTopOf : public CompositeJoiner< + EmbedProperties, CompositeBases...> +{ +public: + + using Impl = EmbedProperties; + using Derived = typename Impl::Derived; + using AspectPropertiesData = typename Impl::AspectPropertiesData; + using AspectProperties = typename Impl::AspectProperties; + using Aspect = typename Impl::Aspect; + using Impl::getAspectProperties; + +protected: + + using Impl::mAspectProperties; + +}; + //============================================================================== /// This is the implementation of a standard combination of embedded-state and /// embedded-properties Aspect. Inherit the EmbedStateAndProperties (next class @@ -282,6 +329,36 @@ class EmbedStateAndProperties : public virtual common::RequiresAspect< }; +//============================================================================== +/// This is an alternative to EmbedStateAndProperties which allows your class to +/// also inherit other Composite objects by listing them as the fourth (and +/// later) template arguments. +template +class EmbedStateAndPropertiesOnTopOf : public CompositeJoiner< + EmbedStateAndProperties, + CompositeBases...> +{ +public: + + using Impl = EmbedStateAndProperties; + using Derived = typename Impl::Derived; + using AspectStateData = typename Impl::AspectStateData; + using AspectState = typename Impl::AspectState; + using AspectPropertiesData = typename Impl::AspectPropertiesData; + using AspectProperties = typename Impl::AspectProperties; + using Aspect = typename Impl::Aspect; + using Impl::getAspectState; + using Impl::getAspectProperties; + +protected: + + using Impl::mAspectState; + using Impl::mAspectProperties; + +}; + + } // namespace common } // namespace dart diff --git a/unittests/testAspect.cpp b/unittests/testAspect.cpp index 3c9c93a42534d..4306be21807c1 100644 --- a/unittests/testAspect.cpp +++ b/unittests/testAspect.cpp @@ -80,6 +80,8 @@ struct EmbeddedStateData } }; +struct SecondEmbeddedStateData { }; + struct EmbeddedPropertiesData { bool b; @@ -107,6 +109,8 @@ struct EmbeddedPropertiesData } }; +struct SecondEmbeddedPropertiesData { }; + class EmbeddedStateComposite : public EmbedState { @@ -159,6 +163,63 @@ class EmbeddedStateAndPropertiesComposite : }; +class InheritAndEmbedStateComposite : + public EmbedStateOnTopOf< + InheritAndEmbedStateComposite, + SecondEmbeddedStateData, + EmbeddedStateComposite> +{ +public: + + InheritAndEmbedStateComposite() + { + create(); + } + + void setAspectState(const AspectState& state) { mAspectState = state; } +}; + +class InheritAndEmbedPropertiesComposite : + public EmbedPropertiesOnTopOf< + InheritAndEmbedPropertiesComposite, + SecondEmbeddedPropertiesData, + EmbeddedPropertiesComposite> +{ +public: + + InheritAndEmbedPropertiesComposite() + { + create(); + } + + void setAspectProperties(const AspectProperties& properties) + { + mAspectProperties = properties; + } +}; + +class InheritAndEmbedStateAndPropertiesComposite : + public EmbedStateAndPropertiesOnTopOf< + InheritAndEmbedStateAndPropertiesComposite, + SecondEmbeddedStateData, + SecondEmbeddedPropertiesData, + EmbeddedStateAndPropertiesComposite> +{ +public: + + InheritAndEmbedStateAndPropertiesComposite() + { + create(); + } + + void setAspectState(const AspectState& state) { mAspectState = state; } + + void setAspectProperties(const AspectProperties& properties) + { + mAspectProperties = properties; + } +}; + // Testing the creation of an Aspect using the AspectWithState template class class StateAspectTest : public dart::common::AspectWithState< StateAspectTest, dart::common::Empty> @@ -788,6 +849,41 @@ TEST(Aspect, Embedded) sp.get()->getAspectProperties()); EXPECT_EQ(&sp.getAspectProperties(), sp.get()->getAspectProperties()); + + sp.setAspectState(s.getAspectState()); + sp.setAspectProperties(p.getAspectProperties()); + + + // --------- Test Inheritance ----------- + InheritAndEmbedStateComposite s_derived; + EXPECT_NE(s_derived.get()->getState(), + s.get()->getState()); + s_derived.setCompositeState(s.getCompositeState()); + EXPECT_EQ(s_derived.get()->getState(), + s.get()->getState()); + + InheritAndEmbedPropertiesComposite p_derived; + EXPECT_NE(p_derived.get()->getProperties(), + p.get()->getProperties()); + p_derived.setCompositeProperties(p.getCompositeProperties()); + EXPECT_EQ(p_derived.get()->getProperties(), + p.get()->getProperties()); + + InheritAndEmbedStateAndPropertiesComposite sp_derived; + EXPECT_NE(sp_derived.get()->getState(), + sp.get()->getState()); + EXPECT_NE(sp_derived.get()->getProperties(), + sp.get()->getProperties()); + sp_derived.setCompositeState(sp.getCompositeState()); + EXPECT_EQ(sp_derived.get()->getState(), + sp.get()->getState()); + EXPECT_NE(sp_derived.get()->getProperties(), + sp.get()->getProperties()); + sp_derived.setCompositeProperties(sp.getCompositeProperties()); + EXPECT_EQ(sp_derived.get()->getState(), + sp.get()->getState()); + EXPECT_EQ(sp_derived.get()->getProperties(), + sp.get()->getProperties()); } int main(int argc, char* argv[]) From 9da9949414dfffcc9ecf9af2334102a2d1bfdf02 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Wed, 30 Mar 2016 01:12:57 -0400 Subject: [PATCH 17/62] using embedded aspect for Joint properties --- dart/common/CompositeJoiner.h | 15 +- dart/common/EmbeddedAspect.h | 10 + dart/common/SpecializedForAspect.h | 14 +- dart/common/VersionCounter.h | 2 + dart/common/Virtual.h | 6 +- dart/common/detail/EmbeddedAspect.h | 4 + dart/dynamics/BallJoint.cpp | 22 +- dart/dynamics/BallJoint.h | 2 +- dart/dynamics/EulerJoint.cpp | 40 +-- dart/dynamics/EulerJoint.h | 2 +- dart/dynamics/FreeJoint.cpp | 31 +- dart/dynamics/FreeJoint.h | 2 +- dart/dynamics/Joint.cpp | 91 +++--- dart/dynamics/Joint.h | 121 ++------ dart/dynamics/MultiDofJoint.h | 11 +- dart/dynamics/PlanarJoint.cpp | 30 +- dart/dynamics/PlanarJoint.h | 2 +- dart/dynamics/PrismaticJoint.cpp | 20 +- dart/dynamics/PrismaticJoint.h | 2 +- dart/dynamics/RevoluteJoint.cpp | 21 +- dart/dynamics/RevoluteJoint.h | 2 +- dart/dynamics/ScrewJoint.cpp | 20 +- dart/dynamics/ScrewJoint.h | 2 +- dart/dynamics/SingleDofJoint.cpp | 60 ++-- dart/dynamics/Skeleton.cpp | 6 +- dart/dynamics/Skeleton.h | 2 +- dart/dynamics/TranslationalJoint.cpp | 23 +- dart/dynamics/TranslationalJoint.h | 2 +- dart/dynamics/UniversalJoint.cpp | 28 +- dart/dynamics/UniversalJoint.h | 2 +- dart/dynamics/WeldJoint.cpp | 11 +- dart/dynamics/WeldJoint.h | 2 +- dart/dynamics/ZeroDofJoint.cpp | 5 +- dart/dynamics/ZeroDofJoint.h | 2 +- dart/dynamics/detail/Joint.h | 144 ++++++++++ dart/dynamics/detail/MultiDofJoint.h | 405 ++++++++++++++------------- unittests/testAspect.cpp | 14 +- 37 files changed, 660 insertions(+), 518 deletions(-) create mode 100644 dart/dynamics/detail/Joint.h diff --git a/dart/common/CompositeJoiner.h b/dart/common/CompositeJoiner.h index 23042456cd6a0..75d4bebf86e5e 100644 --- a/dart/common/CompositeJoiner.h +++ b/dart/common/CompositeJoiner.h @@ -45,11 +45,19 @@ namespace common { /// Terminator for the variadic template template -class CompositeJoiner { }; +class CompositeJoiner +{ +public: + virtual ~CompositeJoiner() = default; +}; /// Special case of only having 1 class: we do nothing but inherit it. template -class CompositeJoiner : public Base1 { }; +class CompositeJoiner : public Base1 +{ +public: + virtual ~CompositeJoiner() = default; +}; /// CompositeJoiner allows classes that inherit from various /// SpecializedForAspect types to be inherited by a single derived class. @@ -63,6 +71,8 @@ class CompositeJoiner : public Base1, public Base2 /// Default constructor CompositeJoiner() = default; + virtual ~CompositeJoiner() = default; + /// This constructor allows one argument to be passed to the Base1 constructor /// and arbitrarily many arguments to be passed to the Base2 constructor. // @@ -146,6 +156,7 @@ class CompositeJoiner : template CompositeJoiner(Args&&... args); + virtual ~CompositeJoiner() = default; }; } // namespace common diff --git a/dart/common/EmbeddedAspect.h b/dart/common/EmbeddedAspect.h index 4a764d4682719..8c7f087c95e5a 100644 --- a/dart/common/EmbeddedAspect.h +++ b/dart/common/EmbeddedAspect.h @@ -182,6 +182,8 @@ class EmbedProperties : public virtual common::RequiresAspect< using AspectProperties = common::Aspect::PropertiesMixer; using Aspect = common::EmbeddedPropertiesAspect; + virtual ~EmbedProperties() = default; + const AspectProperties& getAspectProperties() const { return mAspectProperties; @@ -211,6 +213,8 @@ class EmbedPropertiesOnTopOf : public CompositeJoiner< using Aspect = typename Impl::Aspect; using Impl::getAspectProperties; + virtual ~EmbedPropertiesOnTopOf() = default; + protected: using Impl::mAspectProperties; @@ -251,6 +255,8 @@ class EmbeddedStateAndPropertiesAspect : EmbeddedStateAndPropertiesAspect( const EmbeddedStateAndPropertiesAspect&) = delete; + virtual ~EmbeddedStateAndPropertiesAspect() = default; + /// Construct using a State and Properties instance EmbeddedStateAndPropertiesAspect( common::Composite* comp, @@ -309,6 +315,8 @@ class EmbedStateAndProperties : public virtual common::RequiresAspect< using Aspect = common::EmbeddedStateAndPropertiesAspect< Derived, AspectState, AspectProperties>; + virtual ~EmbedStateAndProperties() = default; + const AspectState& getAspectState() const { return mAspectState; @@ -351,6 +359,8 @@ class EmbedStateAndPropertiesOnTopOf : public CompositeJoiner< using Impl::getAspectState; using Impl::getAspectProperties; + virtual ~EmbedStateAndPropertiesOnTopOf() = default; + protected: using Impl::mAspectState; diff --git a/dart/common/SpecializedForAspect.h b/dart/common/SpecializedForAspect.h index a48d901cecc84..6a7f038dcd13b 100644 --- a/dart/common/SpecializedForAspect.h +++ b/dart/common/SpecializedForAspect.h @@ -46,7 +46,11 @@ namespace common { /// Declaration of the variadic template template -class SpecializedForAspect { }; +class SpecializedForAspect +{ +public: + virtual ~SpecializedForAspect() = default; +}; //============================================================================== /// SpecializedForAspect allows classes that inherit Composite to have @@ -59,6 +63,8 @@ class SpecializedForAspect : public virtual Composite /// Default Constructor SpecializedForAspect(); + virtual ~SpecializedForAspect() = default; + /// Check if this Composite currently has a certain type of Aspect template bool has() const; @@ -185,7 +191,11 @@ class SpecializedForAspect : public virtual Composite template class SpecializedForAspect : public CompositeJoiner< Virtual< SpecializedForAspect >, - Virtual< SpecializedForAspect > > { }; + Virtual< SpecializedForAspect > > +{ +public: + virtual ~SpecializedForAspect() = default; +}; } // namespace common } // namespace dart diff --git a/dart/common/VersionCounter.h b/dart/common/VersionCounter.h index 5eaaa75d413fa..10bcd951704ba 100644 --- a/dart/common/VersionCounter.h +++ b/dart/common/VersionCounter.h @@ -52,6 +52,8 @@ class VersionCounter /// Get the version number of this object virtual size_t getVersion() const = 0; + + virtual ~VersionCounter() = default; }; } // namespace common diff --git a/dart/common/Virtual.h b/dart/common/Virtual.h index 3872989f875fe..c2ea57916f4b5 100644 --- a/dart/common/Virtual.h +++ b/dart/common/Virtual.h @@ -43,7 +43,11 @@ namespace common { /// This class is used to have CRTP functions inherit their template parameters /// virtually instead of directly. template -class Virtual : public virtual T { }; +class Virtual : public virtual T +{ +public: + virtual ~Virtual() = default; +}; } // namespace common } // namespace dart diff --git a/dart/common/detail/EmbeddedAspect.h b/dart/common/detail/EmbeddedAspect.h index 94744660febfc..4a4a1ceda5e1e 100644 --- a/dart/common/detail/EmbeddedAspect.h +++ b/dart/common/detail/EmbeddedAspect.h @@ -91,6 +91,8 @@ class EmbeddedStateAspect : public BaseT EmbeddedStateAspect() = delete; EmbeddedStateAspect(const EmbeddedStateAspect&) = delete; + virtual ~EmbeddedStateAspect() = default; + /// Construct using a State instance EmbeddedStateAspect(Composite* comp, const State& state = State()) : BaseT(comp), @@ -208,6 +210,8 @@ class EmbeddedPropertiesAspect : public BaseT EmbeddedPropertiesAspect() = delete; EmbeddedPropertiesAspect(const EmbeddedPropertiesAspect&) = delete; + virtual ~EmbeddedPropertiesAspect() = default; + /// Construct using a Properties instance EmbeddedPropertiesAspect( Composite* comp, const Properties& properties = Properties()) diff --git a/dart/dynamics/BallJoint.cpp b/dart/dynamics/BallJoint.cpp index ab8fee85a3220..de7e6300288e2 100644 --- a/dart/dynamics/BallJoint.cpp +++ b/dart/dynamics/BallJoint.cpp @@ -97,14 +97,16 @@ Eigen::Matrix3d BallJoint::convertToRotation(const Eigen::Vector3d& _positions) } //============================================================================== -BallJoint::BallJoint(const Properties& _properties) - : MultiDofJoint<3>(_properties), +BallJoint::BallJoint(const Properties& properties) + : MultiDofJoint<3>(properties), mR(Eigen::Isometry3d::Identity()) { mJacobianDeriv = Eigen::Matrix::Zero(); - setProperties(_properties); - updateDegreeOfFreedomNames(); + // Inherited Aspects must be created in the final joint class in reverse order + // or else we get pure virtual function calls + createMultiDofJointAspect(properties); + createJointAspect(properties); } //============================================================================== @@ -143,11 +145,11 @@ void BallJoint::integratePositions(double _dt) void BallJoint::updateDegreeOfFreedomNames() { if(!mDofs[0]->isNamePreserved()) - mDofs[0]->setName(mJointP.mName + "_x", false); + mDofs[0]->setName(mAspectProperties.mName + "_x", false); if(!mDofs[1]->isNamePreserved()) - mDofs[1]->setName(mJointP.mName + "_y", false); + mDofs[1]->setName(mAspectProperties.mName + "_y", false); if(!mDofs[2]->isNamePreserved()) - mDofs[2]->setName(mJointP.mName + "_z", false); + mDofs[2]->setName(mAspectProperties.mName + "_z", false); } //============================================================================== @@ -155,8 +157,8 @@ void BallJoint::updateLocalTransform() const { mR.linear() = convertToRotation(getPositionsStatic()); - mT = mJointP.mT_ParentBodyToJoint * mR - * mJointP.mT_ChildBodyToJoint.inverse(); + mT = mAspectProperties.mT_ParentBodyToJoint * mR + * mAspectProperties.mT_ChildBodyToJoint.inverse(); assert(math::verifyTransform(mT)); } @@ -165,7 +167,7 @@ void BallJoint::updateLocalTransform() const void BallJoint::updateLocalJacobian(bool _mandatory) const { if (_mandatory) - mJacobian = math::getAdTMatrix(mJointP.mT_ChildBodyToJoint).leftCols<3>(); + mJacobian = math::getAdTMatrix(mAspectProperties.mT_ChildBodyToJoint).leftCols<3>(); } //============================================================================== diff --git a/dart/dynamics/BallJoint.h b/dart/dynamics/BallJoint.h index 77e2beb4df109..46bcd2f37d76a 100644 --- a/dart/dynamics/BallJoint.h +++ b/dart/dynamics/BallJoint.h @@ -102,7 +102,7 @@ class BallJoint : public MultiDofJoint<3> protected: /// Constructor called by Skeleton class - BallJoint(const Properties& _properties); + BallJoint(const Properties& properties); // Documentation inherited Joint* clone() const override; diff --git a/dart/dynamics/EulerJoint.cpp b/dart/dynamics/EulerJoint.cpp index 921d0abc69dad..596b59bbf9001 100644 --- a/dart/dynamics/EulerJoint.cpp +++ b/dart/dynamics/EulerJoint.cpp @@ -212,7 +212,7 @@ Eigen::Matrix EulerJoint::getLocalJacobianStatic( #ifndef NDEBUG if (std::abs(getPositionsStatic()[1]) == DART_PI * 0.5) std::cout << "Singular configuration in ZYX-euler joint [" - << mJointP.mName << "]. (" + << mAspectProperties.mName << "]. (" << _positions[0] << ", " << _positions[1] << ", " << _positions[2] << ")" @@ -238,7 +238,7 @@ Eigen::Matrix EulerJoint::getLocalJacobianStatic( #ifndef NDEBUG if (std::abs(_positions[1]) == DART_PI * 0.5) std::cout << "Singular configuration in ZYX-euler joint [" - << mJointP.mName << "]. (" + << mAspectProperties.mName << "]. (" << _positions[0] << ", " << _positions[1] << ", " << _positions[2] << ")" @@ -254,9 +254,9 @@ Eigen::Matrix EulerJoint::getLocalJacobianStatic( } } - J.col(0) = math::AdT(mJointP.mT_ChildBodyToJoint, J0); - J.col(1) = math::AdT(mJointP.mT_ChildBodyToJoint, J1); - J.col(2) = math::AdT(mJointP.mT_ChildBodyToJoint, J2); + J.col(0) = math::AdT(mAspectProperties.mT_ChildBodyToJoint, J0); + J.col(1) = math::AdT(mAspectProperties.mT_ChildBodyToJoint, J1); + J.col(2) = math::AdT(mAspectProperties.mT_ChildBodyToJoint, J2); assert(!math::isNan(J)); @@ -267,7 +267,7 @@ Eigen::Matrix EulerJoint::getLocalJacobianStatic( double det = luJTJ.determinant(); if (det < 1e-5) { - std::cout << "ill-conditioned Jacobian in joint [" << mJointP.mName << "]." + std::cout << "ill-conditioned Jacobian in joint [" << mAspectProperties.mName << "]." << " The determinant of the Jacobian is (" << det << ")." << std::endl; std::cout << "rank is (" << luJTJ.rank() << ")." << std::endl; @@ -280,14 +280,14 @@ Eigen::Matrix EulerJoint::getLocalJacobianStatic( } //============================================================================== -EulerJoint::EulerJoint(const Properties& _properties) - : detail::EulerJointBase(_properties, common::NoArg) +EulerJoint::EulerJoint(const Properties& properties) + : detail::EulerJointBase(properties, common::NoArg) { - createEulerJointAspect(_properties); - - // Inherited Joint Properties must be set in the final joint class or else we - // get pure virtual function calls - MultiDofJoint<3>::setProperties(_properties); + // Inherited Aspects must be created in the final joint class in reverse order + // or else we get pure virtual function calls + createEulerJointAspect(properties); + createMultiDofJointAspect(properties); + createJointAspect(properties); } //============================================================================== @@ -313,7 +313,7 @@ void EulerJoint::updateDegreeOfFreedomNames() affixes.push_back("_z"); break; default: - dterr << "Unsupported axis order in EulerJoint named '" << mJointP.mName + dterr << "Unsupported axis order in EulerJoint named '" << mAspectProperties.mName << "' (" << static_cast(getAxisOrder()) << ")\n"; } @@ -322,7 +322,7 @@ void EulerJoint::updateDegreeOfFreedomNames() for (size_t i = 0; i < 3; ++i) { if(!mDofs[i]->isNamePreserved()) - mDofs[i]->setName(mJointP.mName + affixes[i], false); + mDofs[i]->setName(mAspectProperties.mName + affixes[i], false); } } } @@ -330,8 +330,8 @@ void EulerJoint::updateDegreeOfFreedomNames() //============================================================================== void EulerJoint::updateLocalTransform() const { - mT = mJointP.mT_ParentBodyToJoint * convertToTransform(getPositionsStatic()) - * mJointP.mT_ChildBodyToJoint.inverse(); + mT = mAspectProperties.mT_ParentBodyToJoint * convertToTransform(getPositionsStatic()) + * mAspectProperties.mT_ChildBodyToJoint.inverse(); assert(math::verifyTransform(mT)); } @@ -410,9 +410,9 @@ void EulerJoint::updateLocalJacobianTimeDeriv() const } } - mJacobianDeriv.col(0) = math::AdT(mJointP.mT_ChildBodyToJoint, dJ0); - mJacobianDeriv.col(1) = math::AdT(mJointP.mT_ChildBodyToJoint, dJ1); - mJacobianDeriv.col(2) = math::AdT(mJointP.mT_ChildBodyToJoint, dJ2); + mJacobianDeriv.col(0) = math::AdT(mAspectProperties.mT_ChildBodyToJoint, dJ0); + mJacobianDeriv.col(1) = math::AdT(mAspectProperties.mT_ChildBodyToJoint, dJ1); + mJacobianDeriv.col(2) = math::AdT(mAspectProperties.mT_ChildBodyToJoint, dJ2); assert(!math::isNan(mJacobianDeriv)); } diff --git a/dart/dynamics/EulerJoint.h b/dart/dynamics/EulerJoint.h index c8e086dfd7801..336c362710a53 100644 --- a/dart/dynamics/EulerJoint.h +++ b/dart/dynamics/EulerJoint.h @@ -153,7 +153,7 @@ class EulerJoint : public detail::EulerJointBase protected: /// Constructor called by Skeleton class - EulerJoint(const Properties& _properties); + EulerJoint(const Properties& properties); // Documentation inherited virtual Joint* clone() const override; diff --git a/dart/dynamics/FreeJoint.cpp b/dart/dynamics/FreeJoint.cpp index 049741f826777..37a94311a433b 100644 --- a/dart/dynamics/FreeJoint.cpp +++ b/dart/dynamics/FreeJoint.cpp @@ -167,9 +167,9 @@ void FreeJoint::setSpatialMotion(const Eigen::Isometry3d* newTransform, void FreeJoint::setRelativeTransform(const Eigen::Isometry3d& newTransform) { setPositionsStatic(convertToPositions( - mJointP.mT_ParentBodyToJoint.inverse() * + mAspectProperties.mT_ParentBodyToJoint.inverse() * newTransform * - mJointP.mT_ChildBodyToJoint)); + mAspectProperties.mT_ChildBodyToJoint)); } //============================================================================== @@ -525,13 +525,16 @@ Eigen::Vector6d FreeJoint::getPositionDifferencesStatic( } //============================================================================== -FreeJoint::FreeJoint(const Properties& _properties) - : MultiDofJoint<6>(_properties), +FreeJoint::FreeJoint(const Properties& properties) + : MultiDofJoint<6>(properties), mQ(Eigen::Isometry3d::Identity()) { mJacobianDeriv = Eigen::Matrix6d::Zero(); - setProperties(_properties); + // Inherited Aspects must be created in the final joint class in reverse order + // or else we get pure virtual function calls + createMultiDofJointAspect(properties); + createJointAspect(properties); } //============================================================================== @@ -573,17 +576,17 @@ void FreeJoint::integratePositions(double _dt) void FreeJoint::updateDegreeOfFreedomNames() { if(!mDofs[0]->isNamePreserved()) - mDofs[0]->setName(mJointP.mName + "_rot_x", false); + mDofs[0]->setName(mAspectProperties.mName + "_rot_x", false); if(!mDofs[1]->isNamePreserved()) - mDofs[1]->setName(mJointP.mName + "_rot_y", false); + mDofs[1]->setName(mAspectProperties.mName + "_rot_y", false); if(!mDofs[2]->isNamePreserved()) - mDofs[2]->setName(mJointP.mName + "_rot_z", false); + mDofs[2]->setName(mAspectProperties.mName + "_rot_z", false); if(!mDofs[3]->isNamePreserved()) - mDofs[3]->setName(mJointP.mName + "_pos_x", false); + mDofs[3]->setName(mAspectProperties.mName + "_pos_x", false); if(!mDofs[4]->isNamePreserved()) - mDofs[4]->setName(mJointP.mName + "_pos_y", false); + mDofs[4]->setName(mAspectProperties.mName + "_pos_y", false); if(!mDofs[5]->isNamePreserved()) - mDofs[5]->setName(mJointP.mName + "_pos_z", false); + mDofs[5]->setName(mAspectProperties.mName + "_pos_z", false); } //============================================================================== @@ -591,8 +594,8 @@ void FreeJoint::updateLocalTransform() const { mQ = convertToTransform(getPositionsStatic()); - mT = mJointP.mT_ParentBodyToJoint * mQ - * mJointP.mT_ChildBodyToJoint.inverse(); + mT = mAspectProperties.mT_ParentBodyToJoint * mQ + * mAspectProperties.mT_ChildBodyToJoint.inverse(); assert(math::verifyTransform(mT)); } @@ -601,7 +604,7 @@ void FreeJoint::updateLocalTransform() const void FreeJoint::updateLocalJacobian(bool _mandatory) const { if (_mandatory) - mJacobian = math::getAdTMatrix(mJointP.mT_ChildBodyToJoint); + mJacobian = math::getAdTMatrix(mAspectProperties.mT_ChildBodyToJoint); } //============================================================================== diff --git a/dart/dynamics/FreeJoint.h b/dart/dynamics/FreeJoint.h index 8a70a26fa8d15..dc1287f8c09e5 100644 --- a/dart/dynamics/FreeJoint.h +++ b/dart/dynamics/FreeJoint.h @@ -271,7 +271,7 @@ class FreeJoint : public MultiDofJoint<6> protected: /// Constructor called by Skeleton class - FreeJoint(const Properties& _properties); + FreeJoint(const Properties& properties); // Documentation inherited Joint* clone() const override; diff --git a/dart/dynamics/Joint.cpp b/dart/dynamics/Joint.cpp index e5fb1906f93f9..f17b11f687318 100644 --- a/dart/dynamics/Joint.cpp +++ b/dart/dynamics/Joint.cpp @@ -50,14 +50,24 @@ namespace dart { namespace dynamics { //============================================================================== -const Joint::ActuatorType Joint::DefaultActuatorType = Joint::FORCE; - -//============================================================================== -Joint::Properties::Properties(const std::string& _name, - const Eigen::Isometry3d& _T_ParentBodyToJoint, - const Eigen::Isometry3d& _T_ChildBodyToJoint, - bool _isPositionLimited, - ActuatorType _actuatorType) +const Joint::ActuatorType Joint::DefaultActuatorType = detail::DefaultActuatorType; +// These declarations are needed for linking to work +constexpr Joint::ActuatorType Joint::FORCE; +constexpr Joint::ActuatorType Joint::PASSIVE; +constexpr Joint::ActuatorType Joint::SERVO; +constexpr Joint::ActuatorType Joint::ACCELERATION; +constexpr Joint::ActuatorType Joint::VELOCITY; +constexpr Joint::ActuatorType Joint::LOCKED; + +namespace detail { + +//============================================================================== +JointProperties::JointProperties( + const std::string& _name, + const Eigen::Isometry3d& _T_ParentBodyToJoint, + const Eigen::Isometry3d& _T_ChildBodyToJoint, + bool _isPositionLimited, + ActuatorType _actuatorType) : mName(_name), mT_ParentBodyToJoint(_T_ParentBodyToJoint), mT_ChildBodyToJoint(_T_ChildBodyToJoint), @@ -67,10 +77,12 @@ Joint::Properties::Properties(const std::string& _name, // Do nothing } +} // namespace detail + //============================================================================== Joint::ExtendedProperties::ExtendedProperties( const Properties& standardProperties, - const AspectProperties& aspectProperties) + const CompositeProperties& aspectProperties) : Properties(standardProperties), mAspectProperties(aspectProperties) { @@ -80,7 +92,7 @@ Joint::ExtendedProperties::ExtendedProperties( //============================================================================== Joint::ExtendedProperties::ExtendedProperties( Properties&& standardProperties, - AspectProperties&& aspectProperties) + CompositeProperties&& aspectProperties) : Properties(std::move(standardProperties)), mAspectProperties(std::move(aspectProperties)) { @@ -94,19 +106,25 @@ Joint::~Joint() } //============================================================================== -void Joint::setProperties(const Properties& _properties) +void Joint::setProperties(const Properties& properties) +{ + setAspectProperties(properties); +} + +//============================================================================== +void Joint::setAspectProperties(const AspectProperties& properties) { - setName(_properties.mName); - setTransformFromParentBodyNode(_properties.mT_ParentBodyToJoint); - setTransformFromChildBodyNode(_properties.mT_ChildBodyToJoint); - setPositionLimitEnforced(_properties.mIsPositionLimited); - setActuatorType(_properties.mActuatorType); + setName(properties.mName); + setTransformFromParentBodyNode(properties.mT_ParentBodyToJoint); + setTransformFromChildBodyNode(properties.mT_ChildBodyToJoint); + setPositionLimitEnforced(properties.mIsPositionLimited); + setActuatorType(properties.mActuatorType); } //============================================================================== const Joint::Properties& Joint::getJointProperties() const { - return mJointP; + return mAspectProperties; } //============================================================================== @@ -137,37 +155,37 @@ Joint& Joint::operator=(const Joint& _otherJoint) //============================================================================== const std::string& Joint::setName(const std::string& _name, bool _renameDofs) { - if (mJointP.mName == _name) + if (mAspectProperties.mName == _name) { if (_renameDofs) updateDegreeOfFreedomNames(); - return mJointP.mName; + return mAspectProperties.mName; } const SkeletonPtr& skel = mChildBodyNode? mChildBodyNode->getSkeleton() : nullptr; if (skel) { - skel->mNameMgrForJoints.removeName(mJointP.mName); - mJointP.mName = _name; + skel->mNameMgrForJoints.removeName(mAspectProperties.mName); + mAspectProperties.mName = _name; skel->addEntryToJointNameMgr(this, _renameDofs); } else { - mJointP.mName = _name; + mAspectProperties.mName = _name; if (_renameDofs) updateDegreeOfFreedomNames(); } - return mJointP.mName; + return mAspectProperties.mName; } //============================================================================== const std::string& Joint::getName() const { - return mJointP.mName; + return mAspectProperties.mName; } //============================================================================== @@ -191,19 +209,19 @@ size_t Joint::getVersion() const //============================================================================== void Joint::setActuatorType(Joint::ActuatorType _actuatorType) { - mJointP.mActuatorType = _actuatorType; + mAspectProperties.mActuatorType = _actuatorType; } //============================================================================== Joint::ActuatorType Joint::getActuatorType() const { - return mJointP.mActuatorType; + return mAspectProperties.mActuatorType; } //============================================================================== bool Joint::isKinematic() const { - switch (mJointP.mActuatorType) + switch (mAspectProperties.mActuatorType) { case FORCE: case PASSIVE: @@ -317,7 +335,7 @@ const Eigen::Vector6d& Joint::getLocalPrimaryAcceleration() const //============================================================================== void Joint::setPositionLimitEnforced(bool _isPositionLimited) { - mJointP.mIsPositionLimited = _isPositionLimited; + mAspectProperties.mIsPositionLimited = _isPositionLimited; } //============================================================================== @@ -329,7 +347,7 @@ void Joint::setPositionLimited(bool _isPositionLimited) //============================================================================== bool Joint::isPositionLimitEnforced() const { - return mJointP.mIsPositionLimited; + return mAspectProperties.mIsPositionLimited; } //============================================================================== @@ -410,7 +428,7 @@ bool Joint::checkSanity(bool _printWarnings) const void Joint::setTransformFromParentBodyNode(const Eigen::Isometry3d& _T) { assert(math::verifyTransform(_T)); - mJointP.mT_ParentBodyToJoint = _T; + mAspectProperties.mT_ParentBodyToJoint = _T; notifyPositionUpdate(); } @@ -418,7 +436,7 @@ void Joint::setTransformFromParentBodyNode(const Eigen::Isometry3d& _T) void Joint::setTransformFromChildBodyNode(const Eigen::Isometry3d& _T) { assert(math::verifyTransform(_T)); - mJointP.mT_ChildBodyToJoint = _T; + mAspectProperties.mT_ChildBodyToJoint = _T; updateLocalJacobian(); notifyPositionUpdate(); } @@ -426,13 +444,13 @@ void Joint::setTransformFromChildBodyNode(const Eigen::Isometry3d& _T) //============================================================================== const Eigen::Isometry3d& Joint::getTransformFromParentBodyNode() const { - return mJointP.mT_ParentBodyToJoint; + return mAspectProperties.mT_ParentBodyToJoint; } //============================================================================== const Eigen::Isometry3d& Joint::getTransformFromChildBodyNode() const { - return mJointP.mT_ChildBodyToJoint; + return mAspectProperties.mT_ChildBodyToJoint; } //============================================================================== @@ -442,9 +460,8 @@ void Joint::applyGLTransform(renderer::RenderInterface* _ri) } //============================================================================== -Joint::Joint(const Properties& _properties) - : mJointP(_properties), - mChildBodyNode(nullptr), +Joint::Joint() + : mChildBodyNode(nullptr), mT(Eigen::Isometry3d::Identity()), mSpatialVelocity(Eigen::Vector6d::Zero()), mSpatialAcceleration(Eigen::Vector6d::Zero()), @@ -456,7 +473,7 @@ Joint::Joint(const Properties& _properties) mIsLocalJacobianDirty(true), mIsLocalJacobianTimeDerivDirty(true) { - // Do nothing + // Do nothing. The Joint::Aspect must be created by a derived class. } //============================================================================== diff --git a/dart/dynamics/Joint.h b/dart/dynamics/Joint.h index 05f5760ff65ae..0be50aab3d494 100644 --- a/dart/dynamics/Joint.h +++ b/dart/dynamics/Joint.h @@ -45,9 +45,10 @@ #include "dart/common/Deprecated.h" #include "dart/common/Subject.h" #include "dart/common/VersionCounter.h" -#include "dart/common/Composite.h" +#include "dart/common/EmbeddedAspect.h" #include "dart/math/MathTypes.h" #include "dart/dynamics/SmartPointer.h" +#include "dart/dynamics/detail/Joint.h" namespace dart { namespace renderer { @@ -65,117 +66,37 @@ class DegreeOfFreedom; /// class Joint class Joint : public virtual common::Subject, public virtual common::VersionCounter, - public virtual common::Composite + public common::EmbedProperties { public: - /// Actuator type - /// - /// The command is taken by setCommand() or setCommands(), and the meaning of - /// command is different depending on the actuator type. The default actuator - /// type is FORCE. (TODO: FreeJoint should be PASSIVE?) - /// - /// FORCE/PASSIVE/SERVO joints are dynamic joints while - /// ACCELERATION/VELOCITY/LOCKED joints are kinematic joints. - /// - /// Note the presence of joint damping force and joint spring force for all - /// the actuator types if the coefficients are non-zero. The default - /// coefficients are zero. - /// - /// \sa setActuatorType(), getActuatorType(), - /// setSpringStiffness(), setDampingCoefficient(), - enum ActuatorType - { - /// Command input is joint force, and the output is joint acceleration. - /// - /// If the command is zero, then it's identical to passive joint. The valid - /// joint constraints are position limit, velocity limit, and Coulomb - /// friction, and the invalid joint constraint is force limit. - FORCE, - - /// Passive joint doesn't take any command input, and the output is joint - /// acceleration. - /// - /// The valid joint constraints are position limit, velocity limit, and - /// Coulomb friction, and the invalid joint constraint is force limit. - PASSIVE, - - /// Command input is desired velocity, and the output is joint acceleration. - /// - /// The constraint solver will try to track the desired velocity within the - /// joint force limit. All the joint constarints are valid. - SERVO, - - /// Command input is joint acceleration, and the output is joint force. - /// - /// The joint acceleration is always satisfied but it doesn't take the joint - /// force limit into account. All the joint constraints are invalid. - ACCELERATION, - - /// Command input is joint velocity, and the output is joint force. - /// - /// The joint velocity is always satisfied but it doesn't take the joint - /// force limit into account. If you want to consider the joint force limit, - /// should use SERVO instead. All the joint constraints are invalid. - VELOCITY, - - /// Locked joint always set the velocity and acceleration to zero so that - /// the joint dosen't move at all (locked), and the output is joint force. - /// force. - /// - /// All the joint constraints are invalid. - LOCKED - }; - - struct Properties - { - /// Joint name - std::string mName; - - /// Transformation from parent BodyNode to this Joint - Eigen::Isometry3d mT_ParentBodyToJoint; + using CompositeProperties = common::Composite::Properties; + using Properties = detail::JointProperties; - /// Transformation from child BodyNode to this Joint - Eigen::Isometry3d mT_ChildBodyToJoint; + typedef detail::ActuatorType ActuatorType; + static constexpr ActuatorType FORCE = detail::FORCE; + static constexpr ActuatorType PASSIVE = detail::PASSIVE; + static constexpr ActuatorType SERVO = detail::SERVO; + static constexpr ActuatorType ACCELERATION = detail::ACCELERATION; + static constexpr ActuatorType VELOCITY = detail::VELOCITY; + static constexpr ActuatorType LOCKED = detail::LOCKED; - /// True if the joint limits should be enforced in dynamic simulation - bool mIsPositionLimited; - - /// Actuator type - ActuatorType mActuatorType; - - /// Constructor - Properties(const std::string& _name = "Joint", - const Eigen::Isometry3d& _T_ParentBodyToJoint = - Eigen::Isometry3d::Identity(), - const Eigen::Isometry3d& _T_ChildBodyToJoint = - Eigen::Isometry3d::Identity(), - bool _isPositionLimited = false, - ActuatorType _actuatorType = DefaultActuatorType); - - virtual ~Properties() = default; - - public: - // To get byte-aligned Eigen vectors - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; - - using AspectProperties = common::Composite::Properties; + DART_BAKE_SPECIALIZED_ASPECT_IRREGULAR(Aspect, JointAspect); struct ExtendedProperties : Properties { /// Composed constructor ExtendedProperties( const Properties& standardProperties = Properties(), - const AspectProperties& aspectProperties = AspectProperties()); + const CompositeProperties& aspectProperties = CompositeProperties()); /// Composed move constructor ExtendedProperties( Properties&& standardProperties, - AspectProperties&& aspectProperties); + CompositeProperties&& aspectProperties); /// Properties of all the Aspects attached to this Joint - AspectProperties mAspectProperties; + CompositeProperties mAspectProperties; }; /// Default actuator type @@ -187,7 +108,10 @@ class Joint : public virtual common::Subject, virtual ~Joint(); /// Set the Properties of this Joint - void setProperties(const Properties& _properties); + void setProperties(const Properties& properties); + + /// Set the AspectProperties of this Joint + void setAspectProperties(const AspectProperties& properties); /// Get the Properties of this Joint const Properties& getJointProperties() const; @@ -737,7 +661,7 @@ class Joint : public virtual common::Subject, protected: /// Constructor called by inheriting class - Joint(const Properties& _properties); + Joint(); /// Create a clone of this Joint. This may only be called by the Skeleton /// class. @@ -938,9 +862,6 @@ class Joint : public virtual common::Subject, protected: - /// Properties of this Joint - Properties mJointP; - /// Child BodyNode pointer that this Joint belongs to BodyNode* mChildBodyNode; diff --git a/dart/dynamics/MultiDofJoint.h b/dart/dynamics/MultiDofJoint.h index 2219b39f07db8..025e49ff11a67 100644 --- a/dart/dynamics/MultiDofJoint.h +++ b/dart/dynamics/MultiDofJoint.h @@ -59,11 +59,14 @@ class Skeleton; /// class MultiDofJoint template class MultiDofJoint : - public Joint, - public virtual common::RequiresAspect< detail::MultiDofJointAspect > + public common::CompositeJoiner>> > { public: + using Base = common::CompositeJoiner>> >; + constexpr static size_t NumDofs = DOF; using Vector = Eigen::Matrix; @@ -134,7 +137,7 @@ class MultiDofJoint : //---------------------------------------------------------------------------- // Documentation inherited - virtual void setCommand(size_t _index, double _command) override; + virtual void setCommand(size_t _index, double command) override; // Documentation inherited virtual double getCommand(size_t _index) const override; @@ -418,7 +421,7 @@ class MultiDofJoint : protected: /// Constructor called by inheriting classes - MultiDofJoint(const Properties& _properties); + MultiDofJoint(const Properties& properties); // Docuemntation inherited void registerDofs() override; diff --git a/dart/dynamics/PlanarJoint.cpp b/dart/dynamics/PlanarJoint.cpp index 41d7350321c86..4cca3a8c40043 100644 --- a/dart/dynamics/PlanarJoint.cpp +++ b/dart/dynamics/PlanarJoint.cpp @@ -193,11 +193,11 @@ Eigen::Matrix PlanarJoint::getLocalJacobianStatic( J.block<3, 1>(0, 2) = getPlanarJointAspect()->getRotAxis(); J.leftCols<2>() - = math::AdTJacFixed(mJointP.mT_ChildBodyToJoint + = math::AdTJacFixed(mAspectProperties.mT_ChildBodyToJoint * math::expAngular(getPlanarJointAspect()->getRotAxis() * -_positions[2]), J.leftCols<2>()); - J.col(2) = math::AdTJac(mJointP.mT_ChildBodyToJoint, J.col(2)); + J.col(2) = math::AdTJac(mAspectProperties.mT_ChildBodyToJoint, J.col(2)); // Verification assert(!math::isNan(J)); @@ -206,14 +206,14 @@ Eigen::Matrix PlanarJoint::getLocalJacobianStatic( } //============================================================================== -PlanarJoint::PlanarJoint(const Properties& _properties) - : detail::PlanarJointBase(_properties, common::NoArg) +PlanarJoint::PlanarJoint(const Properties& properties) + : detail::PlanarJointBase(properties, common::NoArg) { - createPlanarJointAspect(_properties); - - // Inherited Joint Properties must be set in the final joint class or else we - // get pure virtual function calls - MultiDofJoint<3>::setProperties(_properties); + // Inherited Aspects must be created in the final joint class in reverse order + // or else we get pure virtual function calls + createPlanarJointAspect(properties); + createMultiDofJointAspect(properties); + createJointAspect(properties); } //============================================================================== @@ -245,7 +245,7 @@ void PlanarJoint::updateDegreeOfFreedomNames() affixes.push_back("_2"); break; default: - dterr << "Unsupported plane type in PlanarJoint named '" << mJointP.mName + dterr << "Unsupported plane type in PlanarJoint named '" << mAspectProperties.mName << "' (" << static_cast(getPlanarJointAspect()->getPlaneType()) << ")\n"; } @@ -255,7 +255,7 @@ void PlanarJoint::updateDegreeOfFreedomNames() for (size_t i = 0; i < 2; ++i) { if (!mDofs[i]->isNamePreserved()) - mDofs[i]->setName(mJointP.mName + affixes[i], false); + mDofs[i]->setName(mAspectProperties.mName + affixes[i], false); } } } @@ -264,11 +264,11 @@ void PlanarJoint::updateDegreeOfFreedomNames() void PlanarJoint::updateLocalTransform() const { const Eigen::Vector3d& positions = getPositionsStatic(); - mT = mJointP.mT_ParentBodyToJoint + mT = mAspectProperties.mT_ParentBodyToJoint * Eigen::Translation3d(getPlanarJointAspect()->getTransAxis1() * positions[0]) * Eigen::Translation3d(getPlanarJointAspect()->getTransAxis2() * positions[1]) * math::expAngular (getPlanarJointAspect()->getRotAxis() * positions[2]) - * mJointP.mT_ChildBodyToJoint.inverse(); + * mAspectProperties.mT_ChildBodyToJoint.inverse(); // Verification assert(math::verifyTransform(mT)); @@ -292,14 +292,14 @@ void PlanarJoint::updateLocalJacobianTimeDeriv() const const Eigen::Vector3d& velocities = getVelocitiesStatic(); mJacobianDeriv.col(0) = -math::ad(Jacobian.col(2) * velocities[2], - math::AdT(mJointP.mT_ChildBodyToJoint + math::AdT(mAspectProperties.mT_ChildBodyToJoint * math::expAngular(getPlanarJointAspect()->getRotAxis() * -getPositionsStatic()[2]), J.col(0))); mJacobianDeriv.col(1) = -math::ad(Jacobian.col(2) * velocities[2], - math::AdT(mJointP.mT_ChildBodyToJoint + math::AdT(mAspectProperties.mT_ChildBodyToJoint * math::expAngular(getPlanarJointAspect()->getRotAxis() * -getPositionsStatic()[2]), J.col(1))); diff --git a/dart/dynamics/PlanarJoint.h b/dart/dynamics/PlanarJoint.h index fa35b7b39c5eb..57c5536a3f3ad 100644 --- a/dart/dynamics/PlanarJoint.h +++ b/dart/dynamics/PlanarJoint.h @@ -137,7 +137,7 @@ class PlanarJoint : public detail::PlanarJointBase protected: /// Constructor called by Skeleton class - PlanarJoint(const Properties& _properties); + PlanarJoint(const Properties& properties); // Documentation inherited virtual Joint* clone() const override; diff --git a/dart/dynamics/PrismaticJoint.cpp b/dart/dynamics/PrismaticJoint.cpp index b0ece9cc4ac04..25a476d81b7d0 100644 --- a/dart/dynamics/PrismaticJoint.cpp +++ b/dart/dynamics/PrismaticJoint.cpp @@ -129,14 +129,14 @@ const Eigen::Vector3d& PrismaticJoint::getAxis() const } //============================================================================== -PrismaticJoint::PrismaticJoint(const Properties& _properties) - : detail::PrismaticJointBase(_properties, common::NoArg) +PrismaticJoint::PrismaticJoint(const Properties& properties) + : detail::PrismaticJointBase(properties, common::NoArg) { - createPrismaticJointAspect(_properties); - - // Inherited Joint Properties must be set in the final joint class or else we - // get pure virtual function calls - SingleDofJoint::setProperties(_properties); + // Inherited Aspects must be created in the final joint class in reverse order + // or else we get pure virtual function calls + createPrismaticJointAspect(properties); + createSingleDofJointAspect(properties); + createJointAspect(properties); } //============================================================================== @@ -148,9 +148,9 @@ Joint* PrismaticJoint::clone() const //============================================================================== void PrismaticJoint::updateLocalTransform() const { - mT = mJointP.mT_ParentBodyToJoint + mT = mAspectProperties.mT_ParentBodyToJoint * Eigen::Translation3d(getAxis() * getPositionStatic()) - * mJointP.mT_ChildBodyToJoint.inverse(); + * mAspectProperties.mT_ChildBodyToJoint.inverse(); // Verification assert(math::verifyTransform(mT)); @@ -161,7 +161,7 @@ void PrismaticJoint::updateLocalJacobian(bool _mandatory) const { if(_mandatory) { - mJacobian = math::AdTLinear(mJointP.mT_ChildBodyToJoint, getAxis()); + mJacobian = math::AdTLinear(mAspectProperties.mT_ChildBodyToJoint, getAxis()); // Verification assert(!math::isNan(mJacobian)); diff --git a/dart/dynamics/PrismaticJoint.h b/dart/dynamics/PrismaticJoint.h index b114cc5292b55..857b1b0864241 100644 --- a/dart/dynamics/PrismaticJoint.h +++ b/dart/dynamics/PrismaticJoint.h @@ -98,7 +98,7 @@ class PrismaticJoint : public detail::PrismaticJointBase protected: /// Constructor called by Skeleton class - PrismaticJoint(const Properties& _properties); + PrismaticJoint(const Properties& properties); // Documentation inherited virtual Joint* clone() const override; diff --git a/dart/dynamics/RevoluteJoint.cpp b/dart/dynamics/RevoluteJoint.cpp index 9604c9287bcd2..60aafb686e7d8 100644 --- a/dart/dynamics/RevoluteJoint.cpp +++ b/dart/dynamics/RevoluteJoint.cpp @@ -130,15 +130,14 @@ const Eigen::Vector3d& RevoluteJoint::getAxis() const } //============================================================================== -RevoluteJoint::RevoluteJoint(const Properties& _properties) - : detail::RevoluteJointBase(_properties, common::NoArg) -// : detail::RevoluteJointBase(common::NextArgs, _properties) +RevoluteJoint::RevoluteJoint(const Properties& properties) + : detail::RevoluteJointBase(properties, common::NoArg) { - createRevoluteJointAspect(_properties); - - // Inherited Joint Properties must be set in the final joint class or else we - // get pure virtual function calls - SingleDofJoint::setProperties(_properties); + // Inherited Aspects must be created in the final joint class in reverse order + // or else we get pure virtual function calls + createRevoluteJointAspect(properties); + createSingleDofJointAspect(properties); + createJointAspect(properties); } //============================================================================== @@ -150,9 +149,9 @@ Joint* RevoluteJoint::clone() const //============================================================================== void RevoluteJoint::updateLocalTransform() const { - mT = mJointP.mT_ParentBodyToJoint + mT = mAspectProperties.mT_ParentBodyToJoint * math::expAngular(getAxis() * getPositionStatic()) - * mJointP.mT_ChildBodyToJoint.inverse(); + * mAspectProperties.mT_ChildBodyToJoint.inverse(); // Verification assert(math::verifyTransform(mT)); @@ -163,7 +162,7 @@ void RevoluteJoint::updateLocalJacobian(bool _mandatory) const { if(_mandatory) { - mJacobian = math::AdTAngular(mJointP.mT_ChildBodyToJoint, getAxis()); + mJacobian = math::AdTAngular(mAspectProperties.mT_ChildBodyToJoint, getAxis()); // Verification assert(!math::isNan(mJacobian)); diff --git a/dart/dynamics/RevoluteJoint.h b/dart/dynamics/RevoluteJoint.h index f7460fb6afb2b..61c5905458907 100644 --- a/dart/dynamics/RevoluteJoint.h +++ b/dart/dynamics/RevoluteJoint.h @@ -96,7 +96,7 @@ class RevoluteJoint : public detail::RevoluteJointBase protected: /// Constructor called by Skeleton class - RevoluteJoint(const Properties& _properties); + RevoluteJoint(const Properties& properties); // Documentation inherited virtual Joint* clone() const override; diff --git a/dart/dynamics/ScrewJoint.cpp b/dart/dynamics/ScrewJoint.cpp index dcbc1951be075..f93f11e2149ab 100644 --- a/dart/dynamics/ScrewJoint.cpp +++ b/dart/dynamics/ScrewJoint.cpp @@ -142,14 +142,14 @@ double ScrewJoint::getPitch() const } //============================================================================== -ScrewJoint::ScrewJoint(const Properties& _properties) - : detail::ScrewJointBase(_properties, common::NoArg) +ScrewJoint::ScrewJoint(const Properties& properties) + : detail::ScrewJointBase(properties, common::NoArg) { - createScrewJointAspect(_properties); - - // Inherited Joint Properties must be set in the final joint class or else we - // get pure virtual function calls - SingleDofJoint::setProperties(_properties); + // Inherited Aspects must be created in the final joint class in reverse order + // or else we get pure virtual function calls + createScrewJointAspect(properties); + createSingleDofJointAspect(properties); + createJointAspect(properties); } //============================================================================== @@ -164,9 +164,9 @@ void ScrewJoint::updateLocalTransform() const Eigen::Vector6d S = Eigen::Vector6d::Zero(); S.head<3>() = getAxis(); S.tail<3>() = getAxis()*getPitch()/DART_2PI; - mT = mJointP.mT_ParentBodyToJoint + mT = mAspectProperties.mT_ParentBodyToJoint * math::expMap(S * getPositionStatic()) - * mJointP.mT_ChildBodyToJoint.inverse(); + * mAspectProperties.mT_ChildBodyToJoint.inverse(); assert(math::verifyTransform(mT)); } @@ -178,7 +178,7 @@ void ScrewJoint::updateLocalJacobian(bool _mandatory) const Eigen::Vector6d S = Eigen::Vector6d::Zero(); S.head<3>() = getAxis(); S.tail<3>() = getAxis()*getPitch()/DART_2PI; - mJacobian = math::AdT(mJointP.mT_ChildBodyToJoint, S); + mJacobian = math::AdT(mAspectProperties.mT_ChildBodyToJoint, S); assert(!math::isNan(mJacobian)); } } diff --git a/dart/dynamics/ScrewJoint.h b/dart/dynamics/ScrewJoint.h index 4d931fed07603..2a272b3a89e5b 100644 --- a/dart/dynamics/ScrewJoint.h +++ b/dart/dynamics/ScrewJoint.h @@ -104,7 +104,7 @@ class ScrewJoint : public detail::ScrewJointBase protected: /// Constructor called by Skeleton class - ScrewJoint(const Properties& _properties); + ScrewJoint(const Properties& properties); // Documentation inherited virtual Joint* clone() const override; diff --git a/dart/dynamics/SingleDofJoint.cpp b/dart/dynamics/SingleDofJoint.cpp index defe4960b7ead..02211875fbe08 100644 --- a/dart/dynamics/SingleDofJoint.cpp +++ b/dart/dynamics/SingleDofJoint.cpp @@ -55,7 +55,7 @@ #define SINGLEDOFJOINT_REPORT_UNSUPPORTED_ACTUATOR( func ) \ dterr << "[SingleDofJoint::" # func "] Unsupported actuator type (" \ - << mJointP.mActuatorType << ") for Joint [" << getName() << "].\n"; \ + << mAspectProperties.mActuatorType << ") for Joint [" << getName() << "].\n"; \ assert(false); namespace dart { @@ -83,7 +83,7 @@ void SingleDofJoint::setProperties(const UniqueProperties& _properties) //============================================================================== SingleDofJoint::Properties SingleDofJoint::getSingleDofJointProperties() const { - return Properties(mJointP, getSingleDofJointAspect()->getProperties()); + return Properties(mAspectProperties, getSingleDofJointAspect()->getProperties()); } //============================================================================== @@ -239,7 +239,7 @@ void SingleDofJoint::setCommand(size_t _index, double _command) return; } - switch (mJointP.mActuatorType) + switch (mAspectProperties.mActuatorType) { case FORCE: mCommand = math::clip(_command, @@ -498,7 +498,7 @@ void SingleDofJoint::setVelocity(size_t _index, double _velocity) setVelocityStatic(_velocity); #if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) - if (mJointP.mActuatorType == VELOCITY) + if (mAspectProperties.mActuatorType == VELOCITY) mCommand = getVelocityStatic(); // TODO: Remove at DART 5.1. #endif @@ -528,7 +528,7 @@ void SingleDofJoint::setVelocities(const Eigen::VectorXd& _velocities) setVelocityStatic(_velocities[0]); #if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) - if (mJointP.mActuatorType == VELOCITY) + if (mAspectProperties.mActuatorType == VELOCITY) mCommand = getVelocityStatic(); // TODO: Remove at DART 5.1. #endif @@ -661,7 +661,7 @@ void SingleDofJoint::setAcceleration(size_t _index, double _acceleration) setAccelerationStatic(_acceleration); #if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) - if (mJointP.mActuatorType == ACCELERATION) + if (mAspectProperties.mActuatorType == ACCELERATION) mCommand = getAccelerationStatic(); // TODO: Remove at DART 5.1. #endif @@ -691,7 +691,7 @@ void SingleDofJoint::setAccelerations(const Eigen::VectorXd& _accelerations) setAccelerationStatic(_accelerations[0]); #if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) - if (mJointP.mActuatorType == ACCELERATION) + if (mAspectProperties.mActuatorType == ACCELERATION) mCommand = getAccelerationStatic(); // TODO: Remove at DART 5.1. #endif @@ -819,7 +819,7 @@ void SingleDofJoint::setForce(size_t _index, double _force) mForce = _force; #if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) - if (mJointP.mActuatorType == FORCE) + if (mAspectProperties.mActuatorType == FORCE) mCommand = mForce; // TODO: Remove at DART 5.1. #endif @@ -849,7 +849,7 @@ void SingleDofJoint::setForces(const Eigen::VectorXd& _forces) mForce = _forces[0]; #if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) - if (mJointP.mActuatorType == FORCE) + if (mAspectProperties.mActuatorType == FORCE) mCommand = mForce; // TODO: Remove at DART 5.1. #endif @@ -867,7 +867,7 @@ void SingleDofJoint::resetForces() mForce = 0.0; #if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) - if (mJointP.mActuatorType == FORCE) + if (mAspectProperties.mActuatorType == FORCE) mCommand = mForce; // TODO: Remove at DART 5.1. #endif @@ -1144,13 +1144,12 @@ double SingleDofJoint::getPotentialEnergy() const } //============================================================================== -SingleDofJoint::SingleDofJoint(const Properties& _properties) - : detail::SingleDofJointBase(_properties, common::NoArg), - mDof(createDofPointer(0)), +SingleDofJoint::SingleDofJoint(const Properties& properties) + : mDof(createDofPointer(0)), mCommand(0.0), - mPosition(_properties.mInitialPosition), + mPosition(properties.mInitialPosition), mPositionDeriv(0.0), - mVelocity(_properties.mInitialVelocity), + mVelocity(properties.mInitialVelocity), mVelocityDeriv(0.0), mAcceleration(0.0), mAccelerationDeriv(0.0), @@ -1168,7 +1167,8 @@ SingleDofJoint::SingleDofJoint(const Properties& _properties) mInvM_a(0.0), mInvMassMatrixSegment(0.0) { - createSingleDofJointAspect(_properties); + // Do nothing. Joint and SingleDofJoint Aspects must be created by the most + // derived class. } //============================================================================== @@ -1185,7 +1185,7 @@ void SingleDofJoint::updateDegreeOfFreedomNames() { // Same name as the joint it belongs to. if (!mDof->isNamePreserved()) - mDof->setName(mJointP.mName, false); + mDof->setName(mAspectProperties.mName, false); } //============================================================================== @@ -1317,7 +1317,7 @@ void SingleDofJoint::addVelocityChangeTo(Eigen::Vector6d& _velocityChange) void SingleDofJoint::addChildArtInertiaTo( Eigen::Matrix6d& _parentArtInertia, const Eigen::Matrix6d& _childArtInertia) { - switch (mJointP.mActuatorType) + switch (mAspectProperties.mActuatorType) { case FORCE: case PASSIVE: @@ -1366,7 +1366,7 @@ void SingleDofJoint::addChildArtInertiaToKinematic( void SingleDofJoint::addChildArtInertiaImplicitTo( Eigen::Matrix6d& _parentArtInertia, const Eigen::Matrix6d& _childArtInertia) { - switch (mJointP.mActuatorType) + switch (mAspectProperties.mActuatorType) { case FORCE: case PASSIVE: @@ -1415,7 +1415,7 @@ void SingleDofJoint::addChildArtInertiaImplicitToKinematic( void SingleDofJoint::updateInvProjArtInertia( const Eigen::Matrix6d& _artInertia) { - switch (mJointP.mActuatorType) + switch (mAspectProperties.mActuatorType) { case FORCE: case PASSIVE: @@ -1460,7 +1460,7 @@ void SingleDofJoint::updateInvProjArtInertiaImplicit( const Eigen::Matrix6d& _artInertia, double _timeStep) { - switch (mJointP.mActuatorType) + switch (mAspectProperties.mActuatorType) { case FORCE: case PASSIVE: @@ -1513,7 +1513,7 @@ void SingleDofJoint::addChildBiasForceTo( const Eigen::Vector6d& _childBiasForce, const Eigen::Vector6d& _childPartialAcc) { - switch (mJointP.mActuatorType) + switch (mAspectProperties.mActuatorType) { case FORCE: case PASSIVE: @@ -1584,7 +1584,7 @@ void SingleDofJoint::addChildBiasImpulseTo( const Eigen::Matrix6d& _childArtInertia, const Eigen::Vector6d& _childBiasImpulse) { - switch (mJointP.mActuatorType) + switch (mAspectProperties.mActuatorType) { case FORCE: case PASSIVE: @@ -1643,7 +1643,7 @@ void SingleDofJoint::updateTotalForce(const Eigen::Vector6d& _bodyForce, { assert(_timeStep > 0.0); - switch (mJointP.mActuatorType) + switch (mAspectProperties.mActuatorType) { case FORCE: mForce = mCommand; @@ -1703,7 +1703,7 @@ void SingleDofJoint::updateTotalForceKinematic( //============================================================================== void SingleDofJoint::updateTotalImpulse(const Eigen::Vector6d& _bodyImpulse) { - switch (mJointP.mActuatorType) + switch (mAspectProperties.mActuatorType) { case FORCE: case PASSIVE: @@ -1745,7 +1745,7 @@ void SingleDofJoint::resetTotalImpulses() void SingleDofJoint::updateAcceleration(const Eigen::Matrix6d& _artInertia, const Eigen::Vector6d& _spatialAcc) { - switch (mJointP.mActuatorType) + switch (mAspectProperties.mActuatorType) { case FORCE: case PASSIVE: @@ -1789,7 +1789,7 @@ void SingleDofJoint::updateVelocityChange( const Eigen::Matrix6d& _artInertia, const Eigen::Vector6d& _velocityChange) { - switch (mJointP.mActuatorType) + switch (mAspectProperties.mActuatorType) { case FORCE: case PASSIVE: @@ -1864,7 +1864,7 @@ void SingleDofJoint::updateForceFD(const Eigen::Vector6d& _bodyForce, bool _withDampingForces, bool _withSpringForces) { - switch (mJointP.mActuatorType) + switch (mAspectProperties.mActuatorType) { case FORCE: case PASSIVE: @@ -1891,7 +1891,7 @@ void SingleDofJoint::updateImpulseID(const Eigen::Vector6d& _bodyImpulse) //============================================================================== void SingleDofJoint::updateImpulseFD(const Eigen::Vector6d& _bodyImpulse) { - switch (mJointP.mActuatorType) + switch (mAspectProperties.mActuatorType) { case FORCE: case PASSIVE: @@ -1911,7 +1911,7 @@ void SingleDofJoint::updateImpulseFD(const Eigen::Vector6d& _bodyImpulse) //============================================================================== void SingleDofJoint::updateConstrainedTerms(double _timeStep) { - switch (mJointP.mActuatorType) + switch (mAspectProperties.mActuatorType) { case FORCE: case PASSIVE: diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index 5e2bdbf8cef0d..0f4674d3704c9 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -190,7 +190,7 @@ Skeleton::ExtendedProperties::ExtendedProperties( const std::vector& parentNames, const AspectProperties& aspectProperties) : mBodyNodeProperties(bodyNodeProperties), - mJointProperties(jointProperties), + mAspectPropertiesroperties(jointProperties), mParentBodyNodeNames(parentNames), mAspectProperties(aspectProperties) { @@ -444,13 +444,13 @@ const std::string& Skeleton::addEntryToBodyNodeNameMgr(BodyNode* _newNode) const std::string& Skeleton::addEntryToJointNameMgr(Joint* _newJoint, bool _updateDofNames) { - _newJoint->mJointP.mName = + _newJoint->mAspectProperties.mName = mNameMgrForJoints.issueNewNameAndAdd(_newJoint->getName(), _newJoint); if(_updateDofNames) _newJoint->updateDegreeOfFreedomNames(); - return _newJoint->mJointP.mName; + return _newJoint->mAspectProperties.mName; } //============================================================================== diff --git a/dart/dynamics/Skeleton.h b/dart/dynamics/Skeleton.h index 4582d07d79270..eb050b38666c9 100644 --- a/dart/dynamics/Skeleton.h +++ b/dart/dynamics/Skeleton.h @@ -193,7 +193,7 @@ class Skeleton : BodyNodeExtendedProperties mBodyNodeProperties; /// Properties of all the Joints in this Skeleton - JointExtendedProperties mJointProperties; + JointExtendedProperties mAspectPropertiesroperties; /// A list of the name of the parent of each BodyNode in this Skeleton. This /// allows the layout of the Skeleton to be reconstructed. diff --git a/dart/dynamics/TranslationalJoint.cpp b/dart/dynamics/TranslationalJoint.cpp index 9a0af6300cd39..cc110867a3853 100644 --- a/dart/dynamics/TranslationalJoint.cpp +++ b/dart/dynamics/TranslationalJoint.cpp @@ -73,12 +73,13 @@ TranslationalJoint::Properties TranslationalJoint::getTranslationalJointProperti } //============================================================================== -TranslationalJoint::TranslationalJoint(const Properties& _properties) - : MultiDofJoint<3>(_properties) +TranslationalJoint::TranslationalJoint(const Properties& properties) + : MultiDofJoint<3>(properties) { - // Inherited Joint Properties must be set in the final joint class or else we - // get pure virtual function calls - MultiDofJoint<3>::setProperties(_properties); + // Inherited Aspects must be created in the final joint class in reverse order + // or else we get pure virtual function calls + createMultiDofJointAspect(properties); + createJointAspect(properties); } //============================================================================== @@ -110,19 +111,19 @@ bool TranslationalJoint::isCyclic(size_t /*_index*/) const void TranslationalJoint::updateDegreeOfFreedomNames() { if(!mDofs[0]->isNamePreserved()) - mDofs[0]->setName(mJointP.mName + "_x", false); + mDofs[0]->setName(mAspectProperties.mName + "_x", false); if(!mDofs[1]->isNamePreserved()) - mDofs[1]->setName(mJointP.mName + "_y", false); + mDofs[1]->setName(mAspectProperties.mName + "_y", false); if(!mDofs[2]->isNamePreserved()) - mDofs[2]->setName(mJointP.mName + "_z", false); + mDofs[2]->setName(mAspectProperties.mName + "_z", false); } //============================================================================== void TranslationalJoint::updateLocalTransform() const { - mT = mJointP.mT_ParentBodyToJoint + mT = mAspectProperties.mT_ParentBodyToJoint * Eigen::Translation3d(getPositionsStatic()) - * mJointP.mT_ChildBodyToJoint.inverse(); + * mAspectProperties.mT_ChildBodyToJoint.inverse(); // Verification assert(math::verifyTransform(mT)); @@ -133,7 +134,7 @@ void TranslationalJoint::updateLocalJacobian(bool _mandatory) const { if (_mandatory) { - mJacobian.bottomRows<3>() = mJointP.mT_ChildBodyToJoint.linear(); + mJacobian.bottomRows<3>() = mAspectProperties.mT_ChildBodyToJoint.linear(); // Verification assert(mJacobian.topRows<3>() == Eigen::Matrix3d::Zero()); diff --git a/dart/dynamics/TranslationalJoint.h b/dart/dynamics/TranslationalJoint.h index 7506f65774983..07efe0fc5c410 100644 --- a/dart/dynamics/TranslationalJoint.h +++ b/dart/dynamics/TranslationalJoint.h @@ -82,7 +82,7 @@ class TranslationalJoint : public MultiDofJoint<3> protected: /// Constructor called by Skeleton class - TranslationalJoint(const Properties& _properties); + TranslationalJoint(const Properties& properties); // Documentation inherited virtual Joint* clone() const override; diff --git a/dart/dynamics/UniversalJoint.cpp b/dart/dynamics/UniversalJoint.cpp index 396b1fd476208..345d02f923571 100644 --- a/dart/dynamics/UniversalJoint.cpp +++ b/dart/dynamics/UniversalJoint.cpp @@ -146,22 +146,22 @@ Eigen::Matrix UniversalJoint::getLocalJacobianStatic( { Eigen::Matrix J; J.col(0) = math::AdTAngular( - mJointP.mT_ChildBodyToJoint + mAspectProperties.mT_ChildBodyToJoint * math::expAngular(-getAxis2() * _positions[1]), getAxis1()); - J.col(1) = math::AdTAngular(mJointP.mT_ChildBodyToJoint, getAxis2()); + J.col(1) = math::AdTAngular(mAspectProperties.mT_ChildBodyToJoint, getAxis2()); assert(!math::isNan(J)); return J; } //============================================================================== -UniversalJoint::UniversalJoint(const Properties& _properties) - : detail::UniversalJointBase(_properties, common::NoArg) +UniversalJoint::UniversalJoint(const Properties& properties) + : detail::UniversalJointBase(properties, common::NoArg) { - createUniversalJointAspect(_properties); - - // Inherited Joint Properties must be set in the final joint class or else we - // get pure virtual function calls - MultiDofJoint<2>::setProperties(_properties); + // Inherited Aspects must be created in the final joint class in reverse order + // or else we get pure virtual function calls + createUniversalJointAspect(properties); + createMultiDofJointAspect(properties); + createJointAspect(properties); } //============================================================================== @@ -174,19 +174,19 @@ Joint* UniversalJoint::clone() const void UniversalJoint::updateDegreeOfFreedomNames() { if(!mDofs[0]->isNamePreserved()) - mDofs[0]->setName(mJointP.mName + "_1", false); + mDofs[0]->setName(mAspectProperties.mName + "_1", false); if(!mDofs[1]->isNamePreserved()) - mDofs[1]->setName(mJointP.mName + "_2", false); + mDofs[1]->setName(mAspectProperties.mName + "_2", false); } //============================================================================== void UniversalJoint::updateLocalTransform() const { const Eigen::Vector2d& positions = getPositionsStatic(); - mT = mJointP.mT_ParentBodyToJoint + mT = mAspectProperties.mT_ParentBodyToJoint * Eigen::AngleAxisd(positions[0], getAxis1()) * Eigen::AngleAxisd(positions[1], getAxis2()) - * mJointP.mT_ChildBodyToJoint.inverse(); + * mAspectProperties.mT_ChildBodyToJoint.inverse(); assert(math::verifyTransform(mT)); } @@ -206,7 +206,7 @@ void UniversalJoint::updateLocalJacobianTimeDeriv() const -getAxis2() * getPositionsStatic()[1]); Eigen::Vector6d tmpV2 - = math::AdTAngular(mJointP.mT_ChildBodyToJoint * tmpT, getAxis1()); + = math::AdTAngular(mAspectProperties.mT_ChildBodyToJoint * tmpT, getAxis1()); mJacobianDeriv.col(0) = -math::ad(tmpV1, tmpV2); diff --git a/dart/dynamics/UniversalJoint.h b/dart/dynamics/UniversalJoint.h index d6d13083fe747..14d35762d5adb 100644 --- a/dart/dynamics/UniversalJoint.h +++ b/dart/dynamics/UniversalJoint.h @@ -108,7 +108,7 @@ class UniversalJoint : public detail::UniversalJointBase protected: /// Constructor called by Skeleton class - UniversalJoint(const Properties& _properties); + UniversalJoint(const Properties& properties); // Documentation inherited virtual Joint* clone() const override; diff --git a/dart/dynamics/WeldJoint.cpp b/dart/dynamics/WeldJoint.cpp index a6ef7fec0f7eb..be3b93aa235c0 100644 --- a/dart/dynamics/WeldJoint.cpp +++ b/dart/dynamics/WeldJoint.cpp @@ -87,7 +87,7 @@ void WeldJoint::setTransformFromParentBodyNode(const Eigen::Isometry3d& _T) { Joint::setTransformFromParentBodyNode(_T); - mT = mJointP.mT_ParentBodyToJoint * mJointP.mT_ChildBodyToJoint.inverse(); + mT = mAspectProperties.mT_ParentBodyToJoint * mAspectProperties.mT_ChildBodyToJoint.inverse(); } //============================================================================== @@ -95,16 +95,15 @@ void WeldJoint::setTransformFromChildBodyNode(const Eigen::Isometry3d& _T) { Joint::setTransformFromChildBodyNode(_T); - mT = mJointP.mT_ParentBodyToJoint * mJointP.mT_ChildBodyToJoint.inverse(); + mT = mAspectProperties.mT_ParentBodyToJoint * mAspectProperties.mT_ChildBodyToJoint.inverse(); } //============================================================================== -WeldJoint::WeldJoint(const Properties& _properties) - : ZeroDofJoint(_properties) +WeldJoint::WeldJoint(const Properties& properties) { - // Inherited Joint Properties must be set in the final joint class or else we + // Inherited Aspects must be created in the final joint class or else we // get pure virtual function calls - ZeroDofJoint::setProperties(_properties); + createJointAspect(properties); } //============================================================================== diff --git a/dart/dynamics/WeldJoint.h b/dart/dynamics/WeldJoint.h index 6de5219490837..746cc698c8440 100644 --- a/dart/dynamics/WeldJoint.h +++ b/dart/dynamics/WeldJoint.h @@ -85,7 +85,7 @@ class WeldJoint : public ZeroDofJoint protected: /// Constructor called by Skeleton class - WeldJoint(const Properties& _properties); + WeldJoint(const Properties& properties); // Documentation inherited virtual Joint* clone() const override; diff --git a/dart/dynamics/ZeroDofJoint.cpp b/dart/dynamics/ZeroDofJoint.cpp index 2fe667fc2685c..7beadc31046ca 100644 --- a/dart/dynamics/ZeroDofJoint.cpp +++ b/dart/dynamics/ZeroDofJoint.cpp @@ -567,10 +567,9 @@ double ZeroDofJoint::getPotentialEnergy() const } //============================================================================== -ZeroDofJoint::ZeroDofJoint(const Properties& _properties) - : Joint(_properties) +ZeroDofJoint::ZeroDofJoint() { - // Do nothing + // Do nothing. The Joint Aspect must be created by the most derived class. } //============================================================================== diff --git a/dart/dynamics/ZeroDofJoint.h b/dart/dynamics/ZeroDofJoint.h index f9bd0d5ca5393..a587ad5eea316 100644 --- a/dart/dynamics/ZeroDofJoint.h +++ b/dart/dynamics/ZeroDofJoint.h @@ -353,7 +353,7 @@ class ZeroDofJoint : public Joint protected: /// Constructor called by inheriting classes - ZeroDofJoint(const Properties& _properties); + ZeroDofJoint(); // Documentation inherited void registerDofs() override; diff --git a/dart/dynamics/detail/Joint.h b/dart/dynamics/detail/Joint.h new file mode 100644 index 0000000000000..5ac56c2be5669 --- /dev/null +++ b/dart/dynamics/detail/Joint.h @@ -0,0 +1,144 @@ +/* + * 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_JOINT_H_ +#define DART_DYNAMICS_DETAIL_JOINT_H_ + +#include +#include + +namespace dart { +namespace dynamics { +namespace detail { + +/// Actuator type +/// +/// The command is taken by setCommand() or setCommands(), and the meaning of +/// command is different depending on the actuator type. The default actuator +/// type is FORCE. (TODO: FreeJoint should be PASSIVE?) +/// +/// FORCE/PASSIVE/SERVO joints are dynamic joints while +/// ACCELERATION/VELOCITY/LOCKED joints are kinematic joints. +/// +/// Note the presence of joint damping force and joint spring force for all +/// the actuator types if the coefficients are non-zero. The default +/// coefficients are zero. +/// +/// \sa setActuatorType(), getActuatorType(), +/// setSpringStiffness(), setDampingCoefficient(), +enum ActuatorType +{ + /// Command input is joint force, and the output is joint acceleration. + /// + /// If the command is zero, then it's identical to passive joint. The valid + /// joint constraints are position limit, velocity limit, and Coulomb + /// friction, and the invalid joint constraint is force limit. + FORCE, + + /// Passive joint doesn't take any command input, and the output is joint + /// acceleration. + /// + /// The valid joint constraints are position limit, velocity limit, and + /// Coulomb friction, and the invalid joint constraint is force limit. + PASSIVE, + + /// Command input is desired velocity, and the output is joint acceleration. + /// + /// The constraint solver will try to track the desired velocity within the + /// joint force limit. All the joint constarints are valid. + SERVO, + + /// Command input is joint acceleration, and the output is joint force. + /// + /// The joint acceleration is always satisfied but it doesn't take the joint + /// force limit into account. All the joint constraints are invalid. + ACCELERATION, + + /// Command input is joint velocity, and the output is joint force. + /// + /// The joint velocity is always satisfied but it doesn't take the joint + /// force limit into account. If you want to consider the joint force limit, + /// should use SERVO instead. All the joint constraints are invalid. + VELOCITY, + + /// Locked joint always set the velocity and acceleration to zero so that + /// the joint dosen't move at all (locked), and the output is joint force. + /// force. + /// + /// All the joint constraints are invalid. + LOCKED +}; + +const ActuatorType DefaultActuatorType = FORCE; + +struct JointProperties +{ + /// Joint name + std::string mName; + + /// Transformation from parent BodyNode to this Joint + Eigen::Isometry3d mT_ParentBodyToJoint; + + /// Transformation from child BodyNode to this Joint + Eigen::Isometry3d mT_ChildBodyToJoint; + + /// True if the joint limits should be enforced in dynamic simulation + bool mIsPositionLimited; + + /// Actuator type + ActuatorType mActuatorType; + + /// Constructor + JointProperties(const std::string& _name = "Joint", + const Eigen::Isometry3d& _T_ParentBodyToJoint = + Eigen::Isometry3d::Identity(), + const Eigen::Isometry3d& _T_ChildBodyToJoint = + Eigen::Isometry3d::Identity(), + bool _isPositionLimited = false, + ActuatorType _actuatorType = DefaultActuatorType); + + virtual ~JointProperties() = default; + +public: + // To get byte-aligned Eigen vectors + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // namespace detail +} // namespace dynamics +} // namespace dart + +#endif // DART_DYNAMICS_DETAIL_JOINT_H_ diff --git a/dart/dynamics/detail/MultiDofJoint.h b/dart/dynamics/detail/MultiDofJoint.h index 66d7d920c7056..dc8099c2d23eb 100644 --- a/dart/dynamics/detail/MultiDofJoint.h +++ b/dart/dynamics/detail/MultiDofJoint.h @@ -39,22 +39,23 @@ #include "dart/dynamics/MultiDofJoint.h" -#define MULTIDOFJOINT_REPORT_DIM_MISMATCH( func, arg ) \ - dterr << "[MultiDofJoint::" #func "] Mismatch beteween size of " \ - << #arg " [" << arg .size() << "] and the number of " \ - << "DOFs [" << getNumDofs() << "] for Joint named [" \ - << getName() << "].\n"; \ +#define MULTIDOFJOINT_REPORT_DIM_MISMATCH( func, arg )\ + dterr << "[MultiDofJoint::" #func "] Mismatch beteween size of "\ + << #arg " [" << arg .size() << "] and the number of "\ + << "DOFs [" << this->getNumDofs() << "] for Joint named ["\ + << this->getName() << "].\n";\ assert(false); -#define MULTIDOFJOINT_REPORT_OUT_OF_RANGE( func, index ) \ - dterr << "[MultiDofJoint::" << #func << "] The index [" << index \ - << "] is out of range for Joint named [" << getName() \ - << "] which has " << getNumDofs() << " DOFs.\n"; \ +#define MULTIDOFJOINT_REPORT_OUT_OF_RANGE( func, index )\ + dterr << "[MultiDofJoint::" << #func << "] The index [" << index\ + << "] is out of range for Joint named [" << this->getName()\ + << "] which has " << this->getNumDofs() << " DOFs.\n";\ assert(false); -#define MULTIDOFJOINT_REPORT_UNSUPPORTED_ACTUATOR( func ) \ - dterr << "[MultiDofJoint::" # func "] Unsupported actuator type (" \ - << mJointP.mActuatorType << ") for Joint [" << getName() << "].\n"; \ +#define MULTIDOFJOINT_REPORT_UNSUPPORTED_ACTUATOR( func )\ + dterr << "[MultiDofJoint::" # func "] Unsupported actuator type ("\ + << Joint::mAspectProperties.mActuatorType << ") for Joint ["\ + << this->getName() << "].\n";\ assert(false); namespace dart { @@ -116,7 +117,7 @@ typename MultiDofJoint::Properties MultiDofJoint::getMultiDofJointProperties() const { return MultiDofJoint::Properties( - mJointP, getMultiDofJointAspect()->getProperties()); + Joint::mAspectProperties, getMultiDofJointAspect()->getProperties()); } //============================================================================== @@ -182,7 +183,7 @@ const std::string& MultiDofJoint::setDofName(size_t _index, { dterr << "[MultiDofJoint::setDofName] Attempting to set the name of DOF " << "index " << _index << ", which is out of bounds for the Joint [" - << getName() << "]. We will set the name of DOF index 0 instead.\n"; + << this->getName() << "]. We will set the name of DOF index 0 instead.\n"; assert(false); _index = 0; } @@ -192,8 +193,8 @@ const std::string& MultiDofJoint::setDofName(size_t _index, if(_name == dofName) return dofName; - const SkeletonPtr& skel = mChildBodyNode? - mChildBodyNode->getSkeleton() : nullptr; + const SkeletonPtr& skel = this->mChildBodyNode? + this->mChildBodyNode->getSkeleton() : nullptr; if(skel) { dofName = @@ -238,8 +239,8 @@ const std::string& MultiDofJoint::getDofName(size_t _index) const if(DOF <= _index) { dterr << "[MultiDofJoint::getDofName] Requested name of DOF index [" - << _index << "] in Joint [" << getName() << "], but that is out of " - << "bounds (max " << DOF-1 << "). Returning name of DOF 0.\n"; + << _index << "] in Joint [" << this->getName() << "], but that is " + << "out of bounds (max " << DOF-1 << "). Returning name of DOF 0.\n"; assert(false); return getMultiDofJointAspect()->getDofName(0); } @@ -282,53 +283,53 @@ size_t MultiDofJoint::getIndexInTree(size_t _index) const //============================================================================== template -void MultiDofJoint::setCommand(size_t _index, double _command) +void MultiDofJoint::setCommand(size_t _index, double command) { if (_index >= getNumDofs()) { MULTIDOFJOINT_REPORT_OUT_OF_RANGE(setCommand, _index); } - switch (mJointP.mActuatorType) + switch (Joint::mAspectProperties.mActuatorType) { - case FORCE: - mCommands[_index] = math::clip(_command, + case Joint::FORCE: + mCommands[_index] = math::clip(command, getMultiDofJointAspect()->getForceLowerLimit(_index), getMultiDofJointAspect()->getForceUpperLimit(_index)); break; - case PASSIVE: - if(0.0 != _command) + case Joint::PASSIVE: + if(0.0 != command) { dtwarn << "[MultiDofJoint::setCommand] Attempting to set a non-zero (" - << _command << ") command for a PASSIVE joint [" << getName() - << "].\n"; + << command << ") command for a PASSIVE joint [" + << this->getName() << "].\n"; } - mCommands[_index] = _command; + mCommands[_index] = command; break; - case SERVO: - mCommands[_index] = math::clip(_command, + case Joint::SERVO: + mCommands[_index] = math::clip(command, getMultiDofJointAspect()->getVelocityLowerLimit(_index), getMultiDofJointAspect()->getVelocityUpperLimit(_index)); break; - case ACCELERATION: - mCommands[_index] = math::clip(_command, + case Joint::ACCELERATION: + mCommands[_index] = math::clip(command, getMultiDofJointAspect()->getAccelerationLowerLimit(_index), getMultiDofJointAspect()->getAccelerationUpperLimit(_index)); break; - case VELOCITY: - mCommands[_index] = math::clip(_command, + case Joint::VELOCITY: + mCommands[_index] = math::clip(command, getMultiDofJointAspect()->getVelocityLowerLimit(_index), getMultiDofJointAspect()->getVelocityUpperLimit(_index)); // TODO: This possibly makes the acceleration to exceed the limits. break; - case LOCKED: - if(0.0 != _command) + case Joint::LOCKED: + if(0.0 != command) { dtwarn << "[MultiDofJoint::setCommand] Attempting to set a non-zero (" - << _command << ") command for a LOCKED joint [" << getName() + << command << ") command for a LOCKED joint [" << this->getName() << "].\n"; } - mCommands[_index] = _command; + mCommands[_index] = command; break; default: assert(false); @@ -359,44 +360,44 @@ void MultiDofJoint::setCommands(const Eigen::VectorXd& _commands) return; } - switch (mJointP.mActuatorType) + switch (Joint::mAspectProperties.mActuatorType) { - case FORCE: + case Joint::FORCE: mCommands = math::clip(_commands, getMultiDofJointAspect()->getForceLowerLimits(), getMultiDofJointAspect()->getForceUpperLimits()); break; - case PASSIVE: + case Joint::PASSIVE: if(Vector::Zero() != _commands) { dtwarn << "[MultiDofJoint::setCommands] Attempting to set a non-zero (" << _commands.transpose() << ") command for a PASSIVE joint [" - << getName() << "].\n"; + << this->getName() << "].\n"; } mCommands = _commands; break; - case SERVO: + case Joint::SERVO: mCommands = math::clip(_commands, getMultiDofJointAspect()->getVelocityLowerLimits(), getMultiDofJointAspect()->getVelocityUpperLimits()); break; - case ACCELERATION: + case Joint::ACCELERATION: mCommands = math::clip(_commands, getMultiDofJointAspect()->getAccelerationLowerLimits(), getMultiDofJointAspect()->getAccelerationUpperLimits()); break; - case VELOCITY: + case Joint::VELOCITY: mCommands = math::clip(_commands, getMultiDofJointAspect()->getVelocityLowerLimits(), getMultiDofJointAspect()->getVelocityUpperLimits()); // TODO: This possibly makes the acceleration to exceed the limits. break; - case LOCKED: + case Joint::LOCKED: if(Vector::Zero() != _commands) { dtwarn << "[MultiDofJoint::setCommands] Attempting to set a non-zero (" << _commands.transpose() << ") command for a LOCKED joint [" - << getName() << "].\n"; + << this->getName() << "].\n"; } mCommands = _commands; break; @@ -435,7 +436,7 @@ void MultiDofJoint::setPosition(size_t _index, double _position) // Note: It would not make much sense to use setPositionsStatic() here mPositions[_index] = _position; - notifyPositionUpdate(); + this->notifyPositionUpdate(); } //============================================================================== @@ -618,11 +619,11 @@ void MultiDofJoint::setVelocity(size_t _index, double _velocity) // Note: It would not make much sense to use setVelocitiesStatic() here mVelocities[_index] = _velocity; - notifyVelocityUpdate(); + this->notifyVelocityUpdate(); #if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) - if (mJointP.mActuatorType == VELOCITY) - mCommands[_index] = getVelocitiesStatic()[_index]; + if (Joint::mAspectProperties.mActuatorType == Joint::VELOCITY) + mCommands[_index] = this->getVelocitiesStatic()[_index]; // TODO: Remove at DART 5.1. #endif } @@ -653,8 +654,8 @@ void MultiDofJoint::setVelocities(const Eigen::VectorXd& _velocities) setVelocitiesStatic(_velocities); #if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) - if (mJointP.mActuatorType == VELOCITY) - mCommands = getVelocitiesStatic(); + if (Joint::mAspectProperties.mActuatorType == Joint::VELOCITY) + mCommands = this->getVelocitiesStatic(); // TODO: Remove at DART 5.1. #endif } @@ -799,11 +800,11 @@ void MultiDofJoint::setAcceleration(size_t _index, double _acceleration) // Note: It would not make much sense to use setAccelerationsStatic() here mAccelerations[_index] = _acceleration; - notifyAccelerationUpdate(); + this->notifyAccelerationUpdate(); #if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) - if (mJointP.mActuatorType == ACCELERATION) - mCommands[_index] = getAccelerationsStatic()[_index]; + if (Joint::mAspectProperties.mActuatorType == Joint::ACCELERATION) + mCommands[_index] = this->getAccelerationsStatic()[_index]; // TODO: Remove at DART 5.1. #endif } @@ -834,8 +835,8 @@ void MultiDofJoint::setAccelerations(const Eigen::VectorXd& _accelerations) setAccelerationsStatic(_accelerations); #if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) - if (mJointP.mActuatorType == ACCELERATION) - mCommands = getAccelerationsStatic(); + if (Joint::mAspectProperties.mActuatorType == Joint::ACCELERATION) + mCommands = this->getAccelerationsStatic(); // TODO: Remove at DART 5.1. #endif } @@ -916,7 +917,7 @@ void MultiDofJoint::setPositionsStatic(const Vector& _positions) return; mPositions = _positions; - notifyPositionUpdate(); + this->notifyPositionUpdate(); } //============================================================================== @@ -935,7 +936,7 @@ void MultiDofJoint::setVelocitiesStatic(const Vector& _velocities) return; mVelocities = _velocities; - notifyVelocityUpdate(); + this->notifyVelocityUpdate(); } //============================================================================== @@ -954,7 +955,7 @@ void MultiDofJoint::setAccelerationsStatic(const Vector& _accels) return; mAccelerations = _accels; - notifyAccelerationUpdate(); + this->notifyAccelerationUpdate(); } //============================================================================== @@ -978,7 +979,7 @@ void MultiDofJoint::setForce(size_t _index, double _force) mForces[_index] = _force; #if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) - if (mJointP.mActuatorType == FORCE) + if (Joint::mAspectProperties.mActuatorType == Joint::FORCE) mCommands[_index] = mForces[_index]; // TODO: Remove at DART 5.1. #endif @@ -1010,7 +1011,7 @@ void MultiDofJoint::setForces(const Eigen::VectorXd& _forces) mForces = _forces; #if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) - if (mJointP.mActuatorType == FORCE) + if (Joint::mAspectProperties.mActuatorType == Joint::FORCE) mCommands = mForces; // TODO: Remove at DART 5.1. #endif @@ -1030,7 +1031,7 @@ void MultiDofJoint::resetForces() mForces.setZero(); #if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) - if (mJointP.mActuatorType == FORCE) + if (Joint::mAspectProperties.mActuatorType == Joint::FORCE) mCommands = mForces; // TODO: Remove at DART 5.1. #endif @@ -1179,7 +1180,7 @@ Eigen::VectorXd MultiDofJoint::getPositionDifferences( { dterr << "[MultiDofJoint::getPositionsDifference] q1's size [" << _q1.size() << "] or q2's size [" << _q2.size() << "] must both equal the dof [" - << getNumDofs() << "] for Joint [" << getName() << "].\n"; + << this->getNumDofs() << "] for Joint [" << this->getName() << "].\n"; assert(false); return Eigen::VectorXd::Zero(getNumDofs()); } @@ -1240,7 +1241,7 @@ void MultiDofJoint::setRestPosition(size_t _index, double _q0) << "], is out of the limit range [" << getMultiDofJointAspect()->getPositionLowerLimit(_index) << ", " << getMultiDofJointAspect()->getPositionUpperLimit(_index) - << "] for index [" << _index << "] of Joint [" << getName() + << "] for index [" << _index << "] of Joint [" << this->getName() << "].\n"; return; } @@ -1335,18 +1336,18 @@ double MultiDofJoint::getPotentialEnergy() const template Eigen::Vector6d MultiDofJoint::getBodyConstraintWrench() const { - assert(mChildBodyNode); - return mChildBodyNode->getBodyForce() - getLocalJacobianStatic() * mForces; + assert(this->mChildBodyNode); + return this->mChildBodyNode->getBodyForce() + - this->getLocalJacobianStatic() * mForces; } //============================================================================== template -MultiDofJoint::MultiDofJoint(const Properties& _properties) - : Joint(_properties), - mCommands(Vector::Zero()), - mPositions(_properties.mInitialPositions), +MultiDofJoint::MultiDofJoint(const Properties& properties) + : mCommands(Vector::Zero()), + mPositions(properties.mInitialPositions), mPositionDeriv(Vector::Zero()), - mVelocities(_properties.mInitialVelocities), + mVelocities(properties.mInitialVelocities), mVelocitiesDeriv(Vector::Zero()), mAccelerations(Vector::Zero()), mAccelerationsDeriv(Vector::Zero()), @@ -1362,17 +1363,17 @@ MultiDofJoint::MultiDofJoint(const Properties& _properties) mTotalForce(Vector::Zero()), mTotalImpulse(Vector::Zero()) { - createMultiDofJointAspect(_properties); - for (size_t i = 0; i < DOF; ++i) - mDofs[i] = createDofPointer(i); + mDofs[i] = this->createDofPointer(i); + + // Joint and MultiDofJoint Aspects must be created by the most derived class. } //============================================================================== template void MultiDofJoint::registerDofs() { - const SkeletonPtr& skel = mChildBodyNode->getSkeleton(); + const SkeletonPtr& skel = this->mChildBodyNode->getSkeleton(); for (size_t i = 0; i < DOF; ++i) { getMultiDofJointAspect()->mProperties.mDofNames[i] = @@ -1392,10 +1393,10 @@ template const Eigen::Matrix& MultiDofJoint::getLocalJacobianStatic() const { - if(mIsLocalJacobianDirty) + if(this->mIsLocalJacobianDirty) { - updateLocalJacobian(false); - mIsLocalJacobianDirty = false; + this->updateLocalJacobian(false); + this->mIsLocalJacobianDirty = false; } return mJacobian; } @@ -1412,10 +1413,10 @@ math::Jacobian MultiDofJoint::getLocalJacobian( template const math::Jacobian MultiDofJoint::getLocalJacobianTimeDeriv() const { - if(mIsLocalJacobianTimeDerivDirty) + if(this->mIsLocalJacobianTimeDerivDirty) { - updateLocalJacobianTimeDeriv(); - mIsLocalJacobianTimeDerivDirty = false; + this->updateLocalJacobianTimeDeriv(); + this->mIsLocalJacobianTimeDerivDirty = false; } return mJacobianDeriv; } @@ -1425,10 +1426,10 @@ template const Eigen::Matrix& MultiDofJoint::getLocalJacobianTimeDerivStatic() const { - if(mIsLocalJacobianTimeDerivDirty) + if(this->mIsLocalJacobianTimeDerivDirty) { - updateLocalJacobianTimeDeriv(); - mIsLocalJacobianTimeDerivDirty = false; + this->updateLocalJacobianTimeDeriv(); + this->mIsLocalJacobianTimeDerivDirty = false; } return mJacobianDeriv; } @@ -1455,22 +1456,25 @@ MultiDofJoint::getInvProjArtInertiaImplicit() const template void MultiDofJoint::updateLocalSpatialVelocity() const { - mSpatialVelocity = getLocalJacobianStatic() * getVelocitiesStatic(); + this->mSpatialVelocity = + this->getLocalJacobianStatic() * this->getVelocitiesStatic(); } //============================================================================== template void MultiDofJoint::updateLocalSpatialAcceleration() const { - mSpatialAcceleration = getLocalPrimaryAcceleration() - + getLocalJacobianTimeDerivStatic() * getVelocitiesStatic(); + this->mSpatialAcceleration = + this->getLocalPrimaryAcceleration() + + this->getLocalJacobianTimeDerivStatic() * this->getVelocitiesStatic(); } //============================================================================== template void MultiDofJoint::updateLocalPrimaryAcceleration() const { - mPrimaryAcceleration = getLocalJacobianStatic() * getAccelerationsStatic(); + this->mPrimaryAcceleration = + this->getLocalJacobianStatic() * this->getAccelerationsStatic(); } //============================================================================== @@ -1526,19 +1530,19 @@ void MultiDofJoint::addChildArtInertiaTo( Eigen::Matrix6d& _parentArtInertia, const Eigen::Matrix6d& _childArtInertia) { - switch (mJointP.mActuatorType) + switch (Joint::mAspectProperties.mActuatorType) { - case FORCE: - case PASSIVE: - case SERVO: + case Joint::FORCE: + case Joint::PASSIVE: + case Joint::SERVO: addChildArtInertiaToDynamic(_parentArtInertia, _childArtInertia); break; - case ACCELERATION: - case VELOCITY: - case LOCKED: + case Joint::ACCELERATION: + case Joint::VELOCITY: + case Joint::LOCKED: addChildArtInertiaToKinematic(_parentArtInertia, - _childArtInertia); + _childArtInertia); break; default: MULTIDOFJOINT_REPORT_UNSUPPORTED_ACTUATOR(addChildArtInertiaTo); @@ -1560,7 +1564,8 @@ void MultiDofJoint::addChildArtInertiaToDynamic( // Add child body's articulated inertia to parent body's articulated inertia. // Note that mT should be updated. - _parentArtInertia += math::transformInertia(getLocalTransform().inverse(), PI); + _parentArtInertia += math::transformInertia( + this->getLocalTransform().inverse(), PI); } //============================================================================== @@ -1571,8 +1576,8 @@ void MultiDofJoint::addChildArtInertiaToKinematic( { // Add child body's articulated inertia to parent body's articulated inertia. // Note that mT should be updated. - _parentArtInertia += math::transformInertia(getLocalTransform().inverse(), - _childArtInertia); + _parentArtInertia += math::transformInertia( + this->getLocalTransform().inverse(), _childArtInertia); } //============================================================================== @@ -1581,19 +1586,19 @@ void MultiDofJoint::addChildArtInertiaImplicitTo( Eigen::Matrix6d& _parentArtInertia, const Eigen::Matrix6d& _childArtInertia) { - switch (mJointP.mActuatorType) + switch (Joint::mAspectProperties.mActuatorType) { - case FORCE: - case PASSIVE: - case SERVO: + case Joint::FORCE: + case Joint::PASSIVE: + case Joint::SERVO: addChildArtInertiaImplicitToDynamic(_parentArtInertia, - _childArtInertia); + _childArtInertia); break; - case ACCELERATION: - case VELOCITY: - case LOCKED: + case Joint::ACCELERATION: + case Joint::VELOCITY: + case Joint::LOCKED: addChildArtInertiaImplicitToKinematic(_parentArtInertia, - _childArtInertia); + _childArtInertia); break; default: MULTIDOFJOINT_REPORT_UNSUPPORTED_ACTUATOR(addChildArtInertiaImplicitTo); @@ -1615,7 +1620,8 @@ void MultiDofJoint::addChildArtInertiaImplicitToDynamic( // Add child body's articulated inertia to parent body's articulated inertia. // Note that mT should be updated. - _parentArtInertia += math::transformInertia(getLocalTransform().inverse(), PI); + _parentArtInertia += math::transformInertia( + this->getLocalTransform().inverse(), PI); } //============================================================================== @@ -1626,8 +1632,8 @@ void MultiDofJoint::addChildArtInertiaImplicitToKinematic( { // Add child body's articulated inertia to parent body's articulated inertia. // Note that mT should be updated. - _parentArtInertia += math::transformInertia(getLocalTransform().inverse(), - _childArtInertia); + _parentArtInertia += math::transformInertia( + this->getLocalTransform().inverse(), _childArtInertia); } //============================================================================== @@ -1635,16 +1641,16 @@ template void MultiDofJoint::updateInvProjArtInertia( const Eigen::Matrix6d& _artInertia) { - switch (mJointP.mActuatorType) + switch (Joint::mAspectProperties.mActuatorType) { - case FORCE: - case PASSIVE: - case SERVO: + case Joint::FORCE: + case Joint::PASSIVE: + case Joint::SERVO: updateInvProjArtInertiaDynamic(_artInertia); break; - case ACCELERATION: - case VELOCITY: - case LOCKED: + case Joint::ACCELERATION: + case Joint::VELOCITY: + case Joint::LOCKED: updateInvProjArtInertiaKinematic(_artInertia); break; default: @@ -1686,16 +1692,16 @@ void MultiDofJoint::updateInvProjArtInertiaImplicit( const Eigen::Matrix6d& _artInertia, double _timeStep) { - switch (mJointP.mActuatorType) + switch (Joint::mAspectProperties.mActuatorType) { - case FORCE: - case PASSIVE: - case SERVO: + case Joint::FORCE: + case Joint::PASSIVE: + case Joint::SERVO: updateInvProjArtInertiaImplicitDynamic(_artInertia, _timeStep); break; - case ACCELERATION: - case VELOCITY: - case LOCKED: + case Joint::ACCELERATION: + case Joint::VELOCITY: + case Joint::LOCKED: updateInvProjArtInertiaImplicitKinematic(_artInertia, _timeStep); break; default: @@ -1749,19 +1755,19 @@ void MultiDofJoint::addChildBiasForceTo( const Eigen::Vector6d& _childBiasForce, const Eigen::Vector6d& _childPartialAcc) { - switch (mJointP.mActuatorType) + switch (Joint::mAspectProperties.mActuatorType) { - case FORCE: - case PASSIVE: - case SERVO: + case Joint::FORCE: + case Joint::PASSIVE: + case Joint::SERVO: addChildBiasForceToDynamic(_parentBiasForce, _childArtInertia, _childBiasForce, _childPartialAcc); break; - case ACCELERATION: - case VELOCITY: - case LOCKED: + case Joint::ACCELERATION: + case Joint::VELOCITY: + case Joint::LOCKED: addChildBiasForceToKinematic(_parentBiasForce, _childArtInertia, _childBiasForce, @@ -1799,7 +1805,7 @@ void MultiDofJoint::addChildBiasForceToDynamic( // Add child body's bias force to parent body's bias force. Note that mT // should be updated. - _parentBiasForce += math::dAdInvT(getLocalTransform(), beta); + _parentBiasForce += math::dAdInvT(this->getLocalTransform(), beta); } //============================================================================== @@ -1826,7 +1832,7 @@ void MultiDofJoint::addChildBiasForceToKinematic( // Add child body's bias force to parent body's bias force. Note that mT // should be updated. - _parentBiasForce += math::dAdInvT(getLocalTransform(), beta); + _parentBiasForce += math::dAdInvT(this->getLocalTransform(), beta); } //============================================================================== @@ -1836,18 +1842,18 @@ void MultiDofJoint::addChildBiasImpulseTo( const Eigen::Matrix6d& _childArtInertia, const Eigen::Vector6d& _childBiasImpulse) { - switch (mJointP.mActuatorType) + switch (Joint::mAspectProperties.mActuatorType) { - case FORCE: - case PASSIVE: - case SERVO: + case Joint::FORCE: + case Joint::PASSIVE: + case Joint::SERVO: addChildBiasImpulseToDynamic(_parentBiasImpulse, _childArtInertia, _childBiasImpulse); break; - case ACCELERATION: - case VELOCITY: - case LOCKED: + case Joint::ACCELERATION: + case Joint::VELOCITY: + case Joint::LOCKED: addChildBiasImpulseToKinematic(_parentBiasImpulse, _childArtInertia, _childBiasImpulse); @@ -1876,7 +1882,7 @@ void MultiDofJoint::addChildBiasImpulseToDynamic( // Add child body's bias force to parent body's bias force. Note that mT // should be updated. - _parentBiasImpulse += math::dAdInvT(getLocalTransform(), beta); + _parentBiasImpulse += math::dAdInvT(this->getLocalTransform(), beta); } //============================================================================== @@ -1888,7 +1894,8 @@ void MultiDofJoint::addChildBiasImpulseToKinematic( { // Add child body's bias force to parent body's bias force. Note that mT // should be updated. - _parentBiasImpulse += math::dAdInvT(getLocalTransform(), _childBiasImpulse); + _parentBiasImpulse += math::dAdInvT( + this->getLocalTransform(), _childBiasImpulse); } //============================================================================== @@ -1899,26 +1906,26 @@ void MultiDofJoint::updateTotalForce( { assert(_timeStep > 0.0); - switch (mJointP.mActuatorType) + switch (Joint::mAspectProperties.mActuatorType) { - case FORCE: + case Joint::FORCE: mForces = mCommands; updateTotalForceDynamic(_bodyForce, _timeStep); break; - case PASSIVE: - case SERVO: + case Joint::PASSIVE: + case Joint::SERVO: mForces.setZero(); updateTotalForceDynamic(_bodyForce, _timeStep); break; - case ACCELERATION: + case Joint::ACCELERATION: setAccelerationsStatic(mCommands); updateTotalForceKinematic(_bodyForce, _timeStep); break; - case VELOCITY: + case Joint::VELOCITY: setAccelerationsStatic( (mCommands - getVelocitiesStatic()) / _timeStep ); updateTotalForceKinematic(_bodyForce, _timeStep); break; - case LOCKED: + case Joint::LOCKED: setVelocitiesStatic(Eigen::Matrix::Zero()); setAccelerationsStatic(Eigen::Matrix::Zero()); updateTotalForceKinematic(_bodyForce, _timeStep); @@ -1965,16 +1972,16 @@ template void MultiDofJoint::updateTotalImpulse( const Eigen::Vector6d& _bodyImpulse) { - switch (mJointP.mActuatorType) + switch (Joint::mAspectProperties.mActuatorType) { - case FORCE: - case PASSIVE: - case SERVO: + case Joint::FORCE: + case Joint::PASSIVE: + case Joint::SERVO: updateTotalImpulseDynamic(_bodyImpulse); break; - case ACCELERATION: - case VELOCITY: - case LOCKED: + case Joint::ACCELERATION: + case Joint::VELOCITY: + case Joint::LOCKED: updateTotalImpulseKinematic(_bodyImpulse); break; default: @@ -2014,16 +2021,16 @@ void MultiDofJoint::updateAcceleration( const Eigen::Matrix6d& _artInertia, const Eigen::Vector6d& _spatialAcc) { - switch (mJointP.mActuatorType) + switch (Joint::mAspectProperties.mActuatorType) { - case FORCE: - case PASSIVE: - case SERVO: + case Joint::FORCE: + case Joint::PASSIVE: + case Joint::SERVO: updateAccelerationDynamic(_artInertia, _spatialAcc); break; - case ACCELERATION: - case VELOCITY: - case LOCKED: + case Joint::ACCELERATION: + case Joint::VELOCITY: + case Joint::LOCKED: updateAccelerationKinematic(_artInertia, _spatialAcc); break; default: @@ -2041,7 +2048,7 @@ void MultiDofJoint::updateAccelerationDynamic( // setAccelerationsStatic( getInvProjArtInertiaImplicit() * (mTotalForce - getLocalJacobianStatic().transpose() - *_artInertia*math::AdInvT(getLocalTransform(), _spatialAcc)) ); + *_artInertia*math::AdInvT(this->getLocalTransform(), _spatialAcc)) ); // Verification assert(!math::isNan(getAccelerationsStatic())); @@ -2062,16 +2069,16 @@ void MultiDofJoint::updateVelocityChange( const Eigen::Matrix6d& _artInertia, const Eigen::Vector6d& _velocityChange) { - switch (mJointP.mActuatorType) + switch (Joint::mAspectProperties.mActuatorType) { - case FORCE: - case PASSIVE: - case SERVO: + case Joint::FORCE: + case Joint::PASSIVE: + case Joint::SERVO: updateVelocityChangeDynamic(_artInertia, _velocityChange); break; - case ACCELERATION: - case VELOCITY: - case LOCKED: + case Joint::ACCELERATION: + case Joint::VELOCITY: + case Joint::LOCKED: updateVelocityChangeKinematic(_artInertia, _velocityChange); break; default: @@ -2090,7 +2097,7 @@ void MultiDofJoint::updateVelocityChangeDynamic( mVelocityChanges = getInvProjArtInertia() * (mTotalImpulse - getLocalJacobianStatic().transpose() - *_artInertia*math::AdInvT(getLocalTransform(), _velocityChange)); + *_artInertia*math::AdInvT(this->getLocalTransform(), _velocityChange)); // Verification assert(!math::isNan(mVelocityChanges)); @@ -2141,15 +2148,15 @@ void MultiDofJoint::updateForceFD(const Eigen::Vector6d& _bodyForce, bool _withDampingForces, bool _withSpringForces) { - switch (mJointP.mActuatorType) + switch (Joint::mAspectProperties.mActuatorType) { - case FORCE: - case PASSIVE: - case SERVO: + case Joint::FORCE: + case Joint::PASSIVE: + case Joint::SERVO: break; - case ACCELERATION: - case VELOCITY: - case LOCKED: + case Joint::ACCELERATION: + case Joint::VELOCITY: + case Joint::LOCKED: updateForceID(_bodyForce, _timeStep, _withDampingForces, _withSpringForces); break; @@ -2170,15 +2177,15 @@ void MultiDofJoint::updateImpulseID(const Eigen::Vector6d& _bodyImpulse) template void MultiDofJoint::updateImpulseFD(const Eigen::Vector6d& _bodyImpulse) { - switch (mJointP.mActuatorType) + switch (Joint::mAspectProperties.mActuatorType) { - case FORCE: - case PASSIVE: - case SERVO: + case Joint::FORCE: + case Joint::PASSIVE: + case Joint::SERVO: break; - case ACCELERATION: - case VELOCITY: - case LOCKED: + case Joint::ACCELERATION: + case Joint::VELOCITY: + case Joint::LOCKED: updateImpulseID(_bodyImpulse); break; default: @@ -2191,16 +2198,16 @@ void MultiDofJoint::updateImpulseFD(const Eigen::Vector6d& _bodyImpulse) template void MultiDofJoint::updateConstrainedTerms(double _timeStep) { - switch (mJointP.mActuatorType) + switch (Joint::mAspectProperties.mActuatorType) { - case FORCE: - case PASSIVE: - case SERVO: + case Joint::FORCE: + case Joint::PASSIVE: + case Joint::SERVO: updateConstrainedTermsDynamic(_timeStep); break; - case ACCELERATION: - case VELOCITY: - case LOCKED: + case Joint::ACCELERATION: + case Joint::VELOCITY: + case Joint::LOCKED: updateConstrainedTermsKinematic(_timeStep); break; default: @@ -2247,7 +2254,7 @@ void MultiDofJoint::addChildBiasForceForInvMassMatrix( // Add child body's bias force to parent body's bias force. Note that mT // should be updated. - _parentBiasForce += math::dAdInvT(getLocalTransform(), beta); + _parentBiasForce += math::dAdInvT(this->getLocalTransform(), beta); } //============================================================================== @@ -2267,7 +2274,7 @@ void MultiDofJoint::addChildBiasForceForInvAugMassMatrix( // Add child body's bias force to parent body's bias force. Note that mT // should be updated. - _parentBiasForce += math::dAdInvT(getLocalTransform(), beta); + _parentBiasForce += math::dAdInvT(this->getLocalTransform(), beta); } //============================================================================== @@ -2292,7 +2299,7 @@ void MultiDofJoint::getInvMassMatrixSegment( mInvMassMatrixSegment = getInvProjArtInertia() * (mInvM_a - getLocalJacobianStatic().transpose() - * _artInertia * math::AdInvT(getLocalTransform(), _spatialAcc)); + * _artInertia * math::AdInvT(this->getLocalTransform(), _spatialAcc)); // Verification assert(!math::isNan(mInvMassMatrixSegment)); @@ -2316,7 +2323,7 @@ void MultiDofJoint::getInvAugMassMatrixSegment( mInvMassMatrixSegment = getInvProjArtInertiaImplicit() * (mInvM_a - getLocalJacobianStatic().transpose() - * _artInertia * math::AdInvT(getLocalTransform(), _spatialAcc)); + * _artInertia * math::AdInvT(this->getLocalTransform(), _spatialAcc)); // Verification assert(!math::isNan(mInvMassMatrixSegment)); diff --git a/unittests/testAspect.cpp b/unittests/testAspect.cpp index 4306be21807c1..4cd7475a8e020 100644 --- a/unittests/testAspect.cpp +++ b/unittests/testAspect.cpp @@ -116,9 +116,9 @@ class EmbeddedStateComposite : { public: - EmbeddedStateComposite() + EmbeddedStateComposite(const EmbeddedStateData& state = EmbeddedStateData()) { - create(); + create(state); } void setAspectState(const AspectState& state) { mAspectState = state; } @@ -130,9 +130,10 @@ class EmbeddedPropertiesComposite : { public: - EmbeddedPropertiesComposite() + EmbeddedPropertiesComposite( + const EmbeddedPropertiesData& properties = EmbeddedPropertiesData()) { - create(); + create(properties); } void setAspectProperties(const AspectProperties& properties) @@ -884,6 +885,11 @@ TEST(Aspect, Embedded) sp.get()->getState()); EXPECT_EQ(sp_derived.get()->getProperties(), sp.get()->getProperties()); + + + // --------- Test Construction ----------- + EmbeddedStateComposite s_constructed(state); + EXPECT_EQ(s_constructed.get()->getState(), state); } int main(int argc, char* argv[]) From 2a30fde1d70340de05d4bc70fba43b591163f1f8 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Wed, 30 Mar 2016 02:14:22 -0400 Subject: [PATCH 18/62] fixed bug in sub_ptr implementation --- dart/common/detail/sub_ptr.h | 39 ++++++++++++++++++++++++------------ dart/common/sub_ptr.h | 11 ++++++---- 2 files changed, 33 insertions(+), 17 deletions(-) diff --git a/dart/common/detail/sub_ptr.h b/dart/common/detail/sub_ptr.h index 6040dcd3d49c5..488a51ef38157 100644 --- a/dart/common/detail/sub_ptr.h +++ b/dart/common/detail/sub_ptr.h @@ -37,17 +37,23 @@ #ifndef DART_COMMON_DETAIL_SUB_PTR_H_ #define DART_COMMON_DETAIL_SUB_PTR_H_ +#include "dart/common/sub_ptr.h" + +namespace dart { +namespace common { + //============================================================================== template sub_ptr::sub_ptr() - : mSubject(nullptr) + : mT(nullptr), + mSubjectBase(nullptr) { // Do nothing } //============================================================================== template -sub_ptr::sub_ptr(T* _ptr) : mSubject(nullptr) +sub_ptr::sub_ptr(T* _ptr) : mT(nullptr), mSubjectBase(nullptr) { set(_ptr); } @@ -72,55 +78,62 @@ sub_ptr& sub_ptr::operator =(T* _ptr) template sub_ptr::operator T*() const { - return mSubject; + return mT; } //============================================================================== template T& sub_ptr::operator*() const { - return *mSubject; + return *mT; } //============================================================================== template T* sub_ptr::operator->() const { - return mSubject; + return mT; } //============================================================================== template T* sub_ptr::get() const { - return mSubject; + return mT; } //============================================================================== template void sub_ptr::set(T* _ptr) { - if(mSubject == _ptr) + if(mT == _ptr) return; - removeSubject(mSubject); - mSubject = _ptr; - addSubject(mSubject); + removeSubject(mSubjectBase); + mSubjectBase = dynamic_cast(_ptr); + mT = _ptr; + addSubject(mSubjectBase); } //============================================================================== template bool sub_ptr::valid() { - return mSubject != nullptr; + return mT != nullptr; } //============================================================================== template void sub_ptr::handleDestructionNotification(const Subject* _subject) { - if(_subject == mSubject) - mSubject = nullptr; + if(_subject == mSubjectBase) + { + mT = nullptr; + mSubjectBase = nullptr; + } } +} // namespace common +} // namespace dart + #endif // DART_COMMON_DETAIL_SUB_PTR_H_ diff --git a/dart/common/sub_ptr.h b/dart/common/sub_ptr.h index cba7186898230..e4efadc24967b 100644 --- a/dart/common/sub_ptr.h +++ b/dart/common/sub_ptr.h @@ -86,11 +86,12 @@ class sub_ptr : public Observer virtual void handleDestructionNotification( const Subject* _subject) override; - /// Store the Subject pointer - T* mSubject; -}; + /// Store the pointer to the full object + T* mT; -#include "dart/common/detail/sub_ptr.h" + /// Store the pointer to the virtual Subject base + Subject* mSubjectBase; +}; } // namespace common @@ -100,4 +101,6 @@ using sub_ptr = common::sub_ptr; } // namespace dart +#include "dart/common/detail/sub_ptr.h" + #endif // DART_COMMON_SUB_PTR_H_ From 75dff54c411b05c0f7cc8d496e86f98ee78cc6ac Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Wed, 30 Mar 2016 13:34:58 -0400 Subject: [PATCH 19/62] finished integrating embedded Aspects into all SingleDofJoints --- dart/common/EmbeddedAspect.h | 33 ++- dart/dynamics/BallJoint.cpp | 12 +- dart/dynamics/EulerJoint.cpp | 26 +-- dart/dynamics/FreeJoint.cpp | 22 +- dart/dynamics/Joint.cpp | 4 +- dart/dynamics/Joint.h | 2 +- dart/dynamics/PlanarJoint.cpp | 16 +- dart/dynamics/PrismaticJoint.cpp | 28 ++- dart/dynamics/PrismaticJoint.h | 7 +- dart/dynamics/RevoluteJoint.cpp | 29 ++- dart/dynamics/RevoluteJoint.h | 6 +- dart/dynamics/ScrewJoint.cpp | 39 +++- dart/dynamics/ScrewJoint.h | 4 +- dart/dynamics/SingleDofJoint.cpp | 214 ++++++++++-------- dart/dynamics/SingleDofJoint.h | 6 +- dart/dynamics/TranslationalJoint.cpp | 12 +- dart/dynamics/UniversalJoint.cpp | 14 +- dart/dynamics/WeldJoint.cpp | 4 +- .../detail/PrismaticJointProperties.cpp | 13 -- .../detail/PrismaticJointProperties.h | 17 +- .../detail/RevoluteJointProperties.cpp | 13 -- .../dynamics/detail/RevoluteJointProperties.h | 17 +- dart/dynamics/detail/ScrewJointProperties.cpp | 13 -- dart/dynamics/detail/ScrewJointProperties.h | 19 +- .../detail/SingleDofJointProperties.cpp | 7 - .../detail/SingleDofJointProperties.h | 36 +-- 26 files changed, 294 insertions(+), 319 deletions(-) diff --git a/dart/common/EmbeddedAspect.h b/dart/common/EmbeddedAspect.h index 8c7f087c95e5a..118156ba569b7 100644 --- a/dart/common/EmbeddedAspect.h +++ b/dart/common/EmbeddedAspect.h @@ -70,6 +70,8 @@ class EmbeddedStateAspect : public detail::EmbeddedStateAspect< // Do nothing } + virtual ~EmbeddedStateAspect() = default; + }; //============================================================================== @@ -94,6 +96,12 @@ class EmbedState : public virtual common::RequiresAspect< using AspectStateData = StateDataT; using AspectState = common::Aspect::StateMixer; using Aspect = common::EmbeddedStateAspect; + using Base = common::RequiresAspect; + + // Inherit constructor + using Base::RequiresAspect; + + virtual ~EmbedState() = default; const AspectState& getAspectState() const { @@ -122,8 +130,14 @@ class EmbedStateOnTopOf : public CompositeJoiner< using AspectStateData = typename Impl::AspectStateData; using AspectState = typename Impl::AspectState; using Aspect = typename Impl::Aspect; + using Base = CompositeJoiner; using Impl::getAspectState; + // Inherit constructor + using Base::CompositeJoiner; + + virtual ~EmbedStateOnTopOf() = default; + protected: using Impl::mAspectState; @@ -156,6 +170,8 @@ class EmbeddedPropertiesAspect : public detail::EmbeddedPropertiesAspect< // Do nothing } + virtual ~EmbeddedPropertiesAspect() = default; + }; //============================================================================== @@ -181,6 +197,10 @@ class EmbedProperties : public virtual common::RequiresAspect< using AspectPropertiesData = PropertiesDataT; using AspectProperties = common::Aspect::PropertiesMixer; using Aspect = common::EmbeddedPropertiesAspect; + using Base = common::RequiresAspect; + + // Inherit constructor + using Base::RequiresAspect; virtual ~EmbedProperties() = default; @@ -206,6 +226,7 @@ class EmbedPropertiesOnTopOf : public CompositeJoiner< { public: + using Base = CompositeJoiner, CompositeBases...>; using Impl = EmbedProperties; using Derived = typename Impl::Derived; using AspectPropertiesData = typename Impl::AspectPropertiesData; @@ -213,6 +234,9 @@ class EmbedPropertiesOnTopOf : public CompositeJoiner< using Aspect = typename Impl::Aspect; using Impl::getAspectProperties; + // Inherit constructor + using Base::CompositeJoiner; + virtual ~EmbedPropertiesOnTopOf() = default; protected: @@ -314,6 +338,10 @@ class EmbedStateAndProperties : public virtual common::RequiresAspect< using AspectProperties = common::Aspect::PropertiesMixer; using Aspect = common::EmbeddedStateAndPropertiesAspect< Derived, AspectState, AspectProperties>; + using Base = common::RequiresAspect; + + // Inherit constructor + using Base::RequiresAspect; virtual ~EmbedStateAndProperties() = default; @@ -358,6 +386,10 @@ class EmbedStateAndPropertiesOnTopOf : public CompositeJoiner< using Aspect = typename Impl::Aspect; using Impl::getAspectState; using Impl::getAspectProperties; + using Base = CompositeJoiner; + + // Inherit constructor + using Base::CompositeJoiner; virtual ~EmbedStateAndPropertiesOnTopOf() = default; @@ -368,7 +400,6 @@ class EmbedStateAndPropertiesOnTopOf : public CompositeJoiner< }; - } // namespace common } // namespace dart diff --git a/dart/dynamics/BallJoint.cpp b/dart/dynamics/BallJoint.cpp index de7e6300288e2..d2c72061b3b6f 100644 --- a/dart/dynamics/BallJoint.cpp +++ b/dart/dynamics/BallJoint.cpp @@ -145,11 +145,11 @@ void BallJoint::integratePositions(double _dt) void BallJoint::updateDegreeOfFreedomNames() { if(!mDofs[0]->isNamePreserved()) - mDofs[0]->setName(mAspectProperties.mName + "_x", false); + mDofs[0]->setName(Joint::mAspectProperties.mName + "_x", false); if(!mDofs[1]->isNamePreserved()) - mDofs[1]->setName(mAspectProperties.mName + "_y", false); + mDofs[1]->setName(Joint::mAspectProperties.mName + "_y", false); if(!mDofs[2]->isNamePreserved()) - mDofs[2]->setName(mAspectProperties.mName + "_z", false); + mDofs[2]->setName(Joint::mAspectProperties.mName + "_z", false); } //============================================================================== @@ -157,8 +157,8 @@ void BallJoint::updateLocalTransform() const { mR.linear() = convertToRotation(getPositionsStatic()); - mT = mAspectProperties.mT_ParentBodyToJoint * mR - * mAspectProperties.mT_ChildBodyToJoint.inverse(); + mT = Joint::mAspectProperties.mT_ParentBodyToJoint * mR + * Joint::mAspectProperties.mT_ChildBodyToJoint.inverse(); assert(math::verifyTransform(mT)); } @@ -167,7 +167,7 @@ void BallJoint::updateLocalTransform() const void BallJoint::updateLocalJacobian(bool _mandatory) const { if (_mandatory) - mJacobian = math::getAdTMatrix(mAspectProperties.mT_ChildBodyToJoint).leftCols<3>(); + mJacobian = math::getAdTMatrix(Joint::mAspectProperties.mT_ChildBodyToJoint).leftCols<3>(); } //============================================================================== diff --git a/dart/dynamics/EulerJoint.cpp b/dart/dynamics/EulerJoint.cpp index 596b59bbf9001..bd1a719e46fbf 100644 --- a/dart/dynamics/EulerJoint.cpp +++ b/dart/dynamics/EulerJoint.cpp @@ -212,7 +212,7 @@ Eigen::Matrix EulerJoint::getLocalJacobianStatic( #ifndef NDEBUG if (std::abs(getPositionsStatic()[1]) == DART_PI * 0.5) std::cout << "Singular configuration in ZYX-euler joint [" - << mAspectProperties.mName << "]. (" + << Joint::mAspectProperties.mName << "]. (" << _positions[0] << ", " << _positions[1] << ", " << _positions[2] << ")" @@ -238,7 +238,7 @@ Eigen::Matrix EulerJoint::getLocalJacobianStatic( #ifndef NDEBUG if (std::abs(_positions[1]) == DART_PI * 0.5) std::cout << "Singular configuration in ZYX-euler joint [" - << mAspectProperties.mName << "]. (" + << Joint::mAspectProperties.mName << "]. (" << _positions[0] << ", " << _positions[1] << ", " << _positions[2] << ")" @@ -254,9 +254,9 @@ Eigen::Matrix EulerJoint::getLocalJacobianStatic( } } - J.col(0) = math::AdT(mAspectProperties.mT_ChildBodyToJoint, J0); - J.col(1) = math::AdT(mAspectProperties.mT_ChildBodyToJoint, J1); - J.col(2) = math::AdT(mAspectProperties.mT_ChildBodyToJoint, J2); + J.col(0) = math::AdT(Joint::mAspectProperties.mT_ChildBodyToJoint, J0); + J.col(1) = math::AdT(Joint::mAspectProperties.mT_ChildBodyToJoint, J1); + J.col(2) = math::AdT(Joint::mAspectProperties.mT_ChildBodyToJoint, J2); assert(!math::isNan(J)); @@ -267,7 +267,7 @@ Eigen::Matrix EulerJoint::getLocalJacobianStatic( double det = luJTJ.determinant(); if (det < 1e-5) { - std::cout << "ill-conditioned Jacobian in joint [" << mAspectProperties.mName << "]." + std::cout << "ill-conditioned Jacobian in joint [" << Joint::mAspectProperties.mName << "]." << " The determinant of the Jacobian is (" << det << ")." << std::endl; std::cout << "rank is (" << luJTJ.rank() << ")." << std::endl; @@ -313,7 +313,7 @@ void EulerJoint::updateDegreeOfFreedomNames() affixes.push_back("_z"); break; default: - dterr << "Unsupported axis order in EulerJoint named '" << mAspectProperties.mName + dterr << "Unsupported axis order in EulerJoint named '" << Joint::mAspectProperties.mName << "' (" << static_cast(getAxisOrder()) << ")\n"; } @@ -322,7 +322,7 @@ void EulerJoint::updateDegreeOfFreedomNames() for (size_t i = 0; i < 3; ++i) { if(!mDofs[i]->isNamePreserved()) - mDofs[i]->setName(mAspectProperties.mName + affixes[i], false); + mDofs[i]->setName(Joint::mAspectProperties.mName + affixes[i], false); } } } @@ -330,8 +330,8 @@ void EulerJoint::updateDegreeOfFreedomNames() //============================================================================== void EulerJoint::updateLocalTransform() const { - mT = mAspectProperties.mT_ParentBodyToJoint * convertToTransform(getPositionsStatic()) - * mAspectProperties.mT_ChildBodyToJoint.inverse(); + mT = Joint::mAspectProperties.mT_ParentBodyToJoint * convertToTransform(getPositionsStatic()) + * Joint::mAspectProperties.mT_ChildBodyToJoint.inverse(); assert(math::verifyTransform(mT)); } @@ -410,9 +410,9 @@ void EulerJoint::updateLocalJacobianTimeDeriv() const } } - mJacobianDeriv.col(0) = math::AdT(mAspectProperties.mT_ChildBodyToJoint, dJ0); - mJacobianDeriv.col(1) = math::AdT(mAspectProperties.mT_ChildBodyToJoint, dJ1); - mJacobianDeriv.col(2) = math::AdT(mAspectProperties.mT_ChildBodyToJoint, dJ2); + mJacobianDeriv.col(0) = math::AdT(Joint::mAspectProperties.mT_ChildBodyToJoint, dJ0); + mJacobianDeriv.col(1) = math::AdT(Joint::mAspectProperties.mT_ChildBodyToJoint, dJ1); + mJacobianDeriv.col(2) = math::AdT(Joint::mAspectProperties.mT_ChildBodyToJoint, dJ2); assert(!math::isNan(mJacobianDeriv)); } diff --git a/dart/dynamics/FreeJoint.cpp b/dart/dynamics/FreeJoint.cpp index 37a94311a433b..a3dfcf053002b 100644 --- a/dart/dynamics/FreeJoint.cpp +++ b/dart/dynamics/FreeJoint.cpp @@ -167,9 +167,9 @@ void FreeJoint::setSpatialMotion(const Eigen::Isometry3d* newTransform, void FreeJoint::setRelativeTransform(const Eigen::Isometry3d& newTransform) { setPositionsStatic(convertToPositions( - mAspectProperties.mT_ParentBodyToJoint.inverse() * + Joint::mAspectProperties.mT_ParentBodyToJoint.inverse() * newTransform * - mAspectProperties.mT_ChildBodyToJoint)); + Joint::mAspectProperties.mT_ChildBodyToJoint)); } //============================================================================== @@ -576,17 +576,17 @@ void FreeJoint::integratePositions(double _dt) void FreeJoint::updateDegreeOfFreedomNames() { if(!mDofs[0]->isNamePreserved()) - mDofs[0]->setName(mAspectProperties.mName + "_rot_x", false); + mDofs[0]->setName(Joint::mAspectProperties.mName + "_rot_x", false); if(!mDofs[1]->isNamePreserved()) - mDofs[1]->setName(mAspectProperties.mName + "_rot_y", false); + mDofs[1]->setName(Joint::mAspectProperties.mName + "_rot_y", false); if(!mDofs[2]->isNamePreserved()) - mDofs[2]->setName(mAspectProperties.mName + "_rot_z", false); + mDofs[2]->setName(Joint::mAspectProperties.mName + "_rot_z", false); if(!mDofs[3]->isNamePreserved()) - mDofs[3]->setName(mAspectProperties.mName + "_pos_x", false); + mDofs[3]->setName(Joint::mAspectProperties.mName + "_pos_x", false); if(!mDofs[4]->isNamePreserved()) - mDofs[4]->setName(mAspectProperties.mName + "_pos_y", false); + mDofs[4]->setName(Joint::mAspectProperties.mName + "_pos_y", false); if(!mDofs[5]->isNamePreserved()) - mDofs[5]->setName(mAspectProperties.mName + "_pos_z", false); + mDofs[5]->setName(Joint::mAspectProperties.mName + "_pos_z", false); } //============================================================================== @@ -594,8 +594,8 @@ void FreeJoint::updateLocalTransform() const { mQ = convertToTransform(getPositionsStatic()); - mT = mAspectProperties.mT_ParentBodyToJoint * mQ - * mAspectProperties.mT_ChildBodyToJoint.inverse(); + mT = Joint::mAspectProperties.mT_ParentBodyToJoint * mQ + * Joint::mAspectProperties.mT_ChildBodyToJoint.inverse(); assert(math::verifyTransform(mT)); } @@ -604,7 +604,7 @@ void FreeJoint::updateLocalTransform() const void FreeJoint::updateLocalJacobian(bool _mandatory) const { if (_mandatory) - mJacobian = math::getAdTMatrix(mAspectProperties.mT_ChildBodyToJoint); + mJacobian = math::getAdTMatrix(Joint::mAspectProperties.mT_ChildBodyToJoint); } //============================================================================== diff --git a/dart/dynamics/Joint.cpp b/dart/dynamics/Joint.cpp index f17b11f687318..122c9753298c1 100644 --- a/dart/dynamics/Joint.cpp +++ b/dart/dynamics/Joint.cpp @@ -84,7 +84,7 @@ Joint::ExtendedProperties::ExtendedProperties( const Properties& standardProperties, const CompositeProperties& aspectProperties) : Properties(standardProperties), - mAspectProperties(aspectProperties) + mCompositeProperties(aspectProperties) { // Do nothing } @@ -94,7 +94,7 @@ Joint::ExtendedProperties::ExtendedProperties( Properties&& standardProperties, CompositeProperties&& aspectProperties) : Properties(std::move(standardProperties)), - mAspectProperties(std::move(aspectProperties)) + mCompositeProperties(std::move(aspectProperties)) { // Do nothing } diff --git a/dart/dynamics/Joint.h b/dart/dynamics/Joint.h index 0be50aab3d494..c603affe8a0e1 100644 --- a/dart/dynamics/Joint.h +++ b/dart/dynamics/Joint.h @@ -96,7 +96,7 @@ class Joint : public virtual common::Subject, CompositeProperties&& aspectProperties); /// Properties of all the Aspects attached to this Joint - CompositeProperties mAspectProperties; + CompositeProperties mCompositeProperties; }; /// Default actuator type diff --git a/dart/dynamics/PlanarJoint.cpp b/dart/dynamics/PlanarJoint.cpp index 4cca3a8c40043..21697028e17aa 100644 --- a/dart/dynamics/PlanarJoint.cpp +++ b/dart/dynamics/PlanarJoint.cpp @@ -193,11 +193,11 @@ Eigen::Matrix PlanarJoint::getLocalJacobianStatic( J.block<3, 1>(0, 2) = getPlanarJointAspect()->getRotAxis(); J.leftCols<2>() - = math::AdTJacFixed(mAspectProperties.mT_ChildBodyToJoint + = math::AdTJacFixed(Joint::mAspectProperties.mT_ChildBodyToJoint * math::expAngular(getPlanarJointAspect()->getRotAxis() * -_positions[2]), J.leftCols<2>()); - J.col(2) = math::AdTJac(mAspectProperties.mT_ChildBodyToJoint, J.col(2)); + J.col(2) = math::AdTJac(Joint::mAspectProperties.mT_ChildBodyToJoint, J.col(2)); // Verification assert(!math::isNan(J)); @@ -245,7 +245,7 @@ void PlanarJoint::updateDegreeOfFreedomNames() affixes.push_back("_2"); break; default: - dterr << "Unsupported plane type in PlanarJoint named '" << mAspectProperties.mName + dterr << "Unsupported plane type in PlanarJoint named '" << Joint::mAspectProperties.mName << "' (" << static_cast(getPlanarJointAspect()->getPlaneType()) << ")\n"; } @@ -255,7 +255,7 @@ void PlanarJoint::updateDegreeOfFreedomNames() for (size_t i = 0; i < 2; ++i) { if (!mDofs[i]->isNamePreserved()) - mDofs[i]->setName(mAspectProperties.mName + affixes[i], false); + mDofs[i]->setName(Joint::mAspectProperties.mName + affixes[i], false); } } } @@ -264,11 +264,11 @@ void PlanarJoint::updateDegreeOfFreedomNames() void PlanarJoint::updateLocalTransform() const { const Eigen::Vector3d& positions = getPositionsStatic(); - mT = mAspectProperties.mT_ParentBodyToJoint + mT = Joint::mAspectProperties.mT_ParentBodyToJoint * Eigen::Translation3d(getPlanarJointAspect()->getTransAxis1() * positions[0]) * Eigen::Translation3d(getPlanarJointAspect()->getTransAxis2() * positions[1]) * math::expAngular (getPlanarJointAspect()->getRotAxis() * positions[2]) - * mAspectProperties.mT_ChildBodyToJoint.inverse(); + * Joint::mAspectProperties.mT_ChildBodyToJoint.inverse(); // Verification assert(math::verifyTransform(mT)); @@ -292,14 +292,14 @@ void PlanarJoint::updateLocalJacobianTimeDeriv() const const Eigen::Vector3d& velocities = getVelocitiesStatic(); mJacobianDeriv.col(0) = -math::ad(Jacobian.col(2) * velocities[2], - math::AdT(mAspectProperties.mT_ChildBodyToJoint + math::AdT(Joint::mAspectProperties.mT_ChildBodyToJoint * math::expAngular(getPlanarJointAspect()->getRotAxis() * -getPositionsStatic()[2]), J.col(0))); mJacobianDeriv.col(1) = -math::ad(Jacobian.col(2) * velocities[2], - math::AdT(mAspectProperties.mT_ChildBodyToJoint + math::AdT(Joint::mAspectProperties.mT_ChildBodyToJoint * math::expAngular(getPlanarJointAspect()->getRotAxis() * -getPositionsStatic()[2]), J.col(1))); diff --git a/dart/dynamics/PrismaticJoint.cpp b/dart/dynamics/PrismaticJoint.cpp index 25a476d81b7d0..a95d0ec662223 100644 --- a/dart/dynamics/PrismaticJoint.cpp +++ b/dart/dynamics/PrismaticJoint.cpp @@ -62,14 +62,19 @@ void PrismaticJoint::setProperties(const Properties& _properties) //============================================================================== void PrismaticJoint::setProperties(const UniqueProperties& _properties) { - setAxis(_properties.mAxis); + setAspectProperties(_properties); +} + +//============================================================================== +void PrismaticJoint::setAspectProperties(const AspectProperties& properties) +{ + setAxis(properties.mAxis); } //============================================================================== PrismaticJoint::Properties PrismaticJoint::getPrismaticJointProperties() const { - return Properties(getSingleDofJointProperties(), - getPrismaticJointAspect()->getProperties()); + return Properties(getSingleDofJointProperties(), mAspectProperties); } //============================================================================== @@ -119,18 +124,23 @@ bool PrismaticJoint::isCyclic(size_t /*_index*/) const //============================================================================== void PrismaticJoint::setAxis(const Eigen::Vector3d& _axis) { - getPrismaticJointAspect()->setAxis(_axis); + if(_axis == mAspectProperties.mAxis) + return; + + mAspectProperties.mAxis = _axis.normalized(); + Joint::notifyPositionUpdate(); + Joint::incrementVersion(); } //============================================================================== const Eigen::Vector3d& PrismaticJoint::getAxis() const { - return getPrismaticJointAspect()->getAxis(); + return mAspectProperties.mAxis; } //============================================================================== PrismaticJoint::PrismaticJoint(const Properties& properties) - : detail::PrismaticJointBase(properties, common::NoArg) + : detail::PrismaticJointBase(common::NoArg, properties) { // Inherited Aspects must be created in the final joint class in reverse order // or else we get pure virtual function calls @@ -148,9 +158,9 @@ Joint* PrismaticJoint::clone() const //============================================================================== void PrismaticJoint::updateLocalTransform() const { - mT = mAspectProperties.mT_ParentBodyToJoint + mT = Joint::mAspectProperties.mT_ParentBodyToJoint * Eigen::Translation3d(getAxis() * getPositionStatic()) - * mAspectProperties.mT_ChildBodyToJoint.inverse(); + * Joint::mAspectProperties.mT_ChildBodyToJoint.inverse(); // Verification assert(math::verifyTransform(mT)); @@ -161,7 +171,7 @@ void PrismaticJoint::updateLocalJacobian(bool _mandatory) const { if(_mandatory) { - mJacobian = math::AdTLinear(mAspectProperties.mT_ChildBodyToJoint, getAxis()); + mJacobian = math::AdTLinear(Joint::mAspectProperties.mT_ChildBodyToJoint, getAxis()); // Verification assert(!math::isNan(mJacobian)); diff --git a/dart/dynamics/PrismaticJoint.h b/dart/dynamics/PrismaticJoint.h index 857b1b0864241..1e417f3d0d0c5 100644 --- a/dart/dynamics/PrismaticJoint.h +++ b/dart/dynamics/PrismaticJoint.h @@ -50,8 +50,6 @@ class PrismaticJoint : public detail::PrismaticJointBase friend class Skeleton; using UniqueProperties = detail::PrismaticJointUniqueProperties; using Properties = detail::PrismaticJointProperties; - using Aspect = detail::PrismaticJointAspect; - using Base = detail::PrismaticJointBase; DART_BAKE_SPECIALIZED_ASPECT_IRREGULAR(Aspect, PrismaticJointAspect) @@ -66,6 +64,9 @@ class PrismaticJoint : public detail::PrismaticJointBase /// Set the Properties of this PrismaticJoint void setProperties(const UniqueProperties& _properties); + /// Set the AspectProperties of this PrismaticJoint + void setAspectProperties(const AspectProperties& properties); + /// Get the Properties of this PrismaticJoint Properties getPrismaticJointProperties() const; @@ -93,8 +94,6 @@ class PrismaticJoint : public detail::PrismaticJointBase /// const Eigen::Vector3d& getAxis() const; - template friend void detail::JointPropertyUpdate(AspectType*); - protected: /// Constructor called by Skeleton class diff --git a/dart/dynamics/RevoluteJoint.cpp b/dart/dynamics/RevoluteJoint.cpp index 60aafb686e7d8..ae9ab16ba8c1d 100644 --- a/dart/dynamics/RevoluteJoint.cpp +++ b/dart/dynamics/RevoluteJoint.cpp @@ -63,14 +63,19 @@ void RevoluteJoint::setProperties(const Properties& _properties) //============================================================================== void RevoluteJoint::setProperties(const UniqueProperties& _properties) { - setAxis(_properties.mAxis); + setAspectProperties(_properties); +} + +//============================================================================== +void RevoluteJoint::setAspectProperties(const AspectProperties& properties) +{ + setAxis(properties.mAxis); } //============================================================================== RevoluteJoint::Properties RevoluteJoint::getRevoluteJointProperties() const { - return Properties(getSingleDofJointProperties(), - getRevoluteJointAspect()->getProperties()); + return Properties(getSingleDofJointProperties(), mAspectProperties); } //============================================================================== @@ -120,18 +125,23 @@ const std::string& RevoluteJoint::getStaticType() //============================================================================== void RevoluteJoint::setAxis(const Eigen::Vector3d& _axis) { - getRevoluteJointAspect()->setAxis(_axis); + if(_axis == mAspectProperties.mAxis) + return; + + mAspectProperties.mAxis = _axis.normalized(); + Joint::notifyPositionUpdate(); + Joint::incrementVersion(); } //============================================================================== const Eigen::Vector3d& RevoluteJoint::getAxis() const { - return getRevoluteJointAspect()->getAxis(); + return mAspectProperties.mAxis; } //============================================================================== RevoluteJoint::RevoluteJoint(const Properties& properties) - : detail::RevoluteJointBase(properties, common::NoArg) + : detail::RevoluteJointBase(common::NoArg, properties) { // Inherited Aspects must be created in the final joint class in reverse order // or else we get pure virtual function calls @@ -149,9 +159,9 @@ Joint* RevoluteJoint::clone() const //============================================================================== void RevoluteJoint::updateLocalTransform() const { - mT = mAspectProperties.mT_ParentBodyToJoint + mT = Joint::mAspectProperties.mT_ParentBodyToJoint * math::expAngular(getAxis() * getPositionStatic()) - * mAspectProperties.mT_ChildBodyToJoint.inverse(); + * Joint::mAspectProperties.mT_ChildBodyToJoint.inverse(); // Verification assert(math::verifyTransform(mT)); @@ -162,7 +172,8 @@ void RevoluteJoint::updateLocalJacobian(bool _mandatory) const { if(_mandatory) { - mJacobian = math::AdTAngular(mAspectProperties.mT_ChildBodyToJoint, getAxis()); + mJacobian = math::AdTAngular( + Joint::mAspectProperties.mT_ChildBodyToJoint, getAxis()); // Verification assert(!math::isNan(mJacobian)); diff --git a/dart/dynamics/RevoluteJoint.h b/dart/dynamics/RevoluteJoint.h index 61c5905458907..34310fd412884 100644 --- a/dart/dynamics/RevoluteJoint.h +++ b/dart/dynamics/RevoluteJoint.h @@ -50,7 +50,6 @@ class RevoluteJoint : public detail::RevoluteJointBase friend class Skeleton; using UniqueProperties = detail::RevoluteJointUniqueProperties; using Properties = detail::RevoluteJointProperties; - using Aspect = detail::RevoluteJointAspect; using Base = detail::RevoluteJointBase; DART_BAKE_SPECIALIZED_ASPECT_IRREGULAR(Aspect, RevoluteJointAspect) @@ -66,6 +65,9 @@ class RevoluteJoint : public detail::RevoluteJointBase /// Set the Properties of this RevoluteJoint void setProperties(const UniqueProperties& _properties); + /// Set the AspectProperties of this RevoluteJoint + void setAspectProperties(const AspectProperties& properties); + /// Get the Properties of this RevoluteJoint Properties getRevoluteJointProperties() const; @@ -112,8 +114,6 @@ class RevoluteJoint : public detail::RevoluteJointBase public: - template friend void detail::JointPropertyUpdate(AspectType*); - // To get byte-aligned Eigen vectors EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; diff --git a/dart/dynamics/ScrewJoint.cpp b/dart/dynamics/ScrewJoint.cpp index f93f11e2149ab..bc38b09224dc5 100644 --- a/dart/dynamics/ScrewJoint.cpp +++ b/dart/dynamics/ScrewJoint.cpp @@ -62,15 +62,20 @@ void ScrewJoint::setProperties(const Properties& _properties) //============================================================================== void ScrewJoint::setProperties(const UniqueProperties& _properties) { - setAxis(_properties.mAxis); - setPitch(_properties.mPitch); + setAspectProperties(_properties); +} + +//============================================================================== +void ScrewJoint::setAspectProperties(const AspectProperties& properties) +{ + setAxis(properties.mAxis); + setPitch(properties.mPitch); } //============================================================================== ScrewJoint::Properties ScrewJoint::getScrewJointProperties() const { - return Properties(getSingleDofJointProperties(), - getScrewJointAspect()->getProperties()); + return Properties(getSingleDofJointProperties(), mAspectProperties); } //============================================================================== @@ -120,30 +125,40 @@ bool ScrewJoint::isCyclic(size_t /*_index*/) const //============================================================================== void ScrewJoint::setAxis(const Eigen::Vector3d& _axis) { - getScrewJointAspect()->setAxis(_axis); + if(_axis == mAspectProperties.mAxis) + return; + + mAspectProperties.mAxis = _axis.normalized(); + Joint::notifyPositionUpdate(); + Joint::incrementVersion(); } //============================================================================== const Eigen::Vector3d& ScrewJoint::getAxis() const { - return getScrewJointAspect()->getAxis(); + return mAspectProperties.mAxis; } //============================================================================== void ScrewJoint::setPitch(double _pitch) { - getScrewJointAspect()->setPitch(_pitch); + if(_pitch == mAspectProperties.mPitch) + return; + + mAspectProperties.mPitch = _pitch; + Joint::notifyPositionUpdate(); + Joint::incrementVersion(); } //============================================================================== double ScrewJoint::getPitch() const { - return getScrewJointAspect()->getPitch(); + return mAspectProperties.mPitch; } //============================================================================== ScrewJoint::ScrewJoint(const Properties& properties) - : detail::ScrewJointBase(properties, common::NoArg) + : detail::ScrewJointBase(common::NoArg, properties) { // Inherited Aspects must be created in the final joint class in reverse order // or else we get pure virtual function calls @@ -164,9 +179,9 @@ void ScrewJoint::updateLocalTransform() const Eigen::Vector6d S = Eigen::Vector6d::Zero(); S.head<3>() = getAxis(); S.tail<3>() = getAxis()*getPitch()/DART_2PI; - mT = mAspectProperties.mT_ParentBodyToJoint + mT = Joint::mAspectProperties.mT_ParentBodyToJoint * math::expMap(S * getPositionStatic()) - * mAspectProperties.mT_ChildBodyToJoint.inverse(); + * Joint::mAspectProperties.mT_ChildBodyToJoint.inverse(); assert(math::verifyTransform(mT)); } @@ -178,7 +193,7 @@ void ScrewJoint::updateLocalJacobian(bool _mandatory) const Eigen::Vector6d S = Eigen::Vector6d::Zero(); S.head<3>() = getAxis(); S.tail<3>() = getAxis()*getPitch()/DART_2PI; - mJacobian = math::AdT(mAspectProperties.mT_ChildBodyToJoint, S); + mJacobian = math::AdT(Joint::mAspectProperties.mT_ChildBodyToJoint, S); assert(!math::isNan(mJacobian)); } } diff --git a/dart/dynamics/ScrewJoint.h b/dart/dynamics/ScrewJoint.h index 2a272b3a89e5b..d7bc99c2b752f 100644 --- a/dart/dynamics/ScrewJoint.h +++ b/dart/dynamics/ScrewJoint.h @@ -50,7 +50,6 @@ class ScrewJoint : public detail::ScrewJointBase friend class Skeleton; using UniqueProperties = detail::ScrewJointUniqueProperties; using Properties = detail::ScrewJointProperties; - using Aspect = detail::ScrewJointAspect; using Base = detail::ScrewJointBase; DART_BAKE_SPECIALIZED_ASPECT_IRREGULAR(Aspect, ScrewJointAspect) @@ -66,6 +65,9 @@ class ScrewJoint : public detail::ScrewJointBase /// Set the Properties of this ScrewJoint void setProperties(const UniqueProperties& _properties); + /// Set the AspectProperties of this ScrewJoint + void setAspectProperties(const AspectProperties& properties); + /// Get the Properties of this ScrewJoint Properties getScrewJointProperties() const; diff --git a/dart/dynamics/SingleDofJoint.cpp b/dart/dynamics/SingleDofJoint.cpp index 02211875fbe08..dc48494888707 100644 --- a/dart/dynamics/SingleDofJoint.cpp +++ b/dart/dynamics/SingleDofJoint.cpp @@ -55,9 +55,15 @@ #define SINGLEDOFJOINT_REPORT_UNSUPPORTED_ACTUATOR( func ) \ dterr << "[SingleDofJoint::" # func "] Unsupported actuator type (" \ - << mAspectProperties.mActuatorType << ") for Joint [" << getName() << "].\n"; \ + << Joint::mAspectProperties.mActuatorType << ") for Joint [" << getName() << "].\n"; \ assert(false); +#define SINGLEDOFJOINT_SET_IF_DIFFERENT( mField, value )\ + if( value == mAspectProperties. mField )\ + return;\ + mAspectProperties. mField = value;\ + Joint::incrementVersion(); + namespace dart { namespace dynamics { @@ -77,13 +83,33 @@ void SingleDofJoint::setProperties(const Properties& _properties) //============================================================================== void SingleDofJoint::setProperties(const UniqueProperties& _properties) { - getSingleDofJointAspect()->setProperties(_properties); + setAspectProperties(_properties); +} + +//============================================================================== +void SingleDofJoint::setAspectProperties(const AspectProperties& properties) +{ + setDofName(0, properties.mDofName, properties.mPreserveDofName); + setPositionLowerLimit(0, properties.mPositionLowerLimit); + setPositionUpperLimit(0, properties.mPositionUpperLimit); + setInitialPosition(0, properties.mInitialPosition); + setVelocityLowerLimit(0, properties.mVelocityLowerLimit); + setVelocityUpperLimit(0, properties.mVelocityUpperLimit); + setInitialVelocity(0, properties.mInitialVelocity); + setAccelerationLowerLimit(0, properties.mAccelerationLowerLimit); + setAccelerationUpperLimit(0, properties.mAccelerationUpperLimit); + setForceLowerLimit(0, properties.mForceLowerLimit); + setForceUpperLimit(0, properties.mForceUpperLimit); + setSpringStiffness(0, properties.mSpringStiffness); + setRestPosition(0, properties.mRestPosition); + setDampingCoefficient(0, properties.mDampingCoefficient); + setCoulombFriction(0, properties.mFriction); } //============================================================================== SingleDofJoint::Properties SingleDofJoint::getSingleDofJointProperties() const { - return Properties(mAspectProperties, getSingleDofJointAspect()->getProperties()); + return Properties(Joint::mAspectProperties, mAspectProperties); } //============================================================================== @@ -177,20 +203,20 @@ const std::string& SingleDofJoint::setDofName(size_t _index, preserveDofName(0, _preserveName); - if(_name == getSingleDofJointAspect()->mProperties.mDofName) - return getSingleDofJointAspect()->mProperties.mDofName; + if(_name == mAspectProperties.mDofName) + return mAspectProperties.mDofName; const SkeletonPtr& skel = getSkeleton(); if(skel) { - getSingleDofJointAspect()->mProperties.mDofName = + mAspectProperties.mDofName = skel->mNameMgrForDofs.changeObjectName(mDof, _name); - skel->incrementVersion(); + Joint::incrementVersion(); } else - getSingleDofJointAspect()->mProperties.mDofName = _name; + mAspectProperties.mDofName = _name; - return getSingleDofJointAspect()->mProperties.mDofName; + return mAspectProperties.mDofName; } //============================================================================== @@ -202,7 +228,7 @@ void SingleDofJoint::preserveDofName(size_t _index, bool _preserve) return; } - getSingleDofJointAspect()->setPreserveDofName(_preserve); + SINGLEDOFJOINT_SET_IF_DIFFERENT( mPreserveDofName, _preserve); } //============================================================================== @@ -213,7 +239,7 @@ bool SingleDofJoint::isDofNamePreserved(size_t _index) const SINGLEDOFJOINT_REPORT_OUT_OF_RANGE( isDofNamePreserved, _index ); } - return getSingleDofJointAspect()->getPreserveDofName(); + return mAspectProperties.mPreserveDofName; } //============================================================================== @@ -227,7 +253,7 @@ const std::string& SingleDofJoint::getDofName(size_t _index) const assert(false); } - return getSingleDofJointAspect()->getDofName(); + return mAspectProperties.mDofName; } //============================================================================== @@ -239,12 +265,12 @@ void SingleDofJoint::setCommand(size_t _index, double _command) return; } - switch (mAspectProperties.mActuatorType) + switch (Joint::mAspectProperties.mActuatorType) { case FORCE: mCommand = math::clip(_command, - getSingleDofJointAspect()->getForceLowerLimit(), - getSingleDofJointAspect()->getForceUpperLimit()); + mAspectProperties.mForceLowerLimit, + mAspectProperties.mForceUpperLimit); break; case PASSIVE: if(_command != 0.0) @@ -257,18 +283,18 @@ void SingleDofJoint::setCommand(size_t _index, double _command) break; case SERVO: mCommand = math::clip(_command, - getSingleDofJointAspect()->getVelocityLowerLimit(), - getSingleDofJointAspect()->getVelocityUpperLimit()); + mAspectProperties.mVelocityLowerLimit, + mAspectProperties.mVelocityUpperLimit); break; case ACCELERATION: mCommand = math::clip(_command, - getSingleDofJointAspect()->getAccelerationLowerLimit(), - getSingleDofJointAspect()->getAccelerationUpperLimit()); + mAspectProperties.mAccelerationLowerLimit, + mAspectProperties.mAccelerationUpperLimit); break; case VELOCITY: mCommand = math::clip(_command, - getSingleDofJointAspect()->getVelocityLowerLimit(), - getSingleDofJointAspect()->getVelocityUpperLimit()); + mAspectProperties.mVelocityLowerLimit, + mAspectProperties.mVelocityUpperLimit); // TODO: This possibly makes the acceleration to exceed the limits. break; case LOCKED: @@ -373,7 +399,7 @@ void SingleDofJoint::setPositionLowerLimit(size_t _index, double _position) return; } - getSingleDofJointAspect()->setPositionLowerLimit(_position); + SINGLEDOFJOINT_SET_IF_DIFFERENT(mPositionLowerLimit, _position) } //============================================================================== @@ -385,7 +411,7 @@ double SingleDofJoint::getPositionLowerLimit(size_t _index) const return 0.0; } - return getSingleDofJointAspect()->getPositionLowerLimit(); + return mAspectProperties.mPositionLowerLimit; } //============================================================================== @@ -397,7 +423,7 @@ void SingleDofJoint::setPositionUpperLimit(size_t _index, double _position) return; } - getSingleDofJointAspect()->setPositionUpperLimit(_position); + SINGLEDOFJOINT_SET_IF_DIFFERENT( mPositionUpperLimit, _position ) } //============================================================================== @@ -409,7 +435,7 @@ double SingleDofJoint::getPositionUpperLimit(size_t _index) const return 0.0; } - return getSingleDofJointAspect()->getPositionUpperLimit(); + return mAspectProperties.mPositionUpperLimit; } //============================================================================== @@ -421,8 +447,8 @@ bool SingleDofJoint::hasPositionLimit(size_t _index) const return true; } - return std::isfinite(getSingleDofJointAspect()->getPositionLowerLimit()) - || std::isfinite(getSingleDofJointAspect()->getPositionUpperLimit()); + return std::isfinite(mAspectProperties.mPositionLowerLimit) + || std::isfinite(mAspectProperties.mPositionUpperLimit); } //============================================================================== @@ -434,13 +460,13 @@ void SingleDofJoint::resetPosition(size_t _index) return; } - setPositionStatic(getSingleDofJointAspect()->getInitialPosition()); + setPositionStatic(mAspectProperties.mInitialPosition); } //============================================================================== void SingleDofJoint::resetPositions() { - setPositionStatic(getSingleDofJointAspect()->getInitialPosition()); + setPositionStatic(mAspectProperties.mInitialPosition); } //============================================================================== @@ -452,7 +478,7 @@ void SingleDofJoint::setInitialPosition(size_t _index, double _initial) return; } - getSingleDofJointAspect()->setInitialPosition(_initial); + SINGLEDOFJOINT_SET_IF_DIFFERENT( mInitialPosition, _initial); } //============================================================================== @@ -464,7 +490,7 @@ double SingleDofJoint::getInitialPosition(size_t _index) const return 0.0; } - return getSingleDofJointAspect()->getInitialPosition(); + return mAspectProperties.mInitialPosition; } //============================================================================== @@ -483,7 +509,7 @@ void SingleDofJoint::setInitialPositions(const Eigen::VectorXd& _initial) Eigen::VectorXd SingleDofJoint::getInitialPositions() const { return Eigen::Matrix::Constant( - getSingleDofJointAspect()->getInitialPosition()); + mAspectProperties.mInitialPosition); } //============================================================================== @@ -498,7 +524,7 @@ void SingleDofJoint::setVelocity(size_t _index, double _velocity) setVelocityStatic(_velocity); #if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) - if (mAspectProperties.mActuatorType == VELOCITY) + if (Joint::mAspectProperties.mActuatorType == VELOCITY) mCommand = getVelocityStatic(); // TODO: Remove at DART 5.1. #endif @@ -528,7 +554,7 @@ void SingleDofJoint::setVelocities(const Eigen::VectorXd& _velocities) setVelocityStatic(_velocities[0]); #if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) - if (mAspectProperties.mActuatorType == VELOCITY) + if (Joint::mAspectProperties.mActuatorType == VELOCITY) mCommand = getVelocityStatic(); // TODO: Remove at DART 5.1. #endif @@ -549,7 +575,7 @@ void SingleDofJoint::setVelocityLowerLimit(size_t _index, double _velocity) return; } - getSingleDofJointAspect()->setVelocityLowerLimit(_velocity); + SINGLEDOFJOINT_SET_IF_DIFFERENT( mVelocityLowerLimit, _velocity); } //============================================================================== @@ -561,7 +587,7 @@ double SingleDofJoint::getVelocityLowerLimit(size_t _index) const return 0.0; } - return getSingleDofJointAspect()->getVelocityLowerLimit(); + return mAspectProperties.mVelocityLowerLimit; } //============================================================================== @@ -573,7 +599,7 @@ void SingleDofJoint::setVelocityUpperLimit(size_t _index, double _velocity) return; } - getSingleDofJointAspect()->setVelocityUpperLimit(_velocity); + SINGLEDOFJOINT_SET_IF_DIFFERENT( mVelocityUpperLimit, _velocity); } //============================================================================== @@ -585,7 +611,7 @@ double SingleDofJoint::getVelocityUpperLimit(size_t _index) const return 0.0; } - return getSingleDofJointAspect()->getVelocityUpperLimit(); + return mAspectProperties.mVelocityUpperLimit; } //============================================================================== @@ -597,13 +623,13 @@ void SingleDofJoint::resetVelocity(size_t _index) return; } - setVelocityStatic(getSingleDofJointAspect()->getInitialVelocity()); + setVelocityStatic(mAspectProperties.mInitialVelocity); } //============================================================================== void SingleDofJoint::resetVelocities() { - setVelocityStatic(getSingleDofJointAspect()->getInitialVelocity()); + setVelocityStatic(mAspectProperties.mInitialVelocity); } //============================================================================== @@ -615,7 +641,7 @@ void SingleDofJoint::setInitialVelocity(size_t _index, double _initial) return; } - getSingleDofJointAspect()->setInitialVelocity(_initial); + SINGLEDOFJOINT_SET_IF_DIFFERENT( mInitialVelocity, _initial ); } //============================================================================== @@ -627,7 +653,7 @@ double SingleDofJoint::getInitialVelocity(size_t _index) const return 0.0; } - return getSingleDofJointAspect()->getInitialVelocity(); + return mAspectProperties.mInitialVelocity; } //============================================================================== @@ -646,7 +672,7 @@ void SingleDofJoint::setInitialVelocities(const Eigen::VectorXd& _initial) Eigen::VectorXd SingleDofJoint::getInitialVelocities() const { return Eigen::Matrix::Constant( - getSingleDofJointAspect()->getInitialVelocity()); + mAspectProperties.mInitialVelocity); } //============================================================================== @@ -661,7 +687,7 @@ void SingleDofJoint::setAcceleration(size_t _index, double _acceleration) setAccelerationStatic(_acceleration); #if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) - if (mAspectProperties.mActuatorType == ACCELERATION) + if (Joint::mAspectProperties.mActuatorType == ACCELERATION) mCommand = getAccelerationStatic(); // TODO: Remove at DART 5.1. #endif @@ -691,7 +717,7 @@ void SingleDofJoint::setAccelerations(const Eigen::VectorXd& _accelerations) setAccelerationStatic(_accelerations[0]); #if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) - if (mAspectProperties.mActuatorType == ACCELERATION) + if (Joint::mAspectProperties.mActuatorType == ACCELERATION) mCommand = getAccelerationStatic(); // TODO: Remove at DART 5.1. #endif @@ -719,7 +745,7 @@ void SingleDofJoint::setAccelerationLowerLimit(size_t _index, return; } - getSingleDofJointAspect()->setAccelerationLowerLimit(_acceleration); + SINGLEDOFJOINT_SET_IF_DIFFERENT( mAccelerationLowerLimit, _acceleration); } //============================================================================== @@ -731,7 +757,7 @@ double SingleDofJoint::getAccelerationLowerLimit(size_t _index) const return 0.0; } - return getSingleDofJointAspect()->getAccelerationLowerLimit(); + return mAspectProperties.mAccelerationLowerLimit; } //============================================================================== @@ -744,7 +770,7 @@ void SingleDofJoint::setAccelerationUpperLimit(size_t _index, return; } - getSingleDofJointAspect()->setAccelerationUpperLimit(_acceleration); + SINGLEDOFJOINT_SET_IF_DIFFERENT( mAccelerationUpperLimit, _acceleration); } //============================================================================== @@ -756,7 +782,7 @@ double SingleDofJoint::getAccelerationUpperLimit(size_t _index) const return 0.0; } - return getSingleDofJointAspect()->getAccelerationUpperLimit(); + return mAspectProperties.mAccelerationUpperLimit; } //============================================================================== @@ -819,7 +845,7 @@ void SingleDofJoint::setForce(size_t _index, double _force) mForce = _force; #if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) - if (mAspectProperties.mActuatorType == FORCE) + if (Joint::mAspectProperties.mActuatorType == FORCE) mCommand = mForce; // TODO: Remove at DART 5.1. #endif @@ -849,7 +875,7 @@ void SingleDofJoint::setForces(const Eigen::VectorXd& _forces) mForce = _forces[0]; #if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) - if (mAspectProperties.mActuatorType == FORCE) + if (Joint::mAspectProperties.mActuatorType == FORCE) mCommand = mForce; // TODO: Remove at DART 5.1. #endif @@ -867,7 +893,7 @@ void SingleDofJoint::resetForces() mForce = 0.0; #if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) - if (mAspectProperties.mActuatorType == FORCE) + if (Joint::mAspectProperties.mActuatorType == FORCE) mCommand = mForce; // TODO: Remove at DART 5.1. #endif @@ -882,7 +908,7 @@ void SingleDofJoint::setForceLowerLimit(size_t _index, double _force) return; } - getSingleDofJointAspect()->setForceLowerLimit(_force); + SINGLEDOFJOINT_SET_IF_DIFFERENT( mForceLowerLimit, _force ); } //============================================================================== @@ -894,7 +920,7 @@ double SingleDofJoint::getForceLowerLimit(size_t _index) const return 0.0; } - return getSingleDofJointAspect()->getForceLowerLimit(); + return mAspectProperties.mForceLowerLimit; } //============================================================================== @@ -906,7 +932,7 @@ void SingleDofJoint::setForceUpperLimit(size_t _index, double _force) return; } - getSingleDofJointAspect()->setForceUpperLimit(_force); + SINGLEDOFJOINT_SET_IF_DIFFERENT( mForceUpperLimit, _force); } //============================================================================== @@ -918,7 +944,7 @@ double SingleDofJoint::getForceUpperLimit(size_t _index) const return 0.0; } - return getSingleDofJointAspect()->getForceUpperLimit(); + return mAspectProperties.mForceUpperLimit; } //============================================================================== @@ -1029,7 +1055,7 @@ void SingleDofJoint::setSpringStiffness(size_t _index, double _k) assert(_k >= 0.0); - getSingleDofJointAspect()->setSpringStiffness(_k); + SINGLEDOFJOINT_SET_IF_DIFFERENT( mSpringStiffness, _k); } //============================================================================== @@ -1041,7 +1067,7 @@ double SingleDofJoint::getSpringStiffness(size_t _index) const return 0.0; } - return getSingleDofJointAspect()->getSpringStiffness(); + return mAspectProperties.mSpringStiffness; } //============================================================================== @@ -1053,19 +1079,19 @@ void SingleDofJoint::setRestPosition(size_t _index, double _q0) return; } - if (getSingleDofJointAspect()->getPositionLowerLimit() > _q0 - || getSingleDofJointAspect()->getPositionUpperLimit() < _q0) + if (mAspectProperties.mPositionLowerLimit > _q0 + || mAspectProperties.mPositionUpperLimit < _q0) { dtwarn << "[SingleDofJoint::setRestPosition] Value of _q0 [" << _q0 << "] is out of the limit range [" - << getSingleDofJointAspect()->getPositionLowerLimit() << ", " - << getSingleDofJointAspect()->getPositionUpperLimit() + << mAspectProperties.mPositionLowerLimit << ", " + << mAspectProperties.mPositionUpperLimit << "] for index [" << _index << "] of Joint [" << getName() << "].\n"; return; } - getSingleDofJointAspect()->setRestPosition(_q0); + SINGLEDOFJOINT_SET_IF_DIFFERENT( mRestPosition, _q0); } //============================================================================== @@ -1077,7 +1103,7 @@ double SingleDofJoint::getRestPosition(size_t _index) const return 0.0; } - return getSingleDofJointAspect()->getRestPosition(); + return mAspectProperties.mRestPosition; } //============================================================================== @@ -1091,7 +1117,7 @@ void SingleDofJoint::setDampingCoefficient(size_t _index, double _d) assert(_d >= 0.0); - getSingleDofJointAspect()->setDampingCoefficient(_d); + SINGLEDOFJOINT_SET_IF_DIFFERENT( mDampingCoefficient, _d); } //============================================================================== @@ -1103,7 +1129,7 @@ double SingleDofJoint::getDampingCoefficient(size_t _index) const return 0.0; } - return getSingleDofJointAspect()->getDampingCoefficient(); + return mAspectProperties.mDampingCoefficient; } //============================================================================== @@ -1117,7 +1143,7 @@ void SingleDofJoint::setCoulombFriction(size_t _index, double _friction) assert(_friction >= 0.0); - getSingleDofJointAspect()->setFriction(_friction); + SINGLEDOFJOINT_SET_IF_DIFFERENT( mFriction, _friction); } //============================================================================== @@ -1129,16 +1155,16 @@ double SingleDofJoint::getCoulombFriction(size_t _index) const return 0.0; } - return getSingleDofJointAspect()->getFriction(); + return mAspectProperties.mFriction; } //============================================================================== double SingleDofJoint::getPotentialEnergy() const { // Spring energy - double pe = 0.5 * getSingleDofJointAspect()->getSpringStiffness() - * (getPositionStatic() - getSingleDofJointAspect()->getRestPosition()) - * (getPositionStatic() - getSingleDofJointAspect()->getRestPosition()); + double pe = 0.5 * mAspectProperties.mSpringStiffness + * (getPositionStatic() - mAspectProperties.mRestPosition) + * (getPositionStatic() - mAspectProperties.mRestPosition); return pe; } @@ -1176,7 +1202,7 @@ void SingleDofJoint::registerDofs() { SkeletonPtr skel = getSkeleton(); if(skel) - getSingleDofJointAspect()->mProperties.mDofName = + mAspectProperties.mDofName = skel->mNameMgrForDofs.issueNewNameAndAdd(mDof->getName(), mDof); } @@ -1185,7 +1211,7 @@ void SingleDofJoint::updateDegreeOfFreedomNames() { // Same name as the joint it belongs to. if (!mDof->isNamePreserved()) - mDof->setName(mAspectProperties.mName, false); + mDof->setName(Joint::mAspectProperties.mName, false); } //============================================================================== @@ -1317,7 +1343,7 @@ void SingleDofJoint::addVelocityChangeTo(Eigen::Vector6d& _velocityChange) void SingleDofJoint::addChildArtInertiaTo( Eigen::Matrix6d& _parentArtInertia, const Eigen::Matrix6d& _childArtInertia) { - switch (mAspectProperties.mActuatorType) + switch (Joint::mAspectProperties.mActuatorType) { case FORCE: case PASSIVE: @@ -1366,7 +1392,7 @@ void SingleDofJoint::addChildArtInertiaToKinematic( void SingleDofJoint::addChildArtInertiaImplicitTo( Eigen::Matrix6d& _parentArtInertia, const Eigen::Matrix6d& _childArtInertia) { - switch (mAspectProperties.mActuatorType) + switch (Joint::mAspectProperties.mActuatorType) { case FORCE: case PASSIVE: @@ -1415,7 +1441,7 @@ void SingleDofJoint::addChildArtInertiaImplicitToKinematic( void SingleDofJoint::updateInvProjArtInertia( const Eigen::Matrix6d& _artInertia) { - switch (mAspectProperties.mActuatorType) + switch (Joint::mAspectProperties.mActuatorType) { case FORCE: case PASSIVE: @@ -1460,7 +1486,7 @@ void SingleDofJoint::updateInvProjArtInertiaImplicit( const Eigen::Matrix6d& _artInertia, double _timeStep) { - switch (mAspectProperties.mActuatorType) + switch (Joint::mAspectProperties.mActuatorType) { case FORCE: case PASSIVE: @@ -1488,8 +1514,8 @@ void SingleDofJoint::updateInvProjArtInertiaImplicitDynamic( double projAI = Jacobian.dot(_artInertia * Jacobian); // Add additional inertia for implicit damping and spring force - projAI += _timeStep * getSingleDofJointAspect()->getDampingCoefficient() - + _timeStep * _timeStep * getSingleDofJointAspect()->getSpringStiffness(); + projAI += _timeStep * mAspectProperties.mDampingCoefficient + + _timeStep * _timeStep * mAspectProperties.mSpringStiffness; // Inversion of the projected articulated inertia for implicit damping and // spring force @@ -1513,7 +1539,7 @@ void SingleDofJoint::addChildBiasForceTo( const Eigen::Vector6d& _childBiasForce, const Eigen::Vector6d& _childPartialAcc) { - switch (mAspectProperties.mActuatorType) + switch (Joint::mAspectProperties.mActuatorType) { case FORCE: case PASSIVE: @@ -1584,7 +1610,7 @@ void SingleDofJoint::addChildBiasImpulseTo( const Eigen::Matrix6d& _childArtInertia, const Eigen::Vector6d& _childBiasImpulse) { - switch (mAspectProperties.mActuatorType) + switch (Joint::mAspectProperties.mActuatorType) { case FORCE: case PASSIVE: @@ -1643,7 +1669,7 @@ void SingleDofJoint::updateTotalForce(const Eigen::Vector6d& _bodyForce, { assert(_timeStep > 0.0); - switch (mAspectProperties.mActuatorType) + switch (Joint::mAspectProperties.mActuatorType) { case FORCE: mForce = mCommand; @@ -1681,12 +1707,12 @@ void SingleDofJoint::updateTotalForceDynamic( const double nextPosition = getPositionStatic() + _timeStep*getVelocityStatic(); const double springForce = - -getSingleDofJointAspect()->getSpringStiffness() - * (nextPosition - getSingleDofJointAspect()->getRestPosition()); + -mAspectProperties.mSpringStiffness + * (nextPosition - mAspectProperties.mRestPosition); // Damping force const double dampingForce = - -getSingleDofJointAspect()->getDampingCoefficient() * getVelocityStatic(); + -mAspectProperties.mDampingCoefficient * getVelocityStatic(); // Compute alpha mTotalForce = mForce + springForce + dampingForce @@ -1703,7 +1729,7 @@ void SingleDofJoint::updateTotalForceKinematic( //============================================================================== void SingleDofJoint::updateTotalImpulse(const Eigen::Vector6d& _bodyImpulse) { - switch (mAspectProperties.mActuatorType) + switch (Joint::mAspectProperties.mActuatorType) { case FORCE: case PASSIVE: @@ -1745,7 +1771,7 @@ void SingleDofJoint::resetTotalImpulses() void SingleDofJoint::updateAcceleration(const Eigen::Matrix6d& _artInertia, const Eigen::Vector6d& _spatialAcc) { - switch (mAspectProperties.mActuatorType) + switch (Joint::mAspectProperties.mActuatorType) { case FORCE: case PASSIVE: @@ -1789,7 +1815,7 @@ void SingleDofJoint::updateVelocityChange( const Eigen::Matrix6d& _artInertia, const Eigen::Vector6d& _velocityChange) { - switch (mAspectProperties.mActuatorType) + switch (Joint::mAspectProperties.mActuatorType) { case FORCE: case PASSIVE: @@ -1842,7 +1868,7 @@ void SingleDofJoint::updateForceID(const Eigen::Vector6d& _bodyForce, if (_withDampingForces) { const double dampingForce = - -getSingleDofJointAspect()->getDampingCoefficient() * getVelocityStatic(); + -mAspectProperties.mDampingCoefficient * getVelocityStatic(); mForce -= dampingForce; } @@ -1852,8 +1878,8 @@ void SingleDofJoint::updateForceID(const Eigen::Vector6d& _bodyForce, const double nextPosition = getPositionStatic() + _timeStep*getVelocityStatic(); const double springForce = - -getSingleDofJointAspect()->getSpringStiffness() - *(nextPosition - getSingleDofJointAspect()->getRestPosition()); + -mAspectProperties.mSpringStiffness + *(nextPosition - mAspectProperties.mRestPosition); mForce -= springForce; } } @@ -1864,7 +1890,7 @@ void SingleDofJoint::updateForceFD(const Eigen::Vector6d& _bodyForce, bool _withDampingForces, bool _withSpringForces) { - switch (mAspectProperties.mActuatorType) + switch (Joint::mAspectProperties.mActuatorType) { case FORCE: case PASSIVE: @@ -1891,7 +1917,7 @@ void SingleDofJoint::updateImpulseID(const Eigen::Vector6d& _bodyImpulse) //============================================================================== void SingleDofJoint::updateImpulseFD(const Eigen::Vector6d& _bodyImpulse) { - switch (mAspectProperties.mActuatorType) + switch (Joint::mAspectProperties.mActuatorType) { case FORCE: case PASSIVE: @@ -1911,7 +1937,7 @@ void SingleDofJoint::updateImpulseFD(const Eigen::Vector6d& _bodyImpulse) //============================================================================== void SingleDofJoint::updateConstrainedTerms(double _timeStep) { - switch (mAspectProperties.mActuatorType) + switch (Joint::mAspectProperties.mActuatorType) { case FORCE: case PASSIVE: diff --git a/dart/dynamics/SingleDofJoint.h b/dart/dynamics/SingleDofJoint.h index f643a8f85ca92..a432c15902960 100644 --- a/dart/dynamics/SingleDofJoint.h +++ b/dart/dynamics/SingleDofJoint.h @@ -56,7 +56,6 @@ class SingleDofJoint : public detail::SingleDofJointBase using UniqueProperties = detail::SingleDofJointUniqueProperties; using Properties = detail::SingleDofJointProperties; - using Aspect = detail::SingleDofJointAspect; DART_BAKE_SPECIALIZED_ASPECT_IRREGULAR(Aspect, SingleDofJointAspect) @@ -69,6 +68,9 @@ class SingleDofJoint : public detail::SingleDofJointBase /// Set the Properties of this SingleDofJoint void setProperties(const UniqueProperties& _properties); + /// Set the AspectProperties of this SingleDofJoint + void setAspectProperties(const AspectProperties& properties); + /// Get the Properties of this SingleDofJoint Properties getSingleDofJointProperties() const; @@ -397,8 +399,6 @@ class SingleDofJoint : public detail::SingleDofJointBase // Documentation inherited virtual Eigen::Vector6d getBodyConstraintWrench() const override; - template friend void detail::JointPropertyUpdate(AspectType*); - protected: /// Constructor called inheriting classes diff --git a/dart/dynamics/TranslationalJoint.cpp b/dart/dynamics/TranslationalJoint.cpp index cc110867a3853..70dbfd9103535 100644 --- a/dart/dynamics/TranslationalJoint.cpp +++ b/dart/dynamics/TranslationalJoint.cpp @@ -111,19 +111,19 @@ bool TranslationalJoint::isCyclic(size_t /*_index*/) const void TranslationalJoint::updateDegreeOfFreedomNames() { if(!mDofs[0]->isNamePreserved()) - mDofs[0]->setName(mAspectProperties.mName + "_x", false); + mDofs[0]->setName(Joint::mAspectProperties.mName + "_x", false); if(!mDofs[1]->isNamePreserved()) - mDofs[1]->setName(mAspectProperties.mName + "_y", false); + mDofs[1]->setName(Joint::mAspectProperties.mName + "_y", false); if(!mDofs[2]->isNamePreserved()) - mDofs[2]->setName(mAspectProperties.mName + "_z", false); + mDofs[2]->setName(Joint::mAspectProperties.mName + "_z", false); } //============================================================================== void TranslationalJoint::updateLocalTransform() const { - mT = mAspectProperties.mT_ParentBodyToJoint + mT = Joint::mAspectProperties.mT_ParentBodyToJoint * Eigen::Translation3d(getPositionsStatic()) - * mAspectProperties.mT_ChildBodyToJoint.inverse(); + * Joint::mAspectProperties.mT_ChildBodyToJoint.inverse(); // Verification assert(math::verifyTransform(mT)); @@ -134,7 +134,7 @@ void TranslationalJoint::updateLocalJacobian(bool _mandatory) const { if (_mandatory) { - mJacobian.bottomRows<3>() = mAspectProperties.mT_ChildBodyToJoint.linear(); + mJacobian.bottomRows<3>() = Joint::mAspectProperties.mT_ChildBodyToJoint.linear(); // Verification assert(mJacobian.topRows<3>() == Eigen::Matrix3d::Zero()); diff --git a/dart/dynamics/UniversalJoint.cpp b/dart/dynamics/UniversalJoint.cpp index 345d02f923571..be12e38ce8fb9 100644 --- a/dart/dynamics/UniversalJoint.cpp +++ b/dart/dynamics/UniversalJoint.cpp @@ -146,9 +146,9 @@ Eigen::Matrix UniversalJoint::getLocalJacobianStatic( { Eigen::Matrix J; J.col(0) = math::AdTAngular( - mAspectProperties.mT_ChildBodyToJoint + Joint::mAspectProperties.mT_ChildBodyToJoint * math::expAngular(-getAxis2() * _positions[1]), getAxis1()); - J.col(1) = math::AdTAngular(mAspectProperties.mT_ChildBodyToJoint, getAxis2()); + J.col(1) = math::AdTAngular(Joint::mAspectProperties.mT_ChildBodyToJoint, getAxis2()); assert(!math::isNan(J)); return J; } @@ -174,19 +174,19 @@ Joint* UniversalJoint::clone() const void UniversalJoint::updateDegreeOfFreedomNames() { if(!mDofs[0]->isNamePreserved()) - mDofs[0]->setName(mAspectProperties.mName + "_1", false); + mDofs[0]->setName(Joint::mAspectProperties.mName + "_1", false); if(!mDofs[1]->isNamePreserved()) - mDofs[1]->setName(mAspectProperties.mName + "_2", false); + mDofs[1]->setName(Joint::mAspectProperties.mName + "_2", false); } //============================================================================== void UniversalJoint::updateLocalTransform() const { const Eigen::Vector2d& positions = getPositionsStatic(); - mT = mAspectProperties.mT_ParentBodyToJoint + mT = Joint::mAspectProperties.mT_ParentBodyToJoint * Eigen::AngleAxisd(positions[0], getAxis1()) * Eigen::AngleAxisd(positions[1], getAxis2()) - * mAspectProperties.mT_ChildBodyToJoint.inverse(); + * Joint::mAspectProperties.mT_ChildBodyToJoint.inverse(); assert(math::verifyTransform(mT)); } @@ -206,7 +206,7 @@ void UniversalJoint::updateLocalJacobianTimeDeriv() const -getAxis2() * getPositionsStatic()[1]); Eigen::Vector6d tmpV2 - = math::AdTAngular(mAspectProperties.mT_ChildBodyToJoint * tmpT, getAxis1()); + = math::AdTAngular(Joint::mAspectProperties.mT_ChildBodyToJoint * tmpT, getAxis1()); mJacobianDeriv.col(0) = -math::ad(tmpV1, tmpV2); diff --git a/dart/dynamics/WeldJoint.cpp b/dart/dynamics/WeldJoint.cpp index be3b93aa235c0..9abb2ca66d2a8 100644 --- a/dart/dynamics/WeldJoint.cpp +++ b/dart/dynamics/WeldJoint.cpp @@ -87,7 +87,7 @@ void WeldJoint::setTransformFromParentBodyNode(const Eigen::Isometry3d& _T) { Joint::setTransformFromParentBodyNode(_T); - mT = mAspectProperties.mT_ParentBodyToJoint * mAspectProperties.mT_ChildBodyToJoint.inverse(); + mT = Joint::mAspectProperties.mT_ParentBodyToJoint * Joint::mAspectProperties.mT_ChildBodyToJoint.inverse(); } //============================================================================== @@ -95,7 +95,7 @@ void WeldJoint::setTransformFromChildBodyNode(const Eigen::Isometry3d& _T) { Joint::setTransformFromChildBodyNode(_T); - mT = mAspectProperties.mT_ParentBodyToJoint * mAspectProperties.mT_ChildBodyToJoint.inverse(); + mT = Joint::mAspectProperties.mT_ParentBodyToJoint * Joint::mAspectProperties.mT_ChildBodyToJoint.inverse(); } //============================================================================== diff --git a/dart/dynamics/detail/PrismaticJointProperties.cpp b/dart/dynamics/detail/PrismaticJointProperties.cpp index 162d8da26e291..667f155eec1f7 100644 --- a/dart/dynamics/detail/PrismaticJointProperties.cpp +++ b/dart/dynamics/detail/PrismaticJointProperties.cpp @@ -58,19 +58,6 @@ PrismaticJointProperties::PrismaticJointProperties( // Do nothing } -//============================================================================== -void PrismaticJointAspect::setAxis(const Eigen::Vector3d& _axis) -{ - mProperties.mAxis = _axis.normalized(); - notifyPropertiesUpdate(); -} - -//============================================================================== -const Eigen::Vector3d& PrismaticJointAspect::getAxis() const -{ - return mProperties.mAxis; -} - } // namespace detail } // namespace dynamics } // namespace dart diff --git a/dart/dynamics/detail/PrismaticJointProperties.h b/dart/dynamics/detail/PrismaticJointProperties.h index 01b20eba67be3..a3646d2c705f8 100644 --- a/dart/dynamics/detail/PrismaticJointProperties.h +++ b/dart/dynamics/detail/PrismaticJointProperties.h @@ -76,21 +76,8 @@ struct PrismaticJointProperties : }; //============================================================================== -class PrismaticJointAspect final : - public common::AspectWithVersionedProperties< - PrismaticJointAspect, PrismaticJointUniqueProperties, PrismaticJoint, - detail::JointPropertyUpdate > -{ -public: - DART_COMMON_JOINT_ASPECT_CONSTRUCTOR( PrismaticJointAspect ) - - void setAxis(const Eigen::Vector3d& _axis); - const Eigen::Vector3d& getAxis() const; -}; - -//============================================================================== -using PrismaticJointBase = common::CompositeJoiner< - SingleDofJoint, common::RequiresAspect >; +using PrismaticJointBase = common::EmbedPropertiesOnTopOf< + PrismaticJoint, PrismaticJointUniqueProperties, SingleDofJoint>; } // namespace detail } // namespace dynamics diff --git a/dart/dynamics/detail/RevoluteJointProperties.cpp b/dart/dynamics/detail/RevoluteJointProperties.cpp index 90a31ae2b3742..59de65ac26741 100644 --- a/dart/dynamics/detail/RevoluteJointProperties.cpp +++ b/dart/dynamics/detail/RevoluteJointProperties.cpp @@ -58,19 +58,6 @@ RevoluteJointProperties::RevoluteJointProperties( // Do nothing } -//============================================================================== -void RevoluteJointAspect::setAxis(const Eigen::Vector3d& _axis) -{ - mProperties.mAxis = _axis.normalized(); - notifyPropertiesUpdate(); -} - -//============================================================================== -const Eigen::Vector3d& RevoluteJointAspect::getAxis() const -{ - return mProperties.mAxis; -} - } // namespace detail } // namespace dynamics } // namespace dart diff --git a/dart/dynamics/detail/RevoluteJointProperties.h b/dart/dynamics/detail/RevoluteJointProperties.h index f4d8ba924b9e5..8e1aa62fe8b04 100644 --- a/dart/dynamics/detail/RevoluteJointProperties.h +++ b/dart/dynamics/detail/RevoluteJointProperties.h @@ -76,21 +76,8 @@ struct RevoluteJointProperties : }; //============================================================================== -class RevoluteJointAspect final : - public common::AspectWithVersionedProperties< - RevoluteJointAspect, RevoluteJointUniqueProperties, RevoluteJoint, - detail::JointPropertyUpdate > -{ -public: - DART_COMMON_JOINT_ASPECT_CONSTRUCTOR( RevoluteJointAspect ) - - void setAxis(const Eigen::Vector3d& _axis); - const Eigen::Vector3d& getAxis() const; -}; - -//============================================================================== -using RevoluteJointBase = common::CompositeJoiner< - SingleDofJoint, common::RequiresAspect >; +using RevoluteJointBase = common::EmbedPropertiesOnTopOf< + RevoluteJoint, RevoluteJointUniqueProperties, SingleDofJoint>; } // namespace detail diff --git a/dart/dynamics/detail/ScrewJointProperties.cpp b/dart/dynamics/detail/ScrewJointProperties.cpp index c9b422d56bfa2..9292f236b64ea 100644 --- a/dart/dynamics/detail/ScrewJointProperties.cpp +++ b/dart/dynamics/detail/ScrewJointProperties.cpp @@ -59,19 +59,6 @@ ScrewJointProperties::ScrewJointProperties( // Do nothing } -//============================================================================== -void ScrewJointAspect::setAxis(const Eigen::Vector3d& _axis) -{ - mProperties.mAxis = _axis.normalized(); - notifyPropertiesUpdate(); -} - -//============================================================================== -const Eigen::Vector3d& ScrewJointAspect::getAxis() const -{ - return mProperties.mAxis; -} - } // namespace detail } // namespace dynamics } // namespace dart diff --git a/dart/dynamics/detail/ScrewJointProperties.h b/dart/dynamics/detail/ScrewJointProperties.h index 460c46fd180f9..592ef0b38255d 100644 --- a/dart/dynamics/detail/ScrewJointProperties.h +++ b/dart/dynamics/detail/ScrewJointProperties.h @@ -80,23 +80,8 @@ struct ScrewJointProperties : SingleDofJoint::Properties, }; //============================================================================== -class ScrewJointAspect final : - public common::AspectWithVersionedProperties< - ScrewJointAspect, ScrewJointUniqueProperties, ScrewJoint, - detail::JointPropertyUpdate > -{ -public: - DART_COMMON_JOINT_ASPECT_CONSTRUCTOR( ScrewJointAspect ) - - void setAxis(const Eigen::Vector3d& _axis); - const Eigen::Vector3d& getAxis() const; - - DART_COMMON_SET_GET_ASPECT_PROPERTY(double, Pitch) -}; - -//============================================================================== -using ScrewJointBase = common::CompositeJoiner< - SingleDofJoint, common::RequiresAspect >; +using ScrewJointBase = common::EmbedPropertiesOnTopOf< + ScrewJoint, ScrewJointUniqueProperties, SingleDofJoint>; } // namespace detail } // namespace dynamics diff --git a/dart/dynamics/detail/SingleDofJointProperties.cpp b/dart/dynamics/detail/SingleDofJointProperties.cpp index 65519904eefd0..85c085c539dcd 100644 --- a/dart/dynamics/detail/SingleDofJointProperties.cpp +++ b/dart/dynamics/detail/SingleDofJointProperties.cpp @@ -86,13 +86,6 @@ SingleDofJointProperties::SingleDofJointProperties( // Do nothing } -//============================================================================== -const std::string& SingleDofJointAspect::setDofName( - const std::string& name, bool preserveName) -{ - return getComposite()->setDofName(0, name, preserveName); -} - } // namespace detail } // namespace dynamics } // namespace dart diff --git a/dart/dynamics/detail/SingleDofJointProperties.h b/dart/dynamics/detail/SingleDofJointProperties.h index 52cc985f2109e..ceb297fe6e30f 100644 --- a/dart/dynamics/detail/SingleDofJointProperties.h +++ b/dart/dynamics/detail/SingleDofJointProperties.h @@ -136,40 +136,8 @@ struct SingleDofJointProperties : }; //============================================================================== -class SingleDofJointAspect final : - public common::AspectWithVersionedProperties< - SingleDofJointAspect, SingleDofJointUniqueProperties, - SingleDofJoint, common::detail::NoOp> -{ -public: - DART_COMMON_ASPECT_PROPERTY_CONSTRUCTOR( SingleDofJointAspect, &common::detail::NoOp ) - - DART_COMMON_SET_GET_ASPECT_PROPERTY(double, PositionLowerLimit) - DART_COMMON_SET_GET_ASPECT_PROPERTY(double, PositionUpperLimit) - DART_COMMON_SET_GET_ASPECT_PROPERTY(double, InitialPosition) - DART_COMMON_SET_GET_ASPECT_PROPERTY(double, VelocityLowerLimit) - DART_COMMON_SET_GET_ASPECT_PROPERTY(double, VelocityUpperLimit) - DART_COMMON_SET_GET_ASPECT_PROPERTY(double, InitialVelocity) - DART_COMMON_SET_GET_ASPECT_PROPERTY(double, AccelerationLowerLimit) - DART_COMMON_SET_GET_ASPECT_PROPERTY(double, AccelerationUpperLimit) - DART_COMMON_SET_GET_ASPECT_PROPERTY(double, ForceLowerLimit) - DART_COMMON_SET_GET_ASPECT_PROPERTY(double, ForceUpperLimit) - DART_COMMON_SET_GET_ASPECT_PROPERTY(double, SpringStiffness) - DART_COMMON_SET_GET_ASPECT_PROPERTY(double, RestPosition) - DART_COMMON_SET_GET_ASPECT_PROPERTY(double, DampingCoefficient) - DART_COMMON_SET_GET_ASPECT_PROPERTY(double, Friction) - DART_COMMON_SET_GET_ASPECT_PROPERTY(bool, PreserveDofName) - - const std::string& setDofName(const std::string& name, - bool preserveName = true); - DART_COMMON_GET_ASPECT_PROPERTY(std::string, DofName) - - friend class dart::dynamics::SingleDofJoint; -}; - -//============================================================================== -using SingleDofJointBase = common::CompositeJoiner< - Joint, common::RequiresAspect >; +using SingleDofJointBase = common::EmbedPropertiesOnTopOf< + SingleDofJoint, SingleDofJointUniqueProperties, Joint>; } // namespace detail } // namespace dynamics From b6cd7823d42f1c1c532b6d45941ab6fa15abe4d5 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Wed, 30 Mar 2016 14:02:13 -0400 Subject: [PATCH 20/62] creating tests to ensure structural updates happen correctly --- dart/dynamics/PrismaticJoint.cpp | 1 + dart/dynamics/RevoluteJoint.cpp | 1 + dart/dynamics/ScrewJoint.cpp | 2 ++ dart/dynamics/SingleDofJoint.h | 20 +++++++------- unittests/testSkeleton.cpp | 47 ++++++++++++++++++++++++++++++++ 5 files changed, 61 insertions(+), 10 deletions(-) diff --git a/dart/dynamics/PrismaticJoint.cpp b/dart/dynamics/PrismaticJoint.cpp index a95d0ec662223..975422c44546d 100644 --- a/dart/dynamics/PrismaticJoint.cpp +++ b/dart/dynamics/PrismaticJoint.cpp @@ -129,6 +129,7 @@ void PrismaticJoint::setAxis(const Eigen::Vector3d& _axis) mAspectProperties.mAxis = _axis.normalized(); Joint::notifyPositionUpdate(); + updateLocalJacobian(); Joint::incrementVersion(); } diff --git a/dart/dynamics/RevoluteJoint.cpp b/dart/dynamics/RevoluteJoint.cpp index ae9ab16ba8c1d..6acfdf87012b3 100644 --- a/dart/dynamics/RevoluteJoint.cpp +++ b/dart/dynamics/RevoluteJoint.cpp @@ -130,6 +130,7 @@ void RevoluteJoint::setAxis(const Eigen::Vector3d& _axis) mAspectProperties.mAxis = _axis.normalized(); Joint::notifyPositionUpdate(); + updateLocalJacobian(); Joint::incrementVersion(); } diff --git a/dart/dynamics/ScrewJoint.cpp b/dart/dynamics/ScrewJoint.cpp index bc38b09224dc5..fdf793b31c4fd 100644 --- a/dart/dynamics/ScrewJoint.cpp +++ b/dart/dynamics/ScrewJoint.cpp @@ -130,6 +130,7 @@ void ScrewJoint::setAxis(const Eigen::Vector3d& _axis) mAspectProperties.mAxis = _axis.normalized(); Joint::notifyPositionUpdate(); + updateLocalJacobian(); Joint::incrementVersion(); } @@ -147,6 +148,7 @@ void ScrewJoint::setPitch(double _pitch) mAspectProperties.mPitch = _pitch; Joint::notifyPositionUpdate(); + updateLocalJacobian(); Joint::incrementVersion(); } diff --git a/dart/dynamics/SingleDofJoint.h b/dart/dynamics/SingleDofJoint.h index a432c15902960..08d5a979b06d1 100644 --- a/dart/dynamics/SingleDofJoint.h +++ b/dart/dynamics/SingleDofJoint.h @@ -396,6 +396,16 @@ class SingleDofJoint : public detail::SingleDofJointBase /// Get potential energy virtual double getPotentialEnergy() const override; + // Documentation inherited + const math::Jacobian getLocalJacobian() const override; + + // Documentation inherited + math::Jacobian getLocalJacobian( + const Eigen::VectorXd& _positions) const override; + + // Documentation inherited + const math::Jacobian getLocalJacobianTimeDeriv() const override; + // Documentation inherited virtual Eigen::Vector6d getBodyConstraintWrench() const override; @@ -426,19 +436,9 @@ class SingleDofJoint : public detail::SingleDofJointBase // Documentation inherited void updateLocalPrimaryAcceleration() const override; - // Documentation inherited - const math::Jacobian getLocalJacobian() const override; - /// Fixed-size version of getLocalJacobian() const Eigen::Vector6d& getLocalJacobianStatic() const; - // Documentation inherited - math::Jacobian getLocalJacobian( - const Eigen::VectorXd& _positions) const override; - - // Documentation inherited - const math::Jacobian getLocalJacobianTimeDeriv() const override; - /// Fixed-size version of getLocalJacobianTimeDeriv() const Eigen::Vector6d& getLocalJacobianTimeDerivStatic() const; diff --git a/unittests/testSkeleton.cpp b/unittests/testSkeleton.cpp index 443937f83b87c..91c3d8f89efcd 100644 --- a/unittests/testSkeleton.cpp +++ b/unittests/testSkeleton.cpp @@ -1217,6 +1217,53 @@ TEST(Skeleton, LinearJacobianDerivOverload) linkage->getLinearJacobianDeriv(linkage->getBodyNode(0)); } +TEST(Skeleton, Updating) +{ + // Make sure that structural properties get automatically updated correctly + + // RevoluteJoint + SkeletonPtr skeleton = createTwoLinkRobot(Vector3d::Ones(), DOF_PITCH, + Vector3d::Ones(), DOF_ROLL); + + Joint* joint0 = skeleton->getJoint(0); + Joint* joint1 = skeleton->getJoint(1); + + math::Jacobian J0i = joint0->getLocalJacobian(); + joint0->get()->setProperties( + joint1->get()->getProperties()); + + math::Jacobian J0f = joint0->getLocalJacobian(); + EXPECT_FALSE(equals(J0i, J0f)); + + // PrismaticJoint + skeleton = createTwoLinkRobot(Vector3d::Ones(), DOF_X, + Vector3d::Ones(), DOF_Y); + joint0 = skeleton->getJoint(0); + joint1 = skeleton->getJoint(1); + + J0i = joint0->getLocalJacobian(); + joint0->get()->setProperties( + joint1->get()->getProperties()); + J0f = joint0->getLocalJacobian(); + EXPECT_FALSE(equals(J0i, J0f)); + + skeleton = Skeleton::create(); + ScrewJoint* screw = skeleton->createJointAndBodyNodePair().first; + + screw->setAxis(Eigen::Vector3d::UnitX()); + screw->setPitch(2); + + J0i = screw->getLocalJacobian(); + screw->setAxis(Eigen::Vector3d::UnitY()); + J0f = screw->getLocalJacobian(); + EXPECT_FALSE(equals(J0i, J0f)); + + J0i = J0f; + screw->setPitch(3); + J0f = screw->getLocalJacobian(); + EXPECT_FALSE(equals(J0i, J0f)); +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv); From 8cff9c0232ecd5b6aab64da8ccee66321ae0335e Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Wed, 30 Mar 2016 14:53:12 -0400 Subject: [PATCH 21/62] finished embedding aspect into MultiDofJoint --- dart/dynamics/MultiDofJoint.h | 14 +- dart/dynamics/detail/MultiDofJoint.h | 195 ++++++++++-------- .../dynamics/detail/MultiDofJointProperties.h | 87 +------- 3 files changed, 114 insertions(+), 182 deletions(-) diff --git a/dart/dynamics/MultiDofJoint.h b/dart/dynamics/MultiDofJoint.h index 025e49ff11a67..fdcee1fa56420 100644 --- a/dart/dynamics/MultiDofJoint.h +++ b/dart/dynamics/MultiDofJoint.h @@ -58,21 +58,16 @@ class Skeleton; /// class MultiDofJoint template -class MultiDofJoint : - public common::CompositeJoiner>> > +class MultiDofJoint : public detail::MultiDofJointBase< MultiDofJoint, DOF > { public: - using Base = common::CompositeJoiner>> >; - constexpr static size_t NumDofs = DOF; using Vector = Eigen::Matrix; - + using Base = detail::MultiDofJointBase, DOF>; using UniqueProperties = detail::MultiDofJointUniqueProperties; using Properties = detail::MultiDofJointProperties; - using Aspect = detail::MultiDofJointAspect; + using AspectProperties = typename Base::AspectProperties; DART_BAKE_SPECIALIZED_ASPECT_IRREGULAR( typename MultiDofJoint::Aspect, MultiDofJointAspect ) @@ -87,6 +82,9 @@ class MultiDofJoint : /// Set the Properties of this MultiDofJoint void setProperties(const UniqueProperties& _properties); + /// Set the AspectProperties of this MultiDofJoint + void setAspectProperties(const AspectProperties& properties); + /// Get the Properties of this MultiDofJoint Properties getMultiDofJointProperties() const; diff --git a/dart/dynamics/detail/MultiDofJoint.h b/dart/dynamics/detail/MultiDofJoint.h index dc8099c2d23eb..8d07b9b6a1cda 100644 --- a/dart/dynamics/detail/MultiDofJoint.h +++ b/dart/dynamics/detail/MultiDofJoint.h @@ -58,6 +58,12 @@ << this->getName() << "].\n";\ assert(false); +#define MULTIDOFJOINT_SET_IF_DIFFERENT( mField, value )\ + if( value == Base::mAspectProperties. mField )\ + return;\ + Base::mAspectProperties. mField = value;\ + Joint::incrementVersion(); + namespace dart { namespace dynamics { @@ -90,24 +96,31 @@ void MultiDofJoint::setProperties(const Properties& _properties) //============================================================================== template void MultiDofJoint::setProperties(const UniqueProperties& _properties) +{ + setAspectProperties(_properties); +} + +//============================================================================== +template +void MultiDofJoint::setAspectProperties(const AspectProperties& properties) { for(size_t i=0; i::Properties MultiDofJoint::getMultiDofJointProperties() const { return MultiDofJoint::Properties( - Joint::mAspectProperties, getMultiDofJointAspect()->getProperties()); + Joint::mAspectProperties, Base::mAspectProperties); } //============================================================================== @@ -189,7 +202,7 @@ const std::string& MultiDofJoint::setDofName(size_t _index, } preserveDofName(_index, _preserveName); - std::string& dofName = getMultiDofJointAspect()->_getDofNameReference(_index); + std::string& dofName = Base::mAspectProperties.mDofNames[_index]; if(_name == dofName) return dofName; @@ -216,7 +229,7 @@ void MultiDofJoint::preserveDofName(size_t _index, bool _preserve) return; } - getMultiDofJointAspect()->setPreserveDofName(_index, _preserve); + MULTIDOFJOINT_SET_IF_DIFFERENT( mPreserveDofNames[_index], _preserve ); } //============================================================================== @@ -229,7 +242,7 @@ bool MultiDofJoint::isDofNamePreserved(size_t _index) const _index = 0; } - return getMultiDofJointAspect()->getPreserveDofName(_index); + return Base::mAspectProperties.mPreserveDofNames[_index]; } //============================================================================== @@ -242,10 +255,10 @@ const std::string& MultiDofJoint::getDofName(size_t _index) const << _index << "] in Joint [" << this->getName() << "], but that is " << "out of bounds (max " << DOF-1 << "). Returning name of DOF 0.\n"; assert(false); - return getMultiDofJointAspect()->getDofName(0); + return Base::mAspectProperties.mDofNames[0]; } - return getMultiDofJointAspect()->getDofName(_index); + return Base::mAspectProperties.mDofNames[_index]; } //============================================================================== @@ -294,8 +307,8 @@ void MultiDofJoint::setCommand(size_t _index, double command) { case Joint::FORCE: mCommands[_index] = math::clip(command, - getMultiDofJointAspect()->getForceLowerLimit(_index), - getMultiDofJointAspect()->getForceUpperLimit(_index)); + Base::mAspectProperties.mForceLowerLimits[_index], + Base::mAspectProperties.mForceUpperLimits[_index]); break; case Joint::PASSIVE: if(0.0 != command) @@ -308,18 +321,18 @@ void MultiDofJoint::setCommand(size_t _index, double command) break; case Joint::SERVO: mCommands[_index] = math::clip(command, - getMultiDofJointAspect()->getVelocityLowerLimit(_index), - getMultiDofJointAspect()->getVelocityUpperLimit(_index)); + Base::mAspectProperties.mVelocityLowerLimits[_index], + Base::mAspectProperties.mVelocityUpperLimits[_index]); break; case Joint::ACCELERATION: mCommands[_index] = math::clip(command, - getMultiDofJointAspect()->getAccelerationLowerLimit(_index), - getMultiDofJointAspect()->getAccelerationUpperLimit(_index)); + Base::mAspectProperties.mAccelerationLowerLimits[_index], + Base::mAspectProperties.mAccelerationUpperLimits[_index]); break; case Joint::VELOCITY: mCommands[_index] = math::clip(command, - getMultiDofJointAspect()->getVelocityLowerLimit(_index), - getMultiDofJointAspect()->getVelocityUpperLimit(_index)); + Base::mAspectProperties.mVelocityLowerLimits[_index], + Base::mAspectProperties.mVelocityUpperLimits[_index]); // TODO: This possibly makes the acceleration to exceed the limits. break; case Joint::LOCKED: @@ -364,8 +377,8 @@ void MultiDofJoint::setCommands(const Eigen::VectorXd& _commands) { case Joint::FORCE: mCommands = math::clip(_commands, - getMultiDofJointAspect()->getForceLowerLimits(), - getMultiDofJointAspect()->getForceUpperLimits()); + Base::mAspectProperties.mForceLowerLimits, + Base::mAspectProperties.mForceUpperLimits); break; case Joint::PASSIVE: if(Vector::Zero() != _commands) @@ -378,18 +391,18 @@ void MultiDofJoint::setCommands(const Eigen::VectorXd& _commands) break; case Joint::SERVO: mCommands = math::clip(_commands, - getMultiDofJointAspect()->getVelocityLowerLimits(), - getMultiDofJointAspect()->getVelocityUpperLimits()); + Base::mAspectProperties.mVelocityLowerLimits, + Base::mAspectProperties.mVelocityUpperLimits); break; case Joint::ACCELERATION: mCommands = math::clip(_commands, - getMultiDofJointAspect()->getAccelerationLowerLimits(), - getMultiDofJointAspect()->getAccelerationUpperLimits()); + Base::mAspectProperties.mAccelerationLowerLimits, + Base::mAspectProperties.mAccelerationUpperLimits); break; case Joint::VELOCITY: mCommands = math::clip(_commands, - getMultiDofJointAspect()->getVelocityLowerLimits(), - getMultiDofJointAspect()->getVelocityUpperLimits()); + Base::mAspectProperties.mVelocityLowerLimits, + Base::mAspectProperties.mVelocityUpperLimits); // TODO: This possibly makes the acceleration to exceed the limits. break; case Joint::LOCKED: @@ -482,7 +495,7 @@ void MultiDofJoint::setPositionLowerLimit(size_t _index, double _position) return; } - getMultiDofJointAspect()->setPositionLowerLimit(_index, _position); + MULTIDOFJOINT_SET_IF_DIFFERENT( mPositionLowerLimits[_index], _position ); } //============================================================================== @@ -495,7 +508,7 @@ double MultiDofJoint::getPositionLowerLimit(size_t _index) const return 0.0; } - return getMultiDofJointAspect()->getPositionLowerLimit(_index); + return Base::mAspectProperties.mPositionLowerLimits[_index]; } //============================================================================== @@ -508,7 +521,7 @@ void MultiDofJoint::setPositionUpperLimit(size_t _index, double _position) return; } - getMultiDofJointAspect()->setPositionUpperLimit(_index, _position); + MULTIDOFJOINT_SET_IF_DIFFERENT( mPositionUpperLimits[_index], _position ); } //============================================================================== @@ -521,14 +534,14 @@ void MultiDofJoint::resetPosition(size_t _index) return; } - setPosition(_index, getMultiDofJointAspect()->getInitialPosition(_index)); + setPosition(_index, Base::mAspectProperties.mInitialPositions[_index]); } //============================================================================== template void MultiDofJoint::resetPositions() { - setPositionsStatic(getMultiDofJointAspect()->getInitialPositions()); + setPositionsStatic(Base::mAspectProperties.mInitialPositions); } //============================================================================== @@ -541,7 +554,7 @@ void MultiDofJoint::setInitialPosition(size_t _index, double _initial) return; } - getMultiDofJointAspect()->setInitialPosition(_index, _initial); + MULTIDOFJOINT_SET_IF_DIFFERENT( mInitialPositions[_index], _initial ); } //============================================================================== @@ -554,7 +567,7 @@ double MultiDofJoint::getInitialPosition(size_t _index) const return 0.0; } - return getMultiDofJointAspect()->getInitialPosition(_index); + return Base::mAspectProperties.mInitialPositions[_index]; } //============================================================================== @@ -567,14 +580,14 @@ void MultiDofJoint::setInitialPositions(const Eigen::VectorXd& _initial) return; } - getMultiDofJointAspect()->setInitialPositions(_initial); + MULTIDOFJOINT_SET_IF_DIFFERENT( mInitialPositions, _initial); } //============================================================================== template Eigen::VectorXd MultiDofJoint::getInitialPositions() const { - return getMultiDofJointAspect()->getInitialPositions(); + return Base::mAspectProperties.mInitialPositions; } //============================================================================== @@ -587,7 +600,7 @@ double MultiDofJoint::getPositionUpperLimit(size_t _index) const return 0.0; } - return getMultiDofJointAspect()->getPositionUpperLimit(_index); + return Base::mAspectProperties.mPositionUpperLimits[_index]; } //============================================================================== @@ -600,8 +613,8 @@ bool MultiDofJoint::hasPositionLimit(size_t _index) const return true; } - return std::isfinite(getMultiDofJointAspect()->getPositionUpperLimit(_index)) - || std::isfinite(getMultiDofJointAspect()->getPositionLowerLimit(_index)); + return std::isfinite(Base::mAspectProperties.mPositionUpperLimits[_index]) + || std::isfinite(Base::mAspectProperties.mPositionLowerLimits[_index]); } //============================================================================== @@ -677,7 +690,7 @@ void MultiDofJoint::setVelocityLowerLimit(size_t _index, double _velocity) return; } - getMultiDofJointAspect()->setVelocityLowerLimit(_index, _velocity); + MULTIDOFJOINT_SET_IF_DIFFERENT( mVelocityLowerLimits[_index], _velocity ); } //============================================================================== @@ -690,7 +703,7 @@ double MultiDofJoint::getVelocityLowerLimit(size_t _index) const return 0.0; } - return getMultiDofJointAspect()->getVelocityLowerLimit(_index); + return Base::mAspectProperties.mVelocityLowerLimits[_index]; } //============================================================================== @@ -703,7 +716,7 @@ void MultiDofJoint::setVelocityUpperLimit(size_t _index, double _velocity) return; } - getMultiDofJointAspect()->setVelocityUpperLimit(_index, _velocity); + MULTIDOFJOINT_SET_IF_DIFFERENT( mVelocityUpperLimits[_index], _velocity ); } //============================================================================== @@ -716,7 +729,7 @@ double MultiDofJoint::getVelocityUpperLimit(size_t _index) const return 0.0; } - return getMultiDofJointAspect()->getVelocityUpperLimit(_index); + return Base::mAspectProperties.mVelocityUpperLimits[_index]; } //============================================================================== @@ -729,14 +742,14 @@ void MultiDofJoint::resetVelocity(size_t _index) return; } - setVelocity(_index, getMultiDofJointAspect()->getInitialVelocity(_index)); + setVelocity(_index, Base::mAspectProperties.mInitialVelocities[_index]); } //============================================================================== template void MultiDofJoint::resetVelocities() { - setVelocitiesStatic(getMultiDofJointAspect()->getInitialVelocities()); + setVelocitiesStatic(Base::mAspectProperties.mInitialVelocities); } //============================================================================== @@ -749,7 +762,7 @@ void MultiDofJoint::setInitialVelocity(size_t _index, double _initial) return; } - getMultiDofJointAspect()->setInitialVelocity(_index, _initial); + MULTIDOFJOINT_SET_IF_DIFFERENT( mInitialVelocities[_index], _initial ); } //============================================================================== @@ -762,7 +775,7 @@ double MultiDofJoint::getInitialVelocity(size_t _index) const return 0.0; } - return getMultiDofJointAspect()->getInitialVelocity(_index); + return Base::mAspectProperties.mInitialVelocities[_index]; } //============================================================================== @@ -775,14 +788,14 @@ void MultiDofJoint::setInitialVelocities(const Eigen::VectorXd& _initial) return; } - getMultiDofJointAspect()->setInitialVelocities(_initial); + MULTIDOFJOINT_SET_IF_DIFFERENT( mInitialVelocities, _initial ); } //============================================================================== template Eigen::VectorXd MultiDofJoint::getInitialVelocities() const { - return getMultiDofJointAspect()->getInitialVelocities(); + return Base::mAspectProperties.mInitialVelocities; } //============================================================================== @@ -866,7 +879,8 @@ void MultiDofJoint::setAccelerationLowerLimit(size_t _index, return; } - getMultiDofJointAspect()->setAccelerationLowerLimit(_index, _acceleration); + MULTIDOFJOINT_SET_IF_DIFFERENT( mAccelerationLowerLimits[_index], + _acceleration ); } //============================================================================== @@ -879,7 +893,7 @@ double MultiDofJoint::getAccelerationLowerLimit(size_t _index) const return 0.0; } - return getMultiDofJointAspect()->getAccelerationLowerLimit(_index); + return Base::mAspectProperties.mAccelerationLowerLimits[_index]; } //============================================================================== @@ -893,7 +907,8 @@ void MultiDofJoint::setAccelerationUpperLimit(size_t _index, return; } - getMultiDofJointAspect()->setAccelerationUpperLimit(_index, _acceleration); + MULTIDOFJOINT_SET_IF_DIFFERENT( mAccelerationUpperLimits[_index], + _acceleration); } //============================================================================== @@ -906,7 +921,7 @@ double MultiDofJoint::getAccelerationUpperLimit(size_t _index) const return 0.0; } - return getMultiDofJointAspect()->getAccelerationUpperLimit(_index); + return Base::mAspectProperties.mAccelerationUpperLimits[_index]; } //============================================================================== @@ -1047,7 +1062,7 @@ void MultiDofJoint::setForceLowerLimit(size_t _index, double _force) return; } - getMultiDofJointAspect()->setForceLowerLimit(_index, _force); + MULTIDOFJOINT_SET_IF_DIFFERENT( mForceLowerLimits[_index], _force ); } //============================================================================== @@ -1060,7 +1075,7 @@ double MultiDofJoint::getForceLowerLimit(size_t _index) const return 0.0; } - return getMultiDofJointAspect()->getForceLowerLimit(_index); + return Base::mAspectProperties.mForceLowerLimits[_index]; } //============================================================================== @@ -1073,7 +1088,7 @@ void MultiDofJoint::setForceUpperLimit(size_t _index, double _force) return; } - getMultiDofJointAspect()->setForceUpperLimit(_index, _force); + MULTIDOFJOINT_SET_IF_DIFFERENT( mForceUpperLimits[_index], _force ); } //============================================================================== @@ -1086,7 +1101,7 @@ double MultiDofJoint::getForceUpperLimit(size_t _index) const return 0.0; } - return getMultiDofJointAspect()->getForceUpperLimit(_index); + return Base::mAspectProperties.mForceUpperLimits[_index]; } //============================================================================== @@ -1208,7 +1223,7 @@ void MultiDofJoint::setSpringStiffness(size_t _index, double _k) assert(_k >= 0.0); - getMultiDofJointAspect()->setSpringStiffness(_index, _k); + MULTIDOFJOINT_SET_IF_DIFFERENT( mSpringStiffnesses[_index], _k ); } //============================================================================== @@ -1221,7 +1236,7 @@ double MultiDofJoint::getSpringStiffness(size_t _index) const return 0.0; } - return getMultiDofJointAspect()->getSpringStiffness(_index); + return Base::mAspectProperties.mSpringStiffnesses[_index]; } //============================================================================== @@ -1234,19 +1249,19 @@ void MultiDofJoint::setRestPosition(size_t _index, double _q0) return; } - if (getMultiDofJointAspect()->getPositionLowerLimit(_index) > _q0 - || getMultiDofJointAspect()->getPositionUpperLimit(_index) < _q0) + if (Base::mAspectProperties.mPositionLowerLimits[_index] > _q0 + || Base::mAspectProperties.mPositionUpperLimits[_index] < _q0) { dtwarn << "[MultiDofJoint::setRestPosition] Value of _q0 [" << _q0 << "], is out of the limit range [" - << getMultiDofJointAspect()->getPositionLowerLimit(_index) << ", " - << getMultiDofJointAspect()->getPositionUpperLimit(_index) + << Base::mAspectProperties.mPositionLowerLimits[_index] << ", " + << Base::mAspectProperties.mPositionUpperLimits[_index] << "] for index [" << _index << "] of Joint [" << this->getName() << "].\n"; return; } - getMultiDofJointAspect()->setRestPosition(_index, _q0); + MULTIDOFJOINT_SET_IF_DIFFERENT( mRestPositions[_index], _q0 ); } //============================================================================== @@ -1259,7 +1274,7 @@ double MultiDofJoint::getRestPosition(size_t _index) const return 0.0; } - return getMultiDofJointAspect()->getRestPosition(_index); + return Base::mAspectProperties.mRestPositions[_index]; } //============================================================================== @@ -1274,7 +1289,7 @@ void MultiDofJoint::setDampingCoefficient(size_t _index, double _d) assert(_d >= 0.0); - getMultiDofJointAspect()->setDampingCoefficient(_index, _d); + MULTIDOFJOINT_SET_IF_DIFFERENT( mDampingCoefficients[_index], _d ); } //============================================================================== @@ -1287,7 +1302,7 @@ double MultiDofJoint::getDampingCoefficient(size_t _index) const return 0.0; } - return getMultiDofJointAspect()->getDampingCoefficient(_index); + return Base::mAspectProperties.mDampingCoefficients[_index]; } //============================================================================== @@ -1302,7 +1317,7 @@ void MultiDofJoint::setCoulombFriction(size_t _index, double _friction) assert(_friction >= 0.0); - getMultiDofJointAspect()->setFriction(_index, _friction); + MULTIDOFJOINT_SET_IF_DIFFERENT( mFrictions[_index], _friction ); } //============================================================================== @@ -1315,7 +1330,7 @@ double MultiDofJoint::getCoulombFriction(size_t _index) const return 0.0; } - return getMultiDofJointAspect()->getFriction(_index); + return Base::mAspectProperties.mFrictions[_index]; } //============================================================================== @@ -1324,9 +1339,9 @@ double MultiDofJoint::getPotentialEnergy() const { // Spring energy Eigen::VectorXd displacement = - getPositionsStatic() - getMultiDofJointAspect()->getRestPositions(); + getPositionsStatic() - Base::mAspectProperties.mRestPositions; double pe = 0.5 * displacement.dot( - getMultiDofJointAspect()->getSpringStiffnesses().asDiagonal() + Base::mAspectProperties.mSpringStiffnesses.asDiagonal() * displacement); return pe; @@ -1376,7 +1391,7 @@ void MultiDofJoint::registerDofs() const SkeletonPtr& skel = this->mChildBodyNode->getSkeleton(); for (size_t i = 0; i < DOF; ++i) { - getMultiDofJointAspect()->mProperties.mDofNames[i] = + Base::mAspectProperties.mDofNames[i] = skel->mNameMgrForDofs.issueNewNameAndAdd(mDofs[i]->getName(), mDofs[i]); } } @@ -1725,8 +1740,8 @@ void MultiDofJoint::updateInvProjArtInertiaImplicitDynamic( // Add additional inertia for implicit damping and spring force for (size_t i = 0; i < DOF; ++i) { - projAI(i, i) += _timeStep * getMultiDofJointAspect()->getDampingCoefficient(i) - + _timeStep * _timeStep * getMultiDofJointAspect()->getSpringStiffness(i); + projAI(i, i) += _timeStep * Base::mAspectProperties.mDampingCoefficients[i] + + _timeStep * _timeStep * Base::mAspectProperties.mSpringStiffnesses[i]; } // Inversion of projected articulated inertia @@ -1944,13 +1959,13 @@ void MultiDofJoint::updateTotalForceDynamic( { // Spring force const Eigen::Matrix springForce - = (-getMultiDofJointAspect()->getSpringStiffnesses()).asDiagonal() - *(getPositionsStatic() - getMultiDofJointAspect()->getRestPositions() + = (-Base::mAspectProperties.mSpringStiffnesses).asDiagonal() + *(getPositionsStatic() - Base::mAspectProperties.mRestPositions + getVelocitiesStatic()*_timeStep); // Damping force const Eigen::Matrix dampingForce - = (-getMultiDofJointAspect()->getDampingCoefficients()).asDiagonal()* + = (-Base::mAspectProperties.mDampingCoefficients).asDiagonal()* getVelocitiesStatic(); // @@ -2125,7 +2140,7 @@ void MultiDofJoint::updateForceID(const Eigen::Vector6d& _bodyForce, if (_withDampingForces) { const Eigen::Matrix dampingForces - = (-getMultiDofJointAspect()->getDampingCoefficients()).asDiagonal() + = (-Base::mAspectProperties.mDampingCoefficients).asDiagonal() *getVelocitiesStatic(); mForces -= dampingForces; } @@ -2134,8 +2149,8 @@ void MultiDofJoint::updateForceID(const Eigen::Vector6d& _bodyForce, if (_withSpringForces) { const Eigen::Matrix springForces - = (-getMultiDofJointAspect()->getSpringStiffnesses()).asDiagonal() - *(getPositionsStatic() - getMultiDofJointAspect()->getRestPositions() + = (-Base::mAspectProperties.mSpringStiffnesses).asDiagonal() + *(getPositionsStatic() - Base::mAspectProperties.mRestPositions + getVelocitiesStatic()*_timeStep); mForces -= springForces; } diff --git a/dart/dynamics/detail/MultiDofJointProperties.h b/dart/dynamics/detail/MultiDofJointProperties.h index 006165b4fd90e..e08e2fbdf6b29 100644 --- a/dart/dynamics/detail/MultiDofJointProperties.h +++ b/dart/dynamics/detail/MultiDofJointProperties.h @@ -154,52 +154,6 @@ struct MultiDofJointProperties : EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -//============================================================================== -template -class MultiDofJointAspect final : - public common::AspectWithVersionedProperties< - MultiDofJointAspect, MultiDofJointUniqueProperties, MultiDofJoint, - common::detail::NoOp*> > -{ -public: - MultiDofJointAspect(const MultiDofJointAspect&) = delete; - - MultiDofJointAspect(common::Composite* comp, - const typename MultiDofJointAspect::PropertiesData& properties = - typename MultiDofJointAspect::PropertiesData()); - - constexpr static size_t NumDofs = DOF; - using Vector = Eigen::Matrix; - using BoolArray = std::array; - using StringArray = std::array; - - DART_COMMON_SET_GET_MULTIDOF_ASPECT(double, Vector, PositionLowerLimit) - DART_COMMON_SET_GET_MULTIDOF_ASPECT(double, Vector, PositionUpperLimit) - DART_COMMON_SET_GET_MULTIDOF_ASPECT(double, Vector, InitialPosition) - DART_COMMON_SET_GET_MULTIDOF_ASPECT(double, Vector, VelocityLowerLimit) - DART_COMMON_SET_GET_MULTIDOF_ASPECT(double, Vector, VelocityUpperLimit) - DART_COMMON_IRREGULAR_SET_GET_MULTIDOF_ASPECT(double, Vector, InitialVelocity, InitialVelocities) - DART_COMMON_SET_GET_MULTIDOF_ASPECT(double, Vector, AccelerationLowerLimit) - DART_COMMON_SET_GET_MULTIDOF_ASPECT(double, Vector, AccelerationUpperLimit) - DART_COMMON_SET_GET_MULTIDOF_ASPECT(double, Vector, ForceLowerLimit) - DART_COMMON_SET_GET_MULTIDOF_ASPECT(double, Vector, ForceUpperLimit) - DART_COMMON_IRREGULAR_SET_GET_MULTIDOF_ASPECT(double, Vector, SpringStiffness, SpringStiffnesses) - DART_COMMON_SET_GET_MULTIDOF_ASPECT(double, Vector, RestPosition) - DART_COMMON_SET_GET_MULTIDOF_ASPECT(double, Vector, DampingCoefficient) - DART_COMMON_SET_GET_MULTIDOF_ASPECT(double, Vector, Friction) - DART_COMMON_SET_GET_MULTIDOF_ASPECT(bool, BoolArray, PreserveDofName) - - const std::string& setDofName(size_t index, const std::string& name, bool preserveName); - DART_COMMON_GET_ASPECT_PROPERTY_ARRAY(MultiDofJointAspect, std::string, StringArray, DofName, DofNames, DOF) - - friend class MultiDofJoint; - -private: - /// Used by the MultiDofJoint class to get a mutable reference to one of the - /// DOF names. Only the MultiDofJoint class should ever use this function. - std::string& _getDofNameReference(size_t index); -}; - //============================================================================== // // These namespace-level definitions are required to enable ODR-use of static @@ -284,44 +238,9 @@ MultiDofJointProperties::MultiDofJointProperties( // Do nothing } -//============================================================================== -// -// These namespace-level definitions are required to enable ODR-use of static -// constexpr member variables. -// -// See this StackOverflow answer: http://stackoverflow.com/a/14396189/111426 -// -template -constexpr size_t MultiDofJointAspect::NumDofs; - -//============================================================================== -template -MultiDofJointAspect::MultiDofJointAspect(common::Composite* comp, - const typename MultiDofJointAspect::PropertiesData& properties) - : common::AspectWithVersionedProperties< - typename MultiDofJointAspect::Derived, - typename MultiDofJointAspect::PropertiesData, - typename MultiDofJointAspect::CompositeType, - &common::detail::NoOp::Derived*> >( - comp, properties) -{ - // Do nothing -} - -//============================================================================== -template -const std::string& MultiDofJointAspect::setDofName( - size_t index, const std::string& name, bool preserveName) -{ - return this->getManager()->setDofName(index, name, preserveName); -} - -//============================================================================== -template -std::string& MultiDofJointAspect::_getDofNameReference(size_t index) -{ - return this->mProperties.mDofNames[index]; -} +template +using MultiDofJointBase = common::EmbedPropertiesOnTopOf< + Derived, MultiDofJointUniqueProperties, Joint>; } // namespace detail } // namespace dynamics From b63c8ffb9140dc6d2be1acd7660ea7a81da17a6c Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Wed, 30 Mar 2016 19:10:18 -0400 Subject: [PATCH 22/62] finished embedding aspects into all MultiDofJoint types --- dart/dynamics/EulerJoint.cpp | 19 ++++-- dart/dynamics/EulerJoint.h | 4 +- dart/dynamics/PlanarJoint.cpp | 60 +++++++++++-------- dart/dynamics/PlanarJoint.h | 4 +- dart/dynamics/UniversalJoint.cpp | 27 ++++++--- dart/dynamics/UniversalJoint.h | 4 +- dart/dynamics/detail/EulerJointProperties.h | 15 +---- .../dynamics/detail/PlanarJointProperties.cpp | 41 ++----------- dart/dynamics/detail/PlanarJointProperties.h | 25 +------- .../detail/UniversalJointProperties.cpp | 26 -------- .../detail/UniversalJointProperties.h | 18 +----- 11 files changed, 87 insertions(+), 156 deletions(-) diff --git a/dart/dynamics/EulerJoint.cpp b/dart/dynamics/EulerJoint.cpp index bd1a719e46fbf..60b8ba3531c6e 100644 --- a/dart/dynamics/EulerJoint.cpp +++ b/dart/dynamics/EulerJoint.cpp @@ -63,7 +63,13 @@ void EulerJoint::setProperties(const Properties& _properties) //============================================================================== void EulerJoint::setProperties(const UniqueProperties& _properties) { - getEulerJointAspect()->setProperties(_properties); + setAspectProperties(_properties); +} + +//============================================================================== +void EulerJoint::setAspectProperties(const AspectProperties& properties) +{ + setAxisOrder(properties.mAxisOrder, false); } //============================================================================== @@ -119,16 +125,19 @@ bool EulerJoint::isCyclic(size_t _index) const //============================================================================== void EulerJoint::setAxisOrder(EulerJoint::AxisOrder _order, bool _renameDofs) { - getEulerJointAspect()->setAxisOrder(_order); + mAspectProperties.mAxisOrder = _order; if (_renameDofs) updateDegreeOfFreedomNames(); - // The EulerJoint::Aspect will take care of notifying a position update + + Joint::notifyPositionUpdate(); + updateLocalJacobian(true); + Joint::incrementVersion(); } //============================================================================== EulerJoint::AxisOrder EulerJoint::getAxisOrder() const { - return getEulerJointAspect()->getAxisOrder(); + return mAspectProperties.mAxisOrder; } //============================================================================== @@ -281,7 +290,7 @@ Eigen::Matrix EulerJoint::getLocalJacobianStatic( //============================================================================== EulerJoint::EulerJoint(const Properties& properties) - : detail::EulerJointBase(properties, common::NoArg) + : detail::EulerJointBase(common::NoArg, properties) { // Inherited Aspects must be created in the final joint class in reverse order // or else we get pure virtual function calls diff --git a/dart/dynamics/EulerJoint.h b/dart/dynamics/EulerJoint.h index 336c362710a53..def209bdf8e63 100644 --- a/dart/dynamics/EulerJoint.h +++ b/dart/dynamics/EulerJoint.h @@ -51,7 +51,6 @@ class EulerJoint : public detail::EulerJointBase using AxisOrder = detail::AxisOrder; using UniqueProperties = detail::EulerJointUniqueProperties; using Properties = detail::EulerJointProperties; - using Aspect = detail::EulerJointAspect; using Base = detail::EulerJointBase; DART_BAKE_SPECIALIZED_ASPECT_IRREGULAR(Aspect, EulerJointAspect) @@ -67,6 +66,9 @@ class EulerJoint : public detail::EulerJointBase /// Set the Properties of this EulerJoint void setProperties(const UniqueProperties& _properties); + /// Set the AspectProperties of this EulerJoint + void setAspectProperties(const AspectProperties& properties); + /// Get the Properties of this EulerJoint Properties getEulerJointProperties() const; diff --git a/dart/dynamics/PlanarJoint.cpp b/dart/dynamics/PlanarJoint.cpp index 21697028e17aa..1d41091f54230 100644 --- a/dart/dynamics/PlanarJoint.cpp +++ b/dart/dynamics/PlanarJoint.cpp @@ -63,14 +63,22 @@ void PlanarJoint::setProperties(const Properties& _properties) //============================================================================== void PlanarJoint::setProperties(const UniqueProperties& _properties) { - getPlanarJointAspect()->setProperties(_properties); + setAspectProperties(_properties); +} + +//============================================================================== +void PlanarJoint::setAspectProperties(const AspectProperties& properties) +{ + mAspectProperties = properties; + Joint::notifyPositionUpdate(); + updateLocalJacobian(true); + Joint::incrementVersion(); } //============================================================================== PlanarJoint::Properties PlanarJoint::getPlanarJointProperties() const { - return Properties(getMultiDofJointProperties(), - getPlanarJointAspect()->getProperties()); + return Properties(getMultiDofJointProperties(), mAspectProperties); } //============================================================================== @@ -120,7 +128,7 @@ bool PlanarJoint::isCyclic(size_t _index) const //============================================================================== void PlanarJoint::setXYPlane(bool _renameDofs) { - getPlanarJointAspect()->setXYPlane(); + mAspectProperties.setXYPlane(); if (_renameDofs) updateDegreeOfFreedomNames(); @@ -130,7 +138,7 @@ void PlanarJoint::setXYPlane(bool _renameDofs) //============================================================================== void PlanarJoint::setYZPlane(bool _renameDofs) { - getPlanarJointAspect()->setYZPlane(); + mAspectProperties.setYZPlane(); if (_renameDofs) updateDegreeOfFreedomNames(); @@ -140,7 +148,7 @@ void PlanarJoint::setYZPlane(bool _renameDofs) //============================================================================== void PlanarJoint::setZXPlane(bool _renameDofs) { - getPlanarJointAspect()->setZXPlane(); + mAspectProperties.setZXPlane(); if (_renameDofs) updateDegreeOfFreedomNames(); @@ -152,7 +160,7 @@ void PlanarJoint::setArbitraryPlane(const Eigen::Vector3d& _transAxis1, const Eigen::Vector3d& _transAxis2, bool _renameDofs) { - getPlanarJointAspect()->setArbitraryPlane(_transAxis1, _transAxis2); + mAspectProperties.setArbitraryPlane(_transAxis1, _transAxis2); if (_renameDofs) updateDegreeOfFreedomNames(); @@ -162,25 +170,25 @@ void PlanarJoint::setArbitraryPlane(const Eigen::Vector3d& _transAxis1, //============================================================================== PlanarJoint::PlaneType PlanarJoint::getPlaneType() const { - return getPlanarJointAspect()->getPlaneType(); + return mAspectProperties.mPlaneType; } //============================================================================== const Eigen::Vector3d& PlanarJoint::getRotationalAxis() const { - return getPlanarJointAspect()->getRotAxis(); + return mAspectProperties.mRotAxis; } //============================================================================== const Eigen::Vector3d& PlanarJoint::getTranslationalAxis1() const { - return getPlanarJointAspect()->getTransAxis1(); + return mAspectProperties.mTransAxis1; } //============================================================================== const Eigen::Vector3d& PlanarJoint::getTranslationalAxis2() const { - return getPlanarJointAspect()->getTransAxis2(); + return mAspectProperties.mTransAxis2; } //============================================================================== @@ -188,13 +196,13 @@ Eigen::Matrix PlanarJoint::getLocalJacobianStatic( const Eigen::Vector3d& _positions) const { Eigen::Matrix J = Eigen::Matrix::Zero(); - J.block<3, 1>(3, 0) = getPlanarJointAspect()->getTransAxis1(); - J.block<3, 1>(3, 1) = getPlanarJointAspect()->getTransAxis2(); - J.block<3, 1>(0, 2) = getPlanarJointAspect()->getRotAxis(); + J.block<3, 1>(3, 0) = mAspectProperties.mTransAxis1; + J.block<3, 1>(3, 1) = mAspectProperties.mTransAxis2; + J.block<3, 1>(0, 2) = mAspectProperties.mRotAxis; J.leftCols<2>() = math::AdTJacFixed(Joint::mAspectProperties.mT_ChildBodyToJoint - * math::expAngular(getPlanarJointAspect()->getRotAxis() + * math::expAngular(mAspectProperties.mRotAxis * -_positions[2]), J.leftCols<2>()); J.col(2) = math::AdTJac(Joint::mAspectProperties.mT_ChildBodyToJoint, J.col(2)); @@ -207,7 +215,7 @@ Eigen::Matrix PlanarJoint::getLocalJacobianStatic( //============================================================================== PlanarJoint::PlanarJoint(const Properties& properties) - : detail::PlanarJointBase(properties, common::NoArg) + : detail::PlanarJointBase(common::NoArg, properties) { // Inherited Aspects must be created in the final joint class in reverse order // or else we get pure virtual function calls @@ -226,7 +234,7 @@ Joint* PlanarJoint::clone() const void PlanarJoint::updateDegreeOfFreedomNames() { std::vector affixes; - switch (getPlanarJointAspect()->getPlaneType()) + switch (mAspectProperties.mPlaneType) { case PlaneType::XY: affixes.push_back("_x"); @@ -246,7 +254,7 @@ void PlanarJoint::updateDegreeOfFreedomNames() break; default: dterr << "Unsupported plane type in PlanarJoint named '" << Joint::mAspectProperties.mName - << "' (" << static_cast(getPlanarJointAspect()->getPlaneType()) + << "' (" << static_cast(mAspectProperties.mPlaneType) << ")\n"; } @@ -265,9 +273,9 @@ void PlanarJoint::updateLocalTransform() const { const Eigen::Vector3d& positions = getPositionsStatic(); mT = Joint::mAspectProperties.mT_ParentBodyToJoint - * Eigen::Translation3d(getPlanarJointAspect()->getTransAxis1() * positions[0]) - * Eigen::Translation3d(getPlanarJointAspect()->getTransAxis2() * positions[1]) - * math::expAngular (getPlanarJointAspect()->getRotAxis() * positions[2]) + * Eigen::Translation3d(mAspectProperties.mTransAxis1 * positions[0]) + * Eigen::Translation3d(mAspectProperties.mTransAxis2 * positions[1]) + * math::expAngular (mAspectProperties.mRotAxis * positions[2]) * Joint::mAspectProperties.mT_ChildBodyToJoint.inverse(); // Verification @@ -284,23 +292,23 @@ void PlanarJoint::updateLocalJacobian(bool) const void PlanarJoint::updateLocalJacobianTimeDeriv() const { Eigen::Matrix J = Eigen::Matrix::Zero(); - J.block<3, 1>(3, 0) = getPlanarJointAspect()->getTransAxis1(); - J.block<3, 1>(3, 1) = getPlanarJointAspect()->getTransAxis2(); - J.block<3, 1>(0, 2) = getPlanarJointAspect()->getRotAxis(); + J.block<3, 1>(3, 0) = mAspectProperties.mTransAxis1; + J.block<3, 1>(3, 1) = mAspectProperties.mTransAxis2; + J.block<3, 1>(0, 2) = mAspectProperties.mRotAxis; const Eigen::Matrix& Jacobian = getLocalJacobianStatic(); const Eigen::Vector3d& velocities = getVelocitiesStatic(); mJacobianDeriv.col(0) = -math::ad(Jacobian.col(2) * velocities[2], math::AdT(Joint::mAspectProperties.mT_ChildBodyToJoint - * math::expAngular(getPlanarJointAspect()->getRotAxis() + * math::expAngular(mAspectProperties.mRotAxis * -getPositionsStatic()[2]), J.col(0))); mJacobianDeriv.col(1) = -math::ad(Jacobian.col(2) * velocities[2], math::AdT(Joint::mAspectProperties.mT_ChildBodyToJoint - * math::expAngular(getPlanarJointAspect()->getRotAxis() + * math::expAngular(mAspectProperties.mRotAxis * -getPositionsStatic()[2]), J.col(1))); diff --git a/dart/dynamics/PlanarJoint.h b/dart/dynamics/PlanarJoint.h index 57c5536a3f3ad..1ebff9ef9edb8 100644 --- a/dart/dynamics/PlanarJoint.h +++ b/dart/dynamics/PlanarJoint.h @@ -56,7 +56,6 @@ class PlanarJoint : public detail::PlanarJointBase using PlaneType = detail::PlaneType; using UniqueProperties = detail::PlanarJointUniqueProperties; using Properties = detail::PlanarJointProperties; - using Aspect = detail::PlanarJointAspect; using Base = detail::PlanarJointBase; DART_BAKE_SPECIALIZED_ASPECT_IRREGULAR(Aspect, PlanarJointAspect) @@ -72,6 +71,9 @@ class PlanarJoint : public detail::PlanarJointBase /// Set the Properties of this PlanarJoint void setProperties(const UniqueProperties& _properties); + /// Set the AspectProperties of this PlanarJoint + void setAspectProperties(const AspectProperties& properties); + /// Get the Properties of this PlanarJoint Properties getPlanarJointProperties() const; diff --git a/dart/dynamics/UniversalJoint.cpp b/dart/dynamics/UniversalJoint.cpp index be12e38ce8fb9..9468c36510bba 100644 --- a/dart/dynamics/UniversalJoint.cpp +++ b/dart/dynamics/UniversalJoint.cpp @@ -61,15 +61,20 @@ void UniversalJoint::setProperties(const Properties& _properties) //============================================================================== void UniversalJoint::setProperties(const UniqueProperties& _properties) { - setAxis1(_properties.mAxis[0]); - setAxis2(_properties.mAxis[1]); + setAspectProperties(_properties); +} + +//============================================================================== +void UniversalJoint::setAspectProperties(const AspectProperties& properties) +{ + setAxis1(properties.mAxis[0]); + setAxis2(properties.mAxis[1]); } //============================================================================== UniversalJoint::Properties UniversalJoint::getUniversalJointProperties() const { - return Properties(getMultiDofJointProperties(), - getUniversalJointAspect()->getProperties()); + return Properties(getMultiDofJointProperties(), mAspectProperties); } //============================================================================== @@ -119,25 +124,29 @@ bool UniversalJoint::isCyclic(size_t _index) const //============================================================================== void UniversalJoint::setAxis1(const Eigen::Vector3d& _axis) { - getUniversalJointAspect()->setAxis1(_axis); + mAspectProperties.mAxis[0] = _axis; + Joint::notifyPositionUpdate(); + Joint::incrementVersion(); } //============================================================================== void UniversalJoint::setAxis2(const Eigen::Vector3d& _axis) { - getUniversalJointAspect()->setAxis2(_axis); + mAspectProperties.mAxis[1] = _axis; + Joint::notifyPositionUpdate(); + Joint::incrementVersion(); } //============================================================================== const Eigen::Vector3d& UniversalJoint::getAxis1() const { - return getUniversalJointAspect()->getAxis1(); + return mAspectProperties.mAxis[0]; } //============================================================================== const Eigen::Vector3d& UniversalJoint::getAxis2() const { - return getUniversalJointAspect()->getAxis2(); + return mAspectProperties.mAxis[1]; } //============================================================================== @@ -155,7 +164,7 @@ Eigen::Matrix UniversalJoint::getLocalJacobianStatic( //============================================================================== UniversalJoint::UniversalJoint(const Properties& properties) - : detail::UniversalJointBase(properties, common::NoArg) + : detail::UniversalJointBase(common::NoArg, properties) { // Inherited Aspects must be created in the final joint class in reverse order // or else we get pure virtual function calls diff --git a/dart/dynamics/UniversalJoint.h b/dart/dynamics/UniversalJoint.h index 14d35762d5adb..6cd033bdebceb 100644 --- a/dart/dynamics/UniversalJoint.h +++ b/dart/dynamics/UniversalJoint.h @@ -50,7 +50,6 @@ class UniversalJoint : public detail::UniversalJointBase friend class Skeleton; using UniqueProperties = detail::UniversalJointUniqueProperties; using Properties = detail::UniversalJointProperties; - using Aspect = detail::UniversalJointAspect; using Base = detail::UniversalJointBase; DART_BAKE_SPECIALIZED_ASPECT_IRREGULAR(Aspect, UniversalJointAspect) @@ -66,6 +65,9 @@ class UniversalJoint : public detail::UniversalJointBase /// Set the Properties of this UniversalJoint void setProperties(const UniqueProperties& _properties); + /// Set the AspectProperties of this UniversalJoint + void setAspectProperties(const AspectProperties& properties); + /// Get the Properties of this UniversalJoint Properties getUniversalJointProperties() const; diff --git a/dart/dynamics/detail/EulerJointProperties.h b/dart/dynamics/detail/EulerJointProperties.h index 8587a99edb14d..6cfd9eb3dbdb1 100644 --- a/dart/dynamics/detail/EulerJointProperties.h +++ b/dart/dynamics/detail/EulerJointProperties.h @@ -84,19 +84,8 @@ struct EulerJointProperties : }; //============================================================================== -class EulerJointAspect final : - public common::AspectWithVersionedProperties< - EulerJointAspect, EulerJointUniqueProperties, EulerJoint, - detail::JointPropertyUpdate > -{ -public: - DART_COMMON_JOINT_ASPECT_CONSTRUCTOR( EulerJointAspect ) - DART_COMMON_SET_GET_ASPECT_PROPERTY( AxisOrder, AxisOrder ) -}; - -//============================================================================== -using EulerJointBase = common::CompositeJoiner< - MultiDofJoint<3>, common::RequiresAspect >; +using EulerJointBase = common::EmbedPropertiesOnTopOf< + EulerJoint, EulerJointUniqueProperties, MultiDofJoint<3> >; } // namespace detail } // namespace dynamics diff --git a/dart/dynamics/detail/PlanarJointProperties.cpp b/dart/dynamics/detail/PlanarJointProperties.cpp index f97d903e0dac6..bb170e2d5209a 100644 --- a/dart/dynamics/detail/PlanarJointProperties.cpp +++ b/dart/dynamics/detail/PlanarJointProperties.cpp @@ -91,8 +91,8 @@ PlanarJointUniqueProperties::PlanarJointUniqueProperties( //============================================================================== void PlanarJointUniqueProperties::setXYPlane() { - mPlaneType = PlaneType::XY; - mRotAxis = Eigen::Vector3d::UnitZ(); + mPlaneType = PlaneType::XY; + mRotAxis = Eigen::Vector3d::UnitZ(); mTransAxis1 = Eigen::Vector3d::UnitX(); mTransAxis2 = Eigen::Vector3d::UnitY(); } @@ -100,8 +100,8 @@ void PlanarJointUniqueProperties::setXYPlane() //============================================================================== void PlanarJointUniqueProperties::setYZPlane() { - mPlaneType = PlaneType::YZ; - mRotAxis = Eigen::Vector3d::UnitX(); + mPlaneType = PlaneType::YZ; + mRotAxis = Eigen::Vector3d::UnitX(); mTransAxis1 = Eigen::Vector3d::UnitY(); mTransAxis2 = Eigen::Vector3d::UnitZ(); } @@ -109,8 +109,8 @@ void PlanarJointUniqueProperties::setYZPlane() //============================================================================== void PlanarJointUniqueProperties::setZXPlane() { - mPlaneType = PlaneType::ZX; - mRotAxis = Eigen::Vector3d::UnitY(); + mPlaneType = PlaneType::ZX; + mRotAxis = Eigen::Vector3d::UnitY(); mTransAxis1 = Eigen::Vector3d::UnitZ(); mTransAxis2 = Eigen::Vector3d::UnitX(); } @@ -149,35 +149,6 @@ PlanarJointProperties::PlanarJointProperties( // Do nothing } -//============================================================================== -void PlanarJointAspect::setXYPlane() -{ - mProperties.setXYPlane(); - notifyPropertiesUpdate(); -} - -//============================================================================== -void PlanarJointAspect::setYZPlane() -{ - mProperties.setYZPlane(); - notifyPropertiesUpdate(); -} - -//============================================================================== -void PlanarJointAspect::setZXPlane() -{ - mProperties.setZXPlane(); - notifyPropertiesUpdate(); -} - -//============================================================================== -void PlanarJointAspect::setArbitraryPlane(const Eigen::Vector3d& _axis1, - const Eigen::Vector3d& _axis2) -{ - mProperties.setArbitraryPlane(_axis1, _axis2); - notifyPropertiesUpdate(); -} - } // namespace detail } // namespace dynamics } // namespace dart diff --git a/dart/dynamics/detail/PlanarJointProperties.h b/dart/dynamics/detail/PlanarJointProperties.h index abad427fcf814..263d8356faf82 100644 --- a/dart/dynamics/detail/PlanarJointProperties.h +++ b/dart/dynamics/detail/PlanarJointProperties.h @@ -123,29 +123,8 @@ struct PlanarJointProperties : }; //============================================================================== -class PlanarJointAspect final : - public common::AspectWithVersionedProperties< - PlanarJointAspect, PlanarJointUniqueProperties, PlanarJoint, - detail::JointPropertyUpdate > -{ -public: - DART_COMMON_JOINT_ASPECT_CONSTRUCTOR( PlanarJointAspect ) - - void setXYPlane(); - void setYZPlane(); - void setZXPlane(); - void setArbitraryPlane(const Eigen::Vector3d& _axis1, - const Eigen::Vector3d& _axis2); - - DART_COMMON_GET_ASPECT_PROPERTY( PlaneType, PlaneType ) - DART_COMMON_GET_ASPECT_PROPERTY( Eigen::Vector3d, TransAxis1 ) - DART_COMMON_GET_ASPECT_PROPERTY( Eigen::Vector3d, TransAxis2 ) - DART_COMMON_GET_ASPECT_PROPERTY( Eigen::Vector3d, RotAxis ) -}; - -//============================================================================== -using PlanarJointBase = common::CompositeJoiner< - MultiDofJoint<3>, common::RequiresAspect >; +using PlanarJointBase = common::EmbedPropertiesOnTopOf< + PlanarJoint, PlanarJointUniqueProperties, MultiDofJoint<3> >; } // namespace detail } // namespace dynamics diff --git a/dart/dynamics/detail/UniversalJointProperties.cpp b/dart/dynamics/detail/UniversalJointProperties.cpp index a80698efba582..e9c348478ce33 100644 --- a/dart/dynamics/detail/UniversalJointProperties.cpp +++ b/dart/dynamics/detail/UniversalJointProperties.cpp @@ -58,32 +58,6 @@ UniversalJointProperties::UniversalJointProperties( // Do nothing } -//============================================================================== -void UniversalJointAspect::setAxis1(const Eigen::Vector3d& _axis) -{ - mProperties.mAxis[0] = _axis.normalized(); - notifyPropertiesUpdate(); -} - -//============================================================================== -const Eigen::Vector3d& UniversalJointAspect::getAxis1() const -{ - return mProperties.mAxis[0]; -} - -//============================================================================== -void UniversalJointAspect::setAxis2(const Eigen::Vector3d& _axis) -{ - mProperties.mAxis[1] = _axis.normalized(); - notifyPropertiesUpdate(); -} - -//============================================================================== -const Eigen::Vector3d& UniversalJointAspect::getAxis2() const -{ - return mProperties.mAxis[1]; -} - } // namespace detail } // namespace dynamics } // namespace dart diff --git a/dart/dynamics/detail/UniversalJointProperties.h b/dart/dynamics/detail/UniversalJointProperties.h index 25b300646eb3f..304eafa1c5a87 100644 --- a/dart/dynamics/detail/UniversalJointProperties.h +++ b/dart/dynamics/detail/UniversalJointProperties.h @@ -77,22 +77,8 @@ struct UniversalJointProperties : }; //============================================================================== -class UniversalJointAspect final : - public common::AspectWithVersionedProperties< - UniversalJointAspect, UniversalJointUniqueProperties, UniversalJoint, - detail::JointPropertyUpdate > -{ -public: - DART_COMMON_JOINT_ASPECT_CONSTRUCTOR( UniversalJointAspect ) - void setAxis1(const Eigen::Vector3d& _axis); - const Eigen::Vector3d& getAxis1() const; - void setAxis2(const Eigen::Vector3d& _axis); - const Eigen::Vector3d& getAxis2() const; -}; - -//============================================================================== -using UniversalJointBase = common::CompositeJoiner< - MultiDofJoint<2>, common::RequiresAspect >; +using UniversalJointBase = common::EmbedPropertiesOnTopOf< + UniversalJoint, UniversalJointUniqueProperties, MultiDofJoint<2> >; } // namespace detail } // namespace dynamics From 2c387fa621732b0bce0da951abbe92552f18b25c Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Thu, 31 Mar 2016 23:08:26 -0400 Subject: [PATCH 23/62] greatly improved constructor flexibility for EmbeddedAspects --- dart/common/Aspect.h | 57 ------ dart/common/EmbeddedAspect.h | 127 ++++++++----- dart/common/Extensible.h | 2 + dart/common/StlHelpers.h | 15 ++ dart/common/detail/EmbeddedAspect.h | 170 ++++++++++++++---- dart/dynamics/BodyNode.cpp | 109 +++++------ dart/dynamics/BodyNode.h | 13 +- dart/dynamics/EulerJoint.h | 2 - dart/dynamics/Joint.h | 11 -- dart/dynamics/PlanarJoint.h | 2 - dart/dynamics/ScrewJoint.h | 2 - dart/dynamics/SingleDofJoint.cpp | 111 ++++++------ dart/dynamics/SingleDofJoint.h | 46 +---- dart/dynamics/SoftBodyNode.cpp | 18 +- dart/dynamics/UniversalJoint.h | 2 - dart/dynamics/detail/BodyNodeProperties.h | 8 +- .../detail/SingleDofJointProperties.cpp | 16 ++ .../detail/SingleDofJointProperties.h | 30 +++- unittests/testAspect.cpp | 2 +- 19 files changed, 420 insertions(+), 323 deletions(-) diff --git a/dart/common/Aspect.h b/dart/common/Aspect.h index a54b8716a4995..3cd68849a8670 100644 --- a/dart/common/Aspect.h +++ b/dart/common/Aspect.h @@ -171,10 +171,6 @@ class CompositeTrackingAspect : public Aspect inline ClassName (dart::common::Composite* comp, const PropertiesData& properties = PropertiesData())\ : AspectWithVersionedProperties< Base, Derived, PropertiesData, CompositeType, UpdatePropertiesMacro>(comp, properties) { } -//============================================================================== -#define DART_COMMON_JOINT_ASPECT_CONSTRUCTOR( ClassName )\ - DART_COMMON_ASPECT_PROPERTY_CONSTRUCTOR( ClassName, &detail::JointPropertyUpdate ) - //============================================================================== #define DART_COMMON_ASPECT_STATE_PROPERTY_CONSTRUCTORS(ClassName)\ ClassName (const ClassName &) = delete;\ @@ -202,59 +198,6 @@ class CompositeTrackingAspect : public Aspect DART_COMMON_SET_ASPECT_PROPERTY( Type, Name )\ DART_COMMON_GET_ASPECT_PROPERTY( Type, Name ) -//============================================================================== -#define DART_COMMON_SET_ASPECT_PROPERTY_ARRAY( Class, SingleType, VectorType, SingleName, PluralName, Size )\ - void set ## SingleName (size_t index, const SingleType & value)\ - {\ - if( index >= Size )\ - {\ - dterr << "[" #Class << "::set" #SingleName << "] Invalid index (" << index << "). "\ - << "The specified index must be less than " #Size << "!\n";\ - assert(false); return;\ - }\ - this->mProperties.m ## PluralName [index] = value;\ - this->notifyPropertiesUpdate();\ - }\ - void set ## PluralName (const VectorType & vec)\ - {\ - this->mProperties.m ## PluralName = vec;\ - this->notifyPropertiesUpdate();\ - } - -//============================================================================== -#define DART_COMMON_GET_ASPECT_PROPERTY_ARRAY(Class, SingleType, VectorType, SingleName, PluralName, Size)\ - inline const SingleType& get ## SingleName (size_t index) const\ - {\ - if(index >= Size)\ - {\ - dterr << "[" #Class << "::get" #SingleName << "] Invalid index (" << index << "). "\ - << "The specified index must be less than " #Size << "!\n";\ - assert(false); index = 0;\ - }\ - return this->mProperties.m ## PluralName [index];\ - }\ - inline const VectorType& get ## PluralName () const\ - {\ - return this->mProperties.m ## PluralName;\ - } - -//============================================================================== -#define DART_COMMON_IRREGULAR_SET_GET_ASPECT_PROPERTY_ARRAY( Class, SingleType, VectorType, SingleName, PluralName, Size )\ - DART_COMMON_SET_ASPECT_PROPERTY_ARRAY( Class, SingleType, VectorType, SingleName, PluralName, Size )\ - DART_COMMON_GET_ASPECT_PROPERTY_ARRAY( Class, SingleType, VectorType, SingleName, PluralName, Size ) - -//============================================================================== -#define DART_COMMON_SET_GET_ASPECT_PROPERTY_ARRAY( Class, SingleType, VectorType, SingleName, Size )\ - DART_COMMON_IRREGULAR_SET_GET_ASPECT_PROPERTY_ARRAY( Class, SingleType, VectorType, SingleName, SingleName ## s, Size ) - -//============================================================================== -#define DART_COMMON_IRREGULAR_SET_GET_MULTIDOF_ASPECT( SingleType, VectorType, SingleName, PluralName )\ - DART_COMMON_IRREGULAR_SET_GET_ASPECT_PROPERTY_ARRAY( MultiDofJointAspect, SingleType, VectorType, SingleName, PluralName, DOF ) - -//============================================================================== -#define DART_COMMON_SET_GET_MULTIDOF_ASPECT( SingleType, VectorType, SingleName )\ - DART_COMMON_IRREGULAR_SET_GET_MULTIDOF_ASPECT( SingleType, VectorType, SingleName, SingleName ## s ) - #include "dart/common/detail/Aspect.h" #endif // DART_COMMON_ASPECT_H_ diff --git a/dart/common/EmbeddedAspect.h b/dart/common/EmbeddedAspect.h index 118156ba569b7..a6557ef487efd 100644 --- a/dart/common/EmbeddedAspect.h +++ b/dart/common/EmbeddedAspect.h @@ -51,17 +51,18 @@ namespace common { /// /// For more control over how your embedded-state Aspect is implemented, you can /// use the detail::EmbeddedStateAspect class. -template +template class EmbeddedStateAspect : public detail::EmbeddedStateAspect< CompositeTrackingAspect, - EmbeddedStateAspect, StateT> + EmbeddedStateAspect, StateDataT> { public: - using State = StateT; using Base = CompositeTrackingAspect; - using Derived = EmbeddedStateAspect; - using Impl = detail::EmbeddedStateAspect; + using Derived = EmbeddedStateAspect; + using Impl = detail::EmbeddedStateAspect; + using State = typename Impl::State; + using StateData = typename Impl::StateData; template EmbeddedStateAspect(Args&&... args) @@ -88,14 +89,14 @@ class EmbeddedStateAspect : public detail::EmbeddedStateAspect< /// To embed both state and properties information, use EmbedStateAndProperties. template class EmbedState : public virtual common::RequiresAspect< - common::EmbeddedStateAspect> > + common::EmbeddedStateAspect > { public: using Derived = DerivedT; - using AspectStateData = StateDataT; - using AspectState = common::Aspect::StateMixer; - using Aspect = common::EmbeddedStateAspect; + using Aspect = common::EmbeddedStateAspect; + using AspectState = typename Aspect::State; + using AspectStateData = typename Aspect::StateData; using Base = common::RequiresAspect; // Inherit constructor @@ -151,17 +152,18 @@ class EmbedStateOnTopOf : public CompositeJoiner< /// /// For more control over how your embedded-properties Aspect is implemented, /// you can use the detail::EmbeddedPropertiesAspect class. -template +template class EmbeddedPropertiesAspect : public detail::EmbeddedPropertiesAspect< CompositeTrackingAspect, - EmbeddedPropertiesAspect, PropertiesT> + EmbeddedPropertiesAspect, PropertiesDataT> { public: - using Properties = PropertiesT; using Base = CompositeTrackingAspect; - using Derived = EmbeddedPropertiesAspect; - using Impl = detail::EmbeddedPropertiesAspect; + using Derived = EmbeddedPropertiesAspect; + using Impl = detail::EmbeddedPropertiesAspect; + using Properties = typename Impl::Properties; + using PropertiesData = typename Impl::PropertiesData; template EmbeddedPropertiesAspect(Args&&... args) @@ -188,15 +190,14 @@ class EmbeddedPropertiesAspect : public detail::EmbeddedPropertiesAspect< /// To embed both state and properties information, use EmbedStateAndProperties. template class EmbedProperties : public virtual common::RequiresAspect< - common::EmbeddedPropertiesAspect< - DerivedT, common::Aspect::PropertiesMixer> > + common::EmbeddedPropertiesAspect > { public: using Derived = DerivedT; - using AspectPropertiesData = PropertiesDataT; - using AspectProperties = common::Aspect::PropertiesMixer; - using Aspect = common::EmbeddedPropertiesAspect; + using Aspect = common::EmbeddedPropertiesAspect; + using AspectProperties = typename Aspect::Properties; + using AspectPropertiesData = typename Aspect::PropertiesData; using Base = common::RequiresAspect; // Inherit constructor @@ -226,12 +227,12 @@ class EmbedPropertiesOnTopOf : public CompositeJoiner< { public: - using Base = CompositeJoiner, CompositeBases...>; using Impl = EmbedProperties; using Derived = typename Impl::Derived; using AspectPropertiesData = typename Impl::AspectPropertiesData; using AspectProperties = typename Impl::AspectProperties; using Aspect = typename Impl::Aspect; + using Base = CompositeJoiner; using Impl::getAspectProperties; // Inherit constructor @@ -249,31 +250,41 @@ class EmbedPropertiesOnTopOf : public CompositeJoiner< /// This is the implementation of a standard combination of embedded-state and /// embedded-properties Aspect. Inherit the EmbedStateAndProperties (next class /// down in the header) to use this Aspect implementation. -template +// +// Dev Note: We achieve "multiple inheritance" without the diamond of death +// issue by specifying detail::EmbeddedStateAspect as the base class of +// detail::EmbeddedPropertiesAspect. This allows their implementations to stack +// on top of each other without the conflict that would arise from both of them +// inheriting from common::Aspect. +template class EmbeddedStateAndPropertiesAspect : public detail::EmbeddedPropertiesAspect< detail::EmbeddedStateAspect< CompositeTrackingAspect, - EmbeddedStateAndPropertiesAspect, - StateT>, - EmbeddedStateAndPropertiesAspect, - PropertiesT> + EmbeddedStateAndPropertiesAspect, + StateDataT>, + EmbeddedStateAndPropertiesAspect, + PropertiesDataT> { public: - using Derived = EmbeddedStateAndPropertiesAspect; - using State = StateT; - using Properties = PropertiesT; - using CompositeType = CompositeT; + using Derived = EmbeddedStateAndPropertiesAspect; using AspectStateImpl = detail::EmbeddedStateAspect< - CompositeTrackingAspect, Derived, State>; + CompositeTrackingAspect, Derived, StateDataT>; using AspectPropertiesImpl = detail::EmbeddedPropertiesAspect< - AspectStateImpl, Derived, Properties>; + AspectStateImpl, Derived, PropertiesDataT>; + + using AspectImpl = Derived; + + using State = typename AspectStateImpl::State; + using StateData = typename AspectStateImpl::StateData; - using AspectImpl = EmbeddedStateAndPropertiesAspect< - CompositeType, State, Properties>; + using Properties = typename AspectPropertiesImpl::Properties; + using PropertiesData = typename AspectPropertiesImpl::PropertiesData; + + using CompositeType = CompositeT; EmbeddedStateAndPropertiesAspect() = delete; EmbeddedStateAndPropertiesAspect( @@ -281,11 +292,37 @@ class EmbeddedStateAndPropertiesAspect : virtual ~EmbeddedStateAndPropertiesAspect() = default; + /// Construct using nothing. The object will remain unaffected. + EmbeddedStateAndPropertiesAspect( + common::Composite* comp) + : AspectPropertiesImpl(comp) + { + // Do nothing + } + + /// Construct using a State. The object's Properties will remain unaffected. + EmbeddedStateAndPropertiesAspect( + common::Composite* comp, + const StateData& state) + : AspectPropertiesImpl(comp, state) + { + // Do nothing + } + + /// Construct using Properties. The object's State will remain unaffected. + EmbeddedStateAndPropertiesAspect( + common::Composite* comp, + const PropertiesData& properties) + : AspectPropertiesImpl(comp, properties) + { + // Do nothing + } + /// Construct using a State and Properties instance EmbeddedStateAndPropertiesAspect( common::Composite* comp, - const State& state = State(), - const Properties& properties = Properties()) + const StateData& state, + const PropertiesData& properties) : AspectPropertiesImpl(comp, properties, state) { // Do nothing @@ -294,8 +331,8 @@ class EmbeddedStateAndPropertiesAspect : /// Construct using a Properties and State instance EmbeddedStateAndPropertiesAspect( common::Composite* comp, - const Properties& properties, - const State& state = State()) + const PropertiesData& properties, + const StateData& state) : AspectPropertiesImpl(comp, properties, state) { // Do nothing @@ -325,19 +362,19 @@ class EmbeddedStateAndPropertiesAspect : template class EmbedStateAndProperties : public virtual common::RequiresAspect< common::EmbeddedStateAndPropertiesAspect< - DerivedT, - common::Aspect::StateMixer, - common::Aspect::PropertiesMixer> > + DerivedT, StateDataT, PropertiesDataT> > { public: using Derived = DerivedT; - using AspectStateData = StateDataT; - using AspectState = common::Aspect::StateMixer; - using AspectPropertiesData = PropertiesDataT; - using AspectProperties = common::Aspect::PropertiesMixer; using Aspect = common::EmbeddedStateAndPropertiesAspect< - Derived, AspectState, AspectProperties>; + DerivedT, StateDataT, PropertiesDataT>; + + using AspectState = typename Aspect::State; + using AspectStateData = typename Aspect::StateData; + + using AspectProperties = typename Aspect::Properties; + using AspectPropertiesData = typename Aspect::PropertiesData; using Base = common::RequiresAspect; // Inherit constructor diff --git a/dart/common/Extensible.h b/dart/common/Extensible.h index 232e6b3d79c40..f88e3e6e2b210 100644 --- a/dart/common/Extensible.h +++ b/dart/common/Extensible.h @@ -85,6 +85,8 @@ class ExtensibleMixer : public T, public Mixin { public: + using Data = Mixin; + /// Default constructor. Uses the default constructor of Mixin ExtensibleMixer(); diff --git a/dart/common/StlHelpers.h b/dart/common/StlHelpers.h index ae1f276b3af85..b80aeb7abf42a 100644 --- a/dart/common/StlHelpers.h +++ b/dart/common/StlHelpers.h @@ -70,6 +70,21 @@ static T getVectorObjectIfAvailable(size_t index, const std::vector& vec) return nullptr; } +//============================================================================== +/// static_if_else allows the compiler to choose between two different possible +/// types at runtime based on a runtime boolean. +template +struct static_if_else +{ + using type = T_else; +}; + +template +struct static_if_else +{ + using type = T_if; +}; + } // namespace common } // namespace dart diff --git a/dart/common/detail/EmbeddedAspect.h b/dart/common/detail/EmbeddedAspect.h index 4a4a1ceda5e1e..d40916e6e5d3f 100644 --- a/dart/common/detail/EmbeddedAspect.h +++ b/dart/common/detail/EmbeddedAspect.h @@ -73,7 +73,8 @@ const PropertiesT& DefaultGetEmbeddedProperties(const AspectT* aspect) } //============================================================================== -template , void (*setEmbeddedState)(DerivedT*, const StateT&) = &DefaultSetEmbeddedState, const StateT& (*getEmbeddedState)(const DerivedT*) = @@ -85,28 +86,56 @@ class EmbeddedStateAspect : public BaseT using Base = BaseT; using Derived = DerivedT; using State = StateT; + using StateData = StateDataT; constexpr static void (*SetEmbeddedState)(Derived*, const State&) = setEmbeddedState; constexpr static const State& (*GetEmbeddedState)(const Derived*) = getEmbeddedState; + enum Delegate_t { Delegate }; + EmbeddedStateAspect() = delete; EmbeddedStateAspect(const EmbeddedStateAspect&) = delete; virtual ~EmbeddedStateAspect() = default; - /// Construct using a State instance - EmbeddedStateAspect(Composite* comp, const State& state = State()) - : BaseT(comp), - mTemporaryState(make_unique(state)) + /// Used to identify constructor arguments that can be used as a State + template + struct ConvertIfState + { + using type = typename static_if_else< + std::is_base_of::value, + StateData, T>::type; + }; + + /// Construct this Aspect without affecting the State. + EmbeddedStateAspect(Composite* comp) + : Base(comp) { // Do nothing } - /// Construct this Aspect and pass args into the constructor of the Base class - template + /// Construct this Aspect. If the first argument contains StateData, then it + /// will be used by this Aspect. Otherwise, all arguments will be forwarded to + /// the Base class. + // + // Dev Note: The complex construction pattern used here allows us to satisfy + // three simultaneous design constraints: + // 1. We can identify when the user has passed in relevant State information + // and capture that information. The type can be of **any** class type + // that inherits StateData. + // 2. We can ignore any non-State information that the user has passed in, + // and move that information along to the Base class. + // 3. We can handle arbitrary numbers of arguments of any type to pass along + // to the Base class. + // If anyone can come up with a cleaner way of accomplishing all three of + // these constraints, I would gladly replace this implementation. -(MXG) + template EmbeddedStateAspect( - Composite* comp, const State& state, BaseArgs&&... args) - : Base(comp, std::forward(args)...), - mTemporaryState(make_unique(state)) + Composite* comp, const T& arg1, + RemainingArgs&&... remainingArgs) + : EmbeddedStateAspect( + Delegate, comp, + static_cast::type&>(arg1), + std::forward(remainingArgs)...) { // Do nothing } @@ -157,6 +186,7 @@ class EmbeddedStateAspect : public BaseT return *mTemporaryState; } + // Documentation inherited std::unique_ptr cloneAspect(Composite* newComposite) const override { return make_unique(newComposite, this->getState()); @@ -164,14 +194,34 @@ class EmbeddedStateAspect : public BaseT protected: + /// Construct this Aspect using the StateData, and pass the remaining + /// arguments into the constructor of the Base class. + template + EmbeddedStateAspect( + Delegate_t, Composite* comp, const StateData& state, + RemainingArgs&&... remainingArgs) + : Base(comp, std::forward(remainingArgs)...), + mTemporaryState(make_unique(state)) + { + // Do nothing + } + + /// Construct this Aspect without affecting the State, and pass all the + /// arguments into the constructor of the Base class. + template + EmbeddedStateAspect( + Delegate_t, Composite* comp, BaseArgs&&... args) + : Base(comp, std::forward(args)...) + { + // Do nothing + } + /// Pass the temporary State of this Aspect into the new Composite void setComposite(Composite* newComposite) override { Base::setComposite(newComposite); if(mTemporaryState) SetEmbeddedState(static_cast(this), *mTemporaryState); - else - SetEmbeddedState(static_cast(this), State()); mTemporaryState = nullptr; } @@ -192,18 +242,24 @@ class EmbeddedStateAspect : public BaseT }; //============================================================================== -template , void (*setEmbeddedProperties)(DerivedT*, const PropertiesT&) = &DefaultSetEmbeddedProperties, const PropertiesT& (*getEmbeddedProperties)(const DerivedT*) = &DefaultGetEmbeddedProperties > class EmbeddedPropertiesAspect : public BaseT { +protected: + + enum Delegate_t { Delegate }; + public: using Base = BaseT; using Derived = DerivedT; using Properties = PropertiesT; + using PropertiesData = PropertiesDataT; constexpr static void (*SetEmbeddedProperties)(Derived*, const Properties&) = setEmbeddedProperties; constexpr static const Properties& (*GetEmbeddedProperties)(const Derived*) = getEmbeddedProperties; @@ -212,21 +268,45 @@ class EmbeddedPropertiesAspect : public BaseT virtual ~EmbeddedPropertiesAspect() = default; - /// Construct using a Properties instance - EmbeddedPropertiesAspect( - Composite* comp, const Properties& properties = Properties()) - : BaseT(comp), - mTemporaryProperties(make_unique(properties)) + /// Used to identify constructor arguments that can be used as Properties + template + struct ConvertIfProperties + { + using type = typename static_if_else< + std::is_base_of::value, + PropertiesData, T>::type; + }; + + /// Construct this Aspect without affecting the Properties. + EmbeddedPropertiesAspect(Composite* comp) + : Base(comp) { // Do nothing } - /// Construct this Aspect and pass args into the constructor of the Base class - template + /// Construct this Aspect. If the first argument contains PropertiesData, then + /// it will be used by this Aspect. Otherwise, all arguments will be forwarded + /// to the Base class. + // + // Dev Note: The complex construction pattern used here allows us to satisfy + // three simultaneous design constraints: + // 1. We can identify when the user has passed in relevant Properties + // information and capture that information. The type can be of **any** + // class type that inherits PropertiesData. + // 2. We can ignore any non-Properties information that the user has passed + // in, and move that information along to the Base class. + // 3. We can handle arbitrary numbers of arguments of any type to pass along + // to the Base class. + // If anyone can come up with a cleaner way of accomplishing all three of + // these constraints, I would gladly replace this implementation. -(MXG) + template EmbeddedPropertiesAspect( - Composite* comp, const Properties& properties, BaseArgs&&... args) - : Base(comp, std::forward(args)...), - mTemporaryProperties(make_unique(properties)) + Composite* comp, const T& arg1, + RemainingArgs&&... remainingArgs) + : EmbeddedPropertiesAspect( + Delegate, comp, + static_cast::type&>(arg1), + std::forward(remainingArgs)...) { // Do nothing } @@ -284,14 +364,34 @@ class EmbeddedPropertiesAspect : public BaseT protected: + /// Construct this Aspect using the PropertiesData, and pass the remaining + /// arguments into the constructor of the Base class. + template + EmbeddedPropertiesAspect( + Delegate_t, Composite* comp, const PropertiesData& properties, + RemainingArgs&&... remainingArgs) + : Base(comp, std::forward(remainingArgs)...), + mTemporaryProperties(make_unique(properties)) + { + // Do nothing + } + + /// Construct this Aspect without affecting the Properties, and pass all the + /// arguments into the constructor of the Base class. + template + EmbeddedPropertiesAspect( + Delegate_t, Composite* comp, BaseArgs&&... args) + : Base(comp, std::forward(args)...) + { + // Do nothing + } + /// Pass the temporary Properties of this Aspect into the new Composite void setComposite(Composite* newComposite) override { Base::setComposite(newComposite); if(mTemporaryProperties) SetEmbeddedProperties(static_cast(this), *mTemporaryProperties); - else - SetEmbeddedProperties(static_cast(this), Properties()); // Release the temporary memory mTemporaryProperties = nullptr; @@ -320,35 +420,39 @@ class EmbeddedPropertiesAspect : public BaseT // // See this StackOverflow answer: http://stackoverflow.com/a/14396189/111426 // -template constexpr void (*EmbeddedStateAspect< - BaseT, DerivedT, StateT, setEmbeddedState, + BaseT, DerivedT, StateDataT, StateT, setEmbeddedState, getEmbeddedState>::SetEmbeddedState)(DerivedT*, const StateT&); //============================================================================== -template constexpr const StateT& (*EmbeddedStateAspect< - BaseT, DerivedT, StateT, setEmbeddedState, + BaseT, DerivedT, StateDataT, StateT, setEmbeddedState, getEmbeddedState>::GetEmbeddedState)(const DerivedT*); //============================================================================== -template constexpr void (*EmbeddedPropertiesAspect< - BaseT, DerivedT, PropertiesT, setEmbeddedProperties, + BaseT, DerivedT, PropertiesDataT, PropertiesT, setEmbeddedProperties, getEmbeddedProperties>::SetEmbeddedProperties)(DerivedT*, const PropertiesT&); //============================================================================== -template constexpr const PropertiesT& (*EmbeddedPropertiesAspect< - BaseT, DerivedT, PropertiesT, setEmbeddedProperties, + BaseT, DerivedT, PropertiesDataT, PropertiesT, setEmbeddedProperties, getEmbeddedProperties>::GetEmbeddedProperties)(const DerivedT*); } // namespace detail diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index eaa6bf857643f..9dcafb433dc49 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -144,10 +144,10 @@ BodyNodeProperties::BodyNodeProperties( BodyNodeExtendedProperties::BodyNodeExtendedProperties( const BodyNodeProperties& standardProperties, const NodeProperties& nodeProperties, - const AspectProperties& aspectProperties) + const CompositeProperties& aspectProperties) : BodyNodeProperties(standardProperties), mNodeProperties(nodeProperties), - mAspectProperties(aspectProperties) + mCompositeProperties(aspectProperties) { // Do nothing } @@ -156,11 +156,11 @@ BodyNodeExtendedProperties::BodyNodeExtendedProperties( BodyNodeExtendedProperties::BodyNodeExtendedProperties( BodyNodeProperties&& standardProperties, NodeProperties&& nodeProperties, - AspectProperties&& aspectProperties) + CompositeProperties&& aspectProperties) : BodyNodeProperties(std::move(standardProperties)) { mNodeProperties = std::move(nodeProperties); - mAspectProperties = std::move(aspectProperties); + mCompositeProperties = std::move(aspectProperties); } } // namespace detail @@ -188,7 +188,7 @@ void BodyNode::setProperties(const ExtendedProperties& _properties) setProperties(_properties.mNodeProperties); - setProperties(_properties.mAspectProperties); + setProperties(_properties.mCompositeProperties); } //============================================================================== @@ -231,7 +231,7 @@ void BodyNode::setProperties(const NodeProperties& _properties) } //============================================================================== -void BodyNode::setProperties(const AspectProperties& _properties) +void BodyNode::setProperties(const CompositeProperties& _properties) { setCompositeProperties(_properties); } @@ -246,26 +246,32 @@ void BodyNode::setProperties(const Properties& _properties) //============================================================================== void BodyNode::setProperties(const UniqueProperties& _properties) { - setInertia(_properties.mInertia); - setGravityMode(_properties.mGravityMode); - setFrictionCoeff(_properties.mFrictionCoeff); - setRestitutionCoeff(_properties.mRestitutionCoeff); + setAspectProperties(_properties); +} + +//============================================================================== +void BodyNode::setAspectProperties(const AspectProperties& properties) +{ + setInertia(properties.mInertia); + setGravityMode(properties.mGravityMode); + setFrictionCoeff(properties.mFrictionCoeff); + setRestitutionCoeff(properties.mRestitutionCoeff); - mBodyP.mMarkerProperties = _properties.mMarkerProperties; + mAspectProperties.mMarkerProperties = properties.mMarkerProperties; // Remove current markers for(Marker* marker : mMarkers) delete marker; // Create new markers mMarkers.clear(); - for(const Marker::Properties& marker : mBodyP.mMarkerProperties) + for(const Marker::Properties& marker : mAspectProperties.mMarkerProperties) addMarker(new Marker(marker, this)); } //============================================================================== BodyNode::Properties BodyNode::getBodyNodeProperties() const { - return BodyNode::Properties(mEntityP, mBodyP); + return BodyNode::Properties(mEntityP, mAspectProperties); } //============================================================================== @@ -397,10 +403,10 @@ const std::string& BodyNode::setName(const std::string& _name) //============================================================================== void BodyNode::setGravityMode(bool _gravityMode) { - if (mBodyP.mGravityMode == _gravityMode) + if (mAspectProperties.mGravityMode == _gravityMode) return; - mBodyP.mGravityMode = _gravityMode; + mAspectProperties.mGravityMode = _gravityMode; SKEL_SET_FLAGS(mGravityForces); SKEL_SET_FLAGS(mCoriolisAndGravityForces); @@ -409,19 +415,19 @@ void BodyNode::setGravityMode(bool _gravityMode) //============================================================================== bool BodyNode::getGravityMode() const { - return mBodyP.mGravityMode; + return mAspectProperties.mGravityMode; } //============================================================================== bool BodyNode::isCollidable() const { - return mBodyP.mIsCollidable; + return mAspectProperties.mIsCollidable; } //============================================================================== void BodyNode::setCollidable(bool _isCollidable) { - mBodyP.mIsCollidable = _isCollidable; + mAspectProperties.mIsCollidable = _isCollidable; } //============================================================================== @@ -429,7 +435,7 @@ void BodyNode::setMass(double _mass) { assert(_mass >= 0.0 && "Negative mass is not allowable."); - mBodyP.mInertia.setMass(_mass); + mAspectProperties.mInertia.setMass(_mass); notifyArticulatedInertiaUpdate(); const SkeletonPtr& skel = getSkeleton(); @@ -440,15 +446,16 @@ void BodyNode::setMass(double _mass) //============================================================================== double BodyNode::getMass() const { - return mBodyP.mInertia.getMass(); + return mAspectProperties.mInertia.getMass(); } //============================================================================== void BodyNode::setMomentOfInertia(double _Ixx, double _Iyy, double _Izz, double _Ixy, double _Ixz, double _Iyz) { - mBodyP.mInertia.setMoment(_Ixx, _Iyy, _Izz, - _Ixy, _Ixz, _Iyz); + mAspectProperties.mInertia.setMoment( + _Ixx, _Iyy, _Izz, + _Ixy, _Ixz, _Iyz); notifyArticulatedInertiaUpdate(); } @@ -458,25 +465,25 @@ void BodyNode::getMomentOfInertia( double& _Ixx, double& _Iyy, double& _Izz, double& _Ixy, double& _Ixz, double& _Iyz) const { - _Ixx = mBodyP.mInertia.getParameter(Inertia::I_XX); - _Iyy = mBodyP.mInertia.getParameter(Inertia::I_YY); - _Izz = mBodyP.mInertia.getParameter(Inertia::I_ZZ); + _Ixx = mAspectProperties.mInertia.getParameter(Inertia::I_XX); + _Iyy = mAspectProperties.mInertia.getParameter(Inertia::I_YY); + _Izz = mAspectProperties.mInertia.getParameter(Inertia::I_ZZ); - _Ixy = mBodyP.mInertia.getParameter(Inertia::I_XY); - _Ixz = mBodyP.mInertia.getParameter(Inertia::I_XZ); - _Iyz = mBodyP.mInertia.getParameter(Inertia::I_YZ); + _Ixy = mAspectProperties.mInertia.getParameter(Inertia::I_XY); + _Ixz = mAspectProperties.mInertia.getParameter(Inertia::I_XZ); + _Iyz = mAspectProperties.mInertia.getParameter(Inertia::I_YZ); } //============================================================================== const Eigen::Matrix6d& BodyNode::getSpatialInertia() const { - return mBodyP.mInertia.getSpatialTensor(); + return mAspectProperties.mInertia.getSpatialTensor(); } //============================================================================== void BodyNode::setInertia(const Inertia& _inertia) { - mBodyP.mInertia = _inertia; + mAspectProperties.mInertia = _inertia; notifyArticulatedInertiaUpdate(); const SkeletonPtr& skel = getSkeleton(); @@ -487,7 +494,7 @@ void BodyNode::setInertia(const Inertia& _inertia) //============================================================================== const Inertia& BodyNode::getInertia() const { - return mBodyP.mInertia; + return mAspectProperties.mInertia; } //============================================================================== @@ -513,7 +520,7 @@ const math::Inertia& BodyNode::getArticulatedInertiaImplicit() const //============================================================================== void BodyNode::setLocalCOM(const Eigen::Vector3d& _com) { - mBodyP.mInertia.setLocalCOM(_com); + mAspectProperties.mInertia.setLocalCOM(_com); notifyArticulatedInertiaUpdate(); } @@ -521,7 +528,7 @@ void BodyNode::setLocalCOM(const Eigen::Vector3d& _com) //============================================================================== const Eigen::Vector3d& BodyNode::getLocalCOM() const { - return mBodyP.mInertia.getLocalCOM(); + return mAspectProperties.mInertia.getLocalCOM(); } //============================================================================== @@ -575,13 +582,13 @@ void BodyNode::setFrictionCoeff(double _coeff) { assert(0.0 <= _coeff && "Coefficient of friction should be non-negative value."); - mBodyP.mFrictionCoeff = _coeff; + mAspectProperties.mFrictionCoeff = _coeff; } //============================================================================== double BodyNode::getFrictionCoeff() const { - return mBodyP.mFrictionCoeff; + return mAspectProperties.mFrictionCoeff; } //============================================================================== @@ -589,13 +596,13 @@ void BodyNode::setRestitutionCoeff(double _coeff) { assert(0.0 <= _coeff && _coeff <= 1.0 && "Coefficient of restitution should be in range of [0, 1]."); - mBodyP.mRestitutionCoeff = _coeff; + mAspectProperties.mRestitutionCoeff = _coeff; } //============================================================================== double BodyNode::getRestitutionCoeff() const { - return mBodyP.mRestitutionCoeff; + return mAspectProperties.mRestitutionCoeff; } //============================================================================== @@ -1567,8 +1574,8 @@ void BodyNode::updateTransmittedForceID(const Eigen::Vector3d& _gravity, bool _withExternalForces) { // Gravity force - const Eigen::Matrix6d& mI = mBodyP.mInertia.getSpatialTensor(); - if (mBodyP.mGravityMode == true) + const Eigen::Matrix6d& mI = mAspectProperties.mInertia.getSpatialTensor(); + if (mAspectProperties.mGravityMode == true) mFgravity.noalias() = mI * math::AdInvRLinear(getWorldTransform(),_gravity); else mFgravity.setZero(); @@ -1608,7 +1615,7 @@ void BodyNode::updateTransmittedForceID(const Eigen::Vector3d& _gravity, void BodyNode::updateArtInertia(double _timeStep) const { // Set spatial inertia to the articulated body inertia - const Eigen::Matrix6d& mI = mBodyP.mInertia.getSpatialTensor(); + const Eigen::Matrix6d& mI = mAspectProperties.mInertia.getSpatialTensor(); mArtInertia = mI; mArtInertiaImplicit = mI; @@ -1640,8 +1647,8 @@ void BodyNode::updateBiasForce(const Eigen::Vector3d& _gravity, double _timeStep) { // Gravity force - const Eigen::Matrix6d& mI = mBodyP.mInertia.getSpatialTensor(); - if (mBodyP.mGravityMode == true) + const Eigen::Matrix6d& mI = mAspectProperties.mInertia.getSpatialTensor(); + if (mAspectProperties.mGravityMode == true) mFgravity.noalias() = mI * math::AdInvRLinear(getWorldTransform(),_gravity); else mFgravity.setZero(); @@ -1897,7 +1904,7 @@ const Eigen::Vector6d& BodyNode::getConstraintImpulse() const double BodyNode::getKineticEnergy() const { const Eigen::Vector6d& V = getSpatialVelocity(); - const Eigen::Matrix6d& mI = mBodyP.mInertia.getSpatialTensor(); + const Eigen::Matrix6d& mI = mAspectProperties.mInertia.getSpatialTensor(); return 0.5 * V.dot(mI * V); } @@ -1910,7 +1917,7 @@ double BodyNode::getPotentialEnergy(const Eigen::Vector3d& _gravity) const //============================================================================== Eigen::Vector3d BodyNode::getLinearMomentum() const { - const Eigen::Matrix6d& mI = mBodyP.mInertia.getSpatialTensor(); + const Eigen::Matrix6d& mI = mAspectProperties.mInertia.getSpatialTensor(); return (mI * getSpatialVelocity()).tail<3>(); } @@ -1918,7 +1925,7 @@ Eigen::Vector3d BodyNode::getLinearMomentum() const Eigen::Vector3d BodyNode::getAngularMomentum(const Eigen::Vector3d& _pivot) { Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); - const Eigen::Matrix6d& mI = mBodyP.mInertia.getSpatialTensor(); + const Eigen::Matrix6d& mI = mAspectProperties.mInertia.getSpatialTensor(); T.translation() = _pivot; return math::dAdT(T, mI * getSpatialVelocity()).head<3>(); } @@ -1960,8 +1967,8 @@ void BodyNode::aggregateCoriolisForceVector(Eigen::VectorXd& _C) void BodyNode::aggregateGravityForceVector(Eigen::VectorXd& _g, const Eigen::Vector3d& _gravity) { - const Eigen::Matrix6d& mI = mBodyP.mInertia.getSpatialTensor(); - if (mBodyP.mGravityMode == true) + const Eigen::Matrix6d& mI = mAspectProperties.mInertia.getSpatialTensor(); + if (mAspectProperties.mGravityMode == true) mG_F = mI * math::AdInvRLinear(getWorldTransform(), _gravity); else mG_F.setZero(); @@ -2002,8 +2009,8 @@ void BodyNode::aggregateCombinedVector(Eigen::VectorXd& _Cg, { // H(i) = I(i) * W(i) - // dad{V}(I(i) * V(i)) + sum(k \in children) dAd_{T(i,j)^{-1}}(H(k)) - const Eigen::Matrix6d& mI = mBodyP.mInertia.getSpatialTensor(); - if (mBodyP.mGravityMode == true) + const Eigen::Matrix6d& mI = mAspectProperties.mInertia.getSpatialTensor(); + if (mAspectProperties.mGravityMode == true) mFgravity = mI * math::AdInvRLinear(getWorldTransform(), _gravity); else mFgravity.setZero(); @@ -2091,7 +2098,7 @@ void BodyNode::updateMassMatrix() //============================================================================== void BodyNode::aggregateMassMatrix(Eigen::MatrixXd& _MCol, size_t _col) { - const Eigen::Matrix6d& mI = mBodyP.mInertia.getSpatialTensor(); + const Eigen::Matrix6d& mI = mAspectProperties.mInertia.getSpatialTensor(); // mM_F.noalias() = mI * mM_dV; @@ -2124,7 +2131,7 @@ void BodyNode::aggregateAugMassMatrix(Eigen::MatrixXd& _MCol, size_t _col, double _timeStep) { // TODO(JS): Need to be reimplemented - const Eigen::Matrix6d& mI = mBodyP.mInertia.getSpatialTensor(); + const Eigen::Matrix6d& mI = mAspectProperties.mInertia.getSpatialTensor(); // mM_F.noalias() = mI * mM_dV; diff --git a/dart/dynamics/BodyNode.h b/dart/dynamics/BodyNode.h index 211bfa7e0d01a..251e07133aa73 100644 --- a/dart/dynamics/BodyNode.h +++ b/dart/dynamics/BodyNode.h @@ -46,6 +46,7 @@ #include "dart/config.h" #include "dart/common/Signal.h" +#include "dart/common/EmbeddedAspect.h" #include "dart/math/Geometry.h" #include "dart/dynamics/Node.h" #include "dart/dynamics/Frame.h" @@ -80,7 +81,7 @@ class Marker; /// BodyNode inherits Frame, and a parent Frame of a BodyNode is the parent /// BodyNode of the BodyNode. class BodyNode : - public virtual common::Composite, + public common::EmbedProperties, public virtual BodyNodeSpecializedFor, public SkeletonRefCountingBase, public TemplatedJacobianNode @@ -98,7 +99,7 @@ class BodyNode : using NodePropertiesVector = common::ExtensibleVector< std::unique_ptr >; using NodePropertiesMap = std::map< std::type_index, std::unique_ptr >; using NodeProperties = common::ExtensibleMapHolder; - using AspectProperties = common::Composite::Properties; + using CompositeProperties = common::Composite::Properties; using UniqueProperties = detail::BodyNodeUniqueProperties; using Properties = detail::BodyNodeProperties; @@ -116,7 +117,7 @@ class BodyNode : void setProperties(const NodeProperties& _properties); /// Same as setCompositeProperties() - void setProperties(const AspectProperties& _properties); + void setProperties(const CompositeProperties& _properties); /// Set the Properties of this BodyNode void setProperties(const Properties& _properties); @@ -124,6 +125,9 @@ class BodyNode : /// Set the UniqueProperties of this BodyNode void setProperties(const UniqueProperties& _properties); + /// Set the AspectProperties of this BodyNode + void setAspectProperties(const AspectProperties& properties); + /// Get the Properties of this BodyNode Properties getBodyNodeProperties() const; @@ -1044,9 +1048,6 @@ class BodyNode : /// Counts the number of nodes globally. static size_t msBodyNodeCount; - /// BodyNode-specific properties - UniqueProperties mBodyP; - /// Whether the node is currently in collision with another node. bool mIsColliding; diff --git a/dart/dynamics/EulerJoint.h b/dart/dynamics/EulerJoint.h index def209bdf8e63..ea4d5b52f2df5 100644 --- a/dart/dynamics/EulerJoint.h +++ b/dart/dynamics/EulerJoint.h @@ -150,8 +150,6 @@ class EulerJoint : public detail::EulerJointBase Eigen::Matrix getLocalJacobianStatic( const Eigen::Vector3d& _positions) const override; - template friend void detail::JointPropertyUpdate(AspectType*); - protected: /// Constructor called by Skeleton class diff --git a/dart/dynamics/Joint.h b/dart/dynamics/Joint.h index c603affe8a0e1..b9737ca4490d0 100644 --- a/dart/dynamics/Joint.h +++ b/dart/dynamics/Joint.h @@ -917,17 +917,6 @@ class Joint : public virtual common::Subject, EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -namespace detail { - -template -void JointPropertyUpdate(AspectType* aspect) -{ - aspect->getComposite()->notifyPositionUpdate(); - aspect->getComposite()->updateLocalJacobian(); -} - -} // namespace detail - } // namespace dynamics } // namespace dart diff --git a/dart/dynamics/PlanarJoint.h b/dart/dynamics/PlanarJoint.h index 1ebff9ef9edb8..bc5476a0ed2f5 100644 --- a/dart/dynamics/PlanarJoint.h +++ b/dart/dynamics/PlanarJoint.h @@ -134,8 +134,6 @@ class PlanarJoint : public detail::PlanarJointBase Eigen::Matrix getLocalJacobianStatic( const Eigen::Vector3d& _positions) const override; - template friend void detail::JointPropertyUpdate(AspectType*); - protected: /// Constructor called by Skeleton class diff --git a/dart/dynamics/ScrewJoint.h b/dart/dynamics/ScrewJoint.h index d7bc99c2b752f..20500c8e8aa6b 100644 --- a/dart/dynamics/ScrewJoint.h +++ b/dart/dynamics/ScrewJoint.h @@ -101,8 +101,6 @@ class ScrewJoint : public detail::ScrewJointBase /// double getPitch() const; - template friend void detail::JointPropertyUpdate(AspectType*); - protected: /// Constructor called by Skeleton class diff --git a/dart/dynamics/SingleDofJoint.cpp b/dart/dynamics/SingleDofJoint.cpp index dc48494888707..70643d1ead326 100644 --- a/dart/dynamics/SingleDofJoint.cpp +++ b/dart/dynamics/SingleDofJoint.cpp @@ -86,6 +86,19 @@ void SingleDofJoint::setProperties(const UniqueProperties& _properties) setAspectProperties(_properties); } +//============================================================================== +void SingleDofJoint::setAspectState(const AspectState& state) +{ + setPositionStatic(state.mPosition); + setVelocityStatic(state.mVelocity); + setAccelerationStatic(state.mAcceleration); + setForce(0, state.mForce); + + // We call Command last so that it does not get overwritten due to actuator + // type interference. + setCommand(0, state.mCommand); +} + //============================================================================== void SingleDofJoint::setAspectProperties(const AspectProperties& properties) { @@ -268,7 +281,7 @@ void SingleDofJoint::setCommand(size_t _index, double _command) switch (Joint::mAspectProperties.mActuatorType) { case FORCE: - mCommand = math::clip(_command, + mAspectState.mCommand = math::clip(_command, mAspectProperties.mForceLowerLimit, mAspectProperties.mForceUpperLimit); break; @@ -279,20 +292,20 @@ void SingleDofJoint::setCommand(size_t _index, double _command) << _command << ") command for a PASSIVE joint [" << getName() << "].\n"; } - mCommand = _command; + mAspectState.mCommand = _command; break; case SERVO: - mCommand = math::clip(_command, + mAspectState.mCommand = math::clip(_command, mAspectProperties.mVelocityLowerLimit, mAspectProperties.mVelocityUpperLimit); break; case ACCELERATION: - mCommand = math::clip(_command, + mAspectState.mCommand = math::clip(_command, mAspectProperties.mAccelerationLowerLimit, mAspectProperties.mAccelerationUpperLimit); break; case VELOCITY: - mCommand = math::clip(_command, + mAspectState.mCommand = math::clip(_command, mAspectProperties.mVelocityLowerLimit, mAspectProperties.mVelocityUpperLimit); // TODO: This possibly makes the acceleration to exceed the limits. @@ -304,7 +317,7 @@ void SingleDofJoint::setCommand(size_t _index, double _command) << _command << ") command for a LOCKED joint [" << getName() << "].\n"; } - mCommand = _command; + mAspectState.mCommand = _command; break; default: assert(false); @@ -321,7 +334,7 @@ double SingleDofJoint::getCommand(size_t _index) const return 0.0; } - return mCommand; + return mAspectState.mCommand; } //============================================================================== @@ -339,13 +352,13 @@ void SingleDofJoint::setCommands(const Eigen::VectorXd& _commands) //============================================================================== Eigen::VectorXd SingleDofJoint::getCommands() const { - return Eigen::Matrix::Constant(mCommand); + return Eigen::Matrix::Constant(mAspectState.mCommand); } //============================================================================== void SingleDofJoint::resetCommands() { - mCommand = 0.0; + mAspectState.mCommand = 0.0; } //============================================================================== @@ -525,7 +538,7 @@ void SingleDofJoint::setVelocity(size_t _index, double _velocity) #if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) if (Joint::mAspectProperties.mActuatorType == VELOCITY) - mCommand = getVelocityStatic(); + mAspectState.mCommand = getVelocityStatic(); // TODO: Remove at DART 5.1. #endif } @@ -555,7 +568,7 @@ void SingleDofJoint::setVelocities(const Eigen::VectorXd& _velocities) #if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) if (Joint::mAspectProperties.mActuatorType == VELOCITY) - mCommand = getVelocityStatic(); + mAspectState.mCommand = getVelocityStatic(); // TODO: Remove at DART 5.1. #endif } @@ -688,7 +701,7 @@ void SingleDofJoint::setAcceleration(size_t _index, double _acceleration) #if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) if (Joint::mAspectProperties.mActuatorType == ACCELERATION) - mCommand = getAccelerationStatic(); + mAspectState.mCommand = getAccelerationStatic(); // TODO: Remove at DART 5.1. #endif } @@ -718,7 +731,7 @@ void SingleDofJoint::setAccelerations(const Eigen::VectorXd& _accelerations) #if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) if (Joint::mAspectProperties.mActuatorType == ACCELERATION) - mCommand = getAccelerationStatic(); + mAspectState.mCommand = getAccelerationStatic(); // TODO: Remove at DART 5.1. #endif } @@ -788,49 +801,49 @@ double SingleDofJoint::getAccelerationUpperLimit(size_t _index) const //============================================================================== void SingleDofJoint::setPositionStatic(const double& _position) { - if(mPosition == _position) + if(mAspectState.mPosition == _position) return; - mPosition = _position; + mAspectState.mPosition = _position; notifyPositionUpdate(); } //============================================================================== const double& SingleDofJoint::getPositionStatic() const { - return mPosition; + return mAspectState.mPosition; } //============================================================================== void SingleDofJoint::setVelocityStatic(const double& _velocity) { - if(mVelocity == _velocity) + if(mAspectState.mVelocity == _velocity) return; - mVelocity = _velocity; + mAspectState.mVelocity = _velocity; notifyVelocityUpdate(); } //============================================================================== const double& SingleDofJoint::getVelocityStatic() const { - return mVelocity; + return mAspectState.mVelocity; } //============================================================================== void SingleDofJoint::setAccelerationStatic(const double& _acceleration) { - if(mAcceleration == _acceleration) + if(mAspectState.mAcceleration == _acceleration) return; - mAcceleration = _acceleration; + mAspectState.mAcceleration = _acceleration; notifyAccelerationUpdate(); } //============================================================================== const double& SingleDofJoint::getAccelerationStatic() const { - return mAcceleration; + return mAspectState.mAcceleration; } //============================================================================== @@ -842,11 +855,11 @@ void SingleDofJoint::setForce(size_t _index, double _force) return; } - mForce = _force; + mAspectState.mForce = _force; #if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) if (Joint::mAspectProperties.mActuatorType == FORCE) - mCommand = mForce; + mAspectState.mCommand = mAspectState.mForce; // TODO: Remove at DART 5.1. #endif } @@ -860,7 +873,7 @@ double SingleDofJoint::getForce(size_t _index) return 0.0; } - return mForce; + return mAspectState.mForce; } //============================================================================== @@ -872,11 +885,11 @@ void SingleDofJoint::setForces(const Eigen::VectorXd& _forces) return; } - mForce = _forces[0]; + mAspectState.mForce = _forces[0]; #if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) if (Joint::mAspectProperties.mActuatorType == FORCE) - mCommand = mForce; + mAspectState.mCommand = mAspectState.mForce; // TODO: Remove at DART 5.1. #endif } @@ -884,17 +897,17 @@ void SingleDofJoint::setForces(const Eigen::VectorXd& _forces) //============================================================================== Eigen::VectorXd SingleDofJoint::getForces() const { - return Eigen::Matrix::Constant(mForce); + return Eigen::Matrix::Constant(mAspectState.mForce); } //============================================================================== void SingleDofJoint::resetForces() { - mForce = 0.0; + mAspectState.mForce = 0.0; #if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) if (Joint::mAspectProperties.mActuatorType == FORCE) - mCommand = mForce; + mAspectState.mCommand = mAspectState.mForce; // TODO: Remove at DART 5.1. #endif } @@ -1172,15 +1185,6 @@ double SingleDofJoint::getPotentialEnergy() const //============================================================================== SingleDofJoint::SingleDofJoint(const Properties& properties) : mDof(createDofPointer(0)), - mCommand(0.0), - mPosition(properties.mInitialPosition), - mPositionDeriv(0.0), - mVelocity(properties.mInitialVelocity), - mVelocityDeriv(0.0), - mAcceleration(0.0), - mAccelerationDeriv(0.0), - mForce(0.0), - mForceDeriv(0.0), mVelocityChange(0.0), mImpulse(0.0), mConstraintImpulse(0.0), @@ -1193,8 +1197,8 @@ SingleDofJoint::SingleDofJoint(const Properties& properties) mInvM_a(0.0), mInvMassMatrixSegment(0.0) { - // Do nothing. Joint and SingleDofJoint Aspects must be created by the most - // derived class. + mAspectState.mPosition = properties.mInitialPosition; + mAspectState.mVelocity = properties.mInitialVelocity; } //============================================================================== @@ -1237,7 +1241,8 @@ void SingleDofJoint::updateLocalPrimaryAcceleration() const Eigen::Vector6d SingleDofJoint::getBodyConstraintWrench() const { assert(mChildBodyNode); - return mChildBodyNode->getBodyForce() - getLocalJacobianStatic() * mForce; + return mChildBodyNode->getBodyForce() + - getLocalJacobianStatic() * mAspectState.mForce; } //============================================================================== @@ -1672,20 +1677,20 @@ void SingleDofJoint::updateTotalForce(const Eigen::Vector6d& _bodyForce, switch (Joint::mAspectProperties.mActuatorType) { case FORCE: - mForce = mCommand; + mAspectState.mForce = mAspectState.mCommand; updateTotalForceDynamic(_bodyForce, _timeStep); break; case PASSIVE: case SERVO: - mForce = 0.0; + mAspectState.mForce = 0.0; updateTotalForceDynamic(_bodyForce, _timeStep); break; case ACCELERATION: - setAccelerationStatic(mCommand); + setAccelerationStatic(mAspectState.mCommand); updateTotalForceKinematic(_bodyForce, _timeStep); break; case VELOCITY: - setAccelerationStatic( (mCommand - getVelocityStatic()) / _timeStep ); + setAccelerationStatic( (mAspectState.mCommand - getVelocityStatic()) / _timeStep ); updateTotalForceKinematic(_bodyForce, _timeStep); break; case LOCKED: @@ -1715,7 +1720,7 @@ void SingleDofJoint::updateTotalForceDynamic( -mAspectProperties.mDampingCoefficient * getVelocityStatic(); // Compute alpha - mTotalForce = mForce + springForce + dampingForce + mTotalForce = mAspectState.mForce + springForce + dampingForce - getLocalJacobianStatic().dot(_bodyForce); } @@ -1862,14 +1867,14 @@ void SingleDofJoint::updateForceID(const Eigen::Vector6d& _bodyForce, bool _withDampingForces, bool _withSpringForces) { - mForce = getLocalJacobianStatic().dot(_bodyForce); + mAspectState.mForce = getLocalJacobianStatic().dot(_bodyForce); // Damping force if (_withDampingForces) { const double dampingForce = -mAspectProperties.mDampingCoefficient * getVelocityStatic(); - mForce -= dampingForce; + mAspectState.mForce -= dampingForce; } // Spring force @@ -1880,7 +1885,7 @@ void SingleDofJoint::updateForceID(const Eigen::Vector6d& _bodyForce, const double springForce = -mAspectProperties.mSpringStiffness *(nextPosition - mAspectProperties.mRestPosition); - mForce -= springForce; + mAspectState.mForce -= springForce; } } @@ -1962,13 +1967,13 @@ void SingleDofJoint::updateConstrainedTermsDynamic(double _timeStep) setVelocityStatic(getVelocityStatic() + mVelocityChange); setAccelerationStatic(getAccelerationStatic() + mVelocityChange*invTimeStep); - mForce += mConstraintImpulse*invTimeStep; + mAspectState.mForce += mConstraintImpulse*invTimeStep; } //============================================================================== void SingleDofJoint::updateConstrainedTermsKinematic(double _timeStep) { - mForce += mImpulse / _timeStep; + mAspectState.mForce += mImpulse / _timeStep; } //============================================================================== @@ -2014,7 +2019,7 @@ void SingleDofJoint::updateTotalForceForInvMassMatrix( const Eigen::Vector6d& _bodyForce) { // Compute alpha - mInvM_a = mForce - getLocalJacobianStatic().dot(_bodyForce); + mInvM_a = mAspectState.mForce - getLocalJacobianStatic().dot(_bodyForce); } //============================================================================== diff --git a/dart/dynamics/SingleDofJoint.h b/dart/dynamics/SingleDofJoint.h index 08d5a979b06d1..4d297d2c58ae7 100644 --- a/dart/dynamics/SingleDofJoint.h +++ b/dart/dynamics/SingleDofJoint.h @@ -68,6 +68,9 @@ class SingleDofJoint : public detail::SingleDofJointBase /// Set the Properties of this SingleDofJoint void setProperties(const UniqueProperties& _properties); + /// Set the AspectState of this SingleDofJoint + void setAspectState(const AspectState& state); + /// Set the AspectProperties of this SingleDofJoint void setAspectProperties(const AspectProperties& properties); @@ -587,49 +590,6 @@ class SingleDofJoint : public detail::SingleDofJointBase /// \brief DegreeOfFreedom pointer DegreeOfFreedom* mDof; - /// Command - double mCommand; - - //---------------------------------------------------------------------------- - // Configuration - //---------------------------------------------------------------------------- - - /// Position - double mPosition; - - /// Derivatives w.r.t. an arbitrary scalr variable - double mPositionDeriv; - - //---------------------------------------------------------------------------- - // Velocity - //---------------------------------------------------------------------------- - - /// Generalized velocity - double mVelocity; - - /// Derivatives w.r.t. an arbitrary scalr variable - double mVelocityDeriv; - - //---------------------------------------------------------------------------- - // Acceleration - //---------------------------------------------------------------------------- - - /// Generalized acceleration - double mAcceleration; - - /// Derivatives w.r.t. an arbitrary scalr variable - double mAccelerationDeriv; - - //---------------------------------------------------------------------------- - // Force - //---------------------------------------------------------------------------- - - /// Generalized force - double mForce; - - /// Derivatives w.r.t. an arbitrary scalr variable - double mForceDeriv; - //---------------------------------------------------------------------------- // Impulse //---------------------------------------------------------------------------- diff --git a/dart/dynamics/SoftBodyNode.cpp b/dart/dynamics/SoftBodyNode.cpp index 48772b17e8972..cf0bdfb142fcc 100644 --- a/dart/dynamics/SoftBodyNode.cpp +++ b/dart/dynamics/SoftBodyNode.cpp @@ -537,12 +537,12 @@ void SoftBodyNode::updateAccelerationID() void SoftBodyNode::updateTransmittedForceID(const Eigen::Vector3d& _gravity, bool _withExternalForces) { - const Eigen::Matrix6d& mI = mBodyP.mInertia.getSpatialTensor(); + const Eigen::Matrix6d& mI = mAspectProperties.mInertia.getSpatialTensor(); for (auto& pointMass : mPointMasses) pointMass->updateTransmittedForceID(_gravity, _withExternalForces); // Gravity force - if (mBodyP.mGravityMode == true) + if (mAspectProperties.mGravityMode == true) mFgravity.noalias() = mI * math::AdInvRLinear(getWorldTransform(),_gravity); else mFgravity.setZero(); @@ -617,7 +617,7 @@ void SoftBodyNode::updateJointImpulseFD() //============================================================================== void SoftBodyNode::updateArtInertia(double _timeStep) const { - const Eigen::Matrix6d& mI = mBodyP.mInertia.getSpatialTensor(); + const Eigen::Matrix6d& mI = mAspectProperties.mInertia.getSpatialTensor(); for (auto& pointMass : mPointMasses) pointMass->updateArtInertiaFD(_timeStep); @@ -662,12 +662,12 @@ void SoftBodyNode::updateArtInertia(double _timeStep) const void SoftBodyNode::updateBiasForce(const Eigen::Vector3d& _gravity, double _timeStep) { - const Eigen::Matrix6d& mI = mBodyP.mInertia.getSpatialTensor(); + const Eigen::Matrix6d& mI = mAspectProperties.mInertia.getSpatialTensor(); for (auto& pointMass : mPointMasses) pointMass->updateBiasForceFD(_timeStep, _gravity); // Gravity force - if (mBodyP.mGravityMode == true) + if (mAspectProperties.mGravityMode == true) mFgravity.noalias() = mI * math::AdInvRLinear(getWorldTransform(),_gravity); else mFgravity.setZero(); @@ -845,7 +845,7 @@ void SoftBodyNode::aggregateAugMassMatrix(Eigen::MatrixXd& _MCol, size_t _col, { // TODO(JS): Need to be reimplemented - const Eigen::Matrix6d& mI = mBodyP.mInertia.getSpatialTensor(); + const Eigen::Matrix6d& mI = mAspectProperties.mInertia.getSpatialTensor(); //------------------------ PointMass Part ------------------------------------ for (size_t i = 0; i < mPointMasses.size(); ++i) mPointMasses.at(i)->aggregateAugMassMatrix(_MCol, _col, _timeStep); @@ -1033,13 +1033,13 @@ void SoftBodyNode::aggregateCoriolisForceVector(Eigen::VectorXd& _C) void SoftBodyNode::aggregateGravityForceVector(Eigen::VectorXd& _g, const Eigen::Vector3d& _gravity) { - const Eigen::Matrix6d& mI = mBodyP.mInertia.getSpatialTensor(); + const Eigen::Matrix6d& mI = mAspectProperties.mInertia.getSpatialTensor(); //------------------------ PointMass Part ------------------------------------ for (size_t i = 0; i < mPointMasses.size(); ++i) mPointMasses.at(i)->aggregateGravityForceVector(_g, _gravity); //----------------------- SoftBodyNode Part ---------------------------------- - if (mBodyP.mGravityMode == true) + if (mAspectProperties.mGravityMode == true) mG_F = mI * math::AdInvRLinear(getWorldTransform(), _gravity); else mG_F.setZero(); @@ -1303,7 +1303,7 @@ void SoftBodyNode::updateInertiaWithPointMass() { // TODO(JS): Not implemented - const Eigen::Matrix6d& mI = mBodyP.mInertia.getSpatialTensor(); + const Eigen::Matrix6d& mI = mAspectProperties.mInertia.getSpatialTensor(); mI2 = mI; for (size_t i = 0; i < mPointMasses.size(); ++i) diff --git a/dart/dynamics/UniversalJoint.h b/dart/dynamics/UniversalJoint.h index 6cd033bdebceb..e4af7096845b4 100644 --- a/dart/dynamics/UniversalJoint.h +++ b/dart/dynamics/UniversalJoint.h @@ -105,8 +105,6 @@ class UniversalJoint : public detail::UniversalJointBase Eigen::Matrix getLocalJacobianStatic( const Eigen::Vector2d& _positions) const override; - template friend void detail::JointPropertyUpdate(AspectType*); - protected: /// Constructor called by Skeleton class diff --git a/dart/dynamics/detail/BodyNodeProperties.h b/dart/dynamics/detail/BodyNodeProperties.h index 34867cbc06569..545502735ff02 100644 --- a/dart/dynamics/detail/BodyNodeProperties.h +++ b/dart/dynamics/detail/BodyNodeProperties.h @@ -104,25 +104,25 @@ struct BodyNodeExtendedProperties : BodyNodeProperties using NodePropertiesVector = common::ExtensibleVector< std::unique_ptr >; using NodePropertiesMap = std::map< std::type_index, std::unique_ptr >; using NodeProperties = common::ExtensibleMapHolder; - using AspectProperties = common::Composite::Properties; + using CompositeProperties = common::Composite::Properties; /// Composed constructor BodyNodeExtendedProperties( const BodyNodeProperties& standardProperties = Properties(), const NodeProperties& nodeProperties = NodeProperties(), - const AspectProperties& aspectProperties = AspectProperties()); + const CompositeProperties& aspectProperties = CompositeProperties()); /// Composed move constructor BodyNodeExtendedProperties( BodyNodeProperties&& standardProperties, NodeProperties&& nodeProperties, - AspectProperties&& aspectProperties); + CompositeProperties&& aspectProperties); /// Properties of all the Nodes attached to this BodyNode NodeProperties mNodeProperties; /// Properties of all the Aspects attached to this BodyNode - AspectProperties mAspectProperties; + CompositeProperties mCompositeProperties; }; } // namespace detail diff --git a/dart/dynamics/detail/SingleDofJointProperties.cpp b/dart/dynamics/detail/SingleDofJointProperties.cpp index 85c085c539dcd..17cb99148eff8 100644 --- a/dart/dynamics/detail/SingleDofJointProperties.cpp +++ b/dart/dynamics/detail/SingleDofJointProperties.cpp @@ -40,6 +40,22 @@ namespace dart { namespace dynamics { namespace detail { +//============================================================================== +SingleDofJointState::SingleDofJointState( + double position, + double velocity, + double acceleration, + double force, + double command) + : mPosition(position), + mVelocity(velocity), + mAcceleration(acceleration), + mForce(force), + mCommand(command) +{ + // Do nothing +} + //============================================================================== SingleDofJointUniqueProperties::SingleDofJointUniqueProperties( double _positionLowerLimit, diff --git a/dart/dynamics/detail/SingleDofJointProperties.h b/dart/dynamics/detail/SingleDofJointProperties.h index ceb297fe6e30f..d63635a4919ca 100644 --- a/dart/dynamics/detail/SingleDofJointProperties.h +++ b/dart/dynamics/detail/SingleDofJointProperties.h @@ -50,6 +50,32 @@ class SingleDofJoint; namespace detail { +//============================================================================== +struct SingleDofJointState +{ + /// Position + double mPosition; + + /// Generalized velocity + double mVelocity; + + /// Generalized acceleration + double mAcceleration; + + /// Generalized force + double mForce; + + /// Command + double mCommand; + + SingleDofJointState(double position = 0.0, + double velocity = 0.0, + double acceleration = 0.0, + double force = 0.0, + double command = 0.0); +}; + +//============================================================================== struct SingleDofJointUniqueProperties { /// Lower limit of position @@ -136,8 +162,8 @@ struct SingleDofJointProperties : }; //============================================================================== -using SingleDofJointBase = common::EmbedPropertiesOnTopOf< - SingleDofJoint, SingleDofJointUniqueProperties, Joint>; +using SingleDofJointBase = common::EmbedStateAndPropertiesOnTopOf< + SingleDofJoint, SingleDofJointState, SingleDofJointUniqueProperties, Joint>; } // namespace detail } // namespace dynamics diff --git a/unittests/testAspect.cpp b/unittests/testAspect.cpp index 4cd7475a8e020..182ae4fdc164a 100644 --- a/unittests/testAspect.cpp +++ b/unittests/testAspect.cpp @@ -118,7 +118,7 @@ class EmbeddedStateComposite : EmbeddedStateComposite(const EmbeddedStateData& state = EmbeddedStateData()) { - create(state); + create(static_cast(state)); } void setAspectState(const AspectState& state) { mAspectState = state; } From 83947d3984c36b31e82191ee17cf98b3de8d6886 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Fri, 1 Apr 2016 00:48:42 -0400 Subject: [PATCH 24/62] complying with clang standards --- dart/common/EmbeddedAspect.h | 55 ++++++++++++++++++++++------- dart/common/detail/EmbeddedAspect.h | 2 +- dart/dynamics/Joint.h | 4 ++- dart/dynamics/SoftBodyNode.h | 12 +++---- 4 files changed, 53 insertions(+), 20 deletions(-) diff --git a/dart/common/EmbeddedAspect.h b/dart/common/EmbeddedAspect.h index a6557ef487efd..ab2f9148dcacf 100644 --- a/dart/common/EmbeddedAspect.h +++ b/dart/common/EmbeddedAspect.h @@ -99,8 +99,13 @@ class EmbedState : public virtual common::RequiresAspect< using AspectStateData = typename Aspect::StateData; using Base = common::RequiresAspect; - // Inherit constructor - using Base::RequiresAspect; + // Forwarding constructor + template + EmbedState(Args&&... args) + : Base(std::forward(args)...) + { + // Do nothing + } virtual ~EmbedState() = default; @@ -134,8 +139,13 @@ class EmbedStateOnTopOf : public CompositeJoiner< using Base = CompositeJoiner; using Impl::getAspectState; - // Inherit constructor - using Base::CompositeJoiner; + // Forwarding constructor + template + EmbedStateOnTopOf(Args&&... args) + : Base(std::forward(args)...) + { + // Do nothing + } virtual ~EmbedStateOnTopOf() = default; @@ -165,6 +175,7 @@ class EmbeddedPropertiesAspect : public detail::EmbeddedPropertiesAspect< using Properties = typename Impl::Properties; using PropertiesData = typename Impl::PropertiesData; + // Forwarding constructor template EmbeddedPropertiesAspect(Args&&... args) : Impl(std::forward(args)...) @@ -200,8 +211,13 @@ class EmbedProperties : public virtual common::RequiresAspect< using AspectPropertiesData = typename Aspect::PropertiesData; using Base = common::RequiresAspect; - // Inherit constructor - using Base::RequiresAspect; + // Forwarding constructor + template + EmbedProperties(Args&&... args) + : Base(std::forward(args)...) + { + // Do nothing + } virtual ~EmbedProperties() = default; @@ -235,8 +251,13 @@ class EmbedPropertiesOnTopOf : public CompositeJoiner< using Base = CompositeJoiner; using Impl::getAspectProperties; - // Inherit constructor - using Base::CompositeJoiner; + // Forwarding constructor + template + EmbedPropertiesOnTopOf(Args&&... args) + : Base(std::forward(args)...) + { + // Do nothing + } virtual ~EmbedPropertiesOnTopOf() = default; @@ -377,8 +398,13 @@ class EmbedStateAndProperties : public virtual common::RequiresAspect< using AspectPropertiesData = typename Aspect::PropertiesData; using Base = common::RequiresAspect; - // Inherit constructor - using Base::RequiresAspect; + // Forwarding constructor + template + EmbedStateAndProperties(Args&&... args) + : Base(std::forward(args)...) + { + // Do nothing + } virtual ~EmbedStateAndProperties() = default; @@ -425,8 +451,13 @@ class EmbedStateAndPropertiesOnTopOf : public CompositeJoiner< using Impl::getAspectProperties; using Base = CompositeJoiner; - // Inherit constructor - using Base::CompositeJoiner; + // Forwarding constructor + template + EmbedStateAndPropertiesOnTopOf(Args&&... args) + : Base(std::forward(args)...) + { + // Do nothing + } virtual ~EmbedStateAndPropertiesOnTopOf() = default; diff --git a/dart/common/detail/EmbeddedAspect.h b/dart/common/detail/EmbeddedAspect.h index d40916e6e5d3f..187ed838d87ab 100644 --- a/dart/common/detail/EmbeddedAspect.h +++ b/dart/common/detail/EmbeddedAspect.h @@ -398,7 +398,7 @@ class EmbeddedPropertiesAspect : public BaseT } /// Save the embedded Properties of this Composite before we remove the Aspect - void loseComposite(Composite* oldComposite) + void loseComposite(Composite* oldComposite) override { mTemporaryProperties = make_unique( GetEmbeddedProperties(static_cast(this))); diff --git a/dart/dynamics/Joint.h b/dart/dynamics/Joint.h index b9737ca4490d0..73d56586301f6 100644 --- a/dart/dynamics/Joint.h +++ b/dart/dynamics/Joint.h @@ -72,6 +72,8 @@ class Joint : public virtual common::Subject, using CompositeProperties = common::Composite::Properties; using Properties = detail::JointProperties; + using Aspect = common::EmbedProperties::Aspect; + using AspectProperties = Aspect::Properties; typedef detail::ActuatorType ActuatorType; static constexpr ActuatorType FORCE = detail::FORCE; @@ -81,7 +83,7 @@ class Joint : public virtual common::Subject, static constexpr ActuatorType VELOCITY = detail::VELOCITY; static constexpr ActuatorType LOCKED = detail::LOCKED; - DART_BAKE_SPECIALIZED_ASPECT_IRREGULAR(Aspect, JointAspect); + DART_BAKE_SPECIALIZED_ASPECT_IRREGULAR(Aspect, JointAspect) struct ExtendedProperties : Properties { diff --git a/dart/dynamics/SoftBodyNode.h b/dart/dynamics/SoftBodyNode.h index 139a316029f05..199270f48e905 100644 --- a/dart/dynamics/SoftBodyNode.h +++ b/dart/dynamics/SoftBodyNode.h @@ -50,17 +50,17 @@ class SoftBodyNode : public detail::SoftBodyNodeBase { public: - friend class Skeleton; - friend class PointMass; - friend class PointMassNotifier; - friend void detail::SoftBodyNodeStateUpdate(SoftBodyAspect* aspect); - friend void detail::SoftBodyNodePropertiesUpdate(SoftBodyAspect* aspect); - using UniqueProperties = detail::SoftBodyNodeUniqueProperties; using Properties = detail::SoftBodyNodeProperties; using Aspect = detail::SoftBodyAspect; using Base = detail::SoftBodyNodeBase; + friend class Skeleton; + friend class PointMass; + friend class PointMassNotifier; + friend void detail::SoftBodyNodeStateUpdate(Aspect* aspect); + friend void detail::SoftBodyNodePropertiesUpdate(Aspect* aspect); + DART_BAKE_SPECIALIZED_ASPECT_IRREGULAR(detail::SoftBodyAspect, SoftBodyAspect) /// \brief From d7d23637d49b850dad90b387d0ec283ef4c99e32 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Fri, 1 Apr 2016 02:14:50 -0400 Subject: [PATCH 25/62] embedding Soft Properties and State -- clang is erroring --- dart/dynamics/Joint.h | 2 - dart/dynamics/PointMass.cpp | 71 +++++++- dart/dynamics/PointMass.h | 6 + dart/dynamics/SoftBodyNode.cpp | 155 ++++++++++-------- dart/dynamics/SoftBodyNode.h | 21 +-- dart/dynamics/detail/SoftBodyNodeProperties.h | 43 +---- 6 files changed, 164 insertions(+), 134 deletions(-) diff --git a/dart/dynamics/Joint.h b/dart/dynamics/Joint.h index 73d56586301f6..9faa748e9221c 100644 --- a/dart/dynamics/Joint.h +++ b/dart/dynamics/Joint.h @@ -72,8 +72,6 @@ class Joint : public virtual common::Subject, using CompositeProperties = common::Composite::Properties; using Properties = detail::JointProperties; - using Aspect = common::EmbedProperties::Aspect; - using AspectProperties = Aspect::Properties; typedef detail::ActuatorType ActuatorType; static constexpr ActuatorType FORCE = detail::FORCE; diff --git a/dart/dynamics/PointMass.cpp b/dart/dynamics/PointMass.cpp index b3630fcec5c96..d71a2988af577 100644 --- a/dart/dynamics/PointMass.cpp +++ b/dart/dynamics/PointMass.cpp @@ -49,6 +49,10 @@ using namespace Eigen; namespace dart { namespace dynamics { +#define RETURN_FALSE_IF_OTHER_IS_EQUAL( X )\ + if( other. X != X )\ + return false; + //============================================================================== PointMass::State::State( const Vector3d& positions, @@ -63,6 +67,17 @@ PointMass::State::State( // Do nothing } +//============================================================================== +bool PointMass::State::operator ==(const PointMass::State& other) const +{ + RETURN_FALSE_IF_OTHER_IS_EQUAL(mPositions); + RETURN_FALSE_IF_OTHER_IS_EQUAL(mVelocities); + RETURN_FALSE_IF_OTHER_IS_EQUAL(mAccelerations); + RETURN_FALSE_IF_OTHER_IS_EQUAL(mForces); + + return true; +} + //============================================================================== PointMass::Properties::Properties( const Vector3d& _X0, @@ -103,6 +118,31 @@ void PointMass::Properties::setMass(double _mass) mMass = _mass; } +//============================================================================== +bool PointMass::Properties::operator ==(const PointMass::Properties& other) const +{ + RETURN_FALSE_IF_OTHER_IS_EQUAL(mX0); + RETURN_FALSE_IF_OTHER_IS_EQUAL(mMass); + RETURN_FALSE_IF_OTHER_IS_EQUAL(mConnectedPointMassIndices); + RETURN_FALSE_IF_OTHER_IS_EQUAL(mPositionLowerLimits); + RETURN_FALSE_IF_OTHER_IS_EQUAL(mPositionUpperLimits); + RETURN_FALSE_IF_OTHER_IS_EQUAL(mVelocityLowerLimits); + RETURN_FALSE_IF_OTHER_IS_EQUAL(mVelocityUpperLimits); + RETURN_FALSE_IF_OTHER_IS_EQUAL(mAccelerationLowerLimits); + RETURN_FALSE_IF_OTHER_IS_EQUAL(mAccelerationUpperLimits); + RETURN_FALSE_IF_OTHER_IS_EQUAL(mForceLowerLimits); + RETURN_FALSE_IF_OTHER_IS_EQUAL(mForceUpperLimits); + + // Nothing was inequal, so we return true + return true; +} + +//============================================================================== +bool PointMass::Properties::operator !=(const PointMass::Properties& other) const +{ + return !(other == *this); +} + //============================================================================== PointMass::PointMass(SoftBodyNode* _softBodyNode) : // mIndexInSkeleton(Eigen::Matrix::Zero()), @@ -150,13 +190,13 @@ PointMass::~PointMass() //============================================================================== PointMass::State& PointMass::getState() { - return mParentSoftBodyNode->getPointState(mIndex); + return mParentSoftBodyNode->mAspectState.mPointStates[mIndex]; } //============================================================================== const PointMass::State& PointMass::getState() const { - return mParentSoftBodyNode->getPointState(mIndex); + return mParentSoftBodyNode->mAspectState.mPointStates[mIndex]; } //============================================================================== @@ -169,13 +209,18 @@ size_t PointMass::getIndexInSoftBodyNode() const void PointMass::setMass(double _mass) { assert(0.0 < _mass); - mParentSoftBodyNode->getSoftPropertiesAndInc().mPointProps[mIndex].mMass = _mass; + double& mMass = mParentSoftBodyNode->mAspectProperties.mPointProps[mIndex].mMass; + if(_mass == mMass) + return; + + mMass = _mass; + mParentSoftBodyNode->incrementVersion(); } //============================================================================== double PointMass::getMass() const { - return mParentSoftBodyNode->getSoftProperties().mPointProps[mIndex].mMass; + return mParentSoftBodyNode->mAspectProperties.mPointProps[mIndex].mMass; } //============================================================================== @@ -211,14 +256,15 @@ void PointMass::addConnectedPointMass(PointMass* _pointMass) { assert(_pointMass != nullptr); - mParentSoftBodyNode->getSoftPropertiesAndInc().mPointProps[mIndex]. + mParentSoftBodyNode->mAspectProperties.mPointProps[mIndex]. mConnectedPointMassIndices.push_back(_pointMass->mIndex); + mParentSoftBodyNode->incrementVersion(); } //============================================================================== size_t PointMass::getNumConnectedPointMasses() const { - return mParentSoftBodyNode->getSoftProperties().mPointProps[mIndex]. + return mParentSoftBodyNode->mAspectProperties.mPointProps[mIndex]. mConnectedPointMassIndices.size(); } @@ -228,7 +274,7 @@ PointMass* PointMass::getConnectedPointMass(size_t _idx) assert(_idx < getNumConnectedPointMasses()); return mParentSoftBodyNode->mPointMasses[ - mParentSoftBodyNode->getSoftProperties().mPointProps[mIndex]. + mParentSoftBodyNode->mAspectProperties.mPointProps[mIndex]. mConnectedPointMassIndices[_idx]]; } @@ -554,14 +600,21 @@ void PointMass::clearConstraintImpulse() //============================================================================== void PointMass::setRestingPosition(const Eigen::Vector3d& _p) { - mParentSoftBodyNode->getSoftPropertiesAndInc().mPointProps[mIndex].mX0 = _p; + + Eigen::Vector3d& mRest = + mParentSoftBodyNode->mAspectProperties.mPointProps[mIndex].mX0; + if(_p == mRest) + return; + + mRest = _p; + mParentSoftBodyNode->incrementVersion(); mNotifier->notifyTransformUpdate(); } //============================================================================== const Eigen::Vector3d& PointMass::getRestingPosition() const { - return mParentSoftBodyNode->getSoftProperties().mPointProps[mIndex].mX0; + return mParentSoftBodyNode->mAspectProperties.mPointProps[mIndex].mX0; } //============================================================================== diff --git a/dart/dynamics/PointMass.h b/dart/dynamics/PointMass.h index 29ab40530e9ba..fe6dde7a551b1 100644 --- a/dart/dynamics/PointMass.h +++ b/dart/dynamics/PointMass.h @@ -81,6 +81,8 @@ class PointMass : public common::Subject const Eigen::Vector3d& velocities = Eigen::Vector3d::Zero(), const Eigen::Vector3d& accelerations = Eigen::Vector3d::Zero(), const Eigen::Vector3d& forces = Eigen::Vector3d::Zero()); + + bool operator==(const State& other) const; }; /// Properties for each PointMass @@ -142,6 +144,10 @@ class PointMass : public common::Subject void setRestingPosition(const Eigen::Vector3d& _x); void setMass(double _mass); + + bool operator==(const Properties& other) const; + + bool operator!=(const Properties& other) const; }; //-------------------------------------------------------------------------- diff --git a/dart/dynamics/SoftBodyNode.cpp b/dart/dynamics/SoftBodyNode.cpp index cf0bdfb142fcc..9cef0c737cf40 100644 --- a/dart/dynamics/SoftBodyNode.cpp +++ b/dart/dynamics/SoftBodyNode.cpp @@ -122,23 +122,6 @@ SoftBodyNodeProperties::SoftBodyNodeProperties( // Do nothing } -//============================================================================== -void SoftBodyNodeStateUpdate(SoftBodyAspect* aspect) -{ - if(SoftBodyNode* sbn = aspect->getComposite()) - sbn->mNotifier->notifyTransformUpdate(); -} - -//============================================================================== -void SoftBodyNodePropertiesUpdate(SoftBodyAspect* aspect) -{ - if(SoftBodyNode* sbn = aspect->getComposite()) - { - sbn->configurePointMasses(sbn->mSoftShapeNode.lock()); - SoftBodyNodeStateUpdate(aspect); - } -} - } // namespace detail //============================================================================== @@ -162,13 +145,39 @@ void SoftBodyNode::setProperties(const Properties& _properties) //============================================================================== void SoftBodyNode::setProperties(const UniqueProperties& _properties) { - getSoftBodyAspect()->setProperties(_properties); + setAspectProperties(_properties); +} + +//============================================================================== +void SoftBodyNode::setAspectState(const AspectState& state) +{ + if(mAspectState.mPointStates == state.mPointStates) + return; + + mAspectState = state; + mNotifier->notifyTransformUpdate(); +} + +//============================================================================== +void SoftBodyNode::setAspectProperties(const AspectProperties& properties) +{ + setVertexSpringStiffness(properties.mKv); + setEdgeSpringStiffness(properties.mKe); + setDampingCoefficient(properties.mDampCoeff); + + if(properties.mPointProps != mAspectProperties.mPointProps + || properties.mFaces != mAspectProperties.mFaces) + { + mAspectProperties.mPointProps = properties.mPointProps; + mAspectProperties.mFaces = properties.mFaces; + configurePointMasses(mSoftShapeNode.lock()); + } } //============================================================================== SoftBodyNode::Properties SoftBodyNode::getSoftBodyNodeProperties() const { - return Properties(getBodyNodeProperties(), getSoftProperties()); + return Properties(getBodyNodeProperties(), mAspectProperties); } //============================================================================== @@ -224,13 +233,12 @@ SoftBodyNode::SoftBodyNode(BodyNode* _parentBodyNode, const Properties& _properties) : Entity(Frame::World(), "", false), Frame(Frame::World(), ""), - Base(std::make_tuple(_parentBodyNode, _parentJoint, _properties), - common::NoArg), + Base(common::NoArg, + std::make_tuple(_parentBodyNode, _parentJoint, _properties)), mSoftShapeNode(nullptr) { createSoftBodyAspect(); mNotifier = new PointMassNotifier(this, "PointMassNotifier"); - std::cout << "Creating and assigning mSoftShapeNode" << std::endl; ShapeNode* softNode = createShapeNodeWith< VisualAspect, CollisionAspect, DynamicsAspect>( std::make_shared(this), getName()+"_SoftMeshShape"); @@ -239,7 +247,7 @@ SoftBodyNode::SoftBodyNode(BodyNode* _parentBodyNode, // Dev's Note: We do this workaround (instead of just using setProperties(~)) // because mSoftShapeNode cannot be used until init(SkeletonPtr) has been // called on this BodyNode, but that happens after construction is finished. - getSoftBodyAspect()->mProperties = _properties; + mAspectProperties = _properties; configurePointMasses(softNode); mNotifier->notifyTransformUpdate(); } @@ -262,7 +270,7 @@ BodyNode* SoftBodyNode::clone(BodyNode* _parentBodyNode, //============================================================================== void SoftBodyNode::configurePointMasses(ShapeNode* softNode) { - const UniqueProperties& softProperties = getSoftBodyAspect()->getProperties(); + const UniqueProperties& softProperties = mAspectProperties; size_t newCount = softProperties.mPointProps.size(); size_t oldCount = mPointMasses.size(); @@ -289,7 +297,7 @@ void SoftBodyNode::configurePointMasses(ShapeNode* softNode) } // Resize the number of States in the Aspect - getSoftBodyAspect()->_getState().mPointStates.resize( + mAspectState.mPointStates.resize( softProperties.mPointProps.size(), PointMass::State()); // Access the SoftMeshShape and reallocate its meshes @@ -315,6 +323,9 @@ void SoftBodyNode::configurePointMasses(ShapeNode* softNode) std::cout << "- " << i << ") " << getShapeNode(i)->getName() << std::endl; } } + + incrementVersion(); + mNotifier->notifyTransformUpdate(); } //============================================================================== @@ -390,46 +401,63 @@ double SoftBodyNode::getMass() const void SoftBodyNode::setVertexSpringStiffness(double _kv) { assert(0.0 <= _kv); - getSoftPropertiesAndInc().mKv = _kv; + + if(_kv == mAspectProperties.mKv) + return; + + mAspectProperties.mKv = _kv; + incrementVersion(); } //============================================================================== double SoftBodyNode::getVertexSpringStiffness() const { - return getSoftProperties().mKv; + return mAspectProperties.mKv; } //============================================================================== void SoftBodyNode::setEdgeSpringStiffness(double _ke) { assert(0.0 <= _ke); - getSoftPropertiesAndInc().mKe = _ke; + + if(_ke == mAspectProperties.mKe) + return; + + mAspectProperties.mKe = _ke; + incrementVersion(); } //============================================================================== double SoftBodyNode::getEdgeSpringStiffness() const { - return getSoftProperties().mKe; + return mAspectProperties.mKe; } //============================================================================== void SoftBodyNode::setDampingCoefficient(double _damp) { assert(_damp >= 0.0); - getSoftPropertiesAndInc().mDampCoeff = _damp; + + if(_damp == mAspectProperties.mDampCoeff) + return; + + mAspectProperties.mDampCoeff = _damp; + incrementVersion(); } //============================================================================== double SoftBodyNode::getDampingCoefficient() const { - return getSoftProperties().mDampCoeff; + return mAspectProperties.mDampCoeff; } //============================================================================== void SoftBodyNode::removeAllPointMasses() { mPointMasses.clear(); - getSoftPropertiesAndInc().mPointProps.clear(); + mAspectProperties.mPointProps.clear(); + mAspectProperties.mFaces.clear(); + configurePointMasses(mSoftShapeNode.lock()); } //============================================================================== @@ -437,7 +465,7 @@ PointMass* SoftBodyNode::addPointMass(const PointMass::Properties& _properties) { mPointMasses.push_back(new PointMass(this)); mPointMasses.back()->mIndex = mPointMasses.size()-1; - getSoftPropertiesAndInc().mPointProps.push_back(_properties); + mAspectProperties.mPointProps.push_back(_properties); configurePointMasses(mSoftShapeNode.lock()); return mPointMasses.back(); @@ -451,25 +479,27 @@ void SoftBodyNode::connectPointMasses(size_t _idx1, size_t _idx2) assert(_idx2 < mPointMasses.size()); mPointMasses[_idx1]->addConnectedPointMass(mPointMasses[_idx2]); mPointMasses[_idx2]->addConnectedPointMass(mPointMasses[_idx1]); + // Version incremented in addConnectedPointMass } //============================================================================== void SoftBodyNode::addFace(const Eigen::Vector3i& _face) { - getSoftPropertiesAndInc().addFace(_face); + mAspectProperties.addFace(_face); + configurePointMasses(mSoftShapeNode.lock()); } //============================================================================== const Eigen::Vector3i& SoftBodyNode::getFace(size_t _idx) const { - assert(_idx < getSoftProperties().mFaces.size()); - return getSoftProperties().mFaces[_idx]; + assert(_idx < mAspectProperties.mFaces.size()); + return mAspectProperties.mFaces[_idx]; } //============================================================================== size_t SoftBodyNode::getNumFaces() const { - return getSoftProperties().mFaces.size(); + return mAspectProperties.mFaces.size(); } //============================================================================== @@ -537,12 +567,13 @@ void SoftBodyNode::updateAccelerationID() void SoftBodyNode::updateTransmittedForceID(const Eigen::Vector3d& _gravity, bool _withExternalForces) { - const Eigen::Matrix6d& mI = mAspectProperties.mInertia.getSpatialTensor(); + const Eigen::Matrix6d& mI = + BodyNode::mAspectProperties.mInertia.getSpatialTensor(); for (auto& pointMass : mPointMasses) pointMass->updateTransmittedForceID(_gravity, _withExternalForces); // Gravity force - if (mAspectProperties.mGravityMode == true) + if (BodyNode::mAspectProperties.mGravityMode == true) mFgravity.noalias() = mI * math::AdInvRLinear(getWorldTransform(),_gravity); else mFgravity.setZero(); @@ -617,7 +648,8 @@ void SoftBodyNode::updateJointImpulseFD() //============================================================================== void SoftBodyNode::updateArtInertia(double _timeStep) const { - const Eigen::Matrix6d& mI = mAspectProperties.mInertia.getSpatialTensor(); + const Eigen::Matrix6d& mI = + BodyNode::mAspectProperties.mInertia.getSpatialTensor(); for (auto& pointMass : mPointMasses) pointMass->updateArtInertiaFD(_timeStep); @@ -662,12 +694,13 @@ void SoftBodyNode::updateArtInertia(double _timeStep) const void SoftBodyNode::updateBiasForce(const Eigen::Vector3d& _gravity, double _timeStep) { - const Eigen::Matrix6d& mI = mAspectProperties.mInertia.getSpatialTensor(); + const Eigen::Matrix6d& mI = + BodyNode::mAspectProperties.mInertia.getSpatialTensor(); for (auto& pointMass : mPointMasses) pointMass->updateBiasForceFD(_timeStep, _gravity); // Gravity force - if (mAspectProperties.mGravityMode == true) + if (BodyNode::mAspectProperties.mGravityMode == true) mFgravity.noalias() = mI * math::AdInvRLinear(getWorldTransform(),_gravity); else mFgravity.setZero(); @@ -845,7 +878,8 @@ void SoftBodyNode::aggregateAugMassMatrix(Eigen::MatrixXd& _MCol, size_t _col, { // TODO(JS): Need to be reimplemented - const Eigen::Matrix6d& mI = mAspectProperties.mInertia.getSpatialTensor(); + const Eigen::Matrix6d& mI = + BodyNode::mAspectProperties.mInertia.getSpatialTensor(); //------------------------ PointMass Part ------------------------------------ for (size_t i = 0; i < mPointMasses.size(); ++i) mPointMasses.at(i)->aggregateAugMassMatrix(_MCol, _col, _timeStep); @@ -1033,13 +1067,14 @@ void SoftBodyNode::aggregateCoriolisForceVector(Eigen::VectorXd& _C) void SoftBodyNode::aggregateGravityForceVector(Eigen::VectorXd& _g, const Eigen::Vector3d& _gravity) { - const Eigen::Matrix6d& mI = mAspectProperties.mInertia.getSpatialTensor(); + const Eigen::Matrix6d& mI = + BodyNode::mAspectProperties.mInertia.getSpatialTensor(); //------------------------ PointMass Part ------------------------------------ for (size_t i = 0; i < mPointMasses.size(); ++i) mPointMasses.at(i)->aggregateGravityForceVector(_g, _gravity); //----------------------- SoftBodyNode Part ---------------------------------- - if (mAspectProperties.mGravityMode == true) + if (BodyNode::mAspectProperties.mGravityMode == true) mG_F = mI * math::AdInvRLinear(getWorldTransform(), _gravity); else mG_F.setZero(); @@ -1216,20 +1251,20 @@ void SoftBodyNode::draw(renderer::RenderInterface* _ri, Eigen::Vector3d pos; Eigen::Vector3d pos_normalized; - for (size_t i = 0; i < getSoftProperties().mFaces.size(); ++i) + for (size_t i = 0; i < mAspectProperties.mFaces.size(); ++i) { glEnable(GL_AUTO_NORMAL); glBegin(GL_TRIANGLES); - pos = mPointMasses[getSoftProperties().mFaces[i](0)]->getLocalPosition(); + pos = mPointMasses[mAspectProperties.mFaces[i](0)]->getLocalPosition(); pos_normalized = pos.normalized(); glNormal3f(pos_normalized(0), pos_normalized(1), pos_normalized(2)); glVertex3f(pos(0), pos(1), pos(2)); - pos = mPointMasses[getSoftProperties().mFaces[i](1)]->getLocalPosition(); + pos = mPointMasses[mAspectProperties.mFaces[i](1)]->getLocalPosition(); pos_normalized = pos.normalized(); glNormal3f(pos_normalized(0), pos_normalized(1), pos_normalized(2)); glVertex3f(pos(0), pos(1), pos(2)); - pos = mPointMasses[getSoftProperties().mFaces[i](2)]->getLocalPosition(); + pos = mPointMasses[mAspectProperties.mFaces[i](2)]->getLocalPosition(); pos_normalized = pos.normalized(); glNormal3f(pos_normalized(0), pos_normalized(1), pos_normalized(2)); glVertex3f(pos(0), pos(1), pos(2)); @@ -1250,25 +1285,6 @@ void SoftBodyNode::draw(renderer::RenderInterface* _ri, _ri->popMatrix(); } -//============================================================================== -PointMass::State& SoftBodyNode::getPointState(size_t index) -{ - return getSoftBodyAspect()->_getState().mPointStates.at(index); -} - -//============================================================================== -SoftBodyNode::UniqueProperties& SoftBodyNode::getSoftPropertiesAndInc() -{ - getSoftBodyAspect()->incrementVersion(); - return getSoftBodyAspect()->_getProperties(); -} - -//============================================================================== -const SoftBodyNode::UniqueProperties& SoftBodyNode::getSoftProperties() const -{ - return getSoftBodyAspect()->_getProperties(); -} - //============================================================================== void SoftBodyNode::_addPiToArtInertia(const Eigen::Vector3d& _p, double _Pi) const { @@ -1303,7 +1319,8 @@ void SoftBodyNode::updateInertiaWithPointMass() { // TODO(JS): Not implemented - const Eigen::Matrix6d& mI = mAspectProperties.mInertia.getSpatialTensor(); + const Eigen::Matrix6d& mI = + BodyNode::mAspectProperties.mInertia.getSpatialTensor(); mI2 = mI; for (size_t i = 0; i < mPointMasses.size(); ++i) diff --git a/dart/dynamics/SoftBodyNode.h b/dart/dynamics/SoftBodyNode.h index 199270f48e905..4047c63c3ac81 100644 --- a/dart/dynamics/SoftBodyNode.h +++ b/dart/dynamics/SoftBodyNode.h @@ -52,16 +52,13 @@ class SoftBodyNode : public detail::SoftBodyNodeBase using UniqueProperties = detail::SoftBodyNodeUniqueProperties; using Properties = detail::SoftBodyNodeProperties; - using Aspect = detail::SoftBodyAspect; using Base = detail::SoftBodyNodeBase; friend class Skeleton; friend class PointMass; friend class PointMassNotifier; - friend void detail::SoftBodyNodeStateUpdate(Aspect* aspect); - friend void detail::SoftBodyNodePropertiesUpdate(Aspect* aspect); - DART_BAKE_SPECIALIZED_ASPECT_IRREGULAR(detail::SoftBodyAspect, SoftBodyAspect) + DART_BAKE_SPECIALIZED_ASPECT_IRREGULAR(Aspect, SoftBodyAspect) /// \brief virtual ~SoftBodyNode(); @@ -72,6 +69,12 @@ class SoftBodyNode : public detail::SoftBodyNodeBase /// Set the Properties of this SoftBodyNode void setProperties(const UniqueProperties& _properties); + /// Set the AspectState of this SoftBodyNode + void setAspectState(const AspectState& state); + + /// Set the AspectProperties of this SoftBodyNode + void setAspectProperties(const AspectProperties& properties); + using SkeletonRefCountingBase::getSkeleton; /// Get the Properties of this SoftBodyNode @@ -296,16 +299,6 @@ class SoftBodyNode : public detail::SoftBodyNodeBase protected: - /// Get a reference to the State of a PointMass - PointMass::State& getPointState(size_t index); - - /// Get a reference to the UniqueProperties of this SoftBodyNode and increment - /// its version. - UniqueProperties& getSoftPropertiesAndInc(); - - /// Get a reference to the UniqueProperties of this SoftBodyNode - const UniqueProperties& getSoftProperties() const; - /// \brief List of point masses composing deformable mesh. std::vector mPointMasses; diff --git a/dart/dynamics/detail/SoftBodyNodeProperties.h b/dart/dynamics/detail/SoftBodyNodeProperties.h index c577ed8a37912..aaf5fd7bcee1a 100644 --- a/dart/dynamics/detail/SoftBodyNodeProperties.h +++ b/dart/dynamics/detail/SoftBodyNodeProperties.h @@ -118,46 +118,9 @@ struct SoftBodyNodeProperties }; //============================================================================== -void SoftBodyNodeStateUpdate(SoftBodyAspect* aspect); - -//============================================================================== -void SoftBodyNodePropertiesUpdate(SoftBodyAspect* aspect); - -//============================================================================== -class SoftBodyAspect final : - public common::AspectWithStateAndVersionedProperties< - SoftBodyAspect, SoftBodyNodeUniqueState, SoftBodyNodeUniqueProperties, - SoftBodyNode, &SoftBodyNodeStateUpdate, &SoftBodyNodePropertiesUpdate > -{ -public: - - friend class dart::dynamics::SoftBodyNode; - - DART_COMMON_ASPECT_STATE_PROPERTY_CONSTRUCTORS(SoftBodyAspect) - - DART_COMMON_SET_GET_ASPECT_PROPERTY(double, Kv) - DART_COMMON_SET_GET_ASPECT_PROPERTY(double, Ke) - DART_COMMON_SET_GET_ASPECT_PROPERTY(double, DampCoeff) - -protected: - - /// Get a direct reference to the State of this Aspect - inline StateData& _getState() { return mState; } - - /// Get a direct reference to the State of this Aspect - inline const StateData& _getState() const { return mState; } - - /// Get a direct reference to the Properties of this Aspect - inline PropertiesData& _getProperties() { return mProperties; } - - /// Get a direct const reference to the Properties of this Aspect - inline const PropertiesData& _getProperties() const { return mProperties; } - -}; - -//============================================================================== -using SoftBodyNodeBase = common::CompositeJoiner< - BodyNode, common::RequiresAspect >; +using SoftBodyNodeBase = common::EmbedStateAndPropertiesOnTopOf< + SoftBodyNode, SoftBodyNodeUniqueState, SoftBodyNodeUniqueProperties, + BodyNode>; } // namespace detail } // namespace dynamics From 3ab44771d5451aee995b089e085167564e53ca84 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Fri, 1 Apr 2016 17:11:59 -0400 Subject: [PATCH 26/62] identified and fixed the issue that clang was having --- dart/dynamics/detail/BodyNode.h | 16 ++++++++-------- unittests/testAspect.cpp | 19 +++++++++++++++++++ 2 files changed, 27 insertions(+), 8 deletions(-) diff --git a/dart/dynamics/detail/BodyNode.h b/dart/dynamics/detail/BodyNode.h index a625c6033d804..c6b5072f8c9f4 100644 --- a/dart/dynamics/detail/BodyNode.h +++ b/dart/dynamics/detail/BodyNode.h @@ -178,7 +178,7 @@ ShapeNode* BodyNode::createShapeNodeWith( } //============================================================================== -template +template size_t BodyNode::getNumShapeNodesWith() const { auto count = 0u; @@ -186,7 +186,7 @@ size_t BodyNode::getNumShapeNodesWith() const for (auto i = 0u; i < numShapeNode; ++i) { - if (getShapeNode(i)->has()) + if (getShapeNode(i)->has()) ++count; } @@ -194,7 +194,7 @@ size_t BodyNode::getNumShapeNodesWith() const } //============================================================================== -template +template const std::vector BodyNode::getShapeNodesWith() { std::vector shapeNodes; @@ -205,7 +205,7 @@ const std::vector BodyNode::getShapeNodesWith() { auto shapeNode = getShapeNode(i); - if (shapeNode->has()) + if (shapeNode->has()) shapeNodes.push_back(shapeNode); } @@ -213,7 +213,7 @@ const std::vector BodyNode::getShapeNodesWith() } //============================================================================== -template +template const std::vector BodyNode::getShapeNodesWith() const { std::vector shapeNodes; @@ -224,7 +224,7 @@ const std::vector BodyNode::getShapeNodesWith() const { const auto shapeNode = getShapeNode(i); - if (shapeNode->has()) + if (shapeNode->has()) shapeNodes.push_back(shapeNode); } @@ -232,10 +232,10 @@ const std::vector BodyNode::getShapeNodesWith() const } //============================================================================== -template +template void BodyNode::removeAllShapeNodesWith() { - auto shapeNodes = getShapeNodesWith(); + auto shapeNodes = getShapeNodesWith(); for (auto shapeNode : shapeNodes) shapeNode->remove(); } diff --git a/unittests/testAspect.cpp b/unittests/testAspect.cpp index 182ae4fdc164a..32a11088a4e6f 100644 --- a/unittests/testAspect.cpp +++ b/unittests/testAspect.cpp @@ -50,6 +50,7 @@ #include "dart/common/EmbeddedAspect.h" #include "dart/dynamics/EulerJoint.h" +#include "dart/dynamics/BoxShape.h" using namespace dart::common; @@ -747,6 +748,24 @@ TEST(Aspect, Joints) universal->getMultiDofJointAspect(true); } +TEST(Aspect, BodyNodes) +{ + SkeletonPtr skel = Skeleton::create(); + + BodyNode* bn = + skel->createJointAndBodyNodePair().second; + + bn->createShapeNodeWith< + dart::dynamics::VisualAspect, + dart::dynamics::CollisionAspect, + dart::dynamics::DynamicsAspect>( + std::make_shared(Eigen::Vector3d::Ones())); + + EXPECT_EQ(bn->getNumShapeNodesWith(), 1u); + EXPECT_EQ(bn->getNumShapeNodesWith(), 1u); + EXPECT_EQ(bn->getNumShapeNodesWith(), 1u); +} + TEST(Aspect, Duplication) { Composite mgr1, mgr2; From 275c07b8f069d2302cdb3683e6a3745f59315af8 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Sat, 2 Apr 2016 12:36:06 -0400 Subject: [PATCH 27/62] using embedded aspect state for BodyNode --- dart/dynamics/BodyNode.cpp | 51 ++++++++++++++++------- dart/dynamics/BodyNode.h | 12 +++--- dart/dynamics/SoftBodyNode.cpp | 6 +-- dart/dynamics/detail/BodyNodeProperties.h | 13 ++++++ 4 files changed, 56 insertions(+), 26 deletions(-) diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index 9dcafb433dc49..0954c43a1eb09 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -116,6 +116,16 @@ size_t BodyNode::msBodyNodeCount = 0; namespace detail { +//============================================================================== +BodyNodeState::BodyNodeState( + bool isColliding, + const Eigen::Vector6d& Fext) + : mIsColliding(isColliding), + mFext(Fext) +{ + // Do nothing +} + //============================================================================== BodyNodeUniqueProperties::BodyNodeUniqueProperties( const Inertia& _inertia, @@ -249,6 +259,17 @@ void BodyNode::setProperties(const UniqueProperties& _properties) setAspectProperties(_properties); } +//============================================================================== +void BodyNode::setAspectState(const AspectState& state) +{ + setColliding(state.mIsColliding); + if(mAspectState.mFext != state.mFext) + { + mAspectState.mFext = state.mFext; + SKEL_SET_FLAGS(mExternalForces); + } +} + //============================================================================== void BodyNode::setAspectProperties(const AspectProperties& properties) { @@ -1110,13 +1131,13 @@ const Eigen::Vector6d& BodyNode::getBodyVelocityChange() const //============================================================================== void BodyNode::setColliding(bool _isColliding) { - mIsColliding = _isColliding; + mAspectState.mIsColliding = _isColliding; } //============================================================================== bool BodyNode::isColliding() { - return mIsColliding; + return mAspectState.mIsColliding; } //============================================================================== @@ -1139,7 +1160,7 @@ void BodyNode::addExtForce(const Eigen::Vector3d& _force, else F.tail<3>() = W.linear().transpose() * _force; - mFext += math::dAdInvT(T, F); + mAspectState.mFext += math::dAdInvT(T, F); SKEL_SET_FLAGS(mExternalForces); } @@ -1163,7 +1184,7 @@ void BodyNode::setExtForce(const Eigen::Vector3d& _force, else F.tail<3>() = W.linear().transpose() * _force; - mFext = math::dAdInvT(T, F); + mAspectState.mFext = math::dAdInvT(T, F); SKEL_SET_FLAGS(mExternalForces); } @@ -1172,9 +1193,9 @@ void BodyNode::setExtForce(const Eigen::Vector3d& _force, void BodyNode::addExtTorque(const Eigen::Vector3d& _torque, bool _isLocal) { if (_isLocal) - mFext.head<3>() += _torque; + mAspectState.mFext.head<3>() += _torque; else - mFext.head<3>() += getWorldTransform().linear().transpose() * _torque; + mAspectState.mFext.head<3>() += getWorldTransform().linear().transpose() * _torque; SKEL_SET_FLAGS(mExternalForces); } @@ -1183,9 +1204,9 @@ void BodyNode::addExtTorque(const Eigen::Vector3d& _torque, bool _isLocal) void BodyNode::setExtTorque(const Eigen::Vector3d& _torque, bool _isLocal) { if (_isLocal) - mFext.head<3>() = _torque; + mAspectState.mFext.head<3>() = _torque; else - mFext.head<3>() = getWorldTransform().linear().transpose() * _torque; + mAspectState.mFext.head<3>() = getWorldTransform().linear().transpose() * _torque; SKEL_SET_FLAGS(mExternalForces); } @@ -1197,13 +1218,11 @@ BodyNode::BodyNode(BodyNode* _parentBodyNode, Joint* _parentJoint, Frame(Frame::World(), ""), // Name gets set later by setProperties TemplatedJacobianNode(this), mID(BodyNode::msBodyNodeCount++), - mIsColliding(false), mParentJoint(_parentJoint), mParentBodyNode(nullptr), mPartialAcceleration(Eigen::Vector6d::Zero()), mIsPartialAccelerationDirty(true), mF(Eigen::Vector6d::Zero()), - mFext(Eigen::Vector6d::Zero()), mFgravity(Eigen::Vector6d::Zero()), mArtInertia(Eigen::Matrix6d::Identity()), mArtInertiaImplicit(Eigen::Matrix6d::Identity()), @@ -1585,7 +1604,7 @@ void BodyNode::updateTransmittedForceID(const Eigen::Vector3d& _gravity, // External force if (_withExternalForces) - mF -= mFext; + mF -= mAspectState.mFext; // Verification assert(!math::isNan(mF)); @@ -1655,7 +1674,7 @@ void BodyNode::updateBiasForce(const Eigen::Vector3d& _gravity, // Set bias force const Eigen::Vector6d& V = getSpatialVelocity(); - mBiasForce = -math::dad(V, mI * V) - mFext - mFgravity; + mBiasForce = -math::dad(V, mI * V) - mAspectState.mFext - mFgravity; // Verification assert(!math::isNan(mBiasForce)); @@ -1814,7 +1833,7 @@ void BodyNode::updateConstrainedTerms(double _timeStep) //============================================================================== void BodyNode::clearExternalForces() { - mFext.setZero(); + mAspectState.mFext.setZero(); SKEL_SET_FLAGS(mExternalForces); } @@ -1827,13 +1846,13 @@ void BodyNode::clearInternalForces() //============================================================================== const Eigen::Vector6d& BodyNode::getExternalForceLocal() const { - return mFext; + return mAspectState.mFext; } //============================================================================== Eigen::Vector6d BodyNode::getExternalForceGlobal() const { - return math::dAdInvT(getWorldTransform(), mFext); + return math::dAdInvT(getWorldTransform(), mAspectState.mFext); } //============================================================================== @@ -2039,7 +2058,7 @@ void BodyNode::aggregateCombinedVector(Eigen::VectorXd& _Cg, //============================================================================== void BodyNode::aggregateExternalForces(Eigen::VectorXd& _Fext) { - mFext_F = mFext; + mFext_F = mAspectState.mFext; for (std::vector::const_iterator it = mChildBodyNodes.begin(); it != mChildBodyNodes.end(); ++it) diff --git a/dart/dynamics/BodyNode.h b/dart/dynamics/BodyNode.h index 251e07133aa73..e3aa9e13f9e49 100644 --- a/dart/dynamics/BodyNode.h +++ b/dart/dynamics/BodyNode.h @@ -81,7 +81,8 @@ class Marker; /// BodyNode inherits Frame, and a parent Frame of a BodyNode is the parent /// BodyNode of the BodyNode. class BodyNode : - public common::EmbedProperties, + public common::EmbedStateAndProperties< + BodyNode, detail::BodyNodeState, detail::BodyNodeUniqueProperties>, public virtual BodyNodeSpecializedFor, public SkeletonRefCountingBase, public TemplatedJacobianNode @@ -125,6 +126,9 @@ class BodyNode : /// Set the UniqueProperties of this BodyNode void setProperties(const UniqueProperties& _properties); + /// Set the AspectState of this BodyNode + void setAspectState(const AspectState& state); + /// Set the AspectProperties of this BodyNode void setAspectProperties(const AspectProperties& properties); @@ -1048,9 +1052,6 @@ class BodyNode : /// Counts the number of nodes globally. static size_t msBodyNodeCount; - /// Whether the node is currently in collision with another node. - bool mIsColliding; - //-------------------------------------------------------------------------- // Structural Properties //-------------------------------------------------------------------------- @@ -1127,9 +1128,6 @@ class BodyNode : /// frame Eigen::Vector6d mF; - /// External spatial force - Eigen::Vector6d mFext; - /// Spatial gravity force Eigen::Vector6d mFgravity; diff --git a/dart/dynamics/SoftBodyNode.cpp b/dart/dynamics/SoftBodyNode.cpp index 9cef0c737cf40..6bdaabb151b8a 100644 --- a/dart/dynamics/SoftBodyNode.cpp +++ b/dart/dynamics/SoftBodyNode.cpp @@ -583,7 +583,7 @@ void SoftBodyNode::updateTransmittedForceID(const Eigen::Vector3d& _gravity, // External force if (_withExternalForces) - mF -= mFext; + mF -= BodyNode::mAspectState.mFext; // Verification assert(!math::isNan(mF)); @@ -707,7 +707,7 @@ void SoftBodyNode::updateBiasForce(const Eigen::Vector3d& _gravity, // Set bias force const Eigen::Vector6d& V = getSpatialVelocity(); - mBiasForce = -math::dad(V, mI * V) - mFext - mFgravity; + mBiasForce = -math::dad(V, mI * V) - BodyNode::mAspectState.mFext - mFgravity; // Verifycation assert(!math::isNan(mBiasForce)); @@ -1163,7 +1163,7 @@ void SoftBodyNode::aggregateExternalForces(Eigen::VectorXd& _Fext) mPointMasses.at(i)->aggregateExternalForces(_Fext); //----------------------- SoftBodyNode Part ---------------------------------- - mFext_F = mFext; + mFext_F = BodyNode::mAspectState.mFext; for (std::vector::const_iterator it = mChildBodyNodes.begin(); it != mChildBodyNodes.end(); ++it) diff --git a/dart/dynamics/detail/BodyNodeProperties.h b/dart/dynamics/detail/BodyNodeProperties.h index 545502735ff02..dc8cede831c72 100644 --- a/dart/dynamics/detail/BodyNodeProperties.h +++ b/dart/dynamics/detail/BodyNodeProperties.h @@ -50,6 +50,19 @@ const double DART_DEFAULT_RESTITUTION_COEFF = 0.0; namespace detail { +//============================================================================== +struct BodyNodeState +{ + /// Whether the node is currently in collision with another node. + bool mIsColliding; + + /// External spatial force + Eigen::Vector6d mFext; + + BodyNodeState(bool isColliding = false, + const Eigen::Vector6d& Fext = Eigen::Vector6d::Zero()); +}; + //============================================================================== struct BodyNodeUniqueProperties { From 6343eb35aa5145c0f997b2db6fb717dec8d6a3b9 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Sat, 2 Apr 2016 17:31:45 -0400 Subject: [PATCH 28/62] renaming Extensible to Cloneable --- dart/common/Aspect.h | 14 +- dart/common/AspectWithVersion.h | 4 +- dart/common/Cloneable.h | 299 ++++++++++++++ dart/common/Composite.h | 4 +- dart/common/Extensible.h | 219 ----------- dart/common/detail/AspectWithVersion.h | 4 +- dart/common/detail/Cloneable.h | 518 +++++++++++++++++++++++++ dart/common/detail/EmbeddedAspect.h | 4 +- dart/common/detail/Extensible.h | 324 ---------------- 9 files changed, 832 insertions(+), 558 deletions(-) create mode 100644 dart/common/Cloneable.h delete mode 100644 dart/common/Extensible.h create mode 100644 dart/common/detail/Cloneable.h delete mode 100644 dart/common/detail/Extensible.h diff --git a/dart/common/Aspect.h b/dart/common/Aspect.h index 3cd68849a8670..2942fe38afb42 100644 --- a/dart/common/Aspect.h +++ b/dart/common/Aspect.h @@ -39,7 +39,7 @@ #include -#include "dart/common/Extensible.h" +#include "dart/common/Cloneable.h" #include "dart/common/detail/NoOp.h" namespace dart { @@ -65,12 +65,12 @@ class Aspect /// stored in Composite::Properties. Typically Properties are values that /// only change rarely if ever, whereas State contains values that might /// change as often as every time step. - class State : public Extensible { }; + class State : public Cloneable { }; - /// Use the StateMixer class to easily create a State extension from an + /// Use the MakeState class to easily create a State extension from an /// existing class or struct. template - using StateMixer = ExtensibleMixer; + using MakeState = MakeCloneable; /// If your Aspect has Properties, then that Properties class should inherit this /// Aspect::Properties class. This allows us to safely serialize, store, and @@ -84,12 +84,12 @@ class Aspect /// stored in Composite::Properties. Typically Properties are values that /// only change rarely if ever, whereas State contains values that might /// change as often as every time step. - class Properties : public Extensible { }; + class Properties : public Cloneable { }; - /// Use the PropertiesMixer class to easily create a Properties extension + /// Use the MakeProperties class to easily create a Properties extension /// from an existing class or struct. template - using PropertiesMixer = ExtensibleMixer; + using MakeProperties = MakeCloneable; /// Virtual destructor virtual ~Aspect() = default; diff --git a/dart/common/AspectWithVersion.h b/dart/common/AspectWithVersion.h index 76751c1c7cfd9..eb9adf5a5bcd3 100644 --- a/dart/common/AspectWithVersion.h +++ b/dart/common/AspectWithVersion.h @@ -76,8 +76,8 @@ class AspectWithStateAndVersionedProperties : using StateData = StateDataT; using PropertiesData = PropertiesDataT; using CompositeType = CompositeT; - using State = common::Aspect::StateMixer; - using Properties = common::Aspect::PropertiesMixer; + using State = common::Aspect::MakeState; + using Properties = common::Aspect::MakeProperties; constexpr static void (*UpdateState)(Derived*) = updateState; constexpr static void (*UpdateProperties)(Derived*) = updateProperties; diff --git a/dart/common/Cloneable.h b/dart/common/Cloneable.h new file mode 100644 index 0000000000000..14c8fdef17680 --- /dev/null +++ b/dart/common/Cloneable.h @@ -0,0 +1,299 @@ +/* + * Copyright (c) 2015, 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_COMMON_EXTENSIBLE_H_ +#define DART_COMMON_EXTENSIBLE_H_ + +#include +#include + +namespace dart { +namespace common { + +//============================================================================== +/// Cloneable is a CRTP base class that provides an interface for easily +/// creating data structures that are meant to be extended. Ordinary copying +/// does not work with extended data structures, because information from the +/// derived classes will typically be lost during a copy. The Cloneable class +/// provides an interface for creating a new copy of an extended data structure, +/// as well as copying information between two extended data structures of the +/// same type. +template +class Cloneable +{ +public: + + /// Default constructor + Cloneable() = default; + + /// Virtual destructor + virtual ~Cloneable() = default; + + /// Do not copy this class directly, use clone() or copy() instead + Cloneable(const Cloneable& doNotCopy) = delete; + + /// Do not copy this class directly, use clone() or copy() instead + Cloneable& operator=(const Cloneable& doNotCopy) = delete; + + /// Implement this function to allow your Cloneable type to be copied safely. + virtual std::unique_ptr clone() const = 0; + + /// Copy the contents of anotherCloneable into this one. + virtual void copy(const T& anotherCloneable) = 0; +}; + +//============================================================================== +/// The MakeCloneable class is used to easily create an Cloneable (such as +/// Node::State) which simply takes an existing class (Mixin) and creates an +/// Cloneable that wraps it. This creates all the appropriate copy, move, and +/// clone members, allowing you to follow the Rule Of Zero. You can also +/// construct an instance in the exact same way that you would construct a Mixin +/// instance. +template +class MakeCloneable : public Base, public Mixin +{ +public: + + using Data = Mixin; + + /// Default constructor. Uses the default constructor of Mixin + MakeCloneable(); + + /// Templated constructor. Uses whichever Mixin constructor is able to match + /// the arguments. + template + MakeCloneable(Args&&... args); + + /// Constructs using a Mixin instance + MakeCloneable(const Mixin& mixin); + + /// Constructs using a Mixin rvalue + MakeCloneable(Mixin&& mixin); + + /// Copy constructor + MakeCloneable(const MakeCloneable& other); + + /// Move constructor + MakeCloneable(MakeCloneable&& other); + + /// Copy assignment operator that uses a Mixin instance + MakeCloneable& operator=(const Mixin& mixin); + + /// Move assignment operator that uses a Mixin rvalue + MakeCloneable& operator=(Mixin&& mixin); + + /// Copy assignment operator + MakeCloneable& operator=(const MakeCloneable& other); + + /// Move assignment operator + MakeCloneable& operator=(MakeCloneable&& other); + + // Documentation inherited + std::unique_ptr clone() const override final; + + // Documentation inherited + void copy(const Base& other) override final; + +}; + +//============================================================================== +template +class ProxyCloneable : public Base +{ +public: + + using Data = DataT; + + /// Default constructor. Constructs a default version of Data. + ProxyCloneable(); + + /// Constructor that ties this instance to an Owner. + ProxyCloneable(Owner* owner); + + /// Constructor that ties this instance to an Owner and then sets its data + /// using the remaining arguments. + template + ProxyCloneable(Owner* owner, Args&&... args); + + /// Templated constructor. Uses whichever Data constructor is able to match + /// the arguments. + template + ProxyCloneable(Args&&... args); + + /// Copy constructor + ProxyCloneable(const ProxyCloneable& other); + + /// Move constructor + ProxyCloneable(ProxyCloneable&& other); + + /// Copy assignment operator that uses a Data instance + ProxyCloneable& operator=(const Data& data); + + /// Move assignment operator that uses a Data instance + ProxyCloneable& operator=(Data&& other); + + /// Copy assignment operator + ProxyCloneable& operator=(const ProxyCloneable& other); + + /// Move assignment operator + ProxyCloneable& operator=(ProxyCloneable&& other); + + /// Set the Data of this ProxyCloneable + void set(const Data& data); + + /// Set the Data of this ProxyCloneable + void set(Data&& data); + + /// Set the Data of this ProxyCloneable based on another + void set(const ProxyCloneable& other); + + /// Set the Data of this ProxyCloneable based on another + void set(ProxyCloneable&& other); + + /// Get the Data of this ProxyCloneable + Data get() const; + + // Documentation inherited + std::unique_ptr clone() const override final; + + // Documentation inherited + void copy(const Base& other) override final; + +protected: + + /// The ProxyCloneable will not store any data in mData if it has an Owner. + Owner* mOwner; + + /// The ProxyCloneable will hold data in this field if it does not have an + /// owner + std::unique_ptr mData; + +}; + +//============================================================================== +/// MapHolder is a templated wrapper class that is used to allow maps of +/// Aspect::State and Aspect::Properties to be handled in a semantically +/// palatable way. +template +class CloneableMap final +{ +public: + + /// Default constructor + CloneableMap() = default; + + /// Copy constructor + CloneableMap(const CloneableMap& otherStates); + + /// Move constructor + CloneableMap(CloneableMap&& otherStates); + + /// Map-based constructor + CloneableMap(const MapType& otherMap); + + /// Map-based move constructor + CloneableMap(MapType&& otherMap); + + /// Assignment operator + CloneableMap& operator=(const CloneableMap& otherStates); + + /// Move assignment operator + CloneableMap& operator=(CloneableMap&& otherStates); + + /// Map-based assignment operator + CloneableMap& operator=(const MapType& otherMap); + + /// Map-based move assignment operator + CloneableMap& operator=(MapType&& otherMap); + + /// Get the map that is being held + MapType& getMap(); + + /// Get the map that is being held + const MapType& getMap() const; + +private: + + /// A map containing the collection of States for the Aspect + MapType mMap; +}; + +//============================================================================== +/// The CloneableVector type wraps a std::vector of an Cloneable type allowing +/// it to be handled by an CloneableMapHolder +template +class CloneableVector final +{ +public: + + /// Default constructor + CloneableVector() = default; + + /// Construct from a regular vector + CloneableVector(const std::vector& regularVector); + + /// Construct from a regular vector using move semantics + CloneableVector(std::vector&& regularVector); + + /// Do not copy this class directly, use clone() or copy() instead + CloneableVector(const CloneableVector& doNotCopy) = delete; + + /// Do not copy this class directly, use clone() or copy() instead + CloneableVector& operator=(const CloneableVector& doNotCopy) = delete; + + /// Create a copy of this CloneableVector's contents + std::unique_ptr< CloneableVector > clone() const; + + /// Copy the contents of another extensible vector into this one. + void copy(const CloneableVector& anotherVector); + + /// Get a reference to the std::vector that this class is wrapping + const std::vector& getVector() const; + +private: + + /// The std::vector that this class is wrapping + std::vector mVector; +}; + +} // namespace common +} // namespace dart + +#include "dart/common/detail/Cloneable.h" + +#endif // DART_COMMON_EXTENSIBLE_H_ diff --git a/dart/common/Composite.h b/dart/common/Composite.h index 4d746da47962b..55be521f89bf3 100644 --- a/dart/common/Composite.h +++ b/dart/common/Composite.h @@ -62,10 +62,10 @@ class Composite public: using StateMap = std::map< std::type_index, std::unique_ptr >; - using State = ExtensibleMapHolder; + using State = CloneableMap; using PropertiesMap = std::map< std::type_index, std::unique_ptr >; - using Properties = ExtensibleMapHolder; + using Properties = CloneableMap; using AspectMap = std::map< std::type_index, std::unique_ptr >; using RequiredAspectSet = std::unordered_set; diff --git a/dart/common/Extensible.h b/dart/common/Extensible.h deleted file mode 100644 index f88e3e6e2b210..0000000000000 --- a/dart/common/Extensible.h +++ /dev/null @@ -1,219 +0,0 @@ -/* - * Copyright (c) 2015, 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_COMMON_EXTENSIBLE_H_ -#define DART_COMMON_EXTENSIBLE_H_ - -#include -#include - -namespace dart { -namespace common { - -/// Extensible is a CRTP base class that provides an interface for easily -/// creating data structures that are meant to be extended. Ordinary copying -/// does not work with extended data structures, because information from the -/// derived classes will typically be lost during a copy. The Extensible class -/// provides an interface for creating a new copy of an extended data structure, -/// as well as copying information between two extended data structures of the -/// same type. -template -class Extensible -{ -public: - - /// Default constructor - Extensible() = default; - - /// Virtual destructor - virtual ~Extensible() = default; - - /// Do not copy this class directly, use clone() or copy() instead - Extensible(const Extensible& doNotCopy) = delete; - - /// Do not copy this class directly, use clone() or copy() instead - Extensible& operator=(const Extensible& doNotCopy) = delete; - - /// Implement this function to allow your Extensible type to be copied safely. - virtual std::unique_ptr clone() const = 0; - - /// Copy the contents of anotherExtensible into this one. - virtual void copy(const T& anotherExtensible) = 0; -}; - -/// The ExtensibleMixer class is used to easily create an Extensible (such as -/// Node::State) which simply takes an existing class (Mixin) and creates an -/// Extensible that wraps it. This creates all the appropriate copy, move, and -/// clone members, allowing you to follow the Rule Of Zero. You can also -/// construct an instance in the exact same way that you would construct a Mixin -/// instance. -template -class ExtensibleMixer : public T, public Mixin -{ -public: - - using Data = Mixin; - - /// Default constructor. Uses the default constructor of Mixin - ExtensibleMixer(); - - /// Templated constructor. Uses whichever Mixin constructor is able to match - /// the arguments. - template - ExtensibleMixer(Args&&... args); - - /// Constructs using a Mixin instance - ExtensibleMixer(const Mixin& mixin); - - /// Constructs using a Mixin rvalue - ExtensibleMixer(Mixin&& mixin); - - /// Copy constructor - ExtensibleMixer(const ExtensibleMixer& other); - - /// Move constructor - ExtensibleMixer(ExtensibleMixer&& other); - - /// Copy assignment operator that uses a Mixin instance - ExtensibleMixer& operator=(const Mixin& mixin); - - /// Move assignment operator that uses a Mixin rvalue - ExtensibleMixer& operator=(Mixin&& mixin); - - /// Copy assignment operator - ExtensibleMixer& operator=(const ExtensibleMixer& other); - - /// Move assignment operator - ExtensibleMixer& operator=(ExtensibleMixer&& other); - - // Documentation inherited - std::unique_ptr clone() const override final; - - // Documentation inherited - void copy(const T& other) override final; - -}; - -/// MapHolder is a templated wrapper class that is used to allow maps of -/// Aspect::State and Aspect::Properties to be handled in a semantically -/// palatable way. -template -class ExtensibleMapHolder final -{ -public: - - /// Default constructor - ExtensibleMapHolder() = default; - - /// Copy constructor - ExtensibleMapHolder(const ExtensibleMapHolder& otherStates); - - /// Move constructor - ExtensibleMapHolder(ExtensibleMapHolder&& otherStates); - - /// Map-based constructor - ExtensibleMapHolder(const MapType& otherMap); - - /// Map-based move constructor - ExtensibleMapHolder(MapType&& otherMap); - - /// Assignment operator - ExtensibleMapHolder& operator=(const ExtensibleMapHolder& otherStates); - - /// Move assignment operator - ExtensibleMapHolder& operator=(ExtensibleMapHolder&& otherStates); - - /// Map-based assignment operator - ExtensibleMapHolder& operator=(const MapType& otherMap); - - /// Map-based move assignment operator - ExtensibleMapHolder& operator=(MapType&& otherMap); - - /// Get the map that is being held - MapType& getMap(); - - /// Get the map that is being held - const MapType& getMap() const; - -private: - - /// A map containing the collection of States for the Aspect - MapType mMap; -}; - -/// The ExtensibleVector type wraps a std::vector of an Extensible type allowing -/// it to be handled by an ExtensibleMapHolder -template -class ExtensibleVector final -{ -public: - - /// Default constructor - ExtensibleVector() = default; - - /// Construct from a regular vector - ExtensibleVector(const std::vector& regularVector); - - /// Construct from a regular vector using move semantics - ExtensibleVector(std::vector&& regularVector); - - /// Do not copy this class directly, use clone() or copy() instead - ExtensibleVector(const ExtensibleVector& doNotCopy) = delete; - - /// Do not copy this class directly, use clone() or copy() instead - ExtensibleVector& operator=(const ExtensibleVector& doNotCopy) = delete; - - /// Create a copy of this ExtensibleVector's contents - std::unique_ptr< ExtensibleVector > clone() const; - - /// Copy the contents of another extensible vector into this one. - void copy(const ExtensibleVector& anotherVector); - - /// Get a reference to the std::vector that this class is wrapping - const std::vector& getVector() const; - -private: - - /// The std::vector that this class is wrapping - std::vector mVector; -}; - -} // namespace common -} // namespace dart - -#include "dart/common/detail/Extensible.h" - -#endif // DART_COMMON_EXTENSIBLE_H_ diff --git a/dart/common/detail/AspectWithVersion.h b/dart/common/detail/AspectWithVersion.h index 9c69c71a06527..c7c6bf83cd894 100644 --- a/dart/common/detail/AspectWithVersion.h +++ b/dart/common/detail/AspectWithVersion.h @@ -58,7 +58,7 @@ class AspectWithState : public BaseT using Derived = DerivedT; using StateData = StateDataT; using CompositeType = CompositeT; - using State = Aspect::StateMixer; + using State = Aspect::MakeState; constexpr static void (*UpdateState)(Derived*) = updateState; using AspectImplementation = AspectWithState< @@ -116,7 +116,7 @@ class AspectWithVersionedProperties : public BaseT using Derived = DerivedT; using PropertiesData = PropertiesDataT; using CompositeType = CompositeT; - using Properties = Aspect::PropertiesMixer; + using Properties = Aspect::MakeProperties; constexpr static void (*UpdateProperties)(Derived*) = updateProperties; using AspectImplementation = AspectWithVersionedProperties< diff --git a/dart/common/detail/Cloneable.h b/dart/common/detail/Cloneable.h new file mode 100644 index 0000000000000..6d074136113c8 --- /dev/null +++ b/dart/common/detail/Cloneable.h @@ -0,0 +1,518 @@ +/* + * Copyright (c) 2015, 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_COMMON_DETAIL_EXTENSIBLE_H_ +#define DART_COMMON_DETAIL_EXTENSIBLE_H_ + +#include "dart/common/Cloneable.h" +#include "dart/common/StlHelpers.h" + +namespace dart { +namespace common { + +//============================================================================== +template +MakeCloneable::MakeCloneable() +{ + // Do nothing +} + +//============================================================================== +template +template +MakeCloneable::MakeCloneable(Args&&... args) + : Mixin(std::forward(args)...) +{ + // Do nothing +} + +//============================================================================== +template +MakeCloneable::MakeCloneable(const Mixin& mixin) + : Mixin(mixin) +{ + // Do nothing +} + +//============================================================================== +template +MakeCloneable::MakeCloneable(Mixin&& mixin) + : Mixin(std::move(mixin)) +{ + // Do nothing +} + +//============================================================================== +template +MakeCloneable::MakeCloneable( + const MakeCloneable& other) + : Mixin(other) +{ + // Do nothing +} + +//============================================================================== +template +MakeCloneable::MakeCloneable(MakeCloneable&& other) + : Mixin(other) +{ + // Do nothing +} + +//============================================================================== +template +MakeCloneable& MakeCloneable::operator=( + const Mixin& mixin) +{ + static_cast(*this) = mixin; + return *this; +} + +//============================================================================== +template +MakeCloneable& MakeCloneable::operator=(Mixin&& mixin) +{ + static_cast(*this) = std::move(mixin); + return *this; +} + +//============================================================================== +template +MakeCloneable& MakeCloneable::operator=( + const MakeCloneable& other) +{ + static_cast(*this) = static_cast(other); + return *this; +} + +//============================================================================== +template +MakeCloneable& MakeCloneable::operator=( + MakeCloneable&& other) +{ + static_cast(*this) = std::move(static_cast(other)); +} + +//============================================================================== +template +std::unique_ptr MakeCloneable::clone() const +{ + return common::make_unique>(*this); +} + +//============================================================================== +template +void MakeCloneable::copy(const Base& other) +{ + *this = static_cast&>(other); +} + +//============================================================================== +template +ProxyCloneable::ProxyCloneable() + : mOwner(nullptr), + mData(make_unique()) +{ + // Do nothing +} + +//============================================================================== +template +ProxyCloneable::ProxyCloneable( + Owner* owner) + : mOwner(owner) +{ + // Do nothing +} + +//============================================================================== +template +template +ProxyCloneable::ProxyCloneable( + Owner* owner, Args&&... args) + : mOwner(owner) +{ + set(Data(std::forward(args)...)); +} + +//============================================================================== +template +template +ProxyCloneable::ProxyCloneable( + Args&&... args) + : mOwner(nullptr), + mData(make_unique(std::forward(args)...)) +{ + // Do nothing +} + +//============================================================================== +template +ProxyCloneable::ProxyCloneable( + const ProxyCloneable& other) + : mOwner(nullptr), + mData(nullptr) +{ + set(other); +} + +//============================================================================== +template +ProxyCloneable::ProxyCloneable( + ProxyCloneable&& other) + : mOwner(nullptr) +{ + set(other); +} + +//============================================================================== +template +auto ProxyCloneable::operator =( + const Data& data) -> ProxyCloneable& +{ + set(data); +} + +//============================================================================== +template +auto ProxyCloneable::operator =( + Data&& data) -> ProxyCloneable& +{ + set(data); +} + +//============================================================================== +template +auto ProxyCloneable::operator =( + const ProxyCloneable& other) -> ProxyCloneable& +{ + set(other); +} + +//============================================================================== +template +auto ProxyCloneable::operator =( + ProxyCloneable&& other) -> ProxyCloneable& +{ + set(other); +} + +//============================================================================== +template +void ProxyCloneable::set( + const Data& data) +{ + if(mOwner) + { + (*setData)(mOwner, data); + return; + } + + mData = make_unique(data); +} + +//============================================================================== +template +void ProxyCloneable::set( + Data&& data) +{ + if(mOwner) + { + (*setData)(mOwner, data); + return; + } + + mData = make_unique(std::move(data)); +} + +//============================================================================== +template +void ProxyCloneable::set( + const ProxyCloneable& other) +{ + set(other.get()); +} + +//============================================================================== +template +void ProxyCloneable::set( + ProxyCloneable&& other) +{ + set(other.get()); +} + +//============================================================================== +template +auto ProxyCloneable::get() const -> Data +{ + if(mOwner) + return (*getData)(mOwner); + + return *mData; +} + +//============================================================================== +template +std::unique_ptr ProxyCloneable< + Base, Owner, DataT, setData, getData>::clone() const +{ + return make_unique(get()); +} + +//============================================================================== +template +void ProxyCloneable::copy( + const Base& other) +{ + set(static_cast(other)); +} + +//============================================================================== +template +CloneableMap::CloneableMap( + const CloneableMap& otherHolder) +{ + *this = otherHolder; +} + +//============================================================================== +template +CloneableMap::CloneableMap( + CloneableMap&& otherHolder) +{ + *this = std::move(otherHolder); +} + +//============================================================================== +template +CloneableMap::CloneableMap(const MapType& otherMap) +{ + *this = otherMap; +} + +//============================================================================== +template +CloneableMap::CloneableMap(MapType&& otherMap) +{ + *this = std::move(otherMap); +} + +//============================================================================== +template +CloneableMap& CloneableMap::operator=( + const CloneableMap& otherHolder) +{ + *this = otherHolder.getMap(); + + return *this; +} + +//============================================================================== +template +CloneableMap& CloneableMap::operator=( + CloneableMap&& otherHolder) +{ + mMap = std::move(otherHolder.mMap); + + return *this; +} + +//============================================================================== +template +CloneableMap& CloneableMap::operator=( + const MapType& otherMap) +{ + typename MapType::iterator receiver = mMap.begin(); + typename MapType::const_iterator sender = otherMap.begin(); + + while( otherMap.end() != sender ) + { + if( mMap.end() == receiver ) + { + // If we've reached the end of this CloneableMapHolder's map, then we + // should just add each entry + mMap[sender->first] = sender->second->clone(); + ++sender; + } + else if( receiver->first == sender->first ) + { + // We should copy the incoming object when possible so we can avoid the + // memory allocation overhead of cloning. + if(receiver->second) + receiver->second->copy(*sender->second); + else + receiver->second = sender->second->clone(); + + ++receiver; + ++sender; + } + else if( receiver->first < sender->first ) + { + // Clear this entry in the map, because it does not have an analog in the + // map that we are copying + receiver->second = nullptr; + ++receiver; + } + else + { + mMap[sender->first] = sender->second->clone(); + ++sender; + } + } + + while( mMap.end() != receiver ) + { + mMap.erase(receiver++); + } + + return *this; +} + +//============================================================================== +template +CloneableMap& CloneableMap::operator=( + MapType&& otherHolder) +{ + mMap = std::move(otherHolder); + + return *this; +} + +//============================================================================== +template +MapType& CloneableMap::getMap() +{ + return mMap; +} + +//============================================================================== +template +const MapType& CloneableMap::getMap() const +{ + return mMap; +} + +//============================================================================== +template +CloneableVector::CloneableVector(const std::vector& regularVector) + : mVector(regularVector) +{ + // Do nothing +} + +//============================================================================== +template +CloneableVector::CloneableVector(std::vector&& regularVector) +{ + mVector = std::move(regularVector); +} + +//============================================================================== +template +std::unique_ptr< CloneableVector > CloneableVector::clone() const +{ + std::vector clonedVector; + clonedVector.reserve(mVector.size()); + + for(const T& entry : mVector) + clonedVector.push_back(entry->clone()); + + return common::make_unique< CloneableVector >( std::move(clonedVector) ); +} + +//============================================================================== +template +void CloneableVector::copy(const CloneableVector& anotherVector) +{ + const std::vector& other = anotherVector.getVector(); + mVector.resize(other.size()); + + for(size_t i=0; i < other.size(); ++i) + { + if(mVector[i] && other[i]) + mVector[i]->copy(*other[i]); + else if(other[i]) + mVector[i] = other[i]->clone(); + else + mVector[i] = nullptr; + } +} + +//============================================================================== +template +const std::vector& CloneableVector::getVector() const +{ + return mVector; +} + +} // namespace common +} // namespace dart + +#endif // DART_COMMON_DETAIL_EXTENSIBLE_H_ diff --git a/dart/common/detail/EmbeddedAspect.h b/dart/common/detail/EmbeddedAspect.h index 187ed838d87ab..813fd89079f7f 100644 --- a/dart/common/detail/EmbeddedAspect.h +++ b/dart/common/detail/EmbeddedAspect.h @@ -74,7 +74,7 @@ const PropertiesT& DefaultGetEmbeddedProperties(const AspectT* aspect) //============================================================================== template , + typename StateT = common::Aspect::MakeState, void (*setEmbeddedState)(DerivedT*, const StateT&) = &DefaultSetEmbeddedState, const StateT& (*getEmbeddedState)(const DerivedT*) = @@ -243,7 +243,7 @@ class EmbeddedStateAspect : public BaseT //============================================================================== template , + typename PropertiesT = common::Aspect::MakeProperties, void (*setEmbeddedProperties)(DerivedT*, const PropertiesT&) = &DefaultSetEmbeddedProperties, const PropertiesT& (*getEmbeddedProperties)(const DerivedT*) = diff --git a/dart/common/detail/Extensible.h b/dart/common/detail/Extensible.h deleted file mode 100644 index 29c84401232b6..0000000000000 --- a/dart/common/detail/Extensible.h +++ /dev/null @@ -1,324 +0,0 @@ -/* - * Copyright (c) 2015, 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_COMMON_DETAIL_EXTENSIBLE_H_ -#define DART_COMMON_DETAIL_EXTENSIBLE_H_ - -#include "dart/common/Extensible.h" -#include "dart/common/StlHelpers.h" - -namespace dart { -namespace common { - -//============================================================================== -template -ExtensibleMixer::ExtensibleMixer() -{ - // Do nothing -} - -//============================================================================== -template -template -ExtensibleMixer::ExtensibleMixer(Args&&... args) - : Mixin(std::forward(args)...) -{ - // Do nothing -} - -//============================================================================== -template -ExtensibleMixer::ExtensibleMixer(const Mixin& mixin) - : Mixin(mixin) -{ - // Do nothing -} - -//============================================================================== -template -ExtensibleMixer::ExtensibleMixer(Mixin&& mixin) - : Mixin(std::move(mixin)) -{ - // Do nothing -} - -//============================================================================== -template -ExtensibleMixer::ExtensibleMixer( - const ExtensibleMixer& other) - : Mixin(other) -{ - // Do nothing -} - -//============================================================================== -template -ExtensibleMixer::ExtensibleMixer(ExtensibleMixer&& other) - : Mixin(other) -{ - // Do nothing -} - -//============================================================================== -template -ExtensibleMixer& ExtensibleMixer::operator=( - const Mixin& mixin) -{ - static_cast(*this) = mixin; - return *this; -} - -//============================================================================== -template -ExtensibleMixer& ExtensibleMixer::operator=(Mixin&& mixin) -{ - static_cast(*this) = std::move(mixin); - return *this; -} - -//============================================================================== -template -ExtensibleMixer& ExtensibleMixer::operator=( - const ExtensibleMixer& other) -{ - static_cast(*this) = static_cast(other); - return *this; -} - -//============================================================================== -template -ExtensibleMixer& ExtensibleMixer::operator=( - ExtensibleMixer&& other) -{ - static_cast(*this) = std::move(static_cast(other)); -} - -//============================================================================== -template -std::unique_ptr ExtensibleMixer::clone() const -{ - return common::make_unique>(*this); -} - -//============================================================================== -template -void ExtensibleMixer::copy(const T& other) -{ - *this = static_cast&>(other); -} - -//============================================================================== -template -ExtensibleMapHolder::ExtensibleMapHolder( - const ExtensibleMapHolder& otherHolder) -{ - *this = otherHolder; -} - -//============================================================================== -template -ExtensibleMapHolder::ExtensibleMapHolder( - ExtensibleMapHolder&& otherHolder) -{ - *this = std::move(otherHolder); -} - -//============================================================================== -template -ExtensibleMapHolder::ExtensibleMapHolder(const MapType& otherMap) -{ - *this = otherMap; -} - -//============================================================================== -template -ExtensibleMapHolder::ExtensibleMapHolder(MapType&& otherMap) -{ - *this = std::move(otherMap); -} - -//============================================================================== -template -ExtensibleMapHolder& ExtensibleMapHolder::operator=( - const ExtensibleMapHolder& otherHolder) -{ - *this = otherHolder.getMap(); - - return *this; -} - -//============================================================================== -template -ExtensibleMapHolder& ExtensibleMapHolder::operator=( - ExtensibleMapHolder&& otherHolder) -{ - mMap = std::move(otherHolder.mMap); - - return *this; -} - -//============================================================================== -template -ExtensibleMapHolder& ExtensibleMapHolder::operator=( - const MapType& otherMap) -{ - typename MapType::iterator receiver = mMap.begin(); - typename MapType::const_iterator sender = otherMap.begin(); - - while( otherMap.end() != sender ) - { - if( mMap.end() == receiver ) - { - // If we've reached the end of this ExtensibleMapHolder's map, then we - // should just add each entry - mMap[sender->first] = sender->second->clone(); - ++sender; - } - else if( receiver->first == sender->first ) - { - // We should copy the incoming object when possible so we can avoid the - // memory allocation overhead of cloning. - if(receiver->second) - receiver->second->copy(*sender->second); - else - receiver->second = sender->second->clone(); - - ++receiver; - ++sender; - } - else if( receiver->first < sender->first ) - { - // Clear this entry in the map, because it does not have an analog in the - // map that we are copying - receiver->second = nullptr; - ++receiver; - } - else - { - mMap[sender->first] = sender->second->clone(); - ++sender; - } - } - - while( mMap.end() != receiver ) - { - mMap.erase(receiver++); - } - - return *this; -} - -//============================================================================== -template -ExtensibleMapHolder& ExtensibleMapHolder::operator=( - MapType&& otherHolder) -{ - mMap = std::move(otherHolder); - - return *this; -} - -//============================================================================== -template -MapType& ExtensibleMapHolder::getMap() -{ - return mMap; -} - -//============================================================================== -template -const MapType& ExtensibleMapHolder::getMap() const -{ - return mMap; -} - -//============================================================================== -template -ExtensibleVector::ExtensibleVector(const std::vector& regularVector) - : mVector(regularVector) -{ - // Do nothing -} - -//============================================================================== -template -ExtensibleVector::ExtensibleVector(std::vector&& regularVector) -{ - mVector = std::move(regularVector); -} - -//============================================================================== -template -std::unique_ptr< ExtensibleVector > ExtensibleVector::clone() const -{ - std::vector clonedVector; - clonedVector.reserve(mVector.size()); - - for(const T& entry : mVector) - clonedVector.push_back(entry->clone()); - - return common::make_unique< ExtensibleVector >( std::move(clonedVector) ); -} - -//============================================================================== -template -void ExtensibleVector::copy(const ExtensibleVector& anotherVector) -{ - const std::vector& other = anotherVector.getVector(); - mVector.resize(other.size()); - - for(size_t i=0; i < other.size(); ++i) - { - if(mVector[i] && other[i]) - mVector[i]->copy(*other[i]); - else if(other[i]) - mVector[i] = other[i]->clone(); - else - mVector[i] = nullptr; - } -} - -//============================================================================== -template -const std::vector& ExtensibleVector::getVector() const -{ - return mVector; -} - -} // namespace common -} // namespace dart - -#endif // DART_COMMON_DETAIL_EXTENSIBLE_H_ From 402f8f40e87b172ceae370efe0be9831c000af9b Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Sat, 2 Apr 2016 17:34:30 -0400 Subject: [PATCH 29/62] refactoring name out of Entity --- dart/dynamics/BodyNode.cpp | 73 ++++++++++++--------- dart/dynamics/BodyNode.h | 10 +-- dart/dynamics/EndEffector.cpp | 41 +++++++----- dart/dynamics/EndEffector.h | 17 +++-- dart/dynamics/Entity.cpp | 74 ++------------------- dart/dynamics/Entity.h | 37 +---------- dart/dynamics/FixedFrame.cpp | 6 +- dart/dynamics/FixedFrame.h | 8 ++- dart/dynamics/Frame.cpp | 21 ++++-- dart/dynamics/Frame.h | 16 +++-- dart/dynamics/Inertia.cpp | 6 ++ dart/dynamics/Inertia.h | 3 + dart/dynamics/JacobianNode.cpp | 6 -- dart/dynamics/JacobianNode.h | 9 +-- dart/dynamics/Node.h | 14 ++-- dart/dynamics/PointMass.cpp | 23 ++++++- dart/dynamics/PointMass.h | 14 +++- dart/dynamics/ShapeFrame.cpp | 78 +++++------------------ dart/dynamics/ShapeFrame.h | 44 +++---------- dart/dynamics/ShapeNode.cpp | 56 ++++++++++------ dart/dynamics/ShapeNode.h | 15 +++-- dart/dynamics/SimpleFrame.cpp | 33 ++++++++-- dart/dynamics/SimpleFrame.h | 19 ++++-- dart/dynamics/Skeleton.cpp | 4 +- dart/dynamics/SoftBodyNode.cpp | 6 +- dart/dynamics/detail/BodyNodeProperties.h | 26 +++----- osgDart/InteractiveFrame.cpp | 6 +- unittests/testForwardKinematics.cpp | 4 +- unittests/testNameManagement.cpp | 17 ++++- unittests/testSignal.cpp | 19 +++--- unittests/testSubscriptions.cpp | 4 +- 31 files changed, 340 insertions(+), 369 deletions(-) diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index 0954c43a1eb09..ce523bf45eef8 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -128,10 +128,12 @@ BodyNodeState::BodyNodeState( //============================================================================== BodyNodeUniqueProperties::BodyNodeUniqueProperties( + const std::string& name, const Inertia& _inertia, bool _isCollidable, double _frictionCoeff, double _restitutionCoeff, bool _gravityMode) - : mInertia(_inertia), + : mName(name), + mInertia(_inertia), mIsCollidable(_isCollidable), mFrictionCoeff(_frictionCoeff), mRestitutionCoeff(_restitutionCoeff), @@ -140,24 +142,14 @@ BodyNodeUniqueProperties::BodyNodeUniqueProperties( // Do nothing } -//============================================================================== -BodyNodeProperties::BodyNodeProperties( - const Entity::Properties& _entityProperties, - const BodyNodeUniqueProperties& _bodyNodeProperties) - : Entity::Properties(_entityProperties), - BodyNodeUniqueProperties(_bodyNodeProperties) -{ - // Do nothing -} - //============================================================================== BodyNodeExtendedProperties::BodyNodeExtendedProperties( const BodyNodeProperties& standardProperties, const NodeProperties& nodeProperties, - const CompositeProperties& aspectProperties) + const CompositeProperties& compositeProperties) : BodyNodeProperties(standardProperties), mNodeProperties(nodeProperties), - mCompositeProperties(aspectProperties) + mCompositeProperties(compositeProperties) { // Do nothing } @@ -246,13 +238,6 @@ void BodyNode::setProperties(const CompositeProperties& _properties) setCompositeProperties(_properties); } -//============================================================================== -void BodyNode::setProperties(const Properties& _properties) -{ - Entity::setProperties(static_cast(_properties)); - setProperties(static_cast(_properties)); -} - //============================================================================== void BodyNode::setProperties(const UniqueProperties& _properties) { @@ -273,11 +258,13 @@ void BodyNode::setAspectState(const AspectState& state) //============================================================================== void BodyNode::setAspectProperties(const AspectProperties& properties) { + setName(properties.mName); setInertia(properties.mInertia); 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) @@ -292,7 +279,7 @@ void BodyNode::setAspectProperties(const AspectProperties& properties) //============================================================================== BodyNode::Properties BodyNode::getBodyNodeProperties() const { - return BodyNode::Properties(mEntityP, mAspectProperties); + return mAspectProperties; } //============================================================================== @@ -393,19 +380,21 @@ void BodyNode::matchNodes(const BodyNode* otherBodyNode) const std::string& BodyNode::setName(const std::string& _name) { // If it already has the requested name, do nothing - if(mEntityP.mName == _name) - return mEntityP.mName; + if(mAspectProperties.mName == _name) + return mAspectProperties.mName; + + const std::string oldName = mAspectProperties.mName; // If the BodyNode belongs to a Skeleton, consult the Skeleton's NameManager const SkeletonPtr& skel = getSkeleton(); if(skel) { - skel->mNameMgrForBodyNodes.removeName(mEntityP.mName); + skel->mNameMgrForBodyNodes.removeName(mAspectProperties.mName); SoftBodyNode* softnode = dynamic_cast(this); if(softnode) - skel->mNameMgrForSoftBodyNodes.removeName(mEntityP.mName); + skel->mNameMgrForSoftBodyNodes.removeName(mAspectProperties.mName); - mEntityP.mName = _name; + mAspectProperties.mName = _name; skel->addEntryToBodyNodeNameMgr(this); if(softnode) @@ -413,12 +402,21 @@ const std::string& BodyNode::setName(const std::string& _name) } else { - mEntityP.mName = _name; + mAspectProperties.mName = _name; } + incrementVersion(); + Entity::mNameChangedSignal.raise(this, oldName, mAspectProperties.mName); + // Return the final name (which might have been altered by the Skeleton's // NameManager) - return mEntityP.mName; + return mAspectProperties.mName; +} + +//============================================================================== +const std::string& BodyNode::getName() const +{ + return mAspectProperties.mName; } //============================================================================== @@ -431,6 +429,8 @@ void BodyNode::setGravityMode(bool _gravityMode) SKEL_SET_FLAGS(mGravityForces); SKEL_SET_FLAGS(mCoriolisAndGravityForces); + + incrementVersion(); } //============================================================================== @@ -504,12 +504,17 @@ const Eigen::Matrix6d& BodyNode::getSpatialInertia() const //============================================================================== void BodyNode::setInertia(const Inertia& _inertia) { + if(_inertia == mAspectProperties.mInertia) + return; + mAspectProperties.mInertia = _inertia; notifyArticulatedInertiaUpdate(); const SkeletonPtr& skel = getSkeleton(); if(skel) skel->updateTotalMass(); + + incrementVersion(); } //============================================================================== @@ -601,9 +606,14 @@ Eigen::Vector6d BodyNode::getCOMSpatialAcceleration(const Frame* _relativeTo, //============================================================================== void BodyNode::setFrictionCoeff(double _coeff) { + if(mAspectProperties.mFrictionCoeff == _coeff) + return; + assert(0.0 <= _coeff && "Coefficient of friction should be non-negative value."); mAspectProperties.mFrictionCoeff = _coeff; + + incrementVersion(); } //============================================================================== @@ -615,9 +625,14 @@ double BodyNode::getFrictionCoeff() const //============================================================================== void BodyNode::setRestitutionCoeff(double _coeff) { + if(_coeff == mAspectProperties.mRestitutionCoeff) + return; + assert(0.0 <= _coeff && _coeff <= 1.0 && "Coefficient of restitution should be in range of [0, 1]."); mAspectProperties.mRestitutionCoeff = _coeff; + + incrementVersion(); } //============================================================================== @@ -1215,7 +1230,7 @@ void BodyNode::setExtTorque(const Eigen::Vector3d& _torque, bool _isLocal) BodyNode::BodyNode(BodyNode* _parentBodyNode, Joint* _parentJoint, const Properties& _properties) : Entity(ConstructFrame), - Frame(Frame::World(), ""), // Name gets set later by setProperties + Frame(Frame::World()), TemplatedJacobianNode(this), mID(BodyNode::msBodyNodeCount++), mParentJoint(_parentJoint), diff --git a/dart/dynamics/BodyNode.h b/dart/dynamics/BodyNode.h index e3aa9e13f9e49..1b99a60430b99 100644 --- a/dart/dynamics/BodyNode.h +++ b/dart/dynamics/BodyNode.h @@ -97,9 +97,9 @@ class BodyNode : using StructuralChangeSignal = common::Signal; - using NodePropertiesVector = common::ExtensibleVector< std::unique_ptr >; + using NodePropertiesVector = common::CloneableVector< std::unique_ptr >; using NodePropertiesMap = std::map< std::type_index, std::unique_ptr >; - using NodeProperties = common::ExtensibleMapHolder; + using NodeProperties = common::CloneableMap; using CompositeProperties = common::Composite::Properties; using UniqueProperties = detail::BodyNodeUniqueProperties; @@ -120,9 +120,6 @@ class BodyNode : /// Same as setCompositeProperties() void setProperties(const CompositeProperties& _properties); - /// Set the Properties of this BodyNode - void setProperties(const Properties& _properties); - /// Set the UniqueProperties of this BodyNode void setProperties(const UniqueProperties& _properties); @@ -162,6 +159,9 @@ class BodyNode : /// version which will be used by the Skeleton const std::string& setName(const std::string& _name) override; + // Documentation inherited + const std::string& getName() const override; + /// Set whether gravity affects this body /// \param[in] _gravityMode True to enable gravity void setGravityMode(bool _gravityMode); diff --git a/dart/dynamics/EndEffector.cpp b/dart/dynamics/EndEffector.cpp index e5b9128b2bfdf..0932acbd82a77 100644 --- a/dart/dynamics/EndEffector.cpp +++ b/dart/dynamics/EndEffector.cpp @@ -87,12 +87,10 @@ EndEffector::UniqueProperties::UniqueProperties( //============================================================================== EndEffector::PropertiesData::PropertiesData( - const Entity::Properties& _entityProperties, const UniqueProperties& _effectorProperties, - const common::Composite::Properties& _aspectProperties) - : Entity::Properties(_entityProperties), - UniqueProperties(_effectorProperties), - mAspectProperties(_aspectProperties) + const common::Composite::Properties& _compositeProperties) + : UniqueProperties(_effectorProperties), + mCompositeProperties(_compositeProperties) { // Do nothing } @@ -120,23 +118,22 @@ EndEffector::StateData EndEffector::getEndEffectorState() const //============================================================================== void EndEffector::setProperties(const PropertiesData& _properties, bool _useNow) { - Entity::setProperties(_properties); setProperties(static_cast(_properties), _useNow); - setCompositeProperties(_properties.mAspectProperties); + setCompositeProperties(_properties.mCompositeProperties); } //============================================================================== void EndEffector::setProperties(const UniqueProperties& _properties, bool _useNow) { + setName(_properties.mName); setDefaultRelativeTransform(_properties.mDefaultTransform, _useNow); } //============================================================================== EndEffector::PropertiesData EndEffector::getEndEffectorProperties() const { - return PropertiesData(getEntityProperties(), mEndEffectorP, - getCompositeProperties()); + return PropertiesData(mEndEffectorP, getCompositeProperties()); } //============================================================================== @@ -169,13 +166,24 @@ EndEffector& EndEffector::operator=(const EndEffector& _otherEndEffector) const std::string& EndEffector::setName(const std::string& _name) { // If it already has the requested name, do nothing - if(mEntityP.mName == _name && !_name.empty()) - return mEntityP.mName; + if(mEndEffectorP.mName == _name && !_name.empty()) + return mEndEffectorP.mName; - mEntityP.mName = registerNameChange(_name); + const std::string oldName = mEndEffectorP.mName; + + mEndEffectorP.mName = registerNameChange(_name); + + incrementVersion(); + Entity::mNameChangedSignal.raise(this, oldName, mEndEffectorP.mName); // Return the resulting name, after it has been checked for uniqueness - return mEntityP.mName; + return mEndEffectorP.mName; +} + +//============================================================================== +const std::string& EndEffector::getName() const +{ + return mEndEffectorP.mName; } //============================================================================== @@ -229,9 +237,8 @@ void EndEffector::copyNodePropertiesTo( EndEffector::Properties* properties = static_cast(outputProperties.get()); - static_cast(*properties) = getEntityProperties(); static_cast(*properties) = mEndEffectorP; - copyCompositePropertiesTo(properties->mAspectProperties); + copyCompositePropertiesTo(properties->mCompositeProperties); } else { @@ -382,8 +389,8 @@ void EndEffector::notifyVelocityUpdate() //============================================================================== EndEffector::EndEffector(BodyNode* _parent, const PropertiesData& _properties) : Entity(ConstructFrame), - Frame(_parent, ""), - FixedFrame(_parent, "", _properties.mDefaultTransform), + Frame(_parent), + FixedFrame(_parent, _properties.mDefaultTransform), TemplatedJacobianNode(_parent) { setProperties(_properties); diff --git a/dart/dynamics/EndEffector.h b/dart/dynamics/EndEffector.h index 31f89139fa9e1..e6711bf014bb0 100644 --- a/dart/dynamics/EndEffector.h +++ b/dart/dynamics/EndEffector.h @@ -126,10 +126,13 @@ class EndEffector final : common::Composite::State mAspectStates; }; - using State = Node::StateMixer; + using State = Node::MakeState; struct UniqueProperties { + /// Name of this EndEffector + std::string mName; + /// The relative transform will be set to this whenever /// resetRelativeTransform() is called Eigen::Isometry3d mDefaultTransform; @@ -140,19 +143,18 @@ class EndEffector final : Eigen::Isometry3d::Identity()); }; - struct PropertiesData : Entity::Properties, UniqueProperties + struct PropertiesData : UniqueProperties { PropertiesData( - const Entity::Properties& _entityProperties = Entity::Properties(), const UniqueProperties& _effectorProperties = UniqueProperties(), - const common::Composite::Properties& _aspectProperties = + const common::Composite::Properties& _compositeProperties = common::Composite::Properties()); /// The properties of the EndEffector's Aspects - common::Composite::Properties mAspectProperties; + common::Composite::Properties mCompositeProperties; }; - using Properties = Node::PropertiesMixer; + using Properties = Node::MakeProperties; /// Destructor virtual ~EndEffector() = default; @@ -194,6 +196,9 @@ class EndEffector final : /// version which will be used by the Skeleton const std::string& setName(const std::string& _name) override; + // Documentation inherited + const std::string& getName() const override; + // Documentation inherited void setNodeState(const Node::State& otherState) override final; diff --git a/dart/dynamics/Entity.cpp b/dart/dynamics/Entity.cpp index 2b3ee9989ef8d..a20ebd4d47547 100644 --- a/dart/dynamics/Entity.cpp +++ b/dart/dynamics/Entity.cpp @@ -50,16 +50,8 @@ namespace dynamics { typedef std::set EntityPtrSet; //============================================================================== -Entity::Properties::Properties(const std::string& _name) - : mName(_name) -{ - // Do nothing -} - -//============================================================================== -Entity::Entity(Frame* _refFrame, const std::string& _name, bool _quiet) - : mEntityP(_name), - mParentFrame(nullptr), +Entity::Entity(Frame* _refFrame, bool _quiet) + : mParentFrame(nullptr), mNeedTransformUpdate(true), mNeedVelocityUpdate(true), mNeedAccelerationUpdate(true), @@ -92,63 +84,6 @@ Entity::~Entity() changeParentFrame(nullptr); } -//============================================================================== -void Entity::setProperties(const Properties& _properties) -{ - // Set name - setName(_properties.mName); -} - -//============================================================================== -const Entity::Properties& Entity::getEntityProperties() const -{ - return mEntityP; -} - -//============================================================================== -void Entity::copy(const Entity& _otherEntity) -{ - if(this == &_otherEntity) - return; - - setProperties(_otherEntity.getEntityProperties()); -} - -//============================================================================== -void Entity::copy(const Entity *_otherEntity) -{ - if(nullptr == _otherEntity) - return; - - copy(*_otherEntity); -} - -//============================================================================== -Entity& Entity::operator=(const Entity& _otherEntity) -{ - copy(_otherEntity); - return *this; -} - -//============================================================================== -const std::string& Entity::setName(const std::string& _name) -{ - if (mEntityP.mName == _name) - return mEntityP.mName; - - const std::string oldName = mEntityP.mName; - mEntityP.mName = _name; - mNameChangedSignal.raise(this, oldName, mEntityP.mName); - - return mEntityP.mName; -} - -//============================================================================== -const std::string& Entity::getName() const -{ - return mEntityP.mName; -} - //============================================================================== void Entity::draw(renderer::RenderInterface* /*_ri*/, const Eigen::Vector4d& /*_color*/, @@ -326,12 +261,13 @@ void Entity::changeParentFrame(Frame* _newParentFrame) notifyTransformUpdate(); } - mFrameChangedSignal.raise(this, oldParentFrame, mParentFrame); + if(mParentFrame) + mFrameChangedSignal.raise(this, oldParentFrame, mParentFrame); } //============================================================================== Detachable::Detachable(Frame* _refFrame, const std::string& _name, bool _quiet) - : Entity(_refFrame, _name, _quiet) + : Entity(_refFrame, _quiet) { // Do nothing } diff --git a/dart/dynamics/Entity.h b/dart/dynamics/Entity.h index 96e1abd55a9c3..98418a6de3af5 100644 --- a/dart/dynamics/Entity.h +++ b/dart/dynamics/Entity.h @@ -81,21 +81,8 @@ class Entity : public virtual common::Subject const std::string& _oldName, const std::string& _newName)>; - // TODO(MXG): Deprecate this with class the next major version-up, and move - // mName into the properties of inheriting classes. - struct Properties - { - /// Name of the Entity - std::string mName; - - /// Constructor - Properties(const std::string& _name = ""); - - virtual ~Properties() = default; - }; - /// Constructor for typical usage - explicit Entity(Frame* _refFrame, const std::string& _name, bool _quiet); + explicit Entity(Frame* _refFrame, bool _quiet); /// Default constructor, delegates to Entity(ConstructAbstract_t) Entity(); @@ -105,28 +92,13 @@ class Entity : public virtual common::Subject /// Destructor virtual ~Entity(); - /// Set the Properties of this Entity - void setProperties(const Properties& _properties); - - /// Get the Properties of this Entity - const Properties& getEntityProperties() const; - - /// Copy the Properties of another Entity - void copy(const Entity& _otherEntity); - - /// Copy the Properties of another Entity - void copy(const Entity* _otherEntity); - - /// Same as copy(const Entity&) - Entity& operator=(const Entity& _otherEntity); - /// Set name. Some implementations of Entity may make alterations to the name /// that gets passed in. The final name that this entity will use gets passed /// back in the return of this function. - virtual const std::string& setName(const std::string& _name); + virtual const std::string& setName(const std::string& _name) = 0; /// Return the name of this Entity - virtual const std::string& getName() const; + virtual const std::string& getName() const = 0; /// Render this Entity virtual void draw(renderer::RenderInterface* ri = nullptr, @@ -195,9 +167,6 @@ class Entity : public virtual common::Subject protected: - /// Properties of this Entity - Properties mEntityP; - /// Parent frame of this Entity Frame* mParentFrame; diff --git a/dart/dynamics/FixedFrame.cpp b/dart/dynamics/FixedFrame.cpp index 31733c2c4f8fa..b5a0b51415c58 100644 --- a/dart/dynamics/FixedFrame.cpp +++ b/dart/dynamics/FixedFrame.cpp @@ -43,10 +43,10 @@ namespace dynamics { const Eigen::Vector6d FixedFrame::mZero = Eigen::Vector6d::Zero(); //============================================================================== -FixedFrame::FixedFrame(Frame* _refFrame, const std::string& _name, +FixedFrame::FixedFrame(Frame* _refFrame, const Eigen::Isometry3d& _relativeTransform) - : Entity(_refFrame, _name, false), - Frame(_refFrame, _name), + : Entity(_refFrame, false), + Frame(_refFrame), mRelativeTf(_relativeTransform) { // Do nothing diff --git a/dart/dynamics/FixedFrame.h b/dart/dynamics/FixedFrame.h index a9a10e00320ac..fda83e58dcd35 100644 --- a/dart/dynamics/FixedFrame.h +++ b/dart/dynamics/FixedFrame.h @@ -50,9 +50,10 @@ class FixedFrame : public virtual Frame { public: /// Constructor - explicit FixedFrame(Frame* _refFrame, const std::string& _name, - const Eigen::Isometry3d& _relativeTransform = - Eigen::Isometry3d::Identity()); + explicit FixedFrame( + Frame* _refFrame, + const Eigen::Isometry3d& _relativeTransform = + Eigen::Isometry3d::Identity()); /// Destructor virtual ~FixedFrame(); @@ -74,6 +75,7 @@ class FixedFrame : public virtual Frame protected: /// Relative Transform of this Frame + DEPRECATED(6.0) Eigen::Isometry3d mRelativeTf; /// Used for Relative Velocity and Relative Acceleration of this Frame diff --git a/dart/dynamics/Frame.cpp b/dart/dynamics/Frame.cpp index 7358968e73f33..3ae9910f302af 100644 --- a/dart/dynamics/Frame.cpp +++ b/dart/dynamics/Frame.cpp @@ -519,7 +519,7 @@ void Frame::notifyAccelerationUpdate() } //============================================================================== -Frame::Frame(Frame* _refFrame, const std::string& _name) +Frame::Frame(Frame* _refFrame) : Entity(ConstructFrame), mWorldTransform(Eigen::Isometry3d::Identity()), mVelocity(Eigen::Vector6d::Zero()), @@ -528,7 +528,6 @@ Frame::Frame(Frame* _refFrame, const std::string& _name) mAmShapeFrame(false) { mAmFrame = true; - mEntityP.mName = _name; changeParentFrame(_refFrame); } @@ -606,7 +605,7 @@ void Frame::processRemovedEntity(Entity*) //============================================================================== Frame::Frame(ConstructWorld_t) - : Entity(this, "World", true), + : Entity(this, true), mWorldTransform(Eigen::Isometry3d::Identity()), mVelocity(Eigen::Vector6d::Zero()), mAcceleration(Eigen::Vector6d::Zero()), @@ -649,9 +648,23 @@ const Eigen::Vector6d& WorldFrame::getPartialAcceleration() const return mZero; } +//============================================================================== +const std::string& WorldFrame::setName(const std::string& name) +{ + dterr << "[WorldFrame::setName] attempting to change name of World frame to [" + << name << "], but this is not allowed!\n"; +} + +//============================================================================== +const std::string& WorldFrame::getName() const +{ + static const std::string worldName = "World"; + return worldName; +} + //============================================================================== WorldFrame::WorldFrame() - : Entity(nullptr, "World", true), + : Entity(nullptr, true), Frame(ConstructWorld), mRelativeTf(Eigen::Isometry3d::Identity()) { diff --git a/dart/dynamics/Frame.h b/dart/dynamics/Frame.h index 58e500249a5d8..1a8756597941d 100644 --- a/dart/dynamics/Frame.h +++ b/dart/dynamics/Frame.h @@ -261,7 +261,7 @@ class Frame : public virtual Entity enum ConstructAbstract_t { ConstructAbstract }; /// Constructor for typical usage - explicit Frame(Frame* _refFrame, const std::string& _name); + explicit Frame(Frame* _refFrame); /// Default constructor, delegates to Frame(ConstructAbstract_t) Frame(); @@ -334,19 +334,23 @@ class WorldFrame : public Frame friend class Frame; /// Always returns the Identity Transform - const Eigen::Isometry3d& getRelativeTransform() const override; + const Eigen::Isometry3d& getRelativeTransform() const override final; /// Always returns a zero vector - const Eigen::Vector6d& getRelativeSpatialVelocity() const override; + const Eigen::Vector6d& getRelativeSpatialVelocity() const override final; /// Always returns a zero vector - const Eigen::Vector6d& getRelativeSpatialAcceleration() const override; + const Eigen::Vector6d& getRelativeSpatialAcceleration() const override final; /// Always returns a zero vector - const Eigen::Vector6d& getPrimaryRelativeAcceleration() const override; + const Eigen::Vector6d& getPrimaryRelativeAcceleration() const override final; /// Always returns a zero vector - const Eigen::Vector6d& getPartialAcceleration() const override; + const Eigen::Vector6d& getPartialAcceleration() const override final; + + const std::string& setName(const std::string& name) override final; + + const std::string& getName() const override final; private: /// This may only be constructed by the Frame class diff --git a/dart/dynamics/Inertia.cpp b/dart/dynamics/Inertia.cpp index 829026af0d34b..f6210a8c3c6a8 100644 --- a/dart/dynamics/Inertia.cpp +++ b/dart/dynamics/Inertia.cpp @@ -383,6 +383,12 @@ bool Inertia::verify(bool _printWarnings, double _tolerance) const return verifySpatialTensor(getSpatialTensor(), _printWarnings, _tolerance); } +//============================================================================== +bool Inertia::operator ==(const Inertia& other) const +{ + return (other.mSpatialTensor == mSpatialTensor); +} + //============================================================================== // Note: Taken from Springer Handbook, chapter 2.2.11 void Inertia::computeSpatialTensor() diff --git a/dart/dynamics/Inertia.h b/dart/dynamics/Inertia.h index e075f95ca95e3..32987e47550ef 100644 --- a/dart/dynamics/Inertia.h +++ b/dart/dynamics/Inertia.h @@ -123,6 +123,9 @@ class Inertia bool verify(bool _printWarnings = true, double _tolerance = 1e-8) const; + /// Check for equality + bool operator==(const Inertia& other) const; + protected: /// Compute the spatial tensor based on the inertial parameters diff --git a/dart/dynamics/JacobianNode.cpp b/dart/dynamics/JacobianNode.cpp index 80c907661d017..8be5a1eccb368 100644 --- a/dart/dynamics/JacobianNode.cpp +++ b/dart/dynamics/JacobianNode.cpp @@ -82,12 +82,6 @@ void JacobianNode::clearIK() mIK = nullptr; } -//============================================================================== -const std::string& JacobianNode::getName() const -{ - return mEntityP.mName; -} - //============================================================================== JacobianNode::JacobianNode(BodyNode* bn) : Entity(Entity::ConstructAbstract), diff --git a/dart/dynamics/JacobianNode.h b/dart/dynamics/JacobianNode.h index 15af5fe211b94..d238362ddfcc0 100644 --- a/dart/dynamics/JacobianNode.h +++ b/dart/dynamics/JacobianNode.h @@ -61,6 +61,9 @@ class JacobianNode : public virtual Frame, public Node /// Virtual destructor virtual ~JacobianNode(); + using Node::setName; + using Node::getName; + /// Get a pointer to an IK module for this JacobianNode. If _createIfNull is /// true, then the IK module will be generated if one does not already exist. const std::shared_ptr& getIK(bool _createIfNull = false); @@ -87,12 +90,6 @@ class JacobianNode : public virtual Frame, public Node /// \{ \name Structural Properties //---------------------------------------------------------------------------- - // Documentation inherited - const std::string& setName(const std::string& _name) override = 0; - - // Documentation inherited - const std::string& getName() const override final; - /// Return true if _genCoordIndex-th generalized coordinate virtual bool dependsOn(size_t _genCoordIndex) const = 0; diff --git a/dart/dynamics/Node.h b/dart/dynamics/Node.h index 582545d151cb3..dfbf4ae4fe871 100644 --- a/dart/dynamics/Node.h +++ b/dart/dynamics/Node.h @@ -40,7 +40,7 @@ #include #include "dart/common/Subject.h" -#include "dart/common/Extensible.h" +#include "dart/common/Cloneable.h" #include "dart/common/VersionCounter.h" #include "dart/dynamics/SmartPointer.h" @@ -103,12 +103,12 @@ class Node : /// get stored in BodyNode::ExtendedProperties. Typically Properties are /// values that only change rarely if ever, whereas State contains values that /// might change as often as every time step. - class State : public common::Extensible { }; + class State : public common::Cloneable { }; - /// Use the StateMixer class to easily create a State extension from an + /// Use the MakeState class to easily create a State extension from an /// existing class or struct template - using StateMixer = common::ExtensibleMixer; + using MakeState = common::MakeCloneable; /// If your Node has a Properties class, then that Properties class should /// inherit this Node::Properties class. This allows us to safely serialize, @@ -122,12 +122,12 @@ class Node : /// get stored in BodyNode::ExtendedProperties. Typically Properties are /// values that only change rarely if ever, whereas State contains values that /// might change as often as every time step. - class Properties : public common::Extensible { }; + class Properties : public common::Cloneable { }; - /// Use the PropertiesMixer class to easily create a Properties extension + /// Use the MakeProperties class to easily create a Properties extension /// from an existing class or struct. template - using PropertiesMixer = common::ExtensibleMixer; + using MakeProperties = common::MakeCloneable; /// Virtual destructor virtual ~Node() = default; diff --git a/dart/dynamics/PointMass.cpp b/dart/dynamics/PointMass.cpp index d71a2988af577..92d452e80c67a 100644 --- a/dart/dynamics/PointMass.cpp +++ b/dart/dynamics/PointMass.cpp @@ -1135,7 +1135,7 @@ void PointMass::draw(renderer::RenderInterface* _ri, //============================================================================== PointMassNotifier::PointMassNotifier(SoftBodyNode* _parentSoftBody, const std::string& _name) - : Entity(_parentSoftBody, _name, false), + : Entity(_parentSoftBody, false), mNeedPartialAccelerationUpdate(true), mParentSoftBodyNode(_parentSoftBody) { @@ -1200,5 +1200,26 @@ void PointMassNotifier::notifyAccelerationUpdate() mNeedAccelerationUpdate = true; } +//============================================================================== +const std::string& PointMassNotifier::setName(const std::string& _name) +{ + if(_name == mName) + return mName; + + const std::string oldName = mName; + + mName = _name; + + Entity::mNameChangedSignal.raise(this, oldName, mName); + + return mName; +} + +//============================================================================== +const std::string& PointMassNotifier::getName() const +{ + return mName; +} + } // namespace dynamics } // namespace dart diff --git a/dart/dynamics/PointMass.h b/dart/dynamics/PointMass.h index fe6dde7a551b1..0b1f85168f5c5 100644 --- a/dart/dynamics/PointMass.h +++ b/dart/dynamics/PointMass.h @@ -698,12 +698,20 @@ class PointMassNotifier : public Entity void clearPartialAccelerationNotice(); void clearAccelerationNotice(); - void notifyTransformUpdate(); - void notifyVelocityUpdate(); - void notifyAccelerationUpdate(); + void notifyTransformUpdate() override; + void notifyVelocityUpdate() override; + void notifyAccelerationUpdate() override; + + // Documentation inherited + const std::string& setName(const std::string& _name) override; + + // Documentation inherited + const std::string& getName() const override; protected: + std::string mName; + bool mNeedPartialAccelerationUpdate; SoftBodyNode* mParentSoftBodyNode; diff --git a/dart/dynamics/ShapeFrame.cpp b/dart/dynamics/ShapeFrame.cpp index c84c286027725..0f2d82102a74e 100644 --- a/dart/dynamics/ShapeFrame.cpp +++ b/dart/dynamics/ShapeFrame.cpp @@ -198,85 +198,47 @@ ShapeFrame::UniqueProperties::UniqueProperties(ShapePtr&& shape) // Do nothing } -//============================================================================== -ShapeFrame::Properties::Properties( - const Entity::Properties& entityProperties, - const UniqueProperties& shapeFrameProperties, - const ShapeFrame::AspectProperties& aspectProperties) - : Entity::Properties(entityProperties), - UniqueProperties(shapeFrameProperties), - mAspectProperties(aspectProperties) -{ - // Do nothing -} - -//============================================================================== -void ShapeFrame::setProperties(const ShapeFrame::AspectProperties& properties) -{ - setCompositeProperties(properties); -} - -//============================================================================== -void ShapeFrame::setProperties(const Properties& properties) -{ - Entity::setProperties(static_cast(properties)); - setProperties(static_cast(properties)); - setProperties(properties.mAspectProperties); -} - //============================================================================== void ShapeFrame::setProperties(const ShapeFrame::UniqueProperties& properties) { - setShape(properties.mShape); -} - -//============================================================================== -const ShapeFrame::Properties ShapeFrame::getShapeFrameProperties() const -{ - return Properties(getEntityProperties(), mShapeFrameP, getCompositeProperties()); + setAspectProperties(properties); } //============================================================================== -void ShapeFrame::copy(const ShapeFrame& other) +void ShapeFrame::setAspectProperties(const AspectProperties& properties) { - if (this == &other) - return; - - setProperties(other.getShapeFrameProperties()); + setShape(properties.mShape); } //============================================================================== -void ShapeFrame::copy(const ShapeFrame* other) +const ShapeFrame::AspectProperties& ShapeFrame::getAspectProperties() const { - if (nullptr == other) - return; - - copy(*other); + return mAspectProperties; } //============================================================================== void ShapeFrame::setShape(const ShapePtr& shape) { - if (shape == mShapeFrameP.mShape) + if (shape == ShapeFrame::mAspectProperties.mShape) return; - ShapePtr oldShape = mShapeFrameP.mShape; + ShapePtr oldShape = ShapeFrame::mAspectProperties.mShape; - mShapeFrameP.mShape = shape; + ShapeFrame::mAspectProperties.mShape = shape; - mShapeUpdatedSignal.raise(this, oldShape, mShapeFrameP.mShape); + mShapeUpdatedSignal.raise(this, oldShape, ShapeFrame::mAspectProperties.mShape); } //============================================================================== ShapePtr ShapeFrame::getShape() { - return mShapeFrameP.mShape; + return ShapeFrame::mAspectProperties.mShape; } //============================================================================== ConstShapePtr ShapeFrame::getShape() const { - return mShapeFrameP.mShape; + return ShapeFrame::mAspectProperties.mShape; } //============================================================================== @@ -293,9 +255,9 @@ void ShapeFrame::draw(renderer::RenderInterface* ri, ri->transform(getRelativeTransform()); if (useDefaultColor) - mShapeFrameP.mShape->draw(ri, visualAspect->getRGBA()); + ShapeFrame::mAspectProperties.mShape->draw(ri, visualAspect->getRGBA()); else - mShapeFrameP.mShape->draw(ri, color); + ShapeFrame::mAspectProperties.mShape->draw(ri, color); ri->popMatrix(); } @@ -303,20 +265,20 @@ void ShapeFrame::draw(renderer::RenderInterface* ri, //============================================================================== size_t ShapeFrame::incrementVersion() { - return ++mShapeFrameP.mVersion; + return ++ShapeFrame::mAspectProperties.mVersion; } //============================================================================== size_t ShapeFrame::getVersion() const { - return mShapeFrameP.mVersion; + return ShapeFrame::mAspectProperties.mVersion; } //============================================================================== ShapeFrame::ShapeFrame(Frame* parent, const Properties& properties) : common::Composite(), Entity(ConstructFrame), - Frame(parent, ""), + Frame(parent), mShapeUpdatedSignal(), mRelativeTransformUpdatedSignal(), onShapeUpdated(mShapeUpdatedSignal), @@ -328,22 +290,16 @@ ShapeFrame::ShapeFrame(Frame* parent, const Properties& properties) //============================================================================== ShapeFrame::ShapeFrame(Frame* parent, - const std::string& name, const ShapePtr& shape) : common::Composite(), Entity(ConstructFrame), - Frame(parent, name), + Frame(parent), mShapeUpdatedSignal(), mRelativeTransformUpdatedSignal(), onShapeUpdated(mShapeUpdatedSignal), onRelativeTransformUpdated(mRelativeTransformUpdatedSignal) { mAmShapeFrame = true; - setName(name); - // TODO: As setName() is a virtual function, it is not safe to call this - // function in a constructor especially of abstract class. This line should be - // removed once Entity class is changed to abstract interface class. - setShape(shape); } diff --git a/dart/dynamics/ShapeFrame.h b/dart/dynamics/ShapeFrame.h index 1287b04a88809..b0ffd201b24e1 100644 --- a/dart/dynamics/ShapeFrame.h +++ b/dart/dynamics/ShapeFrame.h @@ -232,8 +232,6 @@ class ShapeFrame : const Eigen::Isometry3d& oldTransform, const Eigen::Isometry3d& newTransform)>; - using AspectProperties = common::Composite::Properties; - struct UniqueProperties { /// Shape pointer @@ -250,43 +248,19 @@ class ShapeFrame : virtual ~UniqueProperties() = default; }; - /// Composition of Entity and ShapeFrame properties - struct Properties : Entity::Properties, UniqueProperties - { - /// Composed constructor - Properties(const Entity::Properties& entityProperties - = Entity::Properties("ShapeFrame"), - const UniqueProperties& shapeFrameProperties - = UniqueProperties(), - const AspectProperties& aspectProperties - = AspectProperties()); - - virtual ~Properties() = default; - - /// Properties of all the Aspects attached to this ShapeFrame - AspectProperties mAspectProperties; - }; + using AspectProperties = UniqueProperties; + using Properties = UniqueProperties; /// Destructor virtual ~ShapeFrame() = default; - /// Same as setAspectProperties() - void setProperties(const AspectProperties& properties); - - /// Set the Properties of this ShapeFrame - void setProperties(const Properties& properties); - /// Set the UniqueProperties of this ShapeFrame void setProperties(const UniqueProperties& properties); - /// Get the Properties of this ShapeFrame - const Properties getShapeFrameProperties() const; - - /// Copy the properties of another ShapeFrame - void copy(const ShapeFrame& other); + /// Set the AspectProperties of this ShapeFrame + void setAspectProperties(const AspectProperties& properties); - /// Copy the properties of another ShapeFrame - void copy(const ShapeFrame* other); + const AspectProperties& getAspectProperties() const; /// Set shape void setShape(const ShapePtr& shape); @@ -317,15 +291,13 @@ class ShapeFrame : protected: /// Constructor - ShapeFrame(Frame* parent, const Properties& properties = Properties()); + ShapeFrame(Frame* parent, const Properties& properties); /// Constructor - ShapeFrame(Frame* parent, - const std::string& name, - const ShapePtr& shape = nullptr); + ShapeFrame(Frame* parent, const ShapePtr& shape = nullptr); /// ShapeFrame properties - Properties mShapeFrameP; + Properties mAspectProperties; /// Shape updated signal ShapeUpdatedSignal mShapeUpdatedSignal; diff --git a/dart/dynamics/ShapeNode.cpp b/dart/dynamics/ShapeNode.cpp index 21ee361852950..e2e6274f7a212 100644 --- a/dart/dynamics/ShapeNode.cpp +++ b/dart/dynamics/ShapeNode.cpp @@ -42,8 +42,10 @@ namespace dynamics { //============================================================================== ShapeNode::UniqueProperties::UniqueProperties( + const std::string& name, const Eigen::Isometry3d& relativeTransform) - : mRelativeTransform(relativeTransform) + : mName(name), + mRelativeTransform(relativeTransform) { // Do nothing } @@ -52,10 +54,10 @@ ShapeNode::UniqueProperties::UniqueProperties( ShapeNode::Properties::Properties( const ShapeFrame::Properties& shapeFrameProperties, const ShapeNode::UniqueProperties& shapeNodeProperties, - const ShapeNode::AspectProperties& aspectProperties) + const CompositeProperties& compositeProperties) : ShapeFrame::Properties(shapeFrameProperties), ShapeNode::UniqueProperties(shapeNodeProperties), - mAspectProperties(aspectProperties) + mCompositeProperties(compositeProperties) { // Do nothing } @@ -64,10 +66,10 @@ ShapeNode::Properties::Properties( ShapeNode::Properties::Properties( ShapeFrame::Properties&& shapeFrameProperties, ShapeNode::UniqueProperties&& shapeNodeProperties, - ShapeNode::AspectProperties&& aspectProperties) + CompositeProperties&& compositeProperties) : ShapeFrame::Properties(std::move(shapeFrameProperties)), ShapeNode::UniqueProperties(std::move(shapeNodeProperties)), - mAspectProperties(std::move(aspectProperties)) + mCompositeProperties(std::move(compositeProperties)) { // Do nothing } @@ -78,19 +80,22 @@ void ShapeNode::setProperties(const Properties& properties) ShapeFrame::setProperties( static_cast(properties)); setProperties(static_cast(properties)); - setCompositeProperties(properties.mAspectProperties); + setCompositeProperties(properties.mCompositeProperties); } //============================================================================== void ShapeNode::setProperties(const ShapeNode::UniqueProperties& properties) { + if(!properties.mName.empty()) + setName(properties.mName); + setRelativeTransform(properties.mRelativeTransform); } //============================================================================== const ShapeNode::Properties ShapeNode::getShapeNodeProperties() const { - return Properties(getShapeFrameProperties(), mShapeNodeP, + return Properties(getAspectProperties(), mShapeNodeP, getCompositeProperties()); } @@ -123,34 +128,48 @@ ShapeNode& ShapeNode::operator=(const ShapeNode& other) const std::string& ShapeNode::setName(const std::string& name) { // If it already has the requested name, do nothing - if(mEntityP.mName == name && !name.empty()) - return mEntityP.mName; + if(mShapeNodeP.mName == name && !name.empty()) + return mShapeNodeP.mName; + + const std::string oldName = mShapeNodeP.mName; + + mShapeNodeP.mName = registerNameChange(name); - mEntityP.mName = registerNameChange(name); + incrementVersion(); + Entity::mNameChangedSignal.raise(this, oldName, mShapeNodeP.mName); // Return the resulting name, after it has been checked for uniqueness - return mEntityP.mName; + return mShapeNodeP.mName; +} + +//============================================================================== +const std::string& ShapeNode::getName() const +{ + return mShapeNodeP.mName; } //============================================================================== size_t ShapeNode::incrementVersion() { - ++mShapeFrameP.mVersion; + ++ShapeFrame::mAspectProperties.mVersion; if(const SkeletonPtr& skel = getSkeleton()) skel->incrementVersion(); - return mShapeFrameP.mVersion; + return ShapeFrame::mAspectProperties.mVersion; } //============================================================================== size_t ShapeNode::getVersion() const { - return mShapeFrameP.mVersion; + return ShapeFrame::mAspectProperties.mVersion; } //============================================================================== void ShapeNode::setRelativeTransform(const Eigen::Isometry3d& transform) { + if(transform.matrix() == mRelativeTf.matrix()) + return; + const Eigen::Isometry3d oldTransform = mRelativeTf; mRelativeTf = transform; @@ -159,6 +178,7 @@ void ShapeNode::setRelativeTransform(const Eigen::Isometry3d& transform) notifyJacobianUpdate(); notifyJacobianDerivUpdate(); + incrementVersion(); mRelativeTransformUpdatedSignal.raise(this, oldTransform, mShapeNodeP.mRelativeTransform); } @@ -316,8 +336,8 @@ const math::Jacobian& ShapeNode::getJacobianClassicDeriv() const //============================================================================== ShapeNode::ShapeNode(BodyNode* bodyNode, const Properties& properties) : Entity(ConstructFrame), - Frame(bodyNode, ""), - FixedFrame(bodyNode, ""), + Frame(bodyNode), + FixedFrame(bodyNode), ShapeFrame(bodyNode), TemplatedJacobianNode(bodyNode), mShapeUpdatedSignal(ShapeUpdatedSignal()), @@ -333,8 +353,8 @@ ShapeNode::ShapeNode(BodyNode* bodyNode, const ShapePtr& shape, const std::string& name) : Entity(ConstructFrame), - Frame(bodyNode, ""), - FixedFrame(bodyNode, ""), + Frame(bodyNode), + FixedFrame(bodyNode), ShapeFrame(bodyNode), TemplatedJacobianNode(bodyNode), mShapeUpdatedSignal(ShapeUpdatedSignal()), diff --git a/dart/dynamics/ShapeNode.h b/dart/dynamics/ShapeNode.h index 0e49a0c948a5f..463dc4013beb9 100644 --- a/dart/dynamics/ShapeNode.h +++ b/dart/dynamics/ShapeNode.h @@ -72,10 +72,13 @@ class ShapeNode : public virtual FixedFrame, const Eigen::Isometry3d& oldTransform, const Eigen::Isometry3d& newTransform)>; - using AspectProperties = common::Composite::Properties; + using CompositeProperties = common::Composite::Properties; struct UniqueProperties { + /// Name of ShapeNode + std::string mName; + /// The current relative transform of the Shape in the ShapeNode Eigen::Isometry3d mRelativeTransform; // TODO(JS): Consider moving this to a FixedFrame::State @@ -84,6 +87,7 @@ class ShapeNode : public virtual FixedFrame, /// Composed constructor UniqueProperties( + const std::string& name = "", const Eigen::Isometry3d& relativeTransform = Eigen::Isometry3d::Identity()); }; @@ -96,16 +100,16 @@ class ShapeNode : public virtual FixedFrame, = ShapeFrame::Properties(), const ShapeNode::UniqueProperties& shapeNodeProperties = ShapeNode::UniqueProperties(), - const AspectProperties& aspectProperties = AspectProperties()); + const CompositeProperties& compositeProperties = CompositeProperties()); /// Composed move constructor Properties( ShapeFrame::Properties&& shapeFrameProperties, ShapeNode::UniqueProperties&& shapeNodeProperties, - AspectProperties&& aspectProperties); + CompositeProperties&& compositeProperties); /// The properties of the ShapeNode's Aspects - AspectProperties mAspectProperties; + CompositeProperties mCompositeProperties; }; /// Destructor @@ -133,6 +137,9 @@ class ShapeNode : public virtual FixedFrame, /// version which will be used by the Skeleton const std::string& setName(const std::string& _name) override; + // Documentation inherited + const std::string& getName() const override; + // Documentation inherited size_t incrementVersion() override; diff --git a/dart/dynamics/SimpleFrame.cpp b/dart/dynamics/SimpleFrame.cpp index 1b6ef98a1740a..3dd6a00076652 100644 --- a/dart/dynamics/SimpleFrame.cpp +++ b/dart/dynamics/SimpleFrame.cpp @@ -45,24 +45,24 @@ namespace dynamics { SimpleFrame::SimpleFrame(Frame* _refFrame, const std::string& _name, const Eigen::Isometry3d& _relativeTransform) : Entity(ConstructFrame), - Frame(_refFrame, _name), + Frame(_refFrame), Detachable(), - ShapeFrame(_refFrame, _name), + ShapeFrame(_refFrame), mRelativeTf(_relativeTransform), mRelativeVelocity(Eigen::Vector6d::Zero()), mRelativeAcceleration(Eigen::Vector6d::Zero()), mPartialAcceleration(Eigen::Vector6d::Zero()) { - // Do nothing + setName(_name); } //============================================================================== SimpleFrame::SimpleFrame(const SimpleFrame& _otherFrame, Frame* _refFrame) : Entity(ConstructFrame), common::Composite(), - Frame(_refFrame, ""), + Frame(_refFrame), Detachable(), - ShapeFrame(_refFrame, ""), + ShapeFrame(_refFrame), mRelativeTf(Eigen::Isometry3d::Identity()), mRelativeVelocity(Eigen::Vector6d::Zero()), mRelativeAcceleration(Eigen::Vector6d::Zero()), @@ -78,6 +78,27 @@ SimpleFrame::~SimpleFrame() // Do nothing } +//============================================================================== +const std::string& SimpleFrame::setName(const std::string& _name) +{ + if(_name == mName) + return mName; + + std::string oldName = mName; + mName = _name; + + incrementVersion(); + Entity::mNameChangedSignal.raise(this, oldName, mName); + + return mName; +} + +//============================================================================== +const std::string& SimpleFrame::getName() const +{ + return mName; +} + //============================================================================== SimpleFramePtr SimpleFrame::clone(Frame* _refFrame) const { @@ -116,7 +137,7 @@ void SimpleFrame::copy(const Frame* _otherFrame, Frame* _refFrame, { const auto shapeFrame = dynamic_cast(_otherFrame); if(shapeFrame) - setProperties(shapeFrame->getShapeFrameProperties()); + setCompositeProperties(shapeFrame->getCompositeProperties()); const auto simpleFrame = dynamic_cast(_otherFrame); if(simpleFrame) diff --git a/dart/dynamics/SimpleFrame.h b/dart/dynamics/SimpleFrame.h index c2d56e46b73c5..3d8b5e6a6365f 100644 --- a/dart/dynamics/SimpleFrame.h +++ b/dart/dynamics/SimpleFrame.h @@ -68,6 +68,12 @@ class SimpleFrame : public Detachable, public ShapeFrame /// Destructor virtual ~SimpleFrame(); + // Documentation inherited + const std::string& setName(const std::string& _name) override; + + // Documentation inherited + const std::string& getName() const override; + /// Create a new SimpleFrame with the same world transform, velocity, and /// acceleration as this one. _refFrame will be used as the reference Frame /// of the new SimpleFrame. @@ -111,7 +117,7 @@ class SimpleFrame : public Detachable, public ShapeFrame const Frame* _withRespectTo = Frame::World()); // Documentation inherited - const Eigen::Isometry3d& getRelativeTransform() const; + const Eigen::Isometry3d& getRelativeTransform() const override; //-------------------------------------------------------------------------- // Velocity @@ -136,7 +142,7 @@ class SimpleFrame : public Detachable, public ShapeFrame const Frame* _inCoordinatesOf); // Documentation inherited - virtual const Eigen::Vector6d& getRelativeSpatialVelocity() const; + virtual const Eigen::Vector6d& getRelativeSpatialVelocity() const override; //-------------------------------------------------------------------------- // Acceleration @@ -157,13 +163,13 @@ class SimpleFrame : public Detachable, public ShapeFrame const Frame* _inCoordinatesOf); // Documentation inherited - virtual const Eigen::Vector6d& getRelativeSpatialAcceleration() const; + virtual const Eigen::Vector6d& getRelativeSpatialAcceleration() const override; // Documentation inherited - virtual const Eigen::Vector6d& getPrimaryRelativeAcceleration() const; + virtual const Eigen::Vector6d& getPrimaryRelativeAcceleration() const override; // Documentation inherited - virtual const Eigen::Vector6d& getPartialAcceleration() const; + virtual const Eigen::Vector6d& getPartialAcceleration() const override; //-------------------------------------------------------------------------- // Classic Method @@ -190,6 +196,9 @@ class SimpleFrame : public Detachable, public ShapeFrame protected: + /// Name of this SimpleFrame + std::string mName; + /// Relative transform of the SimpleFrame Eigen::Isometry3d mRelativeTf; diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index 0f4674d3704c9..fa2ac55abbb48 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -434,10 +434,10 @@ const std::string& Skeleton::getName() const //============================================================================== const std::string& Skeleton::addEntryToBodyNodeNameMgr(BodyNode* _newNode) { - _newNode->mEntityP.mName = + _newNode->BodyNode::mAspectProperties.mName = mNameMgrForBodyNodes.issueNewNameAndAdd(_newNode->getName(), _newNode); - return _newNode->mEntityP.mName; + return _newNode->BodyNode::mAspectProperties.mName; } //============================================================================== diff --git a/dart/dynamics/SoftBodyNode.cpp b/dart/dynamics/SoftBodyNode.cpp index 6bdaabb151b8a..d6ad2d7c76cb0 100644 --- a/dart/dynamics/SoftBodyNode.cpp +++ b/dart/dynamics/SoftBodyNode.cpp @@ -231,14 +231,14 @@ const PointMass* SoftBodyNode::getPointMass(size_t _idx) const SoftBodyNode::SoftBodyNode(BodyNode* _parentBodyNode, Joint* _parentJoint, const Properties& _properties) - : Entity(Frame::World(), "", false), - Frame(Frame::World(), ""), + : Entity(Frame::World(), false), + Frame(Frame::World()), Base(common::NoArg, std::make_tuple(_parentBodyNode, _parentJoint, _properties)), mSoftShapeNode(nullptr) { createSoftBodyAspect(); - mNotifier = new PointMassNotifier(this, "PointMassNotifier"); + mNotifier = new PointMassNotifier(this, getName()+"_PointMassNotifier"); ShapeNode* softNode = createShapeNodeWith< VisualAspect, CollisionAspect, DynamicsAspect>( std::make_shared(this), getName()+"_SoftMeshShape"); diff --git a/dart/dynamics/detail/BodyNodeProperties.h b/dart/dynamics/detail/BodyNodeProperties.h index dc8cede831c72..e3b41905609c7 100644 --- a/dart/dynamics/detail/BodyNodeProperties.h +++ b/dart/dynamics/detail/BodyNodeProperties.h @@ -66,6 +66,9 @@ struct BodyNodeState //============================================================================== struct BodyNodeUniqueProperties { + /// Name of the Entity + std::string mName; + /// Inertia information for the BodyNode Inertia mInertia; @@ -86,6 +89,7 @@ struct BodyNodeUniqueProperties /// Constructor BodyNodeUniqueProperties( + const std::string& name = "BodyNode", const Inertia& _inertia = Inertia(), bool _isCollidable = true, double _frictionCoeff = DART_DEFAULT_FRICTION_COEFF, @@ -98,32 +102,20 @@ struct BodyNodeUniqueProperties EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -//============================================================================== -struct BodyNodeProperties : Entity::Properties, BodyNodeUniqueProperties -{ - /// Composed constructor - BodyNodeProperties( - const Entity::Properties& _entityProperties = - Entity::Properties("BodyNode"), - const BodyNodeUniqueProperties& _bodyNodeProperties = - BodyNodeUniqueProperties()); - - virtual ~BodyNodeProperties() = default; -}; +using BodyNodeProperties = BodyNodeUniqueProperties; //============================================================================== struct BodyNodeExtendedProperties : BodyNodeProperties { - using NodePropertiesVector = common::ExtensibleVector< std::unique_ptr >; + using NodePropertiesVector = common::CloneableVector< std::unique_ptr >; using NodePropertiesMap = std::map< std::type_index, std::unique_ptr >; - using NodeProperties = common::ExtensibleMapHolder; + using NodeProperties = common::CloneableMap; using CompositeProperties = common::Composite::Properties; /// Composed constructor - BodyNodeExtendedProperties( - const BodyNodeProperties& standardProperties = Properties(), + BodyNodeExtendedProperties(const BodyNodeProperties& standardProperties = BodyNodeProperties(), const NodeProperties& nodeProperties = NodeProperties(), - const CompositeProperties& aspectProperties = CompositeProperties()); + const CompositeProperties& compositeProperties = CompositeProperties()); /// Composed move constructor BodyNodeExtendedProperties( diff --git a/osgDart/InteractiveFrame.cpp b/osgDart/InteractiveFrame.cpp index 3e59432c2aa88..5d049d3e804cd 100644 --- a/osgDart/InteractiveFrame.cpp +++ b/osgDart/InteractiveFrame.cpp @@ -46,7 +46,7 @@ namespace osgDart { InteractiveTool::InteractiveTool(InteractiveFrame* frame, double defaultAlpha, const std::string& name) : Entity(ConstructFrame), - Frame(frame, name), + Frame(frame), SimpleFrame(frame, name), mDefaultAlpha(defaultAlpha), mEnabled(true), @@ -158,8 +158,8 @@ InteractiveFrame::InteractiveFrame( const std::string& name, const Eigen::Isometry3d& relativeTransform, double size_scale, double thickness_scale) - : Entity(referenceFrame, name, false), - Frame(referenceFrame, name), + : Entity(referenceFrame, false), + Frame(referenceFrame), SimpleFrame(referenceFrame, name, relativeTransform) { for(size_t i=0; i<3; ++i) diff --git a/unittests/testForwardKinematics.cpp b/unittests/testForwardKinematics.cpp index 33982f03f805d..8fbc6d6eda7e8 100644 --- a/unittests/testForwardKinematics.cpp +++ b/unittests/testForwardKinematics.cpp @@ -71,8 +71,8 @@ TEST(FORWARD_KINEMATICS, YAW_ROLL) for (size_t i = 0; i < numTests; i++) { robot->setPositions(Eigen::VectorXd(joints[i])); - Vector3d actual - = robot->getBodyNode("ee")->getTransform().translation(); + BodyNode* bn = robot->getBodyNode("ee"); + Vector3d actual = bn->getTransform().translation(); bool equality = equals(actual, expectedPos[i], 1e-3); EXPECT_TRUE(equality); if(!equality) diff --git a/unittests/testNameManagement.cpp b/unittests/testNameManagement.cpp index 774acb745c0a4..825e520941703 100644 --- a/unittests/testNameManagement.cpp +++ b/unittests/testNameManagement.cpp @@ -70,6 +70,10 @@ TEST(NameManagement, Skeleton) EXPECT_FALSE(body2->getName() == body3->getName()); EXPECT_FALSE(body3->getName() == body1->getName()); + EXPECT_TRUE(body1->getName() == "BodyNode"); + EXPECT_TRUE(body2->getName() == "BodyNode(1)"); + EXPECT_TRUE(body3->getName() == "BodyNode(2)"); + EXPECT_FALSE(joint1->getName() == joint2->getName()); EXPECT_FALSE(joint2->getName() == joint3->getName()); EXPECT_FALSE(joint3->getName() == joint1->getName()); @@ -179,6 +183,13 @@ TEST(NameManagement, Skeleton) EXPECT_TRUE(skel->getBodyNode("nonexistent_name") == nullptr); EXPECT_TRUE(skel->getJoint("nonexistent_name") == nullptr); EXPECT_TRUE(skel->getSoftBodyNode("nonexistent_name") == nullptr); + + // Test Node Names + EndEffector* ee1 = body1->createEndEffector("ee"); + EndEffector* ee2 = body1->createEndEffector("ee"); + + EXPECT_TRUE(skel->getEndEffector("ee") == ee1); + EXPECT_TRUE(skel->getEndEffector("ee(1)") == ee2); } //============================================================================== @@ -186,9 +197,9 @@ TEST(NameManagement, SetPattern) { dart::common::NameManager< std::shared_ptr > test_mgr("test", "name"); - std::shared_ptr entity0(new Entity(Frame::World(), "name", false)); - std::shared_ptr entity1(new Entity(Frame::World(), "name", false)); - std::shared_ptr entity2(new Entity(Frame::World(), "name", false)); + std::shared_ptr entity0(new SimpleFrame(Frame::World(), "name")); + std::shared_ptr entity1(new SimpleFrame(Frame::World(), "name")); + std::shared_ptr entity2(new SimpleFrame(Frame::World(), "name")); test_mgr.setPattern("%s(%d)"); diff --git a/unittests/testSignal.cpp b/unittests/testSignal.cpp index 843e86e6bcadf..ed7a69b5f1499 100644 --- a/unittests/testSignal.cpp +++ b/unittests/testSignal.cpp @@ -270,9 +270,9 @@ TEST(Signal, ReturnValues) } //============================================================================== -void frameChangecCallback(const Entity* _entity, - const Frame* _oldParentFrame, - const Frame* _newParentFrame) +void frameChangeCallback(const Entity* _entity, + const Frame* _oldParentFrame, + const Frame* _newParentFrame) { assert(_entity); @@ -281,8 +281,11 @@ void frameChangecCallback(const Entity* _entity, std::string newFrameName = _newParentFrame == nullptr ? "(empty)" : _newParentFrame->getName(); - std::cout << "[" << _entity->getName() << "]: " - << oldFrameName << " --> " << newFrameName << std::endl; + if(_newParentFrame) + std::cout << "[" << _entity->getName() << "]: " + << oldFrameName << " --> " << newFrameName << std::endl; + else + std::cout << "Entity (" << _entity << ") has been destroyed" << std::endl; } //============================================================================== @@ -314,9 +317,9 @@ TEST(Signal, FrameSignals) SimpleFrame F2(&F1, "F2", tf2); SimpleFrame F3(&F2, "F3", tf3); - Connection cf1 = F1.onFrameChanged.connect(frameChangecCallback); - Connection cf2 = F2.onFrameChanged.connect(frameChangecCallback); - ScopedConnection cf3(F3.onFrameChanged.connect(frameChangecCallback)); + Connection cf1 = F1.onFrameChanged.connect(frameChangeCallback); + Connection cf2 = F2.onFrameChanged.connect(frameChangeCallback); + ScopedConnection cf3(F3.onFrameChanged.connect(frameChangeCallback)); Connection cv1 = F1.onNameChanged.connect(nameChangedCallback); Connection cv2 = F2.onNameChanged.connect(nameChangedCallback); diff --git a/unittests/testSubscriptions.cpp b/unittests/testSubscriptions.cpp index e2498c120823b..2b7f1a3123399 100644 --- a/unittests/testSubscriptions.cpp +++ b/unittests/testSubscriptions.cpp @@ -45,7 +45,7 @@ using namespace dynamics; TEST(Subjects, Notifications) { - sub_ptr entity_ptr = new Detachable(Frame::World(), "entity", false); + sub_ptr entity_ptr = new SimpleFrame(Frame::World(), "entity"); sub_ptr frame_ptr = new SimpleFrame(Frame::World(), "frame"); EXPECT_TRUE(entity_ptr.valid()); @@ -68,7 +68,7 @@ Entity* getPointer(Entity* _ptr) TEST(Subjects, ImplicitConversion) { - sub_ptr entity_ptr = new Entity(Frame::World(), "entity", false); + sub_ptr entity_ptr = new SimpleFrame(Frame::World(), "entity"); // This checks whether the sub_ptr class can successfully be implicitly // converted to the type of class it's supposed to be pointing to From 2566ff8a8d8bdc63539376d30c6d50085124406a Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Sun, 3 Apr 2016 23:47:42 -0400 Subject: [PATCH 30/62] adding proxy aspects for Nodes --- dart/common/Cloneable.h | 19 ++- dart/common/Composite.cpp | 122 +++++++------- dart/common/ProxyAspect.h | 95 +++++++++++ dart/common/StlHelpers.h | 76 +++++++++ dart/common/detail/Cloneable.h | 162 +++++++++++-------- dart/common/detail/EmbeddedAspect.h | 4 +- dart/common/detail/ProxyAspect.h | 186 ++++++++++++++++++++++ dart/dynamics/BodyNode.cpp | 75 +++++---- dart/dynamics/BodyNode.h | 26 +-- dart/dynamics/Frame.cpp | 2 + dart/dynamics/Skeleton.cpp | 87 +++++++++- dart/dynamics/Skeleton.h | 6 +- dart/dynamics/detail/BodyNodeProperties.h | 40 +++++ dart/dynamics/detail/SkeletonAspect.h | 119 ++++++++++++++ 14 files changed, 833 insertions(+), 186 deletions(-) create mode 100644 dart/common/ProxyAspect.h create mode 100644 dart/common/detail/ProxyAspect.h create mode 100644 dart/dynamics/detail/SkeletonAspect.h diff --git a/dart/common/Cloneable.h b/dart/common/Cloneable.h index 14c8fdef17680..e0082832841ee 100644 --- a/dart/common/Cloneable.h +++ b/dart/common/Cloneable.h @@ -130,25 +130,26 @@ class MakeCloneable : public Base, public Mixin }; //============================================================================== -template +template class ProxyCloneable : public Base { public: using Data = DataT; + using Owner = OwnerT; /// Default constructor. Constructs a default version of Data. ProxyCloneable(); /// Constructor that ties this instance to an Owner. - ProxyCloneable(Owner* owner); + ProxyCloneable(OwnerT* owner); /// Constructor that ties this instance to an Owner and then sets its data /// using the remaining arguments. template - ProxyCloneable(Owner* owner, Args&&... args); + ProxyCloneable(OwnerT* owner, Args&&... args); /// Templated constructor. Uses whichever Data constructor is able to match /// the arguments. @@ -188,6 +189,12 @@ class ProxyCloneable : public Base /// Get the Data of this ProxyCloneable Data get() const; + /// Get the Owner of this ProxyCloneable + OwnerT* getOwner(); + + /// Get the Owner of this ProxyCloneable + const OwnerT* getOwner() const; + // Documentation inherited std::unique_ptr clone() const override final; @@ -197,7 +204,7 @@ class ProxyCloneable : public Base protected: /// The ProxyCloneable will not store any data in mData if it has an Owner. - Owner* mOwner; + OwnerT* mOwner; /// The ProxyCloneable will hold data in this field if it does not have an /// owner diff --git a/dart/common/Composite.cpp b/dart/common/Composite.cpp index bbdd17be15d90..c800110c1fa31 100644 --- a/dart/common/Composite.cpp +++ b/dart/common/Composite.cpp @@ -44,49 +44,22 @@ namespace dart { namespace common { //============================================================================== -void Composite::setCompositeState(const State& newStates) -{ - const StateMap& stateMap = newStates.getMap(); - - AspectMap::iterator aspects = mAspectMap.begin(); - StateMap::const_iterator states = stateMap.begin(); - - while( mAspectMap.end() != aspects && stateMap.end() != states ) - { - if( aspects->first == states->first ) - { - Aspect* aspect = aspects->second.get(); - if(aspect && states->second) - aspect->setAspectState(*states->second); - - ++aspects; - ++states; - } - else if( aspects->first < states->first ) - { - ++aspects; - } - else - { - ++states; - } - } -} - -//============================================================================== -template -void extractMapData(MapType& outgoingMap, const Composite::AspectMap& aspectMap) +template >, + typename DataMap = std::map< std::type_index, std::unique_ptr > > +void extractTypeMapData(DataMap& dataMap, const ObjectMap& objectMap) { // TODO(MXG): Consider placing this function in a header so it can be utilized // by anything that needs to transfer data between maps of extensibles // This method allows us to avoid dynamic allocation (cloning) whenever possible. - for(const auto& aspect : aspectMap) + for(const auto& object : objectMap) { - if(nullptr == aspect.second) + if(nullptr == object.second) continue; - const DataType* data = (aspect.second.get()->*getData)(); + const DataType* data = (object.second.get()->*getData)(); if(data) { // Attempt to insert a nullptr to see whether this entry exists while also @@ -94,10 +67,10 @@ void extractMapData(MapType& outgoingMap, const Composite::AspectMap& aspectMap) // to search for a spot in the map once, instead of searching the map to // see if the entry already exists and then searching the map again in // order to insert the entry if it didn't already exist. - std::pair insertion = - outgoingMap.insert(typename MapType::value_type(aspect.first, nullptr)); + std::pair insertion = + dataMap.insert(typename DataMap::value_type(object.first, nullptr)); - typename MapType::iterator& it = insertion.first; + typename DataMap::iterator& it = insertion.first; bool existed = !insertion.second; if(existed) @@ -123,6 +96,46 @@ void extractMapData(MapType& outgoingMap, const Composite::AspectMap& aspectMap) } } +//============================================================================== +template >, + typename DataMap = std::map< std::type_index, std::unique_ptr > > +void setObjectDataFromMap(ObjectMap& objectMap, const DataMap& dataMap) +{ + typename ObjectMap::iterator objects = objectMap.begin(); + typename DataMap::const_iterator data = dataMap.begin(); + + while( objectMap.end() != objects && dataMap.end() != data ) + { + if( objects->first == data->first ) + { + ObjectType* object = objects->second.get(); + if(object && data->second) + (object->*setData)(*data->second); + + ++objects; + ++data; + } + else if( objects->first < data->first ) + { + ++objects; + } + else + { + ++data; + } + } +} + + +//============================================================================== +void Composite::setCompositeState(const State& newStates) +{ + setObjectDataFromMap( + mAspectMap, newStates.getMap()); +} + //============================================================================== Composite::State Composite::getCompositeState() const { @@ -136,37 +149,16 @@ Composite::State Composite::getCompositeState() const void Composite::copyCompositeStateTo(State& outgoingStates) const { StateMap& states = outgoingStates.getMap(); - extractMapData(states, mAspectMap); + extractTypeMapData( + states, mAspectMap); } //============================================================================== void Composite::setCompositeProperties(const Properties& newProperties) { - const PropertiesMap& propertiesMap = newProperties.getMap(); - - AspectMap::iterator aspects = mAspectMap.begin(); - PropertiesMap::const_iterator props = propertiesMap.begin(); - - while( mAspectMap.end() != aspects && propertiesMap.end() != props ) - { - if( aspects->first == props->first ) - { - Aspect* aspect = aspects->second.get(); - if(aspect) - aspect->setAspectProperties(*props->second); - - ++aspects; - ++props; - } - else if( aspects->first < props->first ) - { - ++aspects; - } - else - { - ++props; - } - } + setObjectDataFromMap< + Aspect, Aspect::Properties, &Aspect::setAspectProperties>( + mAspectMap, newProperties.getMap()); } //============================================================================== @@ -183,7 +175,7 @@ void Composite::copyCompositePropertiesTo( Properties& outgoingProperties) const { PropertiesMap& properties = outgoingProperties.getMap(); - extractMapData( + extractTypeMapData( properties, mAspectMap); } diff --git a/dart/common/ProxyAspect.h b/dart/common/ProxyAspect.h new file mode 100644 index 0000000000000..3909fc8de0482 --- /dev/null +++ b/dart/common/ProxyAspect.h @@ -0,0 +1,95 @@ +/* + * 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_COMMON_PROXYASPECT_H_ +#define DART_COMMON_PROXYASPECT_H_ + +#include "dart/common/detail/ProxyAspect.h" + +namespace dart { +namespace common { + +//============================================================================== +template +using ProxyStateAspect = detail::ProxyStateAspect< + common::CompositeTrackingAspect, CompositeT, StateT>; + +//============================================================================== +template +using ProxyPropertiesAspect = detail::ProxyPropertiesAspect< + common::CompositeTrackingAspect, CompositeT, PropertiesT>; + +//============================================================================== +template +class ProxyStateAndPropertiesAspect : + public detail::ProxyPropertiesAspect< + ProxyStateAspect, + CompositeT, PropertiesT> +{ +public: + + using State = StateT; + using Properties = PropertiesT; + using CompositeType = CompositeT; + + using AspectStateImpl = ProxyStateAspect; + using AspectPropertiesImpl = detail::ProxyPropertiesAspect< + AspectStateImpl, CompositeType, Properties>; + + using Base = AspectPropertiesImpl; + + virtual ~ProxyStateAndPropertiesAspect() = default; + + // Forwarding constructor + template + ProxyStateAndPropertiesAspect(Args&&... args) + : Base(std::forward(args)...) + { + // Do nothing + } + + // Documentation inherited + std::unique_ptr cloneAspect(Composite* newComposite) const override + { + return make_unique(newComposite); + } + +}; + +} // namespace common +} // namespace dart + +#endif // DART_COMMON_PROXYASPECT_H_ diff --git a/dart/common/StlHelpers.h b/dart/common/StlHelpers.h index b80aeb7abf42a..68c8c2414024e 100644 --- a/dart/common/StlHelpers.h +++ b/dart/common/StlHelpers.h @@ -41,6 +41,7 @@ #include #include #include +#include "dart/common/Console.h" // Macro to suppress -Wunused-parameter and -Wunused-variable warnings in // release mode when a variable is only used in assertions. @@ -85,6 +86,81 @@ struct static_if_else using type = T_if; }; +//============================================================================== +/// Templated function for passing each entry in a std::vector into each +/// member of an array of Objects belonging to some Owner class. +/// +/// The ObjectBase argument should be the base class of Object in which the +/// setData function is defined. In many cases, ObjectBase may be the same as +/// Object, but it is not always. +template +void setAllMemberObjectData(Owner* owner, const std::vector& data) +{ + if(!owner) + { + dterr << "[setAllMemberObjectData] Attempting to set [" + << typeid(Data).name() << "] of every [" << typeid(Object).name() + << "] in a nullptr [" << typeid(Owner).name() << "]. Please report " + << "this as a bug!\n"; + assert(false); + return; + } + + size_t numObjects = (owner->*getNumObjects)(); + + if(data.size() != numObjects) + { + dtwarn << "[setAllMemberObjectData] Mismatch between the number of [" + << typeid(Object).name() << "] member objects (" << numObjects + << ") in the [" << typeid(Owner).name() << "] named [" + << owner->getName() << "] (" << owner << ") and the number of [" + << typeid(Object).name() << "] which is (" << data.size() + << ") while setting [" << typeid(Data).name() << "]\n" + << " -- We will set (" << std::min(numObjects, data.size()) + << ") of them.\n"; + numObjects = std::min(numObjects, data.size()); + } + + for(size_t i=0; i < numObjects; ++i) + ((owner->*getObject)(i)->*setData)(data[i]); +} + +//============================================================================== +/// Templated function for aggregating a std::vector out of each member of +/// an array of Objects belonging to some Owner class. +/// +/// The ObjectBase argument should be the base class of Object in which the +/// getData function is defined. In many cases, ObjectBase may be the same as +/// Object, but it is not always. +template +std::vector getAllMemberObjectData(const Owner* owner) +{ + if(!owner) + { + dterr << "[getAllMemberObjectData] Attempting to get the [" + << typeid(Data).name() << "] from every [" << typeid(Object).name() + << "] in a nullptr [" << typeid(Owner).name() << "]. Please report " + << "this as a bug!\n"; + assert(false); + return std::vector(); + } + + const size_t numObjects = (owner->*getNumObjects)(); + std::vector data; + data.reserve(numObjects); + + for(size_t i=0; i < numObjects; ++i) + data.push_back(((owner->*getObject)(i)->*getData)()); + + return data; +} + } // namespace common } // namespace dart diff --git a/dart/common/detail/Cloneable.h b/dart/common/detail/Cloneable.h index 6d074136113c8..72bb2b9e65239 100644 --- a/dart/common/detail/Cloneable.h +++ b/dart/common/detail/Cloneable.h @@ -141,10 +141,10 @@ void MakeCloneable::copy(const Base& other) } //============================================================================== -template -ProxyCloneable::ProxyCloneable() +template +ProxyCloneable::ProxyCloneable() : mOwner(nullptr), mData(make_unique()) { @@ -152,34 +152,34 @@ ProxyCloneable::ProxyCloneable() } //============================================================================== -template -ProxyCloneable::ProxyCloneable( - Owner* owner) +template +ProxyCloneable::ProxyCloneable( + OwnerT* owner) : mOwner(owner) { // Do nothing } //============================================================================== -template +template template -ProxyCloneable::ProxyCloneable( - Owner* owner, Args&&... args) +ProxyCloneable::ProxyCloneable( + OwnerT* owner, Args&&... args) : mOwner(owner) { set(Data(std::forward(args)...)); } //============================================================================== -template +template template -ProxyCloneable::ProxyCloneable( +ProxyCloneable::ProxyCloneable( Args&&... args) : mOwner(nullptr), mData(make_unique(std::forward(args)...)) @@ -188,10 +188,10 @@ ProxyCloneable::ProxyCloneable( } //============================================================================== -template -ProxyCloneable::ProxyCloneable( +template +ProxyCloneable::ProxyCloneable( const ProxyCloneable& other) : mOwner(nullptr), mData(nullptr) @@ -200,10 +200,10 @@ ProxyCloneable::ProxyCloneable( } //============================================================================== -template -ProxyCloneable::ProxyCloneable( +template +ProxyCloneable::ProxyCloneable( ProxyCloneable&& other) : mOwner(nullptr) { @@ -211,50 +211,54 @@ ProxyCloneable::ProxyCloneable( } //============================================================================== -template -auto ProxyCloneable::operator =( +template +auto ProxyCloneable::operator =( const Data& data) -> ProxyCloneable& { set(data); + return *this; } //============================================================================== -template -auto ProxyCloneable::operator =( +template +auto ProxyCloneable::operator =( Data&& data) -> ProxyCloneable& { set(data); + return *this; } //============================================================================== -template -auto ProxyCloneable::operator =( +template +auto ProxyCloneable::operator =( const ProxyCloneable& other) -> ProxyCloneable& { set(other); + return *this; } //============================================================================== -template -auto ProxyCloneable::operator =( +template +auto ProxyCloneable::operator =( ProxyCloneable&& other) -> ProxyCloneable& { set(other); + return *this; } //============================================================================== -template -void ProxyCloneable::set( +template +void ProxyCloneable::set( const Data& data) { if(mOwner) @@ -267,10 +271,10 @@ void ProxyCloneable::set( } //============================================================================== -template -void ProxyCloneable::set( +template +void ProxyCloneable::set( Data&& data) { if(mOwner) @@ -283,30 +287,30 @@ void ProxyCloneable::set( } //============================================================================== -template -void ProxyCloneable::set( +template +void ProxyCloneable::set( const ProxyCloneable& other) { set(other.get()); } //============================================================================== -template -void ProxyCloneable::set( +template +void ProxyCloneable::set( ProxyCloneable&& other) { set(other.get()); } //============================================================================== -template -auto ProxyCloneable::get() const -> Data +template +auto ProxyCloneable::get() const -> Data { if(mOwner) return (*getData)(mOwner); @@ -315,20 +319,38 @@ auto ProxyCloneable::get() const -> Data } //============================================================================== -template +template +OwnerT* ProxyCloneable::getOwner() +{ + return mOwner; +} + +//============================================================================== +template +const OwnerT* ProxyCloneable::getOwner() const +{ + return mOwner; +} + +//============================================================================== +template std::unique_ptr ProxyCloneable< - Base, Owner, DataT, setData, getData>::clone() const + Base, OwnerT, DataT, setData, getData>::clone() const { return make_unique(get()); } //============================================================================== -template -void ProxyCloneable::copy( +template +void ProxyCloneable::copy( const Base& other) { set(static_cast(other)); diff --git a/dart/common/detail/EmbeddedAspect.h b/dart/common/detail/EmbeddedAspect.h index 813fd89079f7f..04331f4e5508a 100644 --- a/dart/common/detail/EmbeddedAspect.h +++ b/dart/common/detail/EmbeddedAspect.h @@ -146,7 +146,7 @@ class EmbeddedStateAspect : public BaseT setState(static_cast(state)); } - // Documentation inherited + /// Set the State of this Aspect void setState(const State& state) { if(this->getComposite()) @@ -166,7 +166,7 @@ class EmbeddedStateAspect : public BaseT return &getState(); } - // Documentation inherited + /// Get the State of this Aspect const State& getState() const { if(this->getComposite()) diff --git a/dart/common/detail/ProxyAspect.h b/dart/common/detail/ProxyAspect.h new file mode 100644 index 0000000000000..9880af5ab924c --- /dev/null +++ b/dart/common/detail/ProxyAspect.h @@ -0,0 +1,186 @@ +/* + * 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_COMMON_DETAIL_PROXYASPECT_H_ +#define DART_COMMON_DETAIL_PROXYASPECT_H_ + +#include "dart/common/Aspect.h" + +namespace dart { +namespace common { +namespace detail { + +//============================================================================== +template +class ProxyStateAspect : public BaseT +{ +public: + + using Base = BaseT; + using CompositeType = CompositeT; + using State = StateT; + + virtual ~ProxyStateAspect() = default; + + /// General constructor + template + ProxyStateAspect(Composite* comp, Args&&... args) + : Base(comp, std::forward(args)...), + mProxyState(dynamic_cast(comp)) + { + // Do nothing + } + + // Documentation inherited + void setAspectState(const Aspect::State& state) override final + { + mProxyState.set(static_cast(state)); + } + + // Documentation inherited + const Aspect::State* getAspectState() const override final + { + return &mProxyState; + } + + // Documentation inherited + std::unique_ptr cloneAspect(Composite* newComposite) const override + { + return make_unique(newComposite); + } + +protected: + + /// Reconfigure the Aspect to link it to this Aspect's new Composite + void setComposite(Composite* newComposite) override + { + Base::setComposite(newComposite); + + // Check if the Composite is the correct Owner type + typename State::Owner* owner = + dynamic_cast(newComposite); + + if(owner && mProxyState.getOwner() != owner) + { + // Link the ProxyState to its new owner + mProxyState = State(owner); + } + } + + /// Reconfigure the Aspect to unlink it from this Aspect's old Composite + void loseComposite(Composite* oldComposite) override + { + mProxyState = State(mProxyState.get()); + Base::loseComposite(oldComposite); + } + + /// Proxy state for this Aspect + State mProxyState; + +}; + +//============================================================================== +template +class ProxyPropertiesAspect : public BaseT +{ +public: + + using Base = BaseT; + using CompositeType = CompositeT; + using Properties = PropertiesT; + + virtual ~ProxyPropertiesAspect() = default; + + /// General constructor + template + ProxyPropertiesAspect(Composite* comp, Args&&... args) + : Base(comp, std::forward(args)...), + mProxyProperties(dynamic_cast(this)) + { + // Do nothing + } + + // Documentation inherited + void setAspectProperties(const Aspect::Properties& properties) override final + { + mProxyProperties.set(static_cast(properties)); + } + + // Documentation inherited + const Aspect::Properties* getAspectProperties() const override final + { + return &mProxyProperties; + } + + // Documentation inherited + std::unique_ptr cloneAspect(Composite* newComposite) const override + { + return make_unique(newComposite); + } + +protected: + + /// Reconfigure the Aspect to link it to this Aspect's new Composite + void setComposite(Composite* newComposite) override + { + Base::setComposite(newComposite); + typename Properties::Owner* comp = + dynamic_cast(newComposite); + + if(comp && mProxyProperties.getOwner() != comp) + { + // Link the ProxyProperties to its new owner + mProxyProperties = Properties(comp); + } + } + + /// Reconfigure the Aspect to unlink it from this Aspect's old Composite + void loseComposite(Composite* oldComposite) override + { + mProxyProperties = Properties(mProxyProperties.get()); + Base::loseComposite(oldComposite); + } + + /// Proxy properties for this Aspect + Properties mProxyProperties; + +}; + +} // namespace detail +} // namespace common +} // namespace dart + +#endif // DART_COMMON_DETAIL_PROXYASPECT_H_ diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index ce523bf45eef8..90eae17308c2a 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -116,6 +116,12 @@ size_t BodyNode::msBodyNodeCount = 0; namespace detail { +//============================================================================== +void setAllNodeStates(BodyNode* bodyNode, const AllNodeStates& states) +{ +// bodyNode->setProperties(states); +} + //============================================================================== BodyNodeState::BodyNodeState( bool isColliding, @@ -188,15 +194,26 @@ void BodyNode::setProperties(const ExtendedProperties& _properties) { setProperties(static_cast(_properties)); - setProperties(_properties.mNodeProperties); + setAllNodeProperties(_properties.mNodeProperties); setProperties(_properties.mCompositeProperties); } //============================================================================== -void BodyNode::setProperties(const NodeProperties& _properties) +void BodyNode::setAllNodeStates(const AllNodeStates& states) { - const NodePropertiesMap& propertiesMap = _properties.getMap(); +// const NodeStateMap& stateMap = states.getMap(); + +// NodeMap::iterator node_it = mNodeMap.begin(); +// NodeStateMap::const_iterator state_it = stateMap.begin(); + +// while( mNodeMap.end() != node_it && stateMap.end() != ) +} + +//============================================================================== +void BodyNode::setAllNodeProperties(const AllNodeProperties& properties) +{ + const NodePropertiesMap& propertiesMap = properties.getMap(); NodeMap::iterator node_it = mNodeMap.begin(); NodePropertiesMap::const_iterator prop_it = propertiesMap.begin(); @@ -232,6 +249,31 @@ void BodyNode::setProperties(const NodeProperties& _properties) } } +//============================================================================== +BodyNode::AllNodeProperties BodyNode::getAllNodeProperties() const +{ + // TODO(MXG): Make a version of this function that will fill in a + // NodeProperties instance instead of creating a new one + detail::NodePropertiesMap nodeProperties; + + for(const auto& entry : mNodeMap) + { + const std::vector& nodes = entry.second; + std::vector< std::unique_ptr > vec; + for(size_t i=0; i < nodes.size(); ++i) + { + std::unique_ptr prop = nodes[i]->getNodeProperties(); + if(prop) + vec.push_back(std::move(prop)); + } + + nodeProperties[entry.first] = common::make_unique< + detail::NodeTypePropertiesVector>(std::move(vec)); + } + + return nodeProperties; +} + //============================================================================== void BodyNode::setProperties(const CompositeProperties& _properties) { @@ -282,36 +324,11 @@ BodyNode::Properties BodyNode::getBodyNodeProperties() const return mAspectProperties; } -//============================================================================== -BodyNode::NodeProperties BodyNode::getAttachedNodeProperties() const -{ - // TODO(MXG): Make a version of this function that will fill in a - // NodeProperties instance instead of creating a new one - NodePropertiesMap nodeProperties; - - for(const auto& entry : mNodeMap) - { - const std::vector& nodes = entry.second; - std::vector< std::unique_ptr > vec; - for(size_t i=0; i < nodes.size(); ++i) - { - std::unique_ptr prop = nodes[i]->getNodeProperties(); - if(prop) - vec.push_back(std::move(prop)); - } - - nodeProperties[entry.first] = common::make_unique( - std::move(vec)); - } - - return nodeProperties; -} - //============================================================================== BodyNode::ExtendedProperties BodyNode::getExtendedProperties() const { return ExtendedProperties(getBodyNodeProperties(), - getAttachedNodeProperties(), + getAllNodeProperties(), getCompositeProperties()); } diff --git a/dart/dynamics/BodyNode.h b/dart/dynamics/BodyNode.h index 1b99a60430b99..61999861e32ec 100644 --- a/dart/dynamics/BodyNode.h +++ b/dart/dynamics/BodyNode.h @@ -96,12 +96,14 @@ class BodyNode : using StructuralChangeSignal = common::Signal; - - using NodePropertiesVector = common::CloneableVector< std::unique_ptr >; - using NodePropertiesMap = std::map< std::type_index, std::unique_ptr >; - using NodeProperties = common::CloneableMap; using CompositeProperties = common::Composite::Properties; + using AllNodeStates = detail::AllNodeStates; + using NodeStateMap = detail::NodeStateMap; + + using AllNodeProperties = detail::AllNodeProperties; + using NodePropertiesMap = detail::NodePropertiesMap; + using UniqueProperties = detail::BodyNodeUniqueProperties; using Properties = detail::BodyNodeProperties; using ExtendedProperties = detail::BodyNodeExtendedProperties; @@ -114,8 +116,17 @@ class BodyNode : /// Set the ExtendedProperties of this BodyNode void setProperties(const ExtendedProperties& _properties); - /// Set the Properties of the attached Nodes - void setProperties(const NodeProperties& _properties); + /// Set the Node::State of all Nodes attached to this BodyNode + void setAllNodeStates(const AllNodeStates& states); + + /// Get the Node::State of all Nodes attached to this BodyNode + AllNodeStates getAllNodeStates() const; + + /// Set the Node::Properties of all Nodes attached to this BodyNode + void setAllNodeProperties(const AllNodeProperties& properties); + + /// Get the Node::Properties of all Nodes attached to this BodyNode + AllNodeProperties getAllNodeProperties() const; /// Same as setCompositeProperties() void setProperties(const CompositeProperties& _properties); @@ -132,9 +143,6 @@ class BodyNode : /// Get the Properties of this BodyNode Properties getBodyNodeProperties() const; - /// Get the the Properties of the Nodes attached to this BodyNode - NodeProperties getAttachedNodeProperties() const; - /// The the full extended Properties of this BodyNode, including the /// Properties of its Aspects, its attached Nodes, and the BodyNode itself. ExtendedProperties getExtendedProperties() const; diff --git a/dart/dynamics/Frame.cpp b/dart/dynamics/Frame.cpp index 3ae9910f302af..2e3f0b49da0e9 100644 --- a/dart/dynamics/Frame.cpp +++ b/dart/dynamics/Frame.cpp @@ -653,6 +653,8 @@ const std::string& WorldFrame::setName(const std::string& name) { dterr << "[WorldFrame::setName] attempting to change name of World frame to [" << name << "], but this is not allowed!\n"; + static const std::string worldName = "World"; + return worldName; } //============================================================================== diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index fa2ac55abbb48..ef98cb4e2e420 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -56,9 +56,6 @@ #include "dart/dynamics/PointMass.h" #include "dart/dynamics/SoftBodyNode.h" -namespace dart { -namespace dynamics { - #define SET_ALL_FLAGS( X ) for(auto& cache : mTreeCache) cache.mDirty. X = true;\ mSkelCache.mDirty. X = true; @@ -83,6 +80,88 @@ namespace dynamics { nonzero_size = V .size(); \ } + +namespace dart { +namespace dynamics { + +namespace detail { + +//============================================================================== +void setAllBodyNodeStates(Skeleton* skel, const BodyNodeStateVector& states) +{ + common::setAllMemberObjectData< + Skeleton, BodyNode, common::Composite, common::Composite::State, + &Skeleton::getNumBodyNodes, &Skeleton::getBodyNode, + &common::Composite::setCompositeState>(skel, states); +} + +//============================================================================== +BodyNodeStateVector getAllBodyNodeStates(const Skeleton* skel) +{ + return common::getAllMemberObjectData< + Skeleton, BodyNode, common::Composite, common::Composite::State, + &Skeleton::getNumBodyNodes, &Skeleton::getBodyNode, + &common::Composite::getCompositeState>(skel); +} + +//============================================================================== +void setAllBodyNodeProperties( + Skeleton* skel, const BodyNodePropertiesVector& properties) +{ + common::setAllMemberObjectData< + Skeleton, BodyNode, common::Composite, common::Composite::Properties, + &Skeleton::getNumBodyNodes, &Skeleton::getBodyNode, + &common::Composite::setCompositeProperties>(skel, properties); +} + +//============================================================================== +BodyNodePropertiesVector getAllBodyNodeProperties(const Skeleton* skel) +{ + return common::getAllMemberObjectData< + Skeleton, BodyNode, common::Composite, common::Composite::Properties, + &Skeleton::getNumBodyNodes, &Skeleton::getBodyNode, + &common::Composite::getCompositeProperties>(skel); +} + +//============================================================================== +void setAllJointStates(Skeleton* skel, const BodyNodeStateVector& states) +{ + common::setAllMemberObjectData< + Skeleton, Joint, common::Composite, common::Composite::State, + &Skeleton::getNumJoints, &Skeleton::getJoint, + &common::Composite::setCompositeState>(skel, states); +} + +//============================================================================== +BodyNodeStateVector getAllJointStates(const Skeleton* skel) +{ + return common::getAllMemberObjectData< + Skeleton, Joint, common::Composite, common::Composite::State, + &Skeleton::getNumJoints, &Skeleton::getJoint, + &common::Composite::getCompositeState>(skel); +} + +//============================================================================== +void setAllJointProperties( + Skeleton* skel, const BodyNodePropertiesVector& properties) +{ + common::setAllMemberObjectData< + Skeleton, Joint, common::Composite, common::Composite::Properties, + &Skeleton::getNumJoints, &Skeleton::getJoint, + &common::Composite::setCompositeProperties>(skel, properties); +} + +//============================================================================== +BodyNodePropertiesVector getAllJointProperties(const Skeleton* skel) +{ + return common::getAllMemberObjectData< + Skeleton, Joint, common::Composite, common::Composite::Properties, + &Skeleton::getNumJoints, &Skeleton::getJoint, + &common::Composite::getCompositeProperties>(skel); +} + +} // namespace detail + //============================================================================== Skeleton::Configuration::Configuration( const Eigen::VectorXd& positions, @@ -1810,6 +1889,8 @@ Skeleton::Skeleton(const Properties& _properties) mIsImpulseApplied(false), mUnionSize(1) { + Composite::create(); + Composite::create(); setProperties(_properties); } diff --git a/dart/dynamics/Skeleton.h b/dart/dynamics/Skeleton.h index eb050b38666c9..38454af9064d7 100644 --- a/dart/dynamics/Skeleton.h +++ b/dart/dynamics/Skeleton.h @@ -49,6 +49,7 @@ #include "dart/dynamics/EndEffector.h" #include "dart/dynamics/detail/BodyNodeProperties.h" #include "dart/dynamics/SpecializedNodeManager.h" +#include "dart/dynamics/detail/SkeletonAspect.h" namespace dart { namespace renderer { @@ -62,9 +63,10 @@ namespace dynamics { /// class Skeleton class Skeleton : public virtual common::VersionCounter, - public virtual common::Composite, public MetaSkeleton, - public virtual SkeletonSpecializedFor + public common::CompositeJoiner< + SkeletonSpecializedFor, + detail::SkeletonProxyAspects> { public: diff --git a/dart/dynamics/detail/BodyNodeProperties.h b/dart/dynamics/detail/BodyNodeProperties.h index e3b41905609c7..e17387e7afa7f 100644 --- a/dart/dynamics/detail/BodyNodeProperties.h +++ b/dart/dynamics/detail/BodyNodeProperties.h @@ -41,10 +41,13 @@ #include "dart/dynamics/Inertia.h" #include "dart/dynamics/Node.h" #include "dart/dynamics/Marker.h" +#include "dart/common/ProxyAspect.h" namespace dart { namespace dynamics { +class Skeleton; + const double DART_DEFAULT_FRICTION_COEFF = 1.0; const double DART_DEFAULT_RESTITUTION_COEFF = 0.0; @@ -130,6 +133,43 @@ struct BodyNodeExtendedProperties : BodyNodeProperties CompositeProperties mCompositeProperties; }; +//============================================================================== +using NodeTypeStateVector = common::CloneableVector< std::unique_ptr >; +using NodeStateMap = std::map< std::type_index, std::unique_ptr >; +using AllNodeStates = common::CloneableMap; + +//============================================================================== +using NodeTypePropertiesVector = common::CloneableVector< std::unique_ptr >; +using NodePropertiesMap = std::map< std::type_index, std::unique_ptr >; +using AllNodeProperties = common::CloneableMap; + +//============================================================================== +void setAllNodeStates(BodyNode* bodyNode, const AllNodeStates& states); + +//============================================================================== +AllNodeStates getAllNodeStates(const BodyNode* bodyNode); + +//============================================================================== +void setAllNodeProperties( + BodyNode* bodyNode, const AllNodeProperties& properties); + +//============================================================================== +AllNodeProperties getAllNodeProperties(const BodyNode* bodyNode); + +//============================================================================== +using NodeVectorProxyAspectState = common::ProxyCloneable< + common::Aspect::State, BodyNode, AllNodeStates, + &setAllNodeStates, &getAllNodeStates>; + +//============================================================================== +using NodeVectorProxyAspectProperties = common::ProxyCloneable< + common::Aspect::Properties, BodyNode, AllNodeProperties, + &setAllNodeProperties, &getAllNodeProperties>; + +//============================================================================== +using NodeVectorProxyAspect = common::ProxyStateAndPropertiesAspect; + } // namespace detail } // namespace dynamics } // namespace dart diff --git a/dart/dynamics/detail/SkeletonAspect.h b/dart/dynamics/detail/SkeletonAspect.h new file mode 100644 index 0000000000000..c5aff44d94214 --- /dev/null +++ b/dart/dynamics/detail/SkeletonAspect.h @@ -0,0 +1,119 @@ +/* + * 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_SKELETONASPECT_H_ +#define DART_DYNAMICS_DETAIL_SKELETONASPECT_H_ + +#include "dart/common/Composite.h" +#include "dart/common/ProxyAspect.h" +#include "dart/common/RequiresAspect.h" + +namespace dart { +namespace dynamics { + +class Skeleton; + +namespace detail { + +//============================================================================== +using BodyNodeStateVector = std::vector; +using BodyNodePropertiesVector = std::vector; +using JointStateVector = std::vector; +using JointPropertiesVector = std::vector; + +//============================================================================== +void setAllBodyNodeStates(Skeleton* skel, const BodyNodeStateVector& states); + +//============================================================================== +BodyNodeStateVector getAllBodyNodeStates(const Skeleton* skel); + +//============================================================================== +void setAllBodyNodeProperties( + Skeleton* skel, const BodyNodePropertiesVector& properties); + +//============================================================================== +BodyNodePropertiesVector getAllBodyNodeProperties(const Skeleton* skel); + +//============================================================================== +using BodyNodeVectorProxyAspectState = common::ProxyCloneable< + common::Aspect::State, Skeleton, BodyNodeStateVector, + &setAllBodyNodeStates, &getAllBodyNodeStates>; + +//============================================================================== +using BodyNodeVectorProxyAspectProperties = common::ProxyCloneable< + common::Aspect::Properties, Skeleton, BodyNodePropertiesVector, + &setAllBodyNodeProperties, &getAllBodyNodeProperties>; + +//============================================================================== +using BodyNodeVectorProxyAspect = common::ProxyStateAndPropertiesAspect; + +//============================================================================== +void setAllJointStates(Skeleton* skel, const JointStateVector& states); + +//============================================================================== +JointStateVector getAllJointStates(const Skeleton* skel); + +//============================================================================== +void setAllJointProperties( + Skeleton* skel, const JointPropertiesVector& properties); + +//============================================================================== +JointPropertiesVector getAllJointProperties(const Skeleton* skel); + +//============================================================================== +using JointVectorProxyAspectState = common::ProxyCloneable< + common::Aspect::State, Skeleton, JointStateVector, + &setAllJointStates, &getAllJointStates>; + +//============================================================================== +using JointVectorProxyAspectProperties = common::ProxyCloneable< + common::Aspect::Properties, Skeleton, JointPropertiesVector, + &setAllJointProperties, &getAllJointProperties>; + +//============================================================================== +using JointVectorProxyAspect = common::ProxyStateAndPropertiesAspect; + +//============================================================================== +using SkeletonProxyAspects = common::RequiresAspect< + BodyNodeVectorProxyAspect, JointVectorProxyAspect>; + +} // namespace detail +} // namespace dynamics +} // namespace dart + +#endif // DART_DYNAMICS_DETAIL_SKELETONASPECT_H_ From c9ed0744adab81fd93ad58565421155f62016fba Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Mon, 4 Apr 2016 13:41:38 -0400 Subject: [PATCH 31/62] finished implementing node proxy aspects -- needs testing --- dart/common/Cloneable.h | 11 +- dart/common/Composite.cpp | 50 ++++--- dart/common/StlHelpers.h | 75 ---------- dart/common/detail/Cloneable.h | 22 +++ dart/dynamics/BodyNode.cpp | 166 ++++++++++++++-------- dart/dynamics/BodyNode.h | 3 +- dart/dynamics/Entity.cpp | 2 +- dart/dynamics/Entity.h | 2 +- dart/dynamics/Skeleton.cpp | 97 +++++++++++-- dart/dynamics/detail/BodyNodeProperties.h | 6 + 10 files changed, 270 insertions(+), 164 deletions(-) diff --git a/dart/common/Cloneable.h b/dart/common/Cloneable.h index e0082832841ee..9b25018783580 100644 --- a/dart/common/Cloneable.h +++ b/dart/common/Cloneable.h @@ -277,11 +277,11 @@ class CloneableVector final /// Construct from a regular vector using move semantics CloneableVector(std::vector&& regularVector); - /// Do not copy this class directly, use clone() or copy() instead - CloneableVector(const CloneableVector& doNotCopy) = delete; + /// Construct this vector empty and then perform copy(other) + CloneableVector(const CloneableVector& other); - /// Do not copy this class directly, use clone() or copy() instead - CloneableVector& operator=(const CloneableVector& doNotCopy) = delete; + /// Call copy(other) on this vector + CloneableVector& operator=(const CloneableVector& other); /// Create a copy of this CloneableVector's contents std::unique_ptr< CloneableVector > clone() const; @@ -289,6 +289,9 @@ class CloneableVector final /// Copy the contents of another extensible vector into this one. void copy(const CloneableVector& anotherVector); + /// Get a reference to the std::vector that this class is wrapping + std::vector& getVector(); + /// Get a reference to the std::vector that this class is wrapping const std::vector& getVector() const; diff --git a/dart/common/Composite.cpp b/dart/common/Composite.cpp index c800110c1fa31..2de96424b8dad 100644 --- a/dart/common/Composite.cpp +++ b/dart/common/Composite.cpp @@ -44,15 +44,24 @@ namespace dart { namespace common { //============================================================================== +/// Type maps are std::map containers which map an object's Type Info to some +/// instance or trait of that type. For example, an ObjectMap will map an +/// object's type to a std::unique_ptr of an instance of that object. a StateMap +/// will map an Object's type to a std::unique_ptr of a State instance for that +/// Object. Type maps are used for the dart::common::Aspect class. +/// +/// This function will move data from an Object instance into a container where +/// the data is sorted by the Object type that it belongs to. If the DataMap +/// that is being filled with data already has an instance of the data for a +/// particular Object type, it will perform a copy instead of a clone to improve +/// performance. template >, typename DataMap = std::map< std::type_index, std::unique_ptr > > -void extractTypeMapData(DataMap& dataMap, const ObjectMap& objectMap) +static void extractDataFromObjectTypeMap( + DataMap& dataMap, const ObjectMap& objectMap) { - // TODO(MXG): Consider placing this function in a header so it can be utilized - // by anything that needs to transfer data between maps of extensibles - // This method allows us to avoid dynamic allocation (cloning) whenever possible. for(const auto& object : objectMap) { @@ -62,16 +71,16 @@ void extractTypeMapData(DataMap& dataMap, const ObjectMap& objectMap) const DataType* data = (object.second.get()->*getData)(); if(data) { - // Attempt to insert a nullptr to see whether this entry exists while also - // creating an itertor to it if it did not already exist. This allows us - // to search for a spot in the map once, instead of searching the map to - // see if the entry already exists and then searching the map again in - // order to insert the entry if it didn't already exist. + // Attempt to insert a nullptr to see whether this data exists while also + // creating an iterator to it if it did not already exist. This allows us + // to search for a spot in the data map once, instead of searching the map + // to see if the data entry already exists and then searching the map + // again in order to insert the entry if it didn't already exist. std::pair insertion = dataMap.insert(typename DataMap::value_type(object.first, nullptr)); typename DataMap::iterator& it = insertion.first; - bool existed = !insertion.second; + const bool existed = !insertion.second; if(existed) { @@ -97,11 +106,21 @@ void extractTypeMapData(DataMap& dataMap, const ObjectMap& objectMap) } //============================================================================== +/// Type maps are std::map containers which map an object's Type Info to some +/// instance or trait of that type. For example, an ObjectMap will map an +/// object's type to a std::unique_ptr of an instance of that object. a StateMap +/// will map an Object's type to a std::unique_ptr of a State instance for that +/// Object. Type maps are used for the dart::common::Aspect class. +/// +/// This function will take a type map of Data and pass its contents into the +/// Objects contained in an ObjectMap for each corresponding Object type which +/// is available. template >, typename DataMap = std::map< std::type_index, std::unique_ptr > > -void setObjectDataFromMap(ObjectMap& objectMap, const DataMap& dataMap) +static void setObjectsFromDataTypeMap( + ObjectMap& objectMap, const DataMap& dataMap) { typename ObjectMap::iterator objects = objectMap.begin(); typename DataMap::const_iterator data = dataMap.begin(); @@ -128,11 +147,10 @@ void setObjectDataFromMap(ObjectMap& objectMap, const DataMap& dataMap) } } - //============================================================================== void Composite::setCompositeState(const State& newStates) { - setObjectDataFromMap( + setObjectsFromDataTypeMap( mAspectMap, newStates.getMap()); } @@ -149,14 +167,14 @@ Composite::State Composite::getCompositeState() const void Composite::copyCompositeStateTo(State& outgoingStates) const { StateMap& states = outgoingStates.getMap(); - extractTypeMapData( + extractDataFromObjectTypeMap( states, mAspectMap); } //============================================================================== void Composite::setCompositeProperties(const Properties& newProperties) { - setObjectDataFromMap< + setObjectsFromDataTypeMap< Aspect, Aspect::Properties, &Aspect::setAspectProperties>( mAspectMap, newProperties.getMap()); } @@ -175,7 +193,7 @@ void Composite::copyCompositePropertiesTo( Properties& outgoingProperties) const { PropertiesMap& properties = outgoingProperties.getMap(); - extractTypeMapData( + extractDataFromObjectTypeMap( properties, mAspectMap); } diff --git a/dart/common/StlHelpers.h b/dart/common/StlHelpers.h index 68c8c2414024e..a34a133632fa5 100644 --- a/dart/common/StlHelpers.h +++ b/dart/common/StlHelpers.h @@ -86,81 +86,6 @@ struct static_if_else using type = T_if; }; -//============================================================================== -/// Templated function for passing each entry in a std::vector into each -/// member of an array of Objects belonging to some Owner class. -/// -/// The ObjectBase argument should be the base class of Object in which the -/// setData function is defined. In many cases, ObjectBase may be the same as -/// Object, but it is not always. -template -void setAllMemberObjectData(Owner* owner, const std::vector& data) -{ - if(!owner) - { - dterr << "[setAllMemberObjectData] Attempting to set [" - << typeid(Data).name() << "] of every [" << typeid(Object).name() - << "] in a nullptr [" << typeid(Owner).name() << "]. Please report " - << "this as a bug!\n"; - assert(false); - return; - } - - size_t numObjects = (owner->*getNumObjects)(); - - if(data.size() != numObjects) - { - dtwarn << "[setAllMemberObjectData] Mismatch between the number of [" - << typeid(Object).name() << "] member objects (" << numObjects - << ") in the [" << typeid(Owner).name() << "] named [" - << owner->getName() << "] (" << owner << ") and the number of [" - << typeid(Object).name() << "] which is (" << data.size() - << ") while setting [" << typeid(Data).name() << "]\n" - << " -- We will set (" << std::min(numObjects, data.size()) - << ") of them.\n"; - numObjects = std::min(numObjects, data.size()); - } - - for(size_t i=0; i < numObjects; ++i) - ((owner->*getObject)(i)->*setData)(data[i]); -} - -//============================================================================== -/// Templated function for aggregating a std::vector out of each member of -/// an array of Objects belonging to some Owner class. -/// -/// The ObjectBase argument should be the base class of Object in which the -/// getData function is defined. In many cases, ObjectBase may be the same as -/// Object, but it is not always. -template -std::vector getAllMemberObjectData(const Owner* owner) -{ - if(!owner) - { - dterr << "[getAllMemberObjectData] Attempting to get the [" - << typeid(Data).name() << "] from every [" << typeid(Object).name() - << "] in a nullptr [" << typeid(Owner).name() << "]. Please report " - << "this as a bug!\n"; - assert(false); - return std::vector(); - } - - const size_t numObjects = (owner->*getNumObjects)(); - std::vector data; - data.reserve(numObjects); - - for(size_t i=0; i < numObjects; ++i) - data.push_back(((owner->*getObject)(i)->*getData)()); - - return data; -} - } // namespace common } // namespace dart diff --git a/dart/common/detail/Cloneable.h b/dart/common/detail/Cloneable.h index 72bb2b9e65239..72b87f6fda53c 100644 --- a/dart/common/detail/Cloneable.h +++ b/dart/common/detail/Cloneable.h @@ -496,6 +496,21 @@ CloneableVector::CloneableVector(std::vector&& regularVector) mVector = std::move(regularVector); } +//============================================================================== +template +CloneableVector::CloneableVector(const CloneableVector& other) +{ + copy(other); +} + +//============================================================================== +template +CloneableVector& CloneableVector::operator =(const CloneableVector& other) +{ + copy(other); + return this; +} + //============================================================================== template std::unique_ptr< CloneableVector > CloneableVector::clone() const @@ -527,6 +542,13 @@ void CloneableVector::copy(const CloneableVector& anotherVector) } } +//============================================================================== +template +std::vector& CloneableVector::getVector() +{ + return mVector; +} + //============================================================================== template const std::vector& CloneableVector::getVector() const diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index 90eae17308c2a..61b0eda039785 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -56,6 +56,77 @@ namespace dart { namespace dynamics { +//============================================================================== +template (Node::*getData)() const, + typename VectorType = common::CloneableVector< std::unique_ptr >, + typename DataMap = std::map< std::type_index, std::unique_ptr > > +static void extractDataFromNodeTypeMap( + DataMap& dataMap, const BodyNode::NodeMap& nodeMap) +{ + for(const auto& node_it : nodeMap) + { + const std::vector& nodes = node_it.second; + + std::pair insertion = + dataMap.insert(typename DataMap::value_type( + node_it.first, nullptr)); + + typename DataMap::iterator& it = insertion.first; + + std::unique_ptr& data = it->second; + if(!data) + data = common::make_unique(); + + data->getVector().resize(nodes.size()); + + for(size_t i=0; i < nodes.size(); ++i) + { + std::unique_ptr& datum = data->getVector()[i]; + datum = (nodes[i]->*getData)(); + } + } +} + +//============================================================================== +template >, + typename DataMap = std::map< std::type_index, std::unique_ptr > > +static void setNodesFromDataTypeMap( + BodyNode::NodeMap& nodeMap, const DataMap& dataMap) +{ + typename BodyNode::NodeMap::iterator node_it = nodeMap.begin(); + typename DataMap::const_iterator data_it = dataMap.begin(); + + while( nodeMap.end() != node_it && dataMap.end() != data_it) + { + if( node_it->first == data_it->first ) + { + const std::vector& node_vec = node_it->second; + const std::vector< std::unique_ptr >& data_vec = + data_it->second->getVector(); + + // TODO(MXG): Should we report if the dimensions are mismatched? + std::size_t stop = std::min(node_vec.size(), data_vec.size()); + for(std::size_t i=0; i < stop; ++i) + { + if(data_vec[i]) + (node_vec[i]->*setData)(*data_vec[i]); + } + + ++node_it; + ++data_it; + } + else if( node_it->first < data_it->first ) + { + ++node_it; + } + else + { + ++data_it; + } + } +} + //============================================================================== SkeletonRefCountingBase::SkeletonRefCountingBase() : mReferenceCount(0), @@ -119,7 +190,26 @@ namespace detail { //============================================================================== void setAllNodeStates(BodyNode* bodyNode, const AllNodeStates& states) { -// bodyNode->setProperties(states); + bodyNode->setAllNodeStates(states); +} + +//============================================================================== +AllNodeStates getAllNodeStates(const BodyNode* bodyNode) +{ + return bodyNode->getAllNodeStates(); +} + +//============================================================================== +void setAllNodeProperties(BodyNode* bodyNode, + const AllNodeProperties& properties) +{ + bodyNode->setAllNodeProperties(properties); +} + +//============================================================================== +AllNodeProperties getAllNodeProperties(const BodyNode* bodyNode) +{ + return bodyNode->getAllNodeProperties(); } //============================================================================== @@ -202,51 +292,24 @@ void BodyNode::setProperties(const ExtendedProperties& _properties) //============================================================================== void BodyNode::setAllNodeStates(const AllNodeStates& states) { -// const NodeStateMap& stateMap = states.getMap(); - -// NodeMap::iterator node_it = mNodeMap.begin(); -// NodeStateMap::const_iterator state_it = stateMap.begin(); + setNodesFromDataTypeMap( + mNodeMap, states.getMap()); +} -// while( mNodeMap.end() != node_it && stateMap.end() != ) +//============================================================================== +BodyNode::AllNodeStates BodyNode::getAllNodeStates() const +{ + detail::NodeStateMap nodeStates; + extractDataFromNodeTypeMap( + nodeStates, mNodeMap); + return nodeStates; } //============================================================================== void BodyNode::setAllNodeProperties(const AllNodeProperties& properties) { - const NodePropertiesMap& propertiesMap = properties.getMap(); - - NodeMap::iterator node_it = mNodeMap.begin(); - NodePropertiesMap::const_iterator prop_it = propertiesMap.begin(); - - while( mNodeMap.end() != node_it && propertiesMap.end() != prop_it ) - { - if( node_it->first == prop_it->first ) - { - if( prop_it->second ) - { - const std::vector& node_vec = node_it->second; - const std::vector< std::unique_ptr >& prop_vec = - prop_it->second->getVector(); - size_t stop = std::min(node_vec.size(), prop_vec.size()); - for(size_t i=0; i < stop; ++i) - { - if(prop_vec[i]) - node_vec[i]->setNodeProperties(*prop_vec[i]); - } - } - - ++node_it; - ++prop_it; - } - else if( node_it->first < prop_it->first ) - { - ++node_it; - } - else - { - ++prop_it; - } - } + setNodesFromDataTypeMap( + mNodeMap, properties.getMap()); } //============================================================================== @@ -255,22 +318,8 @@ BodyNode::AllNodeProperties BodyNode::getAllNodeProperties() const // TODO(MXG): Make a version of this function that will fill in a // NodeProperties instance instead of creating a new one detail::NodePropertiesMap nodeProperties; - - for(const auto& entry : mNodeMap) - { - const std::vector& nodes = entry.second; - std::vector< std::unique_ptr > vec; - for(size_t i=0; i < nodes.size(); ++i) - { - std::unique_ptr prop = nodes[i]->getNodeProperties(); - if(prop) - vec.push_back(std::move(prop)); - } - - nodeProperties[entry.first] = common::make_unique< - detail::NodeTypePropertiesVector>(std::move(vec)); - } - + extractDataFromNodeTypeMap( + nodeProperties, mNodeMap); return nodeProperties; } @@ -1287,10 +1336,13 @@ BodyNode::BodyNode(BodyNode* _parentBodyNode, Joint* _parentJoint, if(_parentBodyNode) _parentBodyNode->addChildBodyNode(this); + + create(); + create(); } //============================================================================== -BodyNode::BodyNode(const std::tuple& args) +BodyNode::BodyNode(const std::tuple& args) : BodyNode(std::get<0>(args), std::get<1>(args), std::get<2>(args)) { // The initializer list is delegating the construction diff --git a/dart/dynamics/BodyNode.h b/dart/dynamics/BodyNode.h index 61999861e32ec..efd13366f89eb 100644 --- a/dart/dynamics/BodyNode.h +++ b/dart/dynamics/BodyNode.h @@ -81,8 +81,7 @@ class Marker; /// BodyNode inherits Frame, and a parent Frame of a BodyNode is the parent /// BodyNode of the BodyNode. class BodyNode : - public common::EmbedStateAndProperties< - BodyNode, detail::BodyNodeState, detail::BodyNodeUniqueProperties>, + public detail::BodyNodeAspects, public virtual BodyNodeSpecializedFor, public SkeletonRefCountingBase, public TemplatedJacobianNode diff --git a/dart/dynamics/Entity.cpp b/dart/dynamics/Entity.cpp index a20ebd4d47547..cbc2e020ca7a1 100644 --- a/dart/dynamics/Entity.cpp +++ b/dart/dynamics/Entity.cpp @@ -266,7 +266,7 @@ void Entity::changeParentFrame(Frame* _newParentFrame) } //============================================================================== -Detachable::Detachable(Frame* _refFrame, const std::string& _name, bool _quiet) +Detachable::Detachable(Frame* _refFrame, bool _quiet) : Entity(_refFrame, _quiet) { // Do nothing diff --git a/dart/dynamics/Entity.h b/dart/dynamics/Entity.h index 98418a6de3af5..f15cd1583535e 100644 --- a/dart/dynamics/Entity.h +++ b/dart/dynamics/Entity.h @@ -228,7 +228,7 @@ class Detachable : public virtual Entity { public: /// Constructor - explicit Detachable(Frame* _refFrame, const std::string& _name, bool _quiet); + explicit Detachable(Frame* _refFrame, bool _quiet); /// Allows the user to change the parent Frame of this Entity virtual void setParentFrame(Frame* _newParentFrame); diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index ef98cb4e2e420..0eab1bca47321 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -86,10 +86,91 @@ namespace dynamics { namespace detail { +//============================================================================== +/// Templated function for passing each entry in a std::vector into each +/// member of an array of Objects belonging to some Owner class. +/// +/// The ObjectBase argument should be the base class of Object in which the +/// setData function is defined. In many cases, ObjectBase may be the same as +/// Object, but it is not always. +// +// TODO(MXG): Consider putting this in an accessible header if it might be +// useful in other places. +template +void setAllMemberObjectData(Owner* owner, const std::vector& data) +{ + if(!owner) + { + dterr << "[setAllMemberObjectData] Attempting to set [" + << typeid(Data).name() << "] of every [" << typeid(Object).name() + << "] in a nullptr [" << typeid(Owner).name() << "]. Please report " + << "this as a bug!\n"; + assert(false); + return; + } + + size_t numObjects = (owner->*getNumObjects)(); + + if(data.size() != numObjects) + { + dtwarn << "[setAllMemberObjectData] Mismatch between the number of [" + << typeid(Object).name() << "] member objects (" << numObjects + << ") in the [" << typeid(Owner).name() << "] named [" + << owner->getName() << "] (" << owner << ") and the number of [" + << typeid(Object).name() << "] which is (" << data.size() + << ") while setting [" << typeid(Data).name() << "]\n" + << " -- We will set (" << std::min(numObjects, data.size()) + << ") of them.\n"; + numObjects = std::min(numObjects, data.size()); + } + + for(size_t i=0; i < numObjects; ++i) + ((owner->*getObject)(i)->*setData)(data[i]); +} + +//============================================================================== +/// Templated function for aggregating a std::vector out of each member of +/// an array of Objects belonging to some Owner class. +/// +/// The ObjectBase argument should be the base class of Object in which the +/// getData function is defined. In many cases, ObjectBase may be the same as +/// Object, but it is not always. +// +// TODO(MXG): Consider putting this in an accessible header if it might be +// useful in other places. +template +std::vector getAllMemberObjectData(const Owner* owner) +{ + if(!owner) + { + dterr << "[getAllMemberObjectData] Attempting to get the [" + << typeid(Data).name() << "] from every [" << typeid(Object).name() + << "] in a nullptr [" << typeid(Owner).name() << "]. Please report " + << "this as a bug!\n"; + assert(false); + return std::vector(); + } + + const size_t numObjects = (owner->*getNumObjects)(); + std::vector data; + data.reserve(numObjects); + + for(size_t i=0; i < numObjects; ++i) + data.push_back(((owner->*getObject)(i)->*getData)()); + + return data; +} + //============================================================================== void setAllBodyNodeStates(Skeleton* skel, const BodyNodeStateVector& states) { - common::setAllMemberObjectData< + setAllMemberObjectData< Skeleton, BodyNode, common::Composite, common::Composite::State, &Skeleton::getNumBodyNodes, &Skeleton::getBodyNode, &common::Composite::setCompositeState>(skel, states); @@ -98,7 +179,7 @@ void setAllBodyNodeStates(Skeleton* skel, const BodyNodeStateVector& states) //============================================================================== BodyNodeStateVector getAllBodyNodeStates(const Skeleton* skel) { - return common::getAllMemberObjectData< + return getAllMemberObjectData< Skeleton, BodyNode, common::Composite, common::Composite::State, &Skeleton::getNumBodyNodes, &Skeleton::getBodyNode, &common::Composite::getCompositeState>(skel); @@ -108,7 +189,7 @@ BodyNodeStateVector getAllBodyNodeStates(const Skeleton* skel) void setAllBodyNodeProperties( Skeleton* skel, const BodyNodePropertiesVector& properties) { - common::setAllMemberObjectData< + setAllMemberObjectData< Skeleton, BodyNode, common::Composite, common::Composite::Properties, &Skeleton::getNumBodyNodes, &Skeleton::getBodyNode, &common::Composite::setCompositeProperties>(skel, properties); @@ -117,7 +198,7 @@ void setAllBodyNodeProperties( //============================================================================== BodyNodePropertiesVector getAllBodyNodeProperties(const Skeleton* skel) { - return common::getAllMemberObjectData< + return getAllMemberObjectData< Skeleton, BodyNode, common::Composite, common::Composite::Properties, &Skeleton::getNumBodyNodes, &Skeleton::getBodyNode, &common::Composite::getCompositeProperties>(skel); @@ -126,7 +207,7 @@ BodyNodePropertiesVector getAllBodyNodeProperties(const Skeleton* skel) //============================================================================== void setAllJointStates(Skeleton* skel, const BodyNodeStateVector& states) { - common::setAllMemberObjectData< + setAllMemberObjectData< Skeleton, Joint, common::Composite, common::Composite::State, &Skeleton::getNumJoints, &Skeleton::getJoint, &common::Composite::setCompositeState>(skel, states); @@ -135,7 +216,7 @@ void setAllJointStates(Skeleton* skel, const BodyNodeStateVector& states) //============================================================================== BodyNodeStateVector getAllJointStates(const Skeleton* skel) { - return common::getAllMemberObjectData< + return getAllMemberObjectData< Skeleton, Joint, common::Composite, common::Composite::State, &Skeleton::getNumJoints, &Skeleton::getJoint, &common::Composite::getCompositeState>(skel); @@ -145,7 +226,7 @@ BodyNodeStateVector getAllJointStates(const Skeleton* skel) void setAllJointProperties( Skeleton* skel, const BodyNodePropertiesVector& properties) { - common::setAllMemberObjectData< + setAllMemberObjectData< Skeleton, Joint, common::Composite, common::Composite::Properties, &Skeleton::getNumJoints, &Skeleton::getJoint, &common::Composite::setCompositeProperties>(skel, properties); @@ -154,7 +235,7 @@ void setAllJointProperties( //============================================================================== BodyNodePropertiesVector getAllJointProperties(const Skeleton* skel) { - return common::getAllMemberObjectData< + return getAllMemberObjectData< Skeleton, Joint, common::Composite, common::Composite::Properties, &Skeleton::getNumJoints, &Skeleton::getJoint, &common::Composite::getCompositeProperties>(skel); diff --git a/dart/dynamics/detail/BodyNodeProperties.h b/dart/dynamics/detail/BodyNodeProperties.h index e17387e7afa7f..cfb93899426f9 100644 --- a/dart/dynamics/detail/BodyNodeProperties.h +++ b/dart/dynamics/detail/BodyNodeProperties.h @@ -42,6 +42,7 @@ #include "dart/dynamics/Node.h" #include "dart/dynamics/Marker.h" #include "dart/common/ProxyAspect.h" +#include "dart/common/EmbeddedAspect.h" namespace dart { namespace dynamics { @@ -170,6 +171,11 @@ using NodeVectorProxyAspectProperties = common::ProxyCloneable< using NodeVectorProxyAspect = common::ProxyStateAndPropertiesAspect; +//============================================================================== +using BodyNodeAspects = common::EmbedStateAndPropertiesOnTopOf< + BodyNode, BodyNodeState, BodyNodeUniqueProperties, + common::RequiresAspect >; + } // namespace detail } // namespace dynamics } // namespace dart From 7b60b84c0282c2a7fa8876f63147539d81a288d2 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Tue, 5 Apr 2016 00:35:35 -0400 Subject: [PATCH 32/62] converting all objects to be compositions of Aspects --- dart/common/Composite.h | 8 +- dart/common/CompositeJoiner.h | 6 +- dart/common/VersionCounter.cpp | 89 ++++++++++ dart/common/VersionCounter.h | 13 +- dart/common/detail/Composite.h | 22 +-- dart/common/detail/CompositeJoiner.h | 10 +- dart/common/detail/SpecializedForAspect.h | 6 +- dart/dynamics/BodyNode.cpp | 72 ++------ dart/dynamics/BodyNode.h | 16 +- dart/dynamics/EndEffector.cpp | 76 ++------- dart/dynamics/EndEffector.h | 39 +---- dart/dynamics/FixedFrame.cpp | 41 ++++- dart/dynamics/FixedFrame.h | 33 +++- dart/dynamics/Joint.cpp | 18 -- dart/dynamics/Joint.h | 6 - dart/dynamics/Node.cpp | 20 +-- dart/dynamics/Node.h | 8 - dart/dynamics/ShapeFrame.cpp | 18 +- dart/dynamics/ShapeFrame.h | 8 - dart/dynamics/ShapeNode.cpp | 35 +--- dart/dynamics/ShapeNode.h | 24 +-- dart/dynamics/Skeleton.cpp | 199 ++++++++++------------ dart/dynamics/Skeleton.h | 122 +++---------- dart/dynamics/detail/BodyNode.h | 4 +- dart/dynamics/detail/BodyNodeProperties.h | 36 +--- dart/dynamics/detail/Node.h | 2 +- dart/dynamics/detail/SkeletonAspect.h | 46 +++++ unittests/testAspect.cpp | 54 +++--- 28 files changed, 442 insertions(+), 589 deletions(-) create mode 100644 dart/common/VersionCounter.cpp diff --git a/dart/common/Composite.h b/dart/common/Composite.h index 55be521f89bf3..f101ede6a2a45 100644 --- a/dart/common/Composite.h +++ b/dart/common/Composite.h @@ -115,17 +115,17 @@ class Composite /// Construct an Aspect inside of this Composite template - T* create(Args&&... args); + T* createAspect(Args&&... args); /// Remove an Aspect from this Composite. template - void erase(); + void eraseAspect(); /// Remove an Aspect from this Composite, but return its unique_ptr instead /// of letting it be deleted. This allows you to safely use move semantics to /// transfer an Aspect between two Composites. template - std::unique_ptr release(); + std::unique_ptr releaseAspect(); /// Check if this Composite is specialized for a specific type of Aspect. /// @@ -135,7 +135,7 @@ class Composite /// Check if this Composite requires this specific type of Aspect template - bool requires() const; + bool requiresAspect() const; /// Set the states of the aspects in this Composite based on the given /// Composite::State. The states of any Aspect types that do not exist diff --git a/dart/common/CompositeJoiner.h b/dart/common/CompositeJoiner.h index 75d4bebf86e5e..53ea40a84a349 100644 --- a/dart/common/CompositeJoiner.h +++ b/dart/common/CompositeJoiner.h @@ -116,15 +116,15 @@ class CompositeJoiner : public Base1, public Base2 // Documentation inherited template - T* create(Args&&... args); + T* createAspect(Args&&... args); // Documentation inherited template - void erase(); + void eraseAspect(); // Documentation inherited template - std::unique_ptr release(); + std::unique_ptr releaseAspect(); // Documentation inherited template diff --git a/dart/common/VersionCounter.cpp b/dart/common/VersionCounter.cpp new file mode 100644 index 0000000000000..0ac2fa82ced6c --- /dev/null +++ b/dart/common/VersionCounter.cpp @@ -0,0 +1,89 @@ +/* + * 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. + */ + +#include "dart/common/VersionCounter.h" +#include "dart/common/Console.h" + +#include +#include + +namespace dart { +namespace common { + +//============================================================================== +size_t VersionCounter::incrementVersion() +{ + ++mVersion; + if(mDependent) + mDependent->incrementVersion(); + + return mVersion; +} + +//============================================================================== +size_t VersionCounter::getVersion() const +{ + return mVersion; +} + +//============================================================================== +void VersionCounter::setVersionDependentObject(VersionCounter* dependent) +{ + VersionCounter* next = dependent; + do + { + if(next == this) + { + dterr << "[VersionCounter::setVersionDependentObject] Attempting to " + << "create a circular version dependency with the following loop:\n"; + next = dependent; + while(next != this) + { + std::cerr << " -- " << next << "\n"; + next = next->mDependent; + } + std::cerr << " -- " << this << "\n"; + assert(false); + return; + } + + } while( (next = next->mDependent) ); + + mDependent = dependent; +} + +} // namespace common +} // namespace dart diff --git a/dart/common/VersionCounter.h b/dart/common/VersionCounter.h index 10bcd951704ba..d86431e066343 100644 --- a/dart/common/VersionCounter.h +++ b/dart/common/VersionCounter.h @@ -48,12 +48,21 @@ class VersionCounter public: /// Increment the version for this object - virtual size_t incrementVersion() = 0; + virtual size_t incrementVersion(); /// Get the version number of this object - virtual size_t getVersion() const = 0; + virtual size_t getVersion() const; virtual ~VersionCounter() = default; + +protected: + + void setVersionDependentObject(VersionCounter* dependent); + + size_t mVersion; + +private: + VersionCounter* mDependent; }; } // namespace common diff --git a/dart/common/detail/Composite.h b/dart/common/detail/Composite.h index 17fd003ecedad..68fe1d9262da7 100644 --- a/dart/common/detail/Composite.h +++ b/dart/common/detail/Composite.h @@ -40,7 +40,7 @@ #include "dart/common/Composite.h" #define DART_COMMON_CHECK_ILLEGAL_ASPECT_ERASE( Func, T, ReturnType )\ - if(requires< T >())\ + if(requiresAspect< T >())\ {\ dterr << "[Composite::" #Func << "] Illegal request to remove required "\ << "Aspect [" << typeid(T).name() << "]!\n";\ @@ -92,7 +92,7 @@ void Composite::set(std::unique_ptr&& aspect) //============================================================================== template -T* Composite::create(Args&&... args) +T* Composite::createAspect(Args&&... args) { T* aspect = new T(this, std::forward(args)...); mAspectMap[typeid(T)] = std::unique_ptr(aspect); @@ -103,10 +103,10 @@ T* Composite::create(Args&&... args) //============================================================================== template -void Composite::erase() +void Composite::eraseAspect() { AspectMap::iterator it = mAspectMap.find( typeid(T) ); - DART_COMMON_CHECK_ILLEGAL_ASPECT_ERASE(erase, T, DART_BLANK) + DART_COMMON_CHECK_ILLEGAL_ASPECT_ERASE(eraseAspect, T, DART_BLANK) if(mAspectMap.end() != it) { removeFromComposite(it->second.get()); @@ -116,11 +116,11 @@ void Composite::erase() //============================================================================== template -std::unique_ptr Composite::release() +std::unique_ptr Composite::releaseAspect() { std::unique_ptr extraction = nullptr; AspectMap::iterator it = mAspectMap.find( typeid(T) ); - DART_COMMON_CHECK_ILLEGAL_ASPECT_ERASE(release, T, nullptr) + DART_COMMON_CHECK_ILLEGAL_ASPECT_ERASE(releaseAspect, T, nullptr) if(mAspectMap.end() != it) { removeFromComposite(it->second.get()); @@ -139,7 +139,7 @@ constexpr bool Composite::isSpecializedFor() //============================================================================== template -bool Composite::requires() const +bool Composite::requiresAspect() const { return (mRequiredAspects.find(typeid(T)) != mRequiredAspects.end()); } @@ -155,7 +155,7 @@ void createAspects(T* /*mgr*/) template void createAspects(T* mgr) { - mgr->template create(); + mgr->template createAspect(); createAspects(mgr); } @@ -204,17 +204,17 @@ void createAspects(T* mgr) template \ inline TypeName * create ## AspectName (Args&&... args)\ {\ - return this->template create(std::forward(args)...);\ + return this->template createAspect(std::forward(args)...);\ }\ \ inline void erase ## AspectName ()\ {\ - this->template erase();\ + this->template eraseAspect();\ }\ \ inline std::unique_ptr< TypeName > release ## AspectName ()\ {\ - return this->template release();\ + return this->template releaseAspect();\ } //============================================================================== diff --git a/dart/common/detail/CompositeJoiner.h b/dart/common/detail/CompositeJoiner.h index 3bc374807447a..08f3b1499e7eb 100644 --- a/dart/common/detail/CompositeJoiner.h +++ b/dart/common/detail/CompositeJoiner.h @@ -82,8 +82,8 @@ DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(T*, CompositeJoiner, get, (), ()) DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(const T*, CompositeJoiner, get, () const, ()) DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(void, CompositeJoiner, set, (const T* aspect), (aspect)) DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(void, CompositeJoiner, set, (std::unique_ptr&& aspect), (std::move(aspect))) -DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(void, CompositeJoiner, erase, (), ()) -DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(std::unique_ptr, CompositeJoiner, release, (), ()) +DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(void, CompositeJoiner, eraseAspect, (), ()) +DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(std::unique_ptr, CompositeJoiner, releaseAspect, (), ()) //============================================================================== // Because this function requires a comma inside of its template argument list, @@ -91,12 +91,12 @@ DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(std::unique_ptr, CompositeJoin // implement it explicitly. template template -T* CompositeJoiner::create(Args&&... args) +T* CompositeJoiner::createAspect(Args&&... args) { if(Base1::template isSpecializedFor()) - return Base1::template create(std::forward(args)...); + return Base1::template createAspect(std::forward(args)...); - return Base2::template create(std::forward(args)...); + return Base2::template createAspect(std::forward(args)...); } //============================================================================== diff --git a/dart/common/detail/SpecializedForAspect.h b/dart/common/detail/SpecializedForAspect.h index 5f4d8036abe7c..4df189e75f466 100644 --- a/dart/common/detail/SpecializedForAspect.h +++ b/dart/common/detail/SpecializedForAspect.h @@ -244,7 +244,7 @@ template template T* SpecializedForAspect::_create(type, Args&&... args) { - return Composite::create(std::forward(args)...); + return Composite::createAspect(std::forward(args)...); } //============================================================================== @@ -269,7 +269,7 @@ template template void SpecializedForAspect::_erase(type) { - Composite::erase(); + Composite::eraseAspect(); } //============================================================================== @@ -291,7 +291,7 @@ template template std::unique_ptr SpecializedForAspect::_release(type) { - return Composite::release(); + return Composite::releaseAspect(); } //============================================================================== diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index 61b0eda039785..e2e2e4f9172ea 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -223,7 +223,7 @@ BodyNodeState::BodyNodeState( } //============================================================================== -BodyNodeUniqueProperties::BodyNodeUniqueProperties( +BodyNodeAspectProperties::BodyNodeAspectProperties( const std::string& name, const Inertia& _inertia, bool _isCollidable, double _frictionCoeff, @@ -238,29 +238,6 @@ BodyNodeUniqueProperties::BodyNodeUniqueProperties( // Do nothing } -//============================================================================== -BodyNodeExtendedProperties::BodyNodeExtendedProperties( - const BodyNodeProperties& standardProperties, - const NodeProperties& nodeProperties, - const CompositeProperties& compositeProperties) - : BodyNodeProperties(standardProperties), - mNodeProperties(nodeProperties), - mCompositeProperties(compositeProperties) -{ - // Do nothing -} - -//============================================================================== -BodyNodeExtendedProperties::BodyNodeExtendedProperties( - BodyNodeProperties&& standardProperties, - NodeProperties&& nodeProperties, - CompositeProperties&& aspectProperties) - : BodyNodeProperties(std::move(standardProperties)) -{ - mNodeProperties = std::move(nodeProperties); - mCompositeProperties = std::move(aspectProperties); -} - } // namespace detail //============================================================================== @@ -279,16 +256,6 @@ BodyNode::~BodyNode() delete mParentJoint; } -//============================================================================== -void BodyNode::setProperties(const ExtendedProperties& _properties) -{ - setProperties(static_cast(_properties)); - - setAllNodeProperties(_properties.mNodeProperties); - - setProperties(_properties.mCompositeProperties); -} - //============================================================================== void BodyNode::setAllNodeStates(const AllNodeStates& states) { @@ -330,7 +297,7 @@ void BodyNode::setProperties(const CompositeProperties& _properties) } //============================================================================== -void BodyNode::setProperties(const UniqueProperties& _properties) +void BodyNode::setProperties(const AspectProperties& _properties) { setAspectProperties(_properties); } @@ -374,35 +341,27 @@ BodyNode::Properties BodyNode::getBodyNodeProperties() const } //============================================================================== -BodyNode::ExtendedProperties BodyNode::getExtendedProperties() const -{ - return ExtendedProperties(getBodyNodeProperties(), - getAllNodeProperties(), - getCompositeProperties()); -} - -//============================================================================== -void BodyNode::copy(const BodyNode& _otherBodyNode) +void BodyNode::copy(const BodyNode& otherBodyNode) { - if(this == &_otherBodyNode) + if(this == &otherBodyNode) return; - setProperties(_otherBodyNode.getExtendedProperties()); + setCompositeProperties(otherBodyNode.getCompositeProperties()); } //============================================================================== -void BodyNode::copy(const BodyNode* _otherBodyNode) +void BodyNode::copy(const BodyNode* otherBodyNode) { - if(nullptr == _otherBodyNode) + if(nullptr == otherBodyNode) return; - copy(*_otherBodyNode); + copy(*otherBodyNode); } //============================================================================== -BodyNode& BodyNode::operator=(const BodyNode& _otherBodyNode) +BodyNode& BodyNode::operator=(const BodyNode& otherBodyNode) { - copy(_otherBodyNode); + copy(otherBodyNode); return *this; } @@ -792,7 +751,7 @@ bool BodyNode::moveTo(const SkeletonPtr& _newSkeleton, BodyNode* _newParent) SkeletonPtr BodyNode::split(const std::string& _skeletonName) { const SkeletonPtr& skel = - Skeleton::create(getSkeleton()->getSkeletonProperties()); + Skeleton::create(getSkeleton()->getAspectProperties()); skel->setName(_skeletonName); moveTo(skel, nullptr); return skel; @@ -830,7 +789,7 @@ SkeletonPtr BodyNode::copyAs(const std::string& _skeletonName, bool _recursive) const { const SkeletonPtr& skel = - Skeleton::create(getSkeleton()->getSkeletonProperties()); + Skeleton::create(getSkeleton()->getAspectProperties()); skel->setName(_skeletonName); copyTo(skel, nullptr, _recursive); return skel; @@ -1337,8 +1296,8 @@ BodyNode::BodyNode(BodyNode* _parentBodyNode, Joint* _parentJoint, if(_parentBodyNode) _parentBodyNode->addChildBodyNode(this); - create(); - create(); + createAspect(); + createAspect(); } //============================================================================== @@ -1382,6 +1341,9 @@ void BodyNode::init(const SkeletonPtr& _skeleton) mReferenceSkeleton = mSkeleton.lock(); } + setVersionDependentObject(mSkeleton.lock().get()); + mParentJoint->setVersionDependentObject(mSkeleton.lock().get()); + // Put the scope around this so that 'lock' releases the mutex immediately // after we're done with it { diff --git a/dart/dynamics/BodyNode.h b/dart/dynamics/BodyNode.h index efd13366f89eb..20ee72e09591f 100644 --- a/dart/dynamics/BodyNode.h +++ b/dart/dynamics/BodyNode.h @@ -103,18 +103,14 @@ class BodyNode : using AllNodeProperties = detail::AllNodeProperties; using NodePropertiesMap = detail::NodePropertiesMap; - using UniqueProperties = detail::BodyNodeUniqueProperties; + using AspectProperties = detail::BodyNodeAspectProperties; using Properties = detail::BodyNodeProperties; - using ExtendedProperties = detail::BodyNodeExtendedProperties; BodyNode(const BodyNode&) = delete; /// Destructor virtual ~BodyNode(); - /// Set the ExtendedProperties of this BodyNode - void setProperties(const ExtendedProperties& _properties); - /// Set the Node::State of all Nodes attached to this BodyNode void setAllNodeStates(const AllNodeStates& states); @@ -131,7 +127,7 @@ class BodyNode : void setProperties(const CompositeProperties& _properties); /// Set the UniqueProperties of this BodyNode - void setProperties(const UniqueProperties& _properties); + void setProperties(const AspectProperties& _properties); /// Set the AspectState of this BodyNode void setAspectState(const AspectState& state); @@ -142,15 +138,11 @@ class BodyNode : /// Get the Properties of this BodyNode Properties getBodyNodeProperties() const; - /// The the full extended Properties of this BodyNode, including the - /// Properties of its Aspects, its attached Nodes, and the BodyNode itself. - ExtendedProperties getExtendedProperties() const; - /// Copy the Properties of another BodyNode - void copy(const BodyNode& _otherBodyNode); + void copy(const BodyNode& otherBodyNode); /// Copy the Properties of another BodyNode - void copy(const BodyNode* _otherBodyNode); + void copy(const BodyNode* otherBodyNode); /// Same as copy(const BodyNode&) BodyNode& operator=(const BodyNode& _otherBodyNode); diff --git a/dart/dynamics/EndEffector.cpp b/dart/dynamics/EndEffector.cpp index 0932acbd82a77..5518fd74483c8 100644 --- a/dart/dynamics/EndEffector.cpp +++ b/dart/dynamics/EndEffector.cpp @@ -67,16 +67,6 @@ bool Support::isActive() const return mState.mActive; } -//============================================================================== -EndEffector::StateData::StateData( - const Eigen::Isometry3d& relativeTransform, - const common::Composite::State& aspectStates) - : mRelativeTransform(relativeTransform), - mAspectStates(aspectStates) -{ - // Do nothing -} - //============================================================================== EndEffector::UniqueProperties::UniqueProperties( const Eigen::Isometry3d& _defaultTransform) @@ -95,26 +85,6 @@ EndEffector::PropertiesData::PropertiesData( // Do nothing } -//============================================================================== -void EndEffector::setState(const StateData& _state) -{ - setRelativeTransform(_state.mRelativeTransform); - setCompositeState(_state.mAspectStates); -} - -//============================================================================== -void EndEffector::setState(StateData&& _state) -{ - setRelativeTransform(_state.mRelativeTransform); - setCompositeState(std::move(_state.mAspectStates)); -} - -//============================================================================== -EndEffector::StateData EndEffector::getEndEffectorState() const -{ - return StateData(mRelativeTf, getCompositeState()); -} - //============================================================================== void EndEffector::setProperties(const PropertiesData& _properties, bool _useNow) { @@ -137,13 +107,13 @@ EndEffector::PropertiesData EndEffector::getEndEffectorProperties() const } //============================================================================== -void EndEffector::copy(const EndEffector& _otherEndEffector) +void EndEffector::copy(const EndEffector& otherEndEffector) { - if(this == &_otherEndEffector) + if(this == &otherEndEffector) return; - setState(_otherEndEffector.getEndEffectorState()); - setProperties(_otherEndEffector.getEndEffectorProperties()); + setCompositeState(otherEndEffector.getCompositeState()); + setCompositeProperties(otherEndEffector.getCompositeProperties()); } //============================================================================== @@ -186,35 +156,6 @@ const std::string& EndEffector::getName() const return mEndEffectorP.mName; } -//============================================================================== -void EndEffector::setNodeState(const Node::State& otherState) -{ - setState(static_cast(otherState)); -} - -//============================================================================== -std::unique_ptr EndEffector::getNodeState() const -{ - return common::make_unique(getEndEffectorState()); -} - -//============================================================================== -void EndEffector::copyNodeStateTo(std::unique_ptr& outputState) const -{ - if(outputState) - { - EndEffector::State* state = - static_cast(outputState.get()); - - state->mRelativeTransform = mRelativeTf; - copyCompositeStateTo(state->mAspectStates); - } - else - { - outputState = getNodeState(); - } -} - //============================================================================== void EndEffector::setNodeProperties(const Node::Properties& otherProperties) { @@ -247,10 +188,13 @@ void EndEffector::copyNodePropertiesTo( } //============================================================================== -void EndEffector::setRelativeTransform(const Eigen::Isometry3d& _newRelativeTf) +void EndEffector::setRelativeTransform(const Eigen::Isometry3d& newRelativeTf) { - mRelativeTf = _newRelativeTf; - notifyTransformUpdate(); + if(newRelativeTf.matrix() == + FixedFrame::mAspectProperties.mRelativeTf.matrix()) + return; + + FixedFrame::setRelativeTransform(newRelativeTf); notifyJacobianUpdate(); notifyJacobianDerivUpdate(); } diff --git a/dart/dynamics/EndEffector.h b/dart/dynamics/EndEffector.h index e6711bf014bb0..8b1bd0b8e0c65 100644 --- a/dart/dynamics/EndEffector.h +++ b/dart/dynamics/EndEffector.h @@ -100,8 +100,8 @@ class Support final : }; class EndEffector final : - public virtual common::SpecializedForAspect, - public FixedFrame, + public common::CompositeJoiner< + common::SpecializedForAspect, common::Virtual >, public AccessoryNode, public TemplatedJacobianNode { @@ -110,24 +110,6 @@ class EndEffector final : friend class Skeleton; friend class BodyNode; - struct StateData - { - StateData(const Eigen::Isometry3d& relativeTransform = - Eigen::Isometry3d::Identity(), - const common::Composite::State& aspectStates = - common::Composite::State()); - - /// The current relative transform of the EndEffector - // TODO(MXG): Consider moving this to a FixedFrame::State struct and then - // inheriting that struct - Eigen::Isometry3d mRelativeTransform; - - /// The current states of the EndEffector's Aspects - common::Composite::State mAspectStates; - }; - - using State = Node::MakeState; - struct UniqueProperties { /// Name of this EndEffector @@ -164,13 +146,13 @@ class EndEffector final : //---------------------------------------------------------------------------- /// Set the State of this EndEffector. - void setState(const StateData& _state); +// void setState(const StateData& _state); /// Set the State of this EndEffector using move semantics - void setState(StateData&& _state); +// void setState(StateData&& _state); /// Get the State of this EndEffector - StateData getEndEffectorState() const; +// StateData getEndEffectorState() const; /// Set the Properties of this EndEffector. If _useNow is true, the current /// Transform will be set to the new default transform. @@ -199,15 +181,6 @@ class EndEffector final : // Documentation inherited const std::string& getName() const override; - // Documentation inherited - void setNodeState(const Node::State& otherState) override final; - - // Documentation inherited - std::unique_ptr getNodeState() const override final; - - // Documentation inherited - void copyNodeStateTo(std::unique_ptr& outputState) const override final; - // Documentation inherited void setNodeProperties(const Node::Properties& otherProperties) override final; @@ -219,7 +192,7 @@ class EndEffector final : std::unique_ptr& outputProperties) const override final; /// Set the current relative transform of this EndEffector - void setRelativeTransform(const Eigen::Isometry3d& _newRelativeTf); + void setRelativeTransform(const Eigen::Isometry3d& newRelativeTf) override; /// Set the default relative transform of this EndEffector. The relative /// transform of this EndEffector will be set to _newDefaultTf the next time diff --git a/dart/dynamics/FixedFrame.cpp b/dart/dynamics/FixedFrame.cpp index b5a0b51415c58..b12a5ba7b1125 100644 --- a/dart/dynamics/FixedFrame.cpp +++ b/dart/dynamics/FixedFrame.cpp @@ -39,17 +39,27 @@ namespace dart { namespace dynamics { +namespace detail { + +//============================================================================== +FixedFrameProperties::FixedFrameProperties(const Eigen::Isometry3d& relativeTf) + : mRelativeTf(relativeTf) +{ + // Do nothing +} + +} // namespace detail + //============================================================================== const Eigen::Vector6d FixedFrame::mZero = Eigen::Vector6d::Zero(); //============================================================================== -FixedFrame::FixedFrame(Frame* _refFrame, - const Eigen::Isometry3d& _relativeTransform) - : Entity(_refFrame, false), - Frame(_refFrame), - mRelativeTf(_relativeTransform) +FixedFrame::FixedFrame(Frame* refFrame, + const Eigen::Isometry3d& relativeTransform) + : Entity(refFrame, false), + Frame(refFrame) { - // Do nothing + create(AspectProperties(relativeTransform)); } //============================================================================== @@ -58,10 +68,27 @@ FixedFrame::~FixedFrame() // Do nothing. The inherited destructors will do all the necessary cleanup. } +//============================================================================== +void FixedFrame::setAspectProperties(const AspectProperties& properties) +{ + setRelativeTransform(properties.mRelativeTf); +} + +//============================================================================== +void FixedFrame::setRelativeTransform(const Eigen::Isometry3d& transform) +{ + if(transform.matrix() == mAspectProperties.mRelativeTf.matrix()) + return; + + mAspectProperties.mRelativeTf = transform; + notifyTransformUpdate(); + incrementVersion(); +} + //============================================================================== const Eigen::Isometry3d& FixedFrame::getRelativeTransform() const { - return mRelativeTf; + return mAspectProperties.mRelativeTf; } //============================================================================== diff --git a/dart/dynamics/FixedFrame.h b/dart/dynamics/FixedFrame.h index fda83e58dcd35..2f059e5e58a98 100644 --- a/dart/dynamics/FixedFrame.h +++ b/dart/dynamics/FixedFrame.h @@ -37,27 +37,47 @@ #ifndef DART_DYNAMICS_FIXEDFRAME_H_ #define DART_DYNAMICS_FIXEDFRAME_H_ -#include "Frame.h" +#include "dart/dynamics/Frame.h" +#include "dart/common/EmbeddedAspect.h" +#include "dart/common/VersionCounter.h" namespace dart { namespace dynamics { +namespace detail { +struct FixedFrameProperties +{ + Eigen::Isometry3d mRelativeTf; + + FixedFrameProperties( + const Eigen::Isometry3d& relativeTf = Eigen::Isometry3d::Identity()); +}; +} // namespace detail + /// The FixedFrame class represents a Frame with zero relative velocity and /// zero relative acceleration. It does not move within its parent Frame after /// its relative transform is set. However, classes that inherit the FixedFrame /// class may alter its relative transform or change what its parent Frame is. -class FixedFrame : public virtual Frame +class FixedFrame : + public virtual Frame, + public virtual common::VersionCounter, + public common::EmbedProperties { public: /// Constructor - explicit FixedFrame( - Frame* _refFrame, - const Eigen::Isometry3d& _relativeTransform = + explicit FixedFrame(Frame* refFrame, + const Eigen::Isometry3d& relativeTransform = Eigen::Isometry3d::Identity()); /// Destructor virtual ~FixedFrame(); + /// Set the AspectProperties of this FixedFrame + void setAspectProperties(const AspectProperties& properties); + + /// Set the relative transform of this FixedFrame + virtual void setRelativeTransform(const Eigen::Isometry3d& transform); + // Documentation inherited const Eigen::Isometry3d& getRelativeTransform() const override; @@ -74,9 +94,6 @@ class FixedFrame : public virtual Frame const Eigen::Vector6d& getPartialAcceleration() const override; protected: - /// Relative Transform of this Frame - DEPRECATED(6.0) - Eigen::Isometry3d mRelativeTf; /// Used for Relative Velocity and Relative Acceleration of this Frame static const Eigen::Vector6d mZero; diff --git a/dart/dynamics/Joint.cpp b/dart/dynamics/Joint.cpp index 122c9753298c1..37dfbb9846da4 100644 --- a/dart/dynamics/Joint.cpp +++ b/dart/dynamics/Joint.cpp @@ -188,24 +188,6 @@ const std::string& Joint::getName() const return mAspectProperties.mName; } -//============================================================================== -size_t Joint::incrementVersion() -{ - if(const auto& skel = getSkeleton()) - return skel->incrementVersion(); - - return 0; -} - -//============================================================================== -size_t Joint::getVersion() const -{ - if(const auto& skel = getSkeleton()) - return skel->getVersion(); - - return 0; -} - //============================================================================== void Joint::setActuatorType(Joint::ActuatorType _actuatorType) { diff --git a/dart/dynamics/Joint.h b/dart/dynamics/Joint.h index 9faa748e9221c..c143899fb730a 100644 --- a/dart/dynamics/Joint.h +++ b/dart/dynamics/Joint.h @@ -137,12 +137,6 @@ class Joint : public virtual common::Subject, /// Get joint name const std::string& getName() const; - /// Increments Skeleton version number - size_t incrementVersion() override; - - /// Gets the Skeleton version number - size_t getVersion() const override; - /// Gets a string representing the joint type virtual const std::string& getType() const = 0; diff --git a/dart/dynamics/Node.cpp b/dart/dynamics/Node.cpp index 100b35abcc2a3..489789b32240a 100644 --- a/dart/dynamics/Node.cpp +++ b/dart/dynamics/Node.cpp @@ -138,24 +138,6 @@ std::shared_ptr Node::getSkeleton() const return mBodyNode->getSkeleton(); } -//============================================================================== -size_t Node::incrementVersion() -{ - if(const SkeletonPtr& skel = getSkeleton()) - return skel->incrementVersion(); - - return 0; -} - -//============================================================================== -size_t Node::getVersion() const -{ - if(const ConstSkeletonPtr& skel = getSkeleton()) - return skel->getVersion(); - - return 0; -} - //============================================================================== std::shared_ptr Node::getOrCreateDestructor() { @@ -182,6 +164,8 @@ Node::Node(BodyNode* _bn) REPORT_INVALID_NODE(Node); return; } + + setVersionDependentObject(mBodyNode); } //============================================================================== diff --git a/dart/dynamics/Node.h b/dart/dynamics/Node.h index dfbf4ae4fe871..7e9309fff0143 100644 --- a/dart/dynamics/Node.h +++ b/dart/dynamics/Node.h @@ -181,14 +181,6 @@ class Node : /// Return the Skeleton that this Node is attached to virtual std::shared_ptr getSkeleton() const; - /// Increment the version of this Node (by default, this will increment the - /// version number of the Skeleton). - size_t incrementVersion() override; - - /// Get the version of this Node (by default, this will get the version - /// number of the Skeleton). - size_t getVersion() const override; - private: std::shared_ptr getOrCreateDestructor(); diff --git a/dart/dynamics/ShapeFrame.cpp b/dart/dynamics/ShapeFrame.cpp index 0f2d82102a74e..a4d6dc09d295f 100644 --- a/dart/dynamics/ShapeFrame.cpp +++ b/dart/dynamics/ShapeFrame.cpp @@ -184,16 +184,14 @@ DynamicsAspect::DynamicsAspect( //============================================================================== ShapeFrame::UniqueProperties::UniqueProperties(const ShapePtr& shape) - : mShape(shape), - mVersion(0) + : mShape(shape) { // Do nothing } //============================================================================== ShapeFrame::UniqueProperties::UniqueProperties(ShapePtr&& shape) - : mShape(std::move(shape)), - mVersion(0) + : mShape(std::move(shape)) { // Do nothing } @@ -262,18 +260,6 @@ void ShapeFrame::draw(renderer::RenderInterface* ri, ri->popMatrix(); } -//============================================================================== -size_t ShapeFrame::incrementVersion() -{ - return ++ShapeFrame::mAspectProperties.mVersion; -} - -//============================================================================== -size_t ShapeFrame::getVersion() const -{ - return ShapeFrame::mAspectProperties.mVersion; -} - //============================================================================== ShapeFrame::ShapeFrame(Frame* parent, const Properties& properties) : common::Composite(), diff --git a/dart/dynamics/ShapeFrame.h b/dart/dynamics/ShapeFrame.h index b0ffd201b24e1..52512112a7038 100644 --- a/dart/dynamics/ShapeFrame.h +++ b/dart/dynamics/ShapeFrame.h @@ -237,8 +237,6 @@ class ShapeFrame : /// Shape pointer ShapePtr mShape; - size_t mVersion; - /// Composed constructor UniqueProperties(const ShapePtr& shape = nullptr); @@ -282,12 +280,6 @@ class ShapeFrame : const Eigen::Vector4d& color = Eigen::Vector4d::Ones(), bool useDefaultColor = true) const; - // Documentation inherited - size_t incrementVersion() override; - - // Documentation inherited - size_t getVersion() const override; - protected: /// Constructor diff --git a/dart/dynamics/ShapeNode.cpp b/dart/dynamics/ShapeNode.cpp index e2e6274f7a212..d793310a87c0f 100644 --- a/dart/dynamics/ShapeNode.cpp +++ b/dart/dynamics/ShapeNode.cpp @@ -95,7 +95,7 @@ void ShapeNode::setProperties(const ShapeNode::UniqueProperties& properties) //============================================================================== const ShapeNode::Properties ShapeNode::getShapeNodeProperties() const { - return Properties(getAspectProperties(), mShapeNodeP, + return Properties(ShapeFrame::getAspectProperties(), mShapeNodeP, getCompositeProperties()); } @@ -148,39 +148,20 @@ const std::string& ShapeNode::getName() const return mShapeNodeP.mName; } -//============================================================================== -size_t ShapeNode::incrementVersion() -{ - ++ShapeFrame::mAspectProperties.mVersion; - if(const SkeletonPtr& skel = getSkeleton()) - skel->incrementVersion(); - - return ShapeFrame::mAspectProperties.mVersion; -} - -//============================================================================== -size_t ShapeNode::getVersion() const -{ - return ShapeFrame::mAspectProperties.mVersion; -} - //============================================================================== void ShapeNode::setRelativeTransform(const Eigen::Isometry3d& transform) { - if(transform.matrix() == mRelativeTf.matrix()) + if(transform.matrix() == FixedFrame::mAspectProperties.mRelativeTf.matrix()) return; - const Eigen::Isometry3d oldTransform = mRelativeTf; + const Eigen::Isometry3d oldTransform = getRelativeTransform(); - mRelativeTf = transform; - mShapeNodeP.mRelativeTransform = transform; - notifyTransformUpdate(); + FixedFrame::setRelativeTransform(transform); notifyJacobianUpdate(); notifyJacobianDerivUpdate(); - incrementVersion(); - mRelativeTransformUpdatedSignal.raise(this, oldTransform, - mShapeNodeP.mRelativeTransform); + mRelativeTransformUpdatedSignal.raise( + this, oldTransform, getRelativeTransform()); } //============================================================================== @@ -338,7 +319,7 @@ ShapeNode::ShapeNode(BodyNode* bodyNode, const Properties& properties) : Entity(ConstructFrame), Frame(bodyNode), FixedFrame(bodyNode), - ShapeFrame(bodyNode), + detail::ShapeNodeCompositeBase(common::NoArg, bodyNode), TemplatedJacobianNode(bodyNode), mShapeUpdatedSignal(ShapeUpdatedSignal()), mRelativeTransformUpdatedSignal(RelativeTransformUpdatedSignal()), @@ -355,7 +336,7 @@ ShapeNode::ShapeNode(BodyNode* bodyNode, : Entity(ConstructFrame), Frame(bodyNode), FixedFrame(bodyNode), - ShapeFrame(bodyNode), + detail::ShapeNodeCompositeBase(common::NoArg, bodyNode), TemplatedJacobianNode(bodyNode), mShapeUpdatedSignal(ShapeUpdatedSignal()), mRelativeTransformUpdatedSignal(RelativeTransformUpdatedSignal()), diff --git a/dart/dynamics/ShapeNode.h b/dart/dynamics/ShapeNode.h index 463dc4013beb9..48eb639ffcfcb 100644 --- a/dart/dynamics/ShapeNode.h +++ b/dart/dynamics/ShapeNode.h @@ -48,15 +48,22 @@ namespace dart { namespace dynamics { +namespace detail { + +using ShapeNodeCompositeBase = common::CompositeJoiner< + common::Virtual, ShapeFrame>; + +} // namespace detail + class VisualAspect; class CollisionAspect; class DynamicsAspect; class ShapeFrame; -class ShapeNode : public virtual FixedFrame, - public ShapeFrame, - public AccessoryNode, - public TemplatedJacobianNode +class ShapeNode : + public detail::ShapeNodeCompositeBase, + public AccessoryNode, + public TemplatedJacobianNode { public: @@ -140,14 +147,8 @@ class ShapeNode : public virtual FixedFrame, // Documentation inherited const std::string& getName() const override; - // Documentation inherited - size_t incrementVersion() override; - - // Documentation inherited - size_t getVersion() const override; - /// Set transformation of this shape node relative to the parent frame - void setRelativeTransform(const Eigen::Isometry3d& transform); + void setRelativeTransform(const Eigen::Isometry3d& transform) override; /// Set rotation of this shape node relative to the parent frame void setRelativeRotation(const Eigen::Matrix3d& rotation); @@ -266,6 +267,7 @@ class ShapeNode : public virtual FixedFrame, protected: /// Properties of this ShapeNode + DEPRECATED(6.0) UniqueProperties mShapeNodeP; /// Cached Jacobian of this ShapeNode diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index 0eab1bca47321..69e045ef762ce 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -167,6 +167,25 @@ std::vector getAllMemberObjectData(const Owner* owner) return data; } +//============================================================================== +SkeletonAspectProperties::SkeletonAspectProperties( + const std::string& _name, + bool _isMobile, + const Eigen::Vector3d& _gravity, + double _timeStep, + bool _enabledSelfCollisionCheck, + bool _enableAdjacentBodyCheck, + size_t _version) + : mName(_name), + mIsMobile(_isMobile), + mGravity(_gravity), + mTimeStep(_timeStep), + mEnabledSelfCollisionCheck(_enabledSelfCollisionCheck), + mEnabledAdjacentBodyCheck(_enableAdjacentBodyCheck) +{ + // Do nothing +} + //============================================================================== void setAllBodyNodeStates(Skeleton* skel, const BodyNodeStateVector& states) { @@ -323,50 +342,16 @@ bool Skeleton::Configuration::operator!=(const Configuration& other) const return !(*this == other); } -//============================================================================== -Skeleton::Properties::Properties( - const std::string& _name, - bool _isMobile, - const Eigen::Vector3d& _gravity, - double _timeStep, - bool _enabledSelfCollisionCheck, - bool _enableAdjacentBodyCheck, - size_t _version) - : mName(_name), - mIsMobile(_isMobile), - mGravity(_gravity), - mTimeStep(_timeStep), - mEnabledSelfCollisionCheck(_enabledSelfCollisionCheck), - mEnabledAdjacentBodyCheck(_enableAdjacentBodyCheck), - mVersion(_version) -{ - // Do nothing -} - -//============================================================================== -Skeleton::ExtendedProperties::ExtendedProperties( - const BodyNodeExtendedProperties& bodyNodeProperties, - const JointExtendedProperties& jointProperties, - const std::vector& parentNames, - const AspectProperties& aspectProperties) - : mBodyNodeProperties(bodyNodeProperties), - mAspectPropertiesroperties(jointProperties), - mParentBodyNodeNames(parentNames), - mAspectProperties(aspectProperties) -{ - // Do nothing -} - //============================================================================== SkeletonPtr Skeleton::create(const std::string& _name) { - return create(Properties(_name)); + return create(AspectPropertiesData(_name)); } //============================================================================== -SkeletonPtr Skeleton::create(const Properties& _properties) +SkeletonPtr Skeleton::create(const AspectPropertiesData& properties) { - SkeletonPtr skel(new Skeleton(_properties)); + SkeletonPtr skel(new Skeleton(properties)); skel->setPtr(skel); return skel; } @@ -466,9 +451,8 @@ SkeletonPtr Skeleton::clone(const std::string& cloneName) const } } - skelClone->setProperties(getSkeletonProperties()); + skelClone->setProperties(getAspectProperties()); skelClone->setName(cloneName); - skelClone->mSkeletonP.mVersion = getVersion(); return skelClone; } @@ -536,59 +520,77 @@ Skeleton::Configuration Skeleton::getConfiguration( } //============================================================================== -void Skeleton::setProperties(const Properties& _properties) +void Skeleton::setProperties(const Properties& properties) { - setName(_properties.mName); - setMobile(_properties.mIsMobile); - setGravity(_properties.mGravity); - setTimeStep(_properties.mTimeStep); + setCompositeProperties(properties); +} - if(_properties.mEnabledSelfCollisionCheck) - enableSelfCollision(_properties.mEnabledAdjacentBodyCheck); +//============================================================================== +Skeleton::Properties Skeleton::getProperties() const +{ + return getCompositeProperties(); +} + +//============================================================================== +void Skeleton::setProperties(const AspectProperties& properties) +{ + setAspectProperties(properties); +} + +//============================================================================== +void Skeleton::setAspectProperties(const AspectProperties& properties) +{ + setName(properties.mName); + setMobile(properties.mIsMobile); + setGravity(properties.mGravity); + setTimeStep(properties.mTimeStep); + + if(properties.mEnabledSelfCollisionCheck) + enableSelfCollision(properties.mEnabledAdjacentBodyCheck); else disableSelfCollision(); } //============================================================================== -const Skeleton::Properties& Skeleton::getSkeletonProperties() const +const Skeleton::AspectProperties& Skeleton::getSkeletonProperties() const { - return mSkeletonP; + return mAspectProperties; } //============================================================================== const std::string& Skeleton::setName(const std::string& _name) { - if(_name == mSkeletonP.mName && !_name.empty()) - return mSkeletonP.mName; + if(_name == mAspectProperties.mName && !_name.empty()) + return mAspectProperties.mName; - const std::string oldName = mSkeletonP.mName; - mSkeletonP.mName = _name; + const std::string oldName = mAspectProperties.mName; + mAspectProperties.mName = _name; mNameMgrForBodyNodes.setManagerName( - "Skeleton::BodyNode | "+mSkeletonP.mName); + "Skeleton::BodyNode | "+mAspectProperties.mName); mNameMgrForSoftBodyNodes.setManagerName( - "Skeleton::SoftBodyNode | "+mSkeletonP.mName); + "Skeleton::SoftBodyNode | "+mAspectProperties.mName); mNameMgrForJoints.setManagerName( - "Skeleton::Joint | "+mSkeletonP.mName); + "Skeleton::Joint | "+mAspectProperties.mName); mNameMgrForDofs.setManagerName( - "Skeleton::DegreeOfFreedom | "+mSkeletonP.mName); + "Skeleton::DegreeOfFreedom | "+mAspectProperties.mName); mNameMgrForMarkers.setManagerName( - "Skeleton::Marker | "+mSkeletonP.mName); + "Skeleton::Marker | "+mAspectProperties.mName); for(auto& mgr : mNodeNameMgrMap) mgr.second.setManagerName( std::string("Skeleton::") + mgr.first.name() - + " | " + mSkeletonP.mName ); + + " | " + mAspectProperties.mName ); ConstMetaSkeletonPtr me = mPtr.lock(); - mNameChangedSignal.raise(me, oldName, mSkeletonP.mName); + mNameChangedSignal.raise(me, oldName, mAspectProperties.mName); - return mSkeletonP.mName; + return mAspectProperties.mName; } //============================================================================== const std::string& Skeleton::getName() const { - return mSkeletonP.mName; + return mAspectProperties.mName; } //============================================================================== @@ -647,46 +649,46 @@ const std::string& Skeleton::addEntryToMarkerNameMgr(Marker* _newMarker) //============================================================================== void Skeleton::enableSelfCollision(bool _enableAdjacentBodyCheck) { - mSkeletonP.mEnabledSelfCollisionCheck = true; - mSkeletonP.mEnabledAdjacentBodyCheck = _enableAdjacentBodyCheck; + mAspectProperties.mEnabledSelfCollisionCheck = true; + mAspectProperties.mEnabledAdjacentBodyCheck = _enableAdjacentBodyCheck; } //============================================================================== void Skeleton::disableSelfCollision() { - mSkeletonP.mEnabledSelfCollisionCheck = false; - mSkeletonP.mEnabledAdjacentBodyCheck = false; + mAspectProperties.mEnabledSelfCollisionCheck = false; + mAspectProperties.mEnabledAdjacentBodyCheck = false; } //============================================================================== bool Skeleton::isEnabledSelfCollisionCheck() const { - return mSkeletonP.mEnabledSelfCollisionCheck; + return mAspectProperties.mEnabledSelfCollisionCheck; } //============================================================================== bool Skeleton::isEnabledAdjacentBodyCheck() const { - return mSkeletonP.mEnabledAdjacentBodyCheck; + return mAspectProperties.mEnabledAdjacentBodyCheck; } //============================================================================== void Skeleton::setMobile(bool _isMobile) { - mSkeletonP.mIsMobile = _isMobile; + mAspectProperties.mIsMobile = _isMobile; } //============================================================================== bool Skeleton::isMobile() const { - return mSkeletonP.mIsMobile; + return mAspectProperties.mIsMobile; } //============================================================================== void Skeleton::setTimeStep(double _timeStep) { assert(_timeStep > 0.0); - mSkeletonP.mTimeStep = _timeStep; + mAspectProperties.mTimeStep = _timeStep; for(size_t i=0; i(); - Composite::create(); - setProperties(_properties); + createAspect(properties); + createAspect(); + createAspect(); } //============================================================================== @@ -2111,7 +2100,7 @@ void Skeleton::registerJoint(Joint* _newJoint) if (nullptr == _newJoint) { dterr << "[Skeleton::registerJoint] Error: Attempting to add a nullptr " - "Joint to the Skeleton named [" << mSkeletonP.mName << "]. Report " + "Joint to the Skeleton named [" << mAspectProperties.mName << "]. Report " "this as a bug!\n"; assert(false); return; @@ -2171,7 +2160,7 @@ void Skeleton::registerNode(Node* _newNode) if(mNodeNameMgrMap.end() == it) { mNodeNameMgrMap[info] = common::NameManager( - std::string("Skeleton::") + info.name() + " | " + mSkeletonP.mName, + std::string("Skeleton::") + info.name() + " | " + mAspectProperties.mName, info.name() ); it = mNodeNameMgrMap.find(info); @@ -2614,7 +2603,7 @@ void Skeleton::updateArticulatedInertia(size_t _tree) const for (std::vector::const_reverse_iterator it = cache.mBodyNodes.rbegin(); it != cache.mBodyNodes.rend(); ++it) { - (*it)->updateArtInertia(mSkeletonP.mTimeStep); + (*it)->updateArtInertia(mAspectProperties.mTimeStep); } cache.mDirty.mArticulatedInertia = false; @@ -2767,7 +2756,7 @@ void Skeleton::updateAugMassMatrix(size_t _treeIdx) const for (std::vector::const_reverse_iterator it = cache.mBodyNodes.rbegin(); it != cache.mBodyNodes.rend(); ++it) { - (*it)->aggregateAugMassMatrix(cache.mAugM, j, mSkeletonP.mTimeStep); + (*it)->aggregateAugMassMatrix(cache.mAugM, j, mAspectProperties.mTimeStep); size_t localDof = (*it)->mParentJoint->getNumDofs(); if (localDof > 0) { @@ -2959,7 +2948,7 @@ void Skeleton::updateInvAugMassMatrix(size_t _treeIdx) const for (std::vector::const_iterator it = cache.mBodyNodes.begin(); it != cache.mBodyNodes.end(); ++it) { - (*it)->aggregateInvAugMassMatrix(cache.mInvAugM, j, mSkeletonP.mTimeStep); + (*it)->aggregateInvAugMassMatrix(cache.mInvAugM, j, mAspectProperties.mTimeStep); size_t localDof = (*it)->mParentJoint->getNumDofs(); if (localDof > 0) { @@ -3090,7 +3079,7 @@ void Skeleton::updateGravityForces(size_t _treeIdx) const for (std::vector::const_reverse_iterator it = cache.mBodyNodes.rbegin(); it != cache.mBodyNodes.rend(); ++it) { - (*it)->aggregateGravityForceVector(cache.mG, mSkeletonP.mGravity); + (*it)->aggregateGravityForceVector(cache.mG, mAspectProperties.mGravity); } cache.mDirty.mGravityForces = false; @@ -3147,7 +3136,7 @@ void Skeleton::updateCoriolisAndGravityForces(size_t _treeIdx) const for (std::vector::const_reverse_iterator it = cache.mBodyNodes.rbegin(); it != cache.mBodyNodes.rend(); ++it) { - (*it)->aggregateCombinedVector(cache.mCg, mSkeletonP.mGravity); + (*it)->aggregateCombinedVector(cache.mCg, mAspectProperties.mGravity); } cache.mDirty.mCoriolisAndGravityForces = false; @@ -3283,7 +3272,7 @@ const Eigen::VectorXd& Skeleton::computeConstraintForces(DataCache& cache) const cache.mFc[i] += cache.mDofs[i]->getConstraintImpulse(); // Get force by dividing the impulse by the time step - cache.mFc = cache.mFc / mSkeletonP.mTimeStep; + cache.mFc = cache.mFc / mAspectProperties.mTimeStep; return cache.mFc; } @@ -3491,14 +3480,14 @@ void Skeleton::computeForwardDynamics() for (auto it = mSkelCache.mBodyNodes.rbegin(); it != mSkelCache.mBodyNodes.rend(); ++it) - (*it)->updateBiasForce(mSkeletonP.mGravity, mSkeletonP.mTimeStep); + (*it)->updateBiasForce(mAspectProperties.mGravity, mAspectProperties.mTimeStep); // Forward recursion for (auto& bodyNode : mSkelCache.mBodyNodes) { bodyNode->updateAccelerationFD(); bodyNode->updateTransmittedForceFD(); - bodyNode->updateJointForceFD(mSkeletonP.mTimeStep, true, true); + bodyNode->updateJointForceFD(mAspectProperties.mTimeStep, true, true); } } @@ -3515,8 +3504,8 @@ void Skeleton::computeInverseDynamics(bool _withExternalForces, for (auto it = mSkelCache.mBodyNodes.rbegin(); it != mSkelCache.mBodyNodes.rend(); ++it) { - (*it)->updateTransmittedForceID(mSkeletonP.mGravity, _withExternalForces); - (*it)->updateJointForceID(mSkeletonP.mTimeStep, + (*it)->updateTransmittedForceID(mAspectProperties.mGravity, _withExternalForces); + (*it)->updateJointForceID(mAspectProperties.mTimeStep, _withDampingForces, _withSpringForces); } @@ -3757,7 +3746,7 @@ void Skeleton::computeImpulseForwardDynamics() bodyNode->updateVelocityChangeFD(); bodyNode->updateTransmittedImpulse(); bodyNode->updateJointImpulseFD(); - bodyNode->updateConstrainedTerms(mSkeletonP.mTimeStep); + bodyNode->updateConstrainedTerms(mAspectProperties.mTimeStep); } } @@ -3784,7 +3773,7 @@ double Skeleton::getPotentialEnergy() const for (std::vector::const_iterator it = mSkelCache.mBodyNodes.begin(); it != mSkelCache.mBodyNodes.end(); ++it) { - PE += (*it)->getPotentialEnergy(mSkeletonP.mGravity); + PE += (*it)->getPotentialEnergy(mAspectProperties.mGravity); PE += (*it)->getParentJoint()->getPotentialEnergy(); } diff --git a/dart/dynamics/Skeleton.h b/dart/dynamics/Skeleton.h index 38454af9064d7..e4b4032048e1e 100644 --- a/dart/dynamics/Skeleton.h +++ b/dart/dynamics/Skeleton.h @@ -64,12 +64,15 @@ namespace dynamics { class Skeleton : public virtual common::VersionCounter, public MetaSkeleton, - public common::CompositeJoiner< - SkeletonSpecializedFor, - detail::SkeletonProxyAspects> + public SkeletonSpecializedFor, + public detail::SkeletonAspectBase { public: + using AspectPropertiesData = detail::SkeletonAspectProperties; + using AspectProperties = common::Aspect::MakeProperties; + using Properties = common::Composite::Properties; + enum ConfigFlag_t { CONFIG_NOTHING = 0, @@ -129,91 +132,6 @@ class Skeleton : bool operator!=(const Configuration& other) const; }; - - - struct State - { - - }; - - /// The Properties of this Skeleton which are independent of the components - /// within the Skeleton, such as its BodyNodes and Joints. This does not - /// include any Properties of the Skeleton's Aspects. - struct Properties - { - /// Name of the Skeleton - std::string mName; - - /// If the skeleton is not mobile, its dynamic effect is equivalent - /// to having infinite mass. If the configuration of an immobile skeleton is - /// manually changed, the collision results might not be correct. - bool mIsMobile; - - /// Gravity vector. - Eigen::Vector3d mGravity; - - /// Time step for implicit joint damping force. - double mTimeStep; - - /// True if self collision check is enabled. Use mEnabledAdjacentBodyCheck - /// to disable collision checks between adjacent bodies. - bool mEnabledSelfCollisionCheck; - - /// True if self collision check is enabled, including adjacent bodies. - /// Note: If mEnabledSelfCollisionCheck is false, then this value will be - /// ignored. - bool mEnabledAdjacentBodyCheck; - - /// Version number of the Skeleton. This will get incremented each time any - /// Property of the Skeleton or a component within the Skeleton is changed. - /// If you create a custom Aspect or Node, you must increment the Skeleton - /// version number each time one of its Properties is changed, or else the - /// machinery used to record Skeletons and Worlds might fail to capture its - /// Property changes. - size_t mVersion; - - /// Default constructor - Properties( - const std::string& _name = "Skeleton", - bool _isMobile = true, - const Eigen::Vector3d& _gravity = Eigen::Vector3d(0.0, 0.0, -9.81), - double _timeStep = 0.001, - bool _enabledSelfCollisionCheck = false, - bool _enableAdjacentBodyCheck = false, - size_t _version = 0); - }; - - using BodyNodeExtendedProperties = std::vector; - using JointExtendedProperties = std::vector; - using AspectProperties = common::Composite::Properties; - - /// The Properties of this Skeleton and everything within the Skeleton, - /// including Aspects and Nodes that are attached to the - struct ExtendedProperties : Properties - { - /// Properties of all the BodyNodes in this Skeleton - BodyNodeExtendedProperties mBodyNodeProperties; - - /// Properties of all the Joints in this Skeleton - JointExtendedProperties mAspectPropertiesroperties; - - /// A list of the name of the parent of each BodyNode in this Skeleton. This - /// allows the layout of the Skeleton to be reconstructed. - std::vector mParentBodyNodeNames; - - /// Properties of any Aspects that are attached directly to this Skeleton - /// object (does NOT include Aspects that are attached to BodyNodes or Joints - /// within this Skeleton). - AspectProperties mAspectProperties; - - /// Default constructor - ExtendedProperties( - const BodyNodeExtendedProperties& bodyNodeProperties = BodyNodeExtendedProperties(), - const JointExtendedProperties& jointProperties = JointExtendedProperties(), - const std::vector& parentNames = std::vector(), - const AspectProperties& aspectProperties = AspectProperties()); - }; - //---------------------------------------------------------------------------- /// \{ \name Constructor and Destructor //---------------------------------------------------------------------------- @@ -222,7 +140,7 @@ class Skeleton : static SkeletonPtr create(const std::string& _name="Skeleton"); /// Create a new Skeleton inside of a shared_ptr - static SkeletonPtr create(const Properties& _properties); + static SkeletonPtr create(const AspectPropertiesData& properties); /// Get the shared_ptr that manages this Skeleton SkeletonPtr getPtr(); @@ -281,11 +199,21 @@ class Skeleton : /// \{ \name Properties //---------------------------------------------------------------------------- + /// Set all properties of this Skeleton + void setProperties(const Properties& properties); + + /// Get all properties of this Skeleton + Properties getProperties() const; + /// Set the Properties of this Skeleton - void setProperties(const Properties& _properties); + void setProperties(const AspectProperties& properties); /// Get the Properties of this Skeleton - const Properties& getSkeletonProperties() const; + DEPRECATED(6.0) + const AspectProperties& getSkeletonProperties() const; + + /// Set the AspectProperties of this Skeleton + void setAspectProperties(const AspectProperties& properties); /// Set name. const std::string& setName(const std::string& _name) override; @@ -328,13 +256,6 @@ class Skeleton : /// Get 3-dim gravitational acceleration. const Eigen::Vector3d& getGravity() const; - /// Increment the version number of this Skeleton and return the resulting - /// (new) version number. - size_t incrementVersion() override; - - /// Get the current version number of this Skeleton - size_t getVersion() const override; - /// \} //---------------------------------------------------------------------------- @@ -988,7 +909,7 @@ class Skeleton : struct DataCache; /// Constructor called by create() - Skeleton(const Properties& _properties); + Skeleton(const AspectPropertiesData& _properties); /// Setup this Skeleton with its shared_ptr void setPtr(const SkeletonPtr& _ptr); @@ -1160,9 +1081,6 @@ class Skeleton : protected: - /// Properties of this Skeleton - Properties mSkeletonP; - /// The resource-managing pointer to this Skeleton std::weak_ptr mPtr; diff --git a/dart/dynamics/detail/BodyNode.h b/dart/dynamics/detail/BodyNode.h index c6b5072f8c9f4..b39789d8a6da5 100644 --- a/dart/dynamics/detail/BodyNode.h +++ b/dart/dynamics/detail/BodyNode.h @@ -72,7 +72,7 @@ template SkeletonPtr BodyNode::split(const std::string& _skeletonName, const typename JointType::Properties& _joint) { - SkeletonPtr skel = Skeleton::create(getSkeleton()->getSkeletonProperties()); + SkeletonPtr skel = Skeleton::create(getSkeleton()->getAspectProperties()); skel->setName(_skeletonName); moveTo(skel, nullptr, _joint); return skel; @@ -117,7 +117,7 @@ template SkeletonPtr BodyNode::copyAs(const std::string& _skeletonName, const typename JointType::Properties& _joint, bool _recursive) const { - SkeletonPtr skel = Skeleton::create(getSkeleton()->getSkeletonProperties()); + SkeletonPtr skel = Skeleton::create(getSkeleton()->getAspectProperties()); skel->setName(_skeletonName); copyTo(skel, nullptr, _joint, _recursive); return skel; diff --git a/dart/dynamics/detail/BodyNodeProperties.h b/dart/dynamics/detail/BodyNodeProperties.h index cfb93899426f9..d751645025a28 100644 --- a/dart/dynamics/detail/BodyNodeProperties.h +++ b/dart/dynamics/detail/BodyNodeProperties.h @@ -68,7 +68,7 @@ struct BodyNodeState }; //============================================================================== -struct BodyNodeUniqueProperties +struct BodyNodeAspectProperties { /// Name of the Entity std::string mName; @@ -92,7 +92,7 @@ struct BodyNodeUniqueProperties std::vector mMarkerProperties; /// Constructor - BodyNodeUniqueProperties( + BodyNodeAspectProperties( const std::string& name = "BodyNode", const Inertia& _inertia = Inertia(), bool _isCollidable = true, @@ -100,39 +100,13 @@ struct BodyNodeUniqueProperties double _restitutionCoeff = DART_DEFAULT_RESTITUTION_COEFF, bool _gravityMode = true); - virtual ~BodyNodeUniqueProperties() = default; + virtual ~BodyNodeAspectProperties() = default; // To get byte-aligned Eigen vectors EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -using BodyNodeProperties = BodyNodeUniqueProperties; - -//============================================================================== -struct BodyNodeExtendedProperties : BodyNodeProperties -{ - using NodePropertiesVector = common::CloneableVector< std::unique_ptr >; - using NodePropertiesMap = std::map< std::type_index, std::unique_ptr >; - using NodeProperties = common::CloneableMap; - using CompositeProperties = common::Composite::Properties; - - /// Composed constructor - BodyNodeExtendedProperties(const BodyNodeProperties& standardProperties = BodyNodeProperties(), - const NodeProperties& nodeProperties = NodeProperties(), - const CompositeProperties& compositeProperties = CompositeProperties()); - - /// Composed move constructor - BodyNodeExtendedProperties( - BodyNodeProperties&& standardProperties, - NodeProperties&& nodeProperties, - CompositeProperties&& aspectProperties); - - /// Properties of all the Nodes attached to this BodyNode - NodeProperties mNodeProperties; - - /// Properties of all the Aspects attached to this BodyNode - CompositeProperties mCompositeProperties; -}; +using BodyNodeProperties = BodyNodeAspectProperties; //============================================================================== using NodeTypeStateVector = common::CloneableVector< std::unique_ptr >; @@ -173,7 +147,7 @@ using NodeVectorProxyAspect = common::ProxyStateAndPropertiesAspect >; } // namespace detail diff --git a/dart/dynamics/detail/Node.h b/dart/dynamics/detail/Node.h index 7c6ee652c2c45..a3723fbd80ac7 100644 --- a/dart/dynamics/detail/Node.h +++ b/dart/dynamics/detail/Node.h @@ -202,7 +202,7 @@ void AccessoryNode::reattach() mSkelCache.mNodeMap[typeid( NodeName )] = std::vector(); \ m ## PluralName ## Iterator = mSkelCache.mNodeMap.find(typeid( NodeName )); \ mNodeNameMgrMap[typeid( NodeName )] = dart::common::NameManager( \ - std::string("Skeleton::") + #NodeName + " | " + mSkeletonP.mName ); \ + std::string("Skeleton::") + #NodeName + " | " + mAspectProperties.mName ); \ mNameMgrFor ## PluralName = &mNodeNameMgrMap.find(typeid( NodeName) )->second; //============================================================================== diff --git a/dart/dynamics/detail/SkeletonAspect.h b/dart/dynamics/detail/SkeletonAspect.h index c5aff44d94214..f1c722150a529 100644 --- a/dart/dynamics/detail/SkeletonAspect.h +++ b/dart/dynamics/detail/SkeletonAspect.h @@ -39,7 +39,9 @@ #include "dart/common/Composite.h" #include "dart/common/ProxyAspect.h" +#include "dart/common/EmbeddedAspect.h" #include "dart/common/RequiresAspect.h" +#include namespace dart { namespace dynamics { @@ -48,6 +50,46 @@ class Skeleton; namespace detail { +//============================================================================== +/// The Properties of this Skeleton which are independent of the components +/// within the Skeleton, such as its BodyNodes and Joints. This does not +/// include any Properties of the Skeleton's Aspects. +struct SkeletonAspectProperties +{ + /// Name of the Skeleton + std::string mName; + + /// If the skeleton is not mobile, its dynamic effect is equivalent + /// to having infinite mass. If the configuration of an immobile skeleton is + /// manually changed, the collision results might not be correct. + bool mIsMobile; + + /// Gravity vector. + Eigen::Vector3d mGravity; + + /// Time step for implicit joint damping force. + double mTimeStep; + + /// True if self collision check is enabled. Use mEnabledAdjacentBodyCheck + /// to disable collision checks between adjacent bodies. + bool mEnabledSelfCollisionCheck; + + /// True if self collision check is enabled, including adjacent bodies. + /// Note: If mEnabledSelfCollisionCheck is false, then this value will be + /// ignored. + bool mEnabledAdjacentBodyCheck; + + /// Default constructor + SkeletonAspectProperties( + const std::string& _name = "Skeleton", + bool _isMobile = true, + const Eigen::Vector3d& _gravity = Eigen::Vector3d(0.0, 0.0, -9.81), + double _timeStep = 0.001, + bool _enabledSelfCollisionCheck = false, + bool _enableAdjacentBodyCheck = false, + size_t _version = 0); +}; + //============================================================================== using BodyNodeStateVector = std::vector; using BodyNodePropertiesVector = std::vector; @@ -112,6 +154,10 @@ using JointVectorProxyAspect = common::ProxyStateAndPropertiesAspect; +//============================================================================== +using SkeletonAspectBase = common::EmbedPropertiesOnTopOf< + Skeleton, SkeletonAspectProperties, SkeletonProxyAspects>; + } // namespace detail } // namespace dynamics } // namespace dart diff --git a/unittests/testAspect.cpp b/unittests/testAspect.cpp index 32a11088a4e6f..da800c9f9fe64 100644 --- a/unittests/testAspect.cpp +++ b/unittests/testAspect.cpp @@ -175,7 +175,7 @@ class InheritAndEmbedStateComposite : InheritAndEmbedStateComposite() { - create(); + createAspect(); } void setAspectState(const AspectState& state) { mAspectState = state; } @@ -191,7 +191,7 @@ class InheritAndEmbedPropertiesComposite : InheritAndEmbedPropertiesComposite() { - create(); + createAspect(); } void setAspectProperties(const AspectProperties& properties) @@ -211,7 +211,7 @@ class InheritAndEmbedStateAndPropertiesComposite : InheritAndEmbedStateAndPropertiesComposite() { - create(); + createAspect(); } void setAspectState(const AspectState& state) { mAspectState = state; } @@ -402,12 +402,12 @@ TEST(Aspect, Generic) EXPECT_TRUE( mgr.get() == nullptr ); - sub_ptr aspect = mgr.create(); + sub_ptr aspect = mgr.createAspect(); GenericAspect* rawAspect = aspect; EXPECT_FALSE( nullptr == aspect ); EXPECT_TRUE( mgr.get() == aspect ); - GenericAspect* newAspect = mgr.create(); + GenericAspect* newAspect = mgr.createAspect(); EXPECT_FALSE( nullptr == newAspect ); EXPECT_TRUE( nullptr == aspect ); EXPECT_FALSE( rawAspect == newAspect ); @@ -474,12 +474,12 @@ TEST(Aspect, Releasing) EXPECT_TRUE( sender.get() == nullptr ); EXPECT_TRUE( receiver.get() == nullptr ); - sub_ptr aspect = sender.create(); + sub_ptr aspect = sender.createAspect(); EXPECT_TRUE( sender.get() == aspect ); EXPECT_TRUE( receiver.get() == nullptr ); - receiver.set(sender.release()); + receiver.set(sender.releaseAspect()); EXPECT_FALSE( nullptr == aspect ); @@ -493,7 +493,7 @@ TEST(Aspect, Releasing) EXPECT_TRUE( sender.get() == aspect ); EXPECT_TRUE( receiver.get() == nullptr ); - sender.release(); + sender.releaseAspect(); EXPECT_TRUE( nullptr == aspect ); EXPECT_TRUE( sender.get() == nullptr ); @@ -505,13 +505,13 @@ TEST(Aspect, Releasing) EXPECT_TRUE( sender.get() == nullptr ); // EXPECT_TRUE( receiver.getSpecializedAspect() == nullptr ); - sub_ptr spec = sender.create(); + sub_ptr spec = sender.createAspect(); EXPECT_TRUE( sender.get() == spec ); // EXPECT_TRUE( receiver.getSpecializedAspect() == nullptr ); // receiver.setSpecializedAspect(sender.release()); - receiver.set(sender.release()); + receiver.set(sender.releaseAspect()); EXPECT_FALSE( nullptr == spec ); @@ -526,7 +526,7 @@ TEST(Aspect, Releasing) EXPECT_TRUE( sender.get() == spec ); // EXPECT_TRUE( receiver.getSpecializedAspect() == nullptr ); - sender.release(); + sender.releaseAspect(); EXPECT_TRUE( nullptr == spec ); EXPECT_TRUE( sender.get() == nullptr ); @@ -539,7 +539,7 @@ TEST(Aspect, Releasing) // The set() methods being used in this block of code will make clones of // the aspects that are being passed in instead of transferring their // ownership like the previous blocks of code were. - sub_ptr aspect = sender.create(); + sub_ptr aspect = sender.createAspect(); // This should create a copy of the GenericAspect without taking the aspect // away from 'sender' @@ -608,16 +608,16 @@ void makePropertiesDifferent(AspectT* aspect, const AspectT* differentFrom) TEST(Aspect, StateAndProperties) { Composite mgr1; - mgr1.create(); - mgr1.create(); - mgr1.create(); - mgr1.create(); + mgr1.createAspect(); + mgr1.createAspect(); + mgr1.createAspect(); + mgr1.createAspect(); Composite mgr2; - mgr2.create(); - mgr2.create(); + mgr2.createAspect(); + mgr2.createAspect(); - mgr1.create(); + mgr1.createAspect(); // ---- Test state transfer ---- @@ -685,14 +685,14 @@ TEST(Aspect, Construction) { Composite mgr; - mgr.create(); + mgr.createAspect(); double s = dart::math::random(0, 100); - mgr.create(s); + mgr.createAspect(s); EXPECT_EQ(mgr.get()->mState.val, s); double p = dart::math::random(0, 100); - mgr.create(dart::math::random(0, 100), p); + mgr.createAspect(dart::math::random(0, 100), p); EXPECT_EQ(mgr.get()->mProperties.val, p); } @@ -770,12 +770,12 @@ TEST(Aspect, Duplication) { Composite mgr1, mgr2; - mgr1.create(); - mgr1.create(); - mgr1.create(); - mgr1.create(); + mgr1.createAspect(); + mgr1.createAspect(); + mgr1.createAspect(); + mgr1.createAspect(); - mgr2.create(); + mgr2.createAspect(); mgr2.duplicateAspects(&mgr1); From bcec8afb85c8ebfc2b1ab5897871349f4dba0b1b Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Wed, 6 Apr 2016 10:53:23 -0400 Subject: [PATCH 33/62] eliminating duplication of code --- dart/common/Cloneable.h | 10 +- dart/common/SpecializedForAspect.h | 18 +- dart/common/VersionCounter.cpp | 8 + dart/common/VersionCounter.h | 3 + dart/common/detail/Cloneable.h | 44 +++-- dart/common/detail/SpecializedForAspect.h | 24 +-- dart/dynamics/BodyNode.cpp | 1 + dart/dynamics/CompositeNode.h | 123 ++++++++++++ dart/dynamics/EndEffector.cpp | 227 +--------------------- dart/dynamics/EndEffector.h | 159 ++------------- dart/dynamics/EntityNode.h | 82 ++++++++ dart/dynamics/FixedFrame.cpp | 17 +- dart/dynamics/FixedFrame.h | 6 + dart/dynamics/FixedJacobianNode.cpp | 218 +++++++++++++++++++++ dart/dynamics/FixedJacobianNode.h | 184 ++++++++++++++++++ dart/dynamics/Node.cpp | 3 +- dart/dynamics/Node.h | 4 + dart/dynamics/ShapeNode.cpp | 188 +----------------- dart/dynamics/ShapeNode.h | 128 +----------- dart/dynamics/detail/CompositeNode.h | 97 +++++++++ dart/dynamics/detail/EntityNode.h | 85 ++++++++ dart/dynamics/detail/EntityNodeAspect.h | 101 ++++++++++ unittests/testAspect.cpp | 20 +- unittests/testSkeleton.cpp | 2 +- 24 files changed, 1037 insertions(+), 715 deletions(-) create mode 100644 dart/dynamics/CompositeNode.h create mode 100644 dart/dynamics/EntityNode.h create mode 100644 dart/dynamics/FixedJacobianNode.cpp create mode 100644 dart/dynamics/FixedJacobianNode.h create mode 100644 dart/dynamics/detail/CompositeNode.h create mode 100644 dart/dynamics/detail/EntityNode.h create mode 100644 dart/dynamics/detail/EntityNodeAspect.h diff --git a/dart/common/Cloneable.h b/dart/common/Cloneable.h index 9b25018783580..638f517430826 100644 --- a/dart/common/Cloneable.h +++ b/dart/common/Cloneable.h @@ -217,7 +217,7 @@ class ProxyCloneable : public Base /// Aspect::State and Aspect::Properties to be handled in a semantically /// palatable way. template -class CloneableMap final +class CloneableMap { public: @@ -248,6 +248,12 @@ class CloneableMap final /// Map-based move assignment operator CloneableMap& operator=(MapType&& otherMap); + /// Copy the contents of another cloneable map into this one. + void copy(const CloneableMap& otherMap); + + /// Copy the contents of a map into this one. + void copy(const MapType& otherMap); + /// Get the map that is being held MapType& getMap(); @@ -264,7 +270,7 @@ class CloneableMap final /// The CloneableVector type wraps a std::vector of an Cloneable type allowing /// it to be handled by an CloneableMapHolder template -class CloneableVector final +class CloneableVector { public: diff --git a/dart/common/SpecializedForAspect.h b/dart/common/SpecializedForAspect.h index 6a7f038dcd13b..828ba15410ca6 100644 --- a/dart/common/SpecializedForAspect.h +++ b/dart/common/SpecializedForAspect.h @@ -91,17 +91,17 @@ class SpecializedForAspect : public virtual Composite /// Construct an Aspect inside of this Composite template - T* create(Args&&... args); + T* createAspect(Args&&... args); /// Remove an Aspect from this Composite template - void erase(); + void eraseAspect(); /// Remove an Aspect from this Composite, but return its unique_ptr instead /// of letting it be deleted. This allows you to safely use move semantics to /// transfer an Aspect between two Composites. template - std::unique_ptr release(); + std::unique_ptr releaseAspect(); /// Check if this Composite is specialized for a specific type of Aspect template @@ -152,25 +152,25 @@ class SpecializedForAspect : public virtual Composite /// Redirect to Composite::create() template - T* _create(type, Args&&... args); + T* _createAspect(type, Args&&... args); /// Specialized implementation of create() template - SpecAspect* _create(type, Args&&... args); + SpecAspect* _createAspect(type, Args&&... args); /// Redirect to Composite::erase() template - void _erase(type); + void _eraseAspect(type); /// Specialized implementation of erase() - void _erase(type); + void _eraseAspect(type); /// Redirect to Composite::release() template - std::unique_ptr _release(type); + std::unique_ptr _releaseAspect(type); /// Specialized implementation of release() - std::unique_ptr _release(type); + std::unique_ptr _releaseAspect(type); /// Return false template diff --git a/dart/common/VersionCounter.cpp b/dart/common/VersionCounter.cpp index 0ac2fa82ced6c..6eb8209c343d5 100644 --- a/dart/common/VersionCounter.cpp +++ b/dart/common/VersionCounter.cpp @@ -43,6 +43,14 @@ namespace dart { namespace common { +//============================================================================== +VersionCounter::VersionCounter() + : mVersion(0), + mDependent(nullptr) +{ + // Do nothing +} + //============================================================================== size_t VersionCounter::incrementVersion() { diff --git a/dart/common/VersionCounter.h b/dart/common/VersionCounter.h index d86431e066343..d3aa14ed4c37e 100644 --- a/dart/common/VersionCounter.h +++ b/dart/common/VersionCounter.h @@ -47,6 +47,9 @@ class VersionCounter { public: + /// Default constructor + VersionCounter(); + /// Increment the version for this object virtual size_t incrementVersion(); diff --git a/dart/common/detail/Cloneable.h b/dart/common/detail/Cloneable.h index 72b87f6fda53c..8a9ce1110a345 100644 --- a/dart/common/detail/Cloneable.h +++ b/dart/common/detail/Cloneable.h @@ -361,7 +361,7 @@ template CloneableMap::CloneableMap( const CloneableMap& otherHolder) { - *this = otherHolder; + copy(otherHolder); } //============================================================================== @@ -376,7 +376,7 @@ CloneableMap::CloneableMap( template CloneableMap::CloneableMap(const MapType& otherMap) { - *this = otherMap; + copy(otherMap); } //============================================================================== @@ -391,8 +391,7 @@ template CloneableMap& CloneableMap::operator=( const CloneableMap& otherHolder) { - *this = otherHolder.getMap(); - + copy(otherHolder); return *this; } @@ -410,6 +409,31 @@ CloneableMap& CloneableMap::operator=( template CloneableMap& CloneableMap::operator=( const MapType& otherMap) +{ + copy(otherMap); + return *this; +} + +//============================================================================== +template +CloneableMap& CloneableMap::operator=( + MapType&& otherHolder) +{ + mMap = std::move(otherHolder); + + return *this; +} + +//============================================================================== +template +void CloneableMap::copy(const CloneableMap& otherMap) +{ + copy(otherMap.getMap()); +} + +//============================================================================== +template +void CloneableMap::copy(const MapType& otherMap) { typename MapType::iterator receiver = mMap.begin(); typename MapType::const_iterator sender = otherMap.begin(); @@ -453,18 +477,6 @@ CloneableMap& CloneableMap::operator=( { mMap.erase(receiver++); } - - return *this; -} - -//============================================================================== -template -CloneableMap& CloneableMap::operator=( - MapType&& otherHolder) -{ - mMap = std::move(otherHolder); - - return *this; } //============================================================================== diff --git a/dart/common/detail/SpecializedForAspect.h b/dart/common/detail/SpecializedForAspect.h index 4df189e75f466..8443d45b458b8 100644 --- a/dart/common/detail/SpecializedForAspect.h +++ b/dart/common/detail/SpecializedForAspect.h @@ -100,25 +100,25 @@ void SpecializedForAspect::set(std::unique_ptr&& aspect) //============================================================================== template template -T* SpecializedForAspect::create(Args&&... args) +T* SpecializedForAspect::createAspect(Args&&... args) { - return _create(type(), std::forward(args)...); + return _createAspect(type(), std::forward(args)...); } //============================================================================== template template -void SpecializedForAspect::erase() +void SpecializedForAspect::eraseAspect() { - _erase(type()); + _eraseAspect(type()); } //============================================================================== template template -std::unique_ptr SpecializedForAspect::release() +std::unique_ptr SpecializedForAspect::releaseAspect() { - return _release(type()); + return _releaseAspect(type()); } //============================================================================== @@ -242,7 +242,7 @@ void SpecializedForAspect::_set( //============================================================================== template template -T* SpecializedForAspect::_create(type, Args&&... args) +T* SpecializedForAspect::_createAspect(type, Args&&... args) { return Composite::createAspect(std::forward(args)...); } @@ -250,7 +250,7 @@ T* SpecializedForAspect::_create(type, Args&&... args) //============================================================================== template template -SpecAspect* SpecializedForAspect::_create( +SpecAspect* SpecializedForAspect::_createAspect( type, Args&&... args) { #ifdef DART_UNITTEST_SPECIALIZED_ASPECT_ACCESS @@ -267,14 +267,14 @@ SpecAspect* SpecializedForAspect::_create( //============================================================================== template template -void SpecializedForAspect::_erase(type) +void SpecializedForAspect::_eraseAspect(type) { Composite::eraseAspect(); } //============================================================================== template -void SpecializedForAspect::_erase(type) +void SpecializedForAspect::_eraseAspect(type) { #ifdef DART_UNITTEST_SPECIALIZED_ASPECT_ACCESS usedSpecializedAspectAccess = true; @@ -289,14 +289,14 @@ void SpecializedForAspect::_erase(type) //============================================================================== template template -std::unique_ptr SpecializedForAspect::_release(type) +std::unique_ptr SpecializedForAspect::_releaseAspect(type) { return Composite::releaseAspect(); } //============================================================================== template -std::unique_ptr SpecializedForAspect::_release( +std::unique_ptr SpecializedForAspect::_releaseAspect( type) { #ifdef DART_UNITTEST_SPECIALIZED_ASPECT_ACCESS diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index e2e2e4f9172ea..a10463b9b36cc 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -245,6 +245,7 @@ BodyNode::~BodyNode() { // Delete all Nodes mNodeMap.clear(); + mNodeDestructors.clear(); // Release markers for (std::vector::const_iterator it = mMarkers.begin(); diff --git a/dart/dynamics/CompositeNode.h b/dart/dynamics/CompositeNode.h new file mode 100644 index 0000000000000..0f840b4f9411f --- /dev/null +++ b/dart/dynamics/CompositeNode.h @@ -0,0 +1,123 @@ +/* + * 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_COMPOSITENODE_H_ +#define DART_DYNAMICS_COMPOSITENODE_H_ + +#include "dart/common/Composite.h" +#include "dart/dynamics/Node.h" + +namespace dart { +namespace dynamics { + +//============================================================================== +template +class CompositeStateNode : public Base +{ +public: + + using State = Node::MakeState; + + /// Forwarding constructor + template + CompositeStateNode(Args&&... args) + : Base(std::forward(args)...) + { + // Do nothing + } + + // Documentation inherited from Node + void setNodeState(const Node::State& otherState) override final; + + // Documentation inherited from Node + std::unique_ptr getNodeState() const override final; + + // Documentation inherited from Node + void copyNodeStateTo( + std::unique_ptr& outputState) const override final; +}; + +//============================================================================== +template +class CompositePropertiesNode : public Base +{ +public: + + using Properties = Node::MakeProperties; + + /// Forwarding constructor + template + CompositePropertiesNode(Args&&... args) + : Base(std::forward(args)...) + { + // Do nothing + } + + // Documentation inherited from Node + void setNodeProperties( + const Node::Properties& otherProperties) override final; + + // Documentation inherited from Node + std::unique_ptr getNodeProperties() const override final; + + // Documentation inherited from Node + void copyNodePropertiesTo( + std::unique_ptr& outputProperties) const override final; +}; + +//============================================================================== +template +class CompositeNode : public CompositePropertiesNode > +{ +public: + + /// Forwarding constructor + template + CompositeNode(Args&&... args) + : CompositePropertiesNode>( + std::forward(args)...) + { + // Do nothing + } + +}; + +} // namespace dynamics +} // namespace dart + +#include "dart/dynamics/detail/CompositeNode.h" + +#endif // DART_DYNAMICS_COMPOSITENODE_H_ diff --git a/dart/dynamics/EndEffector.cpp b/dart/dynamics/EndEffector.cpp index 5518fd74483c8..8cb38cea5909f 100644 --- a/dart/dynamics/EndEffector.cpp +++ b/dart/dynamics/EndEffector.cpp @@ -132,73 +132,6 @@ EndEffector& EndEffector::operator=(const EndEffector& _otherEndEffector) return *this; } -//============================================================================== -const std::string& EndEffector::setName(const std::string& _name) -{ - // If it already has the requested name, do nothing - if(mEndEffectorP.mName == _name && !_name.empty()) - return mEndEffectorP.mName; - - const std::string oldName = mEndEffectorP.mName; - - mEndEffectorP.mName = registerNameChange(_name); - - incrementVersion(); - Entity::mNameChangedSignal.raise(this, oldName, mEndEffectorP.mName); - - // Return the resulting name, after it has been checked for uniqueness - return mEndEffectorP.mName; -} - -//============================================================================== -const std::string& EndEffector::getName() const -{ - return mEndEffectorP.mName; -} - -//============================================================================== -void EndEffector::setNodeProperties(const Node::Properties& otherProperties) -{ - setProperties(static_cast(otherProperties)); -} - -//============================================================================== -std::unique_ptr EndEffector::getNodeProperties() const -{ - return common::make_unique( - getEndEffectorProperties()); -} - -//============================================================================== -void EndEffector::copyNodePropertiesTo( - std::unique_ptr& outputProperties) const -{ - if(outputProperties) - { - EndEffector::Properties* properties = - static_cast(outputProperties.get()); - - static_cast(*properties) = mEndEffectorP; - copyCompositePropertiesTo(properties->mCompositeProperties); - } - else - { - outputProperties = getNodeProperties(); - } -} - -//============================================================================== -void EndEffector::setRelativeTransform(const Eigen::Isometry3d& newRelativeTf) -{ - if(newRelativeTf.matrix() == - FixedFrame::mAspectProperties.mRelativeTf.matrix()) - return; - - FixedFrame::setRelativeTransform(newRelativeTf); - notifyJacobianUpdate(); - notifyJacobianDerivUpdate(); -} - //============================================================================== void EndEffector::setDefaultRelativeTransform( const Eigen::Isometry3d& _newDefaultTf, bool _useNow) @@ -215,102 +148,6 @@ void EndEffector::resetRelativeTransform() setRelativeTransform(mEndEffectorP.mDefaultTransform); } -//============================================================================== -bool EndEffector::dependsOn(size_t _genCoordIndex) const -{ - return mBodyNode->dependsOn(_genCoordIndex); -} - -//============================================================================== -size_t EndEffector::getNumDependentGenCoords() const -{ - return mBodyNode->getNumDependentGenCoords(); -} - -//============================================================================== -size_t EndEffector::getDependentGenCoordIndex(size_t _arrayIndex) const -{ - return mBodyNode->getDependentGenCoordIndex(_arrayIndex); -} - -//============================================================================== -const std::vector& EndEffector::getDependentGenCoordIndices() const -{ - return mBodyNode->getDependentGenCoordIndices(); -} - -//============================================================================== -size_t EndEffector::getNumDependentDofs() const -{ - return mBodyNode->getNumDependentDofs(); -} - -//============================================================================== -DegreeOfFreedom* EndEffector::getDependentDof(size_t _index) -{ - return mBodyNode->getDependentDof(_index); -} - -//============================================================================== -const DegreeOfFreedom* EndEffector::getDependentDof(size_t _index) const -{ - return mBodyNode->getDependentDof(_index); -} - -//============================================================================== -const std::vector& EndEffector::getDependentDofs() -{ - return mBodyNode->getDependentDofs(); -} - -//============================================================================== -const std::vector& EndEffector::getDependentDofs() const -{ - return static_cast(mBodyNode)->getDependentDofs(); -} - -//============================================================================== -const std::vector EndEffector::getChainDofs() const -{ - return mBodyNode->getChainDofs(); -} - -//============================================================================== -const math::Jacobian& EndEffector::getJacobian() const -{ - if (mIsBodyJacobianDirty) - updateEffectorJacobian(); - - return mEffectorJacobian; -} - -//============================================================================== -const math::Jacobian& EndEffector::getWorldJacobian() const -{ - if(mIsWorldJacobianDirty) - updateWorldJacobian(); - - return mWorldJacobian; -} - -//============================================================================== -const math::Jacobian& EndEffector::getJacobianSpatialDeriv() const -{ - if(mIsBodyJacobianSpatialDerivDirty) - updateEffectorJacobianSpatialDeriv(); - - return mEffectorJacobianSpatialDeriv; -} - -//============================================================================== -const math::Jacobian& EndEffector::getJacobianClassicDeriv() const -{ - if(mIsWorldJacobianClassicDerivDirty) - updateWorldJacobianClassicDeriv(); - - return mWorldJacobianClassicDeriv; -} - //============================================================================== void EndEffector::notifyTransformUpdate() { @@ -325,19 +162,14 @@ void EndEffector::notifyTransformUpdate() } //============================================================================== -void EndEffector::notifyVelocityUpdate() -{ - Frame::notifyVelocityUpdate(); -} - -//============================================================================== -EndEffector::EndEffector(BodyNode* _parent, const PropertiesData& _properties) +EndEffector::EndEffector(BodyNode* parent, const PropertiesData& properties) : Entity(ConstructFrame), - Frame(_parent), - FixedFrame(_parent, _properties.mDefaultTransform), - TemplatedJacobianNode(_parent) + Frame(parent), + FixedFrame(parent, properties.mDefaultTransform), + detail::EndEffectorCompositeBase( + std::make_tuple(parent, properties.mDefaultTransform), common::NoArg) { - setProperties(_properties); + setProperties(properties); } //============================================================================== @@ -354,52 +186,5 @@ Node* EndEffector::cloneNode(BodyNode* _parent) const return ee; } -//============================================================================== -void EndEffector::updateEffectorJacobian() const -{ - mEffectorJacobian = math::AdInvTJac(getRelativeTransform(), - mBodyNode->getJacobian()); - mIsBodyJacobianDirty = false; -} - -//============================================================================== -void EndEffector::updateWorldJacobian() const -{ - mWorldJacobian = math::AdRJac(getWorldTransform(), getJacobian()); - - mIsWorldJacobianDirty = false; -} - -//============================================================================== -void EndEffector::updateEffectorJacobianSpatialDeriv() const -{ - mEffectorJacobianSpatialDeriv = - math::AdInvTJac(getRelativeTransform(), - mBodyNode->getJacobianSpatialDeriv()); - - mIsBodyJacobianSpatialDerivDirty = false; -} - -//============================================================================== -void EndEffector::updateWorldJacobianClassicDeriv() const -{ - const math::Jacobian& dJ_parent = mBodyNode->getJacobianClassicDeriv(); - const math::Jacobian& J_parent = mBodyNode->getWorldJacobian(); - - const Eigen::Vector3d& v_local = - getLinearVelocity(mBodyNode, Frame::World()); - - const Eigen::Vector3d& w_parent = mBodyNode->getAngularVelocity(); - const Eigen::Vector3d& p = (getWorldTransform().translation() - - mBodyNode->getWorldTransform().translation()).eval(); - - mWorldJacobianClassicDeriv = dJ_parent; - mWorldJacobianClassicDeriv.bottomRows<3>().noalias() += - J_parent.topRows<3>().colwise().cross(v_local + w_parent.cross(p)) - + dJ_parent.topRows<3>().colwise().cross(p); - - mIsWorldJacobianClassicDerivDirty = false; -} - } // namespace dynamics } // namespace dart diff --git a/dart/dynamics/EndEffector.h b/dart/dynamics/EndEffector.h index 8b1bd0b8e0c65..130a409cd2b7e 100644 --- a/dart/dynamics/EndEffector.h +++ b/dart/dynamics/EndEffector.h @@ -40,8 +40,8 @@ #include "dart/common/Aspect.h" #include "dart/common/SpecializedForAspect.h" #include "dart/common/AspectWithVersion.h" -#include "dart/dynamics/FixedFrame.h" -#include "dart/dynamics/TemplatedJacobianNode.h" +#include "dart/dynamics/FixedJacobianNode.h" +#include "dart/dynamics/CompositeNode.h" namespace dart { namespace dynamics { @@ -49,11 +49,11 @@ namespace dynamics { class BodyNode; class Skeleton; class EndEffector; - class Support; -namespace detail -{ +namespace detail { + +//============================================================================== struct SupportStateData { inline SupportStateData(bool active = false) : mActive(active) { } @@ -61,6 +61,7 @@ struct SupportStateData bool mActive; }; +//============================================================================== struct SupportPropertiesData { math::SupportGeometry mGeometry; @@ -68,8 +69,16 @@ struct SupportPropertiesData void SupportUpdate(Support* support); +using EndEffectorCompositeBase = CompositeNode< + common::CompositeJoiner< + FixedJacobianNode, + common::SpecializedForAspect + > +>; + } // namespace detail +//============================================================================== class Support final : public common::AspectWithStateAndVersionedProperties< Support, @@ -99,11 +108,8 @@ class Support final : }; -class EndEffector final : - public common::CompositeJoiner< - common::SpecializedForAspect, common::Virtual >, - public AccessoryNode, - public TemplatedJacobianNode +//============================================================================== +class EndEffector final : public detail::EndEffectorCompositeBase { public: @@ -145,15 +151,6 @@ class EndEffector final : /// \{ \name Structural Properties //---------------------------------------------------------------------------- - /// Set the State of this EndEffector. -// void setState(const StateData& _state); - - /// Set the State of this EndEffector using move semantics -// void setState(StateData&& _state); - - /// Get the State of this EndEffector -// StateData getEndEffectorState() const; - /// Set the Properties of this EndEffector. If _useNow is true, the current /// Transform will be set to the new default transform. void setProperties(const PropertiesData& _properties, bool _useNow=false); @@ -174,26 +171,6 @@ class EndEffector final : /// Copy the State and Properties of another EndEffector EndEffector& operator=(const EndEffector& _otherEndEffector); - /// Set name. If the name is already taken, this will return an altered - /// version which will be used by the Skeleton - const std::string& setName(const std::string& _name) override; - - // Documentation inherited - const std::string& getName() const override; - - // Documentation inherited - void setNodeProperties(const Node::Properties& otherProperties) override final; - - // Documentation inherited - std::unique_ptr getNodeProperties() const override final; - - // Documentation inherited - void copyNodePropertiesTo( - std::unique_ptr& outputProperties) const override final; - - /// Set the current relative transform of this EndEffector - void setRelativeTransform(const Eigen::Isometry3d& newRelativeTf) override; - /// Set the default relative transform of this EndEffector. The relative /// transform of this EndEffector will be set to _newDefaultTf the next time /// resetRelativeTransform() is called. If _useNow is set to true, then @@ -208,68 +185,6 @@ class EndEffector final : DART_BAKE_SPECIALIZED_ASPECT(Support) - // Documentation inherited - bool dependsOn(size_t _genCoordIndex) const override; - - // Documentation inherited - size_t getNumDependentGenCoords() const override; - - // Documentation inherited - size_t getDependentGenCoordIndex(size_t _arrayIndex) const override; - - // Documentation inherited - const std::vector& getDependentGenCoordIndices() const override; - - // Documentation inherited - size_t getNumDependentDofs() const override; - - // Documentation inherited - DegreeOfFreedom* getDependentDof(size_t _index) override; - - // Documentation inherited - const DegreeOfFreedom* getDependentDof(size_t _index) const override; - - // Documentation inherited - const std::vector& getDependentDofs() override; - - // Documentation inherited - const std::vector& getDependentDofs() const override; - - // Documentation inherited - const std::vector getChainDofs() const override; - - /// \} - - //---------------------------------------------------------------------------- - /// \{ \name Jacobian Functions - //---------------------------------------------------------------------------- - - // Documentation inherited - const math::Jacobian& getJacobian() const override final; - - // Prevent the inherited getJacobian functions from being shadowed - using TemplatedJacobianNode::getJacobian; - - // Documentation inherited - const math::Jacobian& getWorldJacobian() const override final; - - // Prevent the inherited getWorldJacobian functions from being shadowed - using TemplatedJacobianNode::getWorldJacobian; - - // Documentation inherited - const math::Jacobian& getJacobianSpatialDeriv() const override final; - - // Prevent the inherited getJacobianSpatialDeriv functions from being shadowed - using TemplatedJacobianNode::getJacobianSpatialDeriv; - - // Documentation inherited - const math::Jacobian& getJacobianClassicDeriv() const override final; - - // Prevent the inherited getJacobianClassicDeriv functions from being shadowed - using TemplatedJacobianNode::getJacobianClassicDeriv; - - /// \} - //---------------------------------------------------------------------------- /// \{ \name Notifications //---------------------------------------------------------------------------- @@ -277,57 +192,21 @@ class EndEffector final : // Documentation inherited virtual void notifyTransformUpdate() override; - // Documentation inherited - virtual void notifyVelocityUpdate() override; + /// \} protected: /// Constructor used by the Skeleton class - explicit EndEffector(BodyNode* _parent, const PropertiesData& _properties); + explicit EndEffector(BodyNode* parent, const PropertiesData& properties); /// Create a clone of this BodyNode. This may only be called by the Skeleton /// class. virtual Node* cloneNode(BodyNode* _parent) const override; - /// Update the Jacobian of this EndEffector. getJacobian() calls this function - /// if mIsEffectorJacobianDirty is true. - void updateEffectorJacobian() const; - - /// Update the World Jacobian cache. - void updateWorldJacobian() const; - - /// Update the spatial time derivative of the end effector Jacobian. - /// getJacobianSpatialDeriv() calls this function if - /// mIsEffectorJacobianSpatialDerivDirty is true. - void updateEffectorJacobianSpatialDeriv() const; - - /// Update the classic time derivative of the end effector Jacobian. - /// getJacobianClassicDeriv() calls this function if - /// mIsWorldJacobianClassicDerivDirty is true. - void updateWorldJacobianClassicDeriv() const; /// Properties of this EndEffector + DEPRECATED(6.0) UniqueProperties mEndEffectorP; - - /// Cached Jacobian of this EndEffector - /// - /// Do not use directly! Use getJacobian() to access this quantity - mutable math::Jacobian mEffectorJacobian; - - /// Cached World Jacobian of this EndEffector - /// - /// Do not use directly! Use getWorldJacobian() to access this quantity - mutable math::Jacobian mWorldJacobian; - - /// Spatial time derivative of end effector Jacobian - /// - /// Do not use directly! Use getJacobianSpatialDeriv() to access this quantity - mutable math::Jacobian mEffectorJacobianSpatialDeriv; - - /// Classic time derivative of the end effector Jacobian - /// - /// Do not use directly! Use getJacobianClassicDeriv() to access this quantity - mutable math::Jacobian mWorldJacobianClassicDeriv; }; } // namespace dynamics diff --git a/dart/dynamics/EntityNode.h b/dart/dynamics/EntityNode.h new file mode 100644 index 0000000000000..0db68633b6984 --- /dev/null +++ b/dart/dynamics/EntityNode.h @@ -0,0 +1,82 @@ +/* + * 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_ENTITYNODE_H_ +#define DART_DYNAMICS_ENTITYNODE_H_ + +#include "dart/dynamics/detail/EntityNodeAspect.h" + +namespace dart { +namespace dynamics { + +//============================================================================== +template +class EntityNode : public detail::EntityNodeBase< + Base, std::is_base_of::value > +{ +public: + + using NameAspect = typename detail::EntityNodeAspectBase::Aspect; + + /// Forwarding constructor + template + EntityNode(Args&&... args) + : detail::EntityNodeBase< + Base, std::is_base_of::value>( + std::forward(args)...) + { + this->template createAspect(); + } + + virtual ~EntityNode() = default; + + /// Set the AspectProperties of this EntityNode + void setAspectProperties(const typename NameAspect::Properties& properties); + + // Documentation inherited from Node + const std::string& setName(const std::string& newName) override; + + // Documentation inherited from Node + const std::string& getName() const override; + +}; + +} // namespace dynamics +} // namespace dart + +#include "dart/dynamics/detail/EntityNode.h" + +#endif // DART_DYNAMICS_ENTITYNODE_H_ diff --git a/dart/dynamics/FixedFrame.cpp b/dart/dynamics/FixedFrame.cpp index b12a5ba7b1125..087231477700e 100644 --- a/dart/dynamics/FixedFrame.cpp +++ b/dart/dynamics/FixedFrame.cpp @@ -59,7 +59,7 @@ FixedFrame::FixedFrame(Frame* refFrame, : Entity(refFrame, false), Frame(refFrame) { - create(AspectProperties(relativeTransform)); + createAspect(AspectProperties(relativeTransform)); } //============================================================================== @@ -115,5 +115,20 @@ const Eigen::Vector6d& FixedFrame::getPartialAcceleration() const return mZero; } +//============================================================================== +FixedFrame::FixedFrame() + : FixedFrame(ConstructAbstract) +{ + // Delegates to the abstract constructor +} + +//============================================================================== +FixedFrame::FixedFrame(ConstructAbstract_t) +{ + dterr << "[FixedFrame::FixedFrame] Attempting to construct a pure abstract " + << "FixedFrame object. This is not allowed!\n"; + assert(false); +} + } // namespace dynamics } // namespace dart diff --git a/dart/dynamics/FixedFrame.h b/dart/dynamics/FixedFrame.h index 2f059e5e58a98..dbe4e918ee593 100644 --- a/dart/dynamics/FixedFrame.h +++ b/dart/dynamics/FixedFrame.h @@ -95,6 +95,12 @@ class FixedFrame : protected: + /// Default constructor -- calls the Abstract constructor + FixedFrame(); + + /// Abstract constructor + explicit FixedFrame(ConstructAbstract_t); + /// Used for Relative Velocity and Relative Acceleration of this Frame static const Eigen::Vector6d mZero; }; diff --git a/dart/dynamics/FixedJacobianNode.cpp b/dart/dynamics/FixedJacobianNode.cpp new file mode 100644 index 0000000000000..6faa614255549 --- /dev/null +++ b/dart/dynamics/FixedJacobianNode.cpp @@ -0,0 +1,218 @@ +/* + * 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. + */ + +#include "dart/dynamics/FixedJacobianNode.h" +#include "dart/dynamics/BodyNode.h" + +namespace dart { +namespace dynamics { + +//============================================================================== +void FixedJacobianNode::setRelativeTransform( + const Eigen::Isometry3d& newRelativeTf) +{ + if(newRelativeTf.matrix() == + FixedFrame::mAspectProperties.mRelativeTf.matrix()) + return; + + FixedFrame::setRelativeTransform(newRelativeTf); + notifyJacobianUpdate(); + notifyJacobianDerivUpdate(); +} + +//============================================================================== +bool FixedJacobianNode::dependsOn(size_t _genCoordIndex) const +{ + return mBodyNode->dependsOn(_genCoordIndex); +} + +//============================================================================== +size_t FixedJacobianNode::getNumDependentGenCoords() const +{ + return mBodyNode->getNumDependentGenCoords(); +} + +//============================================================================== +size_t FixedJacobianNode::getDependentGenCoordIndex(size_t _arrayIndex) const +{ + return mBodyNode->getDependentGenCoordIndex(_arrayIndex); +} + +//============================================================================== +const std::vector& FixedJacobianNode::getDependentGenCoordIndices() const +{ + return mBodyNode->getDependentGenCoordIndices(); +} + +//============================================================================== +size_t FixedJacobianNode::getNumDependentDofs() const +{ + return mBodyNode->getNumDependentDofs(); +} + +//============================================================================== +DegreeOfFreedom* FixedJacobianNode::getDependentDof(size_t _index) +{ + return mBodyNode->getDependentDof(_index); +} + +//============================================================================== +const DegreeOfFreedom* FixedJacobianNode::getDependentDof(size_t _index) const +{ + return mBodyNode->getDependentDof(_index); +} + +//============================================================================== +const std::vector& FixedJacobianNode::getDependentDofs() +{ + return mBodyNode->getDependentDofs(); +} + +//============================================================================== +const std::vector& FixedJacobianNode::getDependentDofs() const +{ + return static_cast(mBodyNode)->getDependentDofs(); +} + +//============================================================================== +const std::vector FixedJacobianNode::getChainDofs() const +{ + return mBodyNode->getChainDofs(); +} + +//============================================================================== +const math::Jacobian& FixedJacobianNode::getJacobian() const +{ + if (mIsBodyJacobianDirty) + updateBodyJacobian(); + + return mCache.mBodyJacobian; +} + +//============================================================================== +const math::Jacobian& FixedJacobianNode::getWorldJacobian() const +{ + if(mIsWorldJacobianDirty) + updateWorldJacobian(); + + return mCache.mWorldJacobian; +} + +//============================================================================== +const math::Jacobian& FixedJacobianNode::getJacobianSpatialDeriv() const +{ + if(mIsBodyJacobianSpatialDerivDirty) + updateBodyJacobianSpatialDeriv(); + + return mCache.mBodyJacobianSpatialDeriv; +} + +//============================================================================== +const math::Jacobian& FixedJacobianNode::getJacobianClassicDeriv() const +{ + if(mIsWorldJacobianClassicDerivDirty) + updateWorldJacobianClassicDeriv(); + + return mCache.mWorldJacobianClassicDeriv; +} + +//============================================================================== +FixedJacobianNode::FixedJacobianNode( + BodyNode* parent, const Eigen::Isometry3d& transform) + : FixedFrame(parent, transform), + detail::FixedJacobianNodeCompositeBase(parent) +{ + // Do nothing +} + +//============================================================================== +FixedJacobianNode::FixedJacobianNode( + const std::tuple& args) + : FixedJacobianNode(std::get<0>(args), std::get<1>(args)) +{ + // Delegating constructor +} + +//============================================================================== +void FixedJacobianNode::updateBodyJacobian() const +{ + mCache.mBodyJacobian = math::AdInvTJac(getRelativeTransform(), + mBodyNode->getJacobian()); + mIsBodyJacobianDirty = false; +} + +//============================================================================== +void FixedJacobianNode::updateWorldJacobian() const +{ + mCache.mWorldJacobian = math::AdRJac(getWorldTransform(), getJacobian()); + + mIsWorldJacobianDirty = false; +} + +//============================================================================== +void FixedJacobianNode::updateBodyJacobianSpatialDeriv() const +{ + mCache.mBodyJacobianSpatialDeriv = + math::AdInvTJac(getRelativeTransform(), + mBodyNode->getJacobianSpatialDeriv()); + + mIsBodyJacobianSpatialDerivDirty = false; +} + +//============================================================================== +void FixedJacobianNode::updateWorldJacobianClassicDeriv() const +{ + const math::Jacobian& dJ_parent = mBodyNode->getJacobianClassicDeriv(); + const math::Jacobian& J_parent = mBodyNode->getWorldJacobian(); + + const Eigen::Vector3d& v_local = + getLinearVelocity(mBodyNode, Frame::World()); + + const Eigen::Vector3d& w_parent = mBodyNode->getAngularVelocity(); + const Eigen::Vector3d& p = (getWorldTransform().translation() + - mBodyNode->getWorldTransform().translation()).eval(); + + mCache.mWorldJacobianClassicDeriv = dJ_parent; + mCache.mWorldJacobianClassicDeriv.bottomRows<3>().noalias() += + J_parent.topRows<3>().colwise().cross(v_local + w_parent.cross(p)) + + dJ_parent.topRows<3>().colwise().cross(p); + + mIsWorldJacobianClassicDerivDirty = false; +} + + +} // namespace dynamics +} // namespace dart diff --git a/dart/dynamics/FixedJacobianNode.h b/dart/dynamics/FixedJacobianNode.h new file mode 100644 index 0000000000000..7bd3af2f14e09 --- /dev/null +++ b/dart/dynamics/FixedJacobianNode.h @@ -0,0 +1,184 @@ +/* + * 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_FIXEDJACOBIANNODE_H_ +#define DART_DYNAMICS_FIXEDJACOBIANNODE_H_ + +#include "dart/dynamics/TemplatedJacobianNode.h" +#include "dart/dynamics/FixedFrame.h" +#include "dart/dynamics/EntityNode.h" + +namespace dart { +namespace dynamics { + +class FixedJacobianNode; + +namespace detail { + +using FixedJacobianNodeCompositeBase = common::CompositeJoiner< + EntityNode< TemplatedJacobianNode >, + common::Virtual +>; + +} // namespace detail + +class FixedJacobianNode : + public detail::FixedJacobianNodeCompositeBase, + public AccessoryNode +{ +public: + + /// Set the current relative transform of this Fixed Frame + void setRelativeTransform(const Eigen::Isometry3d& newRelativeTf) override; + + // Documentation inherited + bool dependsOn(size_t _genCoordIndex) const override; + + // Documentation inherited + size_t getNumDependentGenCoords() const override; + + // Documentation inherited + size_t getDependentGenCoordIndex(size_t _arrayIndex) const override; + + // Documentation inherited + const std::vector& getDependentGenCoordIndices() const override; + + // Documentation inherited + size_t getNumDependentDofs() const override; + + // Documentation inherited + DegreeOfFreedom* getDependentDof(size_t _index) override; + + // Documentation inherited + const DegreeOfFreedom* getDependentDof(size_t _index) const override; + + // Documentation inherited + const std::vector& getDependentDofs() override; + + // Documentation inherited + const std::vector& getDependentDofs() const override; + + // Documentation inherited + const std::vector getChainDofs() const override; + + /// \} + + //---------------------------------------------------------------------------- + /// \{ \name Jacobian Functions + //---------------------------------------------------------------------------- + + // Documentation inherited + const math::Jacobian& getJacobian() const override final; + + // Prevent the inherited getJacobian functions from being shadowed + using TemplatedJacobianNode::getJacobian; + + // Documentation inherited + const math::Jacobian& getWorldJacobian() const override final; + + // Prevent the inherited getWorldJacobian functions from being shadowed + using TemplatedJacobianNode::getWorldJacobian; + + // Documentation inherited + const math::Jacobian& getJacobianSpatialDeriv() const override final; + + // Prevent the inherited getJacobianSpatialDeriv functions from being shadowed + using TemplatedJacobianNode::getJacobianSpatialDeriv; + + // Documentation inherited + const math::Jacobian& getJacobianClassicDeriv() const override final; + + // Prevent the inherited getJacobianClassicDeriv functions from being shadowed + using TemplatedJacobianNode::getJacobianClassicDeriv; + + /// \} + +protected: + + /// Constructor + FixedJacobianNode(BodyNode* parent, const Eigen::Isometry3d& transform); + + /// Tuple constructor + FixedJacobianNode(const std::tuple& args); + + /// Update the Jacobian of this Fixed Frame. getJacobian() calls this function + /// if mIsBodyJacobianDirty is true. + void updateBodyJacobian() const; + + /// Update the World Jacobian cache. + void updateWorldJacobian() const; + + /// Update the spatial time derivative of the Fixed Frame Jacobian. + /// getJacobianSpatialDeriv() calls this function if + /// mIsBodyJacobianSpatialDerivDirty is true. + void updateBodyJacobianSpatialDeriv() const; + + /// Update the classic time derivative of the Fixed Frame Jacobian. + /// getJacobianClassicDeriv() calls this function if + /// mIsWorldJacobianClassicDerivDirty is true. + void updateWorldJacobianClassicDeriv() const; + + struct Cache + { + /// Cached Jacobian of this Fixed Frame + /// + /// Do not use directly! Use getJacobian() to access this quantity + math::Jacobian mBodyJacobian; + + /// Cached World Jacobian of this Fixed Frame + /// + /// Do not use directly! Use getWorldJacobian() to access this quantity + math::Jacobian mWorldJacobian; + + /// Spatial time derivative of Fixed Frame Jacobian + /// + /// Do not use directly! Use getJacobianSpatialDeriv() to access this quantity + math::Jacobian mBodyJacobianSpatialDeriv; + + /// Classic time derivative of the Fixed Frame Jacobian + /// + /// Do not use directly! Use getJacobianClassicDeriv() to access this quantity + math::Jacobian mWorldJacobianClassicDeriv; + }; + + mutable Cache mCache; + +}; + +} // namespace dynamics +} // namespace dart + +#endif // DART_DYNAMICS_FIXEDJACOBIANNODE_H_ diff --git a/dart/dynamics/Node.cpp b/dart/dynamics/Node.cpp index 489789b32240a..42e264efa8292 100644 --- a/dart/dynamics/Node.cpp +++ b/dart/dynamics/Node.cpp @@ -165,7 +165,8 @@ Node::Node(BodyNode* _bn) return; } - setVersionDependentObject(mBodyNode); + if(mBodyNode != this) + setVersionDependentObject(mBodyNode); } //============================================================================== diff --git a/dart/dynamics/Node.h b/dart/dynamics/Node.h index 7e9309fff0143..763ff0dfd28b4 100644 --- a/dart/dynamics/Node.h +++ b/dart/dynamics/Node.h @@ -42,6 +42,7 @@ #include "dart/common/Subject.h" #include "dart/common/Cloneable.h" #include "dart/common/VersionCounter.h" +#include "dart/common/EmbeddedAspect.h" #include "dart/dynamics/SmartPointer.h" @@ -51,6 +52,7 @@ namespace dynamics { class BodyNode; class Node; +//============================================================================== class NodeDestructor final { public: @@ -73,6 +75,7 @@ class NodeDestructor final }; +//============================================================================== /// The Node class is a base class for BodyNode and any object that attaches to /// a BodyNode. This base class handles ownership and reference counting for the /// classes that inherit it. @@ -228,6 +231,7 @@ class Node : size_t mIndexInTree; }; +//============================================================================== /// AccessoryNode provides an interface for Nodes to get their index within the /// list of Nodes, as well as detach and reattach. This uses CRTP to get around /// the diamond of death problem. diff --git a/dart/dynamics/ShapeNode.cpp b/dart/dynamics/ShapeNode.cpp index d793310a87c0f..87fd1c825785a 100644 --- a/dart/dynamics/ShapeNode.cpp +++ b/dart/dynamics/ShapeNode.cpp @@ -124,30 +124,6 @@ ShapeNode& ShapeNode::operator=(const ShapeNode& other) return *this; } -//============================================================================== -const std::string& ShapeNode::setName(const std::string& name) -{ - // If it already has the requested name, do nothing - if(mShapeNodeP.mName == name && !name.empty()) - return mShapeNodeP.mName; - - const std::string oldName = mShapeNodeP.mName; - - mShapeNodeP.mName = registerNameChange(name); - - incrementVersion(); - Entity::mNameChangedSignal.raise(this, oldName, mShapeNodeP.mName); - - // Return the resulting name, after it has been checked for uniqueness - return mShapeNodeP.mName; -} - -//============================================================================== -const std::string& ShapeNode::getName() const -{ - return mShapeNodeP.mName; -} - //============================================================================== void ShapeNode::setRelativeTransform(const Eigen::Isometry3d& transform) { @@ -206,121 +182,13 @@ Eigen::Vector3d ShapeNode::getOffset() const return getRelativeTranslation(); } -//============================================================================== -std::shared_ptr ShapeNode::getSkeleton() -{ - return mBodyNode->getSkeleton(); -} - -//============================================================================== -std::shared_ptr ShapeNode::getSkeleton() const -{ - return mBodyNode->getSkeleton(); -} - -//============================================================================== -bool ShapeNode::dependsOn(size_t genCoordIndex) const -{ - return mBodyNode->dependsOn(genCoordIndex); -} - -//============================================================================== -size_t ShapeNode::getNumDependentGenCoords() const -{ - return mBodyNode->getNumDependentGenCoords(); -} - -//============================================================================== -size_t ShapeNode::getDependentGenCoordIndex(size_t arrayIndex) const -{ - return mBodyNode->getDependentGenCoordIndex(arrayIndex); -} - -//============================================================================== -const std::vector& ShapeNode::getDependentGenCoordIndices() const -{ - return mBodyNode->getDependentGenCoordIndices(); -} - -//============================================================================== -size_t ShapeNode::getNumDependentDofs() const -{ - return mBodyNode->getNumDependentDofs(); -} - -//============================================================================== -DegreeOfFreedom* ShapeNode::getDependentDof(size_t index) -{ - return mBodyNode->getDependentDof(index); -} - -//============================================================================== -const DegreeOfFreedom* ShapeNode::getDependentDof(size_t index) const -{ - return mBodyNode->getDependentDof(index); -} - -//============================================================================== -const std::vector& ShapeNode::getDependentDofs() -{ - return mBodyNode->getDependentDofs(); -} - -//============================================================================== -const std::vector& ShapeNode::getDependentDofs() const -{ - return static_cast(mBodyNode)->getDependentDofs(); -} - -//============================================================================== -const std::vector ShapeNode::getChainDofs() const -{ - return mBodyNode->getChainDofs(); -} - -//============================================================================== -const math::Jacobian& ShapeNode::getJacobian() const -{ - if (mIsBodyJacobianDirty) - updateShapeNodeJacobian(); - - return mShapeNodeJacobian; -} - -//============================================================================== -const math::Jacobian& ShapeNode::getWorldJacobian() const -{ - if(mIsWorldJacobianDirty) - updateWorldJacobian(); - - return mWorldJacobian; -} - -//============================================================================== -const math::Jacobian& ShapeNode::getJacobianSpatialDeriv() const -{ - if(mIsBodyJacobianSpatialDerivDirty) - updateShapeNodeJacobianSpatialDeriv(); - - return mShapeNodeJacobianSpatialDeriv; -} - -//============================================================================== -const math::Jacobian& ShapeNode::getJacobianClassicDeriv() const -{ - if(mIsWorldJacobianClassicDerivDirty) - updateWorldJacobianClassicDeriv(); - - return mWorldJacobianClassicDeriv; -} - //============================================================================== ShapeNode::ShapeNode(BodyNode* bodyNode, const Properties& properties) : Entity(ConstructFrame), Frame(bodyNode), FixedFrame(bodyNode), - detail::ShapeNodeCompositeBase(common::NoArg, bodyNode), - TemplatedJacobianNode(bodyNode), + detail::ShapeNodeCompositeBase( + std::make_tuple(bodyNode, properties.mRelativeTransform), bodyNode), mShapeUpdatedSignal(ShapeUpdatedSignal()), mRelativeTransformUpdatedSignal(RelativeTransformUpdatedSignal()), onShapeUpdated(mShapeUpdatedSignal), @@ -336,13 +204,14 @@ ShapeNode::ShapeNode(BodyNode* bodyNode, : Entity(ConstructFrame), Frame(bodyNode), FixedFrame(bodyNode), - detail::ShapeNodeCompositeBase(common::NoArg, bodyNode), - TemplatedJacobianNode(bodyNode), + detail::ShapeNodeCompositeBase( + std::make_tuple(bodyNode, Eigen::Isometry3d::Identity()), bodyNode), mShapeUpdatedSignal(ShapeUpdatedSignal()), mRelativeTransformUpdatedSignal(RelativeTransformUpdatedSignal()), onShapeUpdated(mShapeUpdatedSignal), onRelativeTransformUpdated(mRelativeTransformUpdatedSignal) { + // TODO(MXG): Consider changing this to a delegating constructor instead Properties prop; prop.mShape = shape; prop.mName = name; @@ -364,53 +233,6 @@ Node* ShapeNode::cloneNode(BodyNode* parent) const return shapeNode; } -//============================================================================== -void ShapeNode::updateShapeNodeJacobian() const -{ - mShapeNodeJacobian = math::AdInvTJac(getRelativeTransform(), - mBodyNode->getJacobian()); - mIsBodyJacobianDirty = false; -} - -//============================================================================== -void ShapeNode::updateWorldJacobian() const -{ - mWorldJacobian = math::AdRJac(getWorldTransform(), getJacobian()); - - mIsWorldJacobianDirty = false; -} - -//============================================================================== -void ShapeNode::updateShapeNodeJacobianSpatialDeriv() const -{ - mShapeNodeJacobianSpatialDeriv = - math::AdInvTJac(getRelativeTransform(), - mBodyNode->getJacobianSpatialDeriv()); - - mIsBodyJacobianSpatialDerivDirty = false; -} - -//============================================================================== -void ShapeNode::updateWorldJacobianClassicDeriv() const -{ - const math::Jacobian& dJ_parent = mBodyNode->getJacobianClassicDeriv(); - const math::Jacobian& J_parent = mBodyNode->getWorldJacobian(); - - const Eigen::Vector3d& v_local = - getLinearVelocity(mBodyNode, Frame::World()); - - const Eigen::Vector3d& w_parent = mBodyNode->getAngularVelocity(); - const Eigen::Vector3d& p = (getWorldTransform().translation() - - mBodyNode->getWorldTransform().translation()).eval(); - - mWorldJacobianClassicDeriv = dJ_parent; - mWorldJacobianClassicDeriv.bottomRows<3>().noalias() += - J_parent.topRows<3>().colwise().cross(v_local + w_parent.cross(p)) - + dJ_parent.topRows<3>().colwise().cross(p); - - mIsWorldJacobianClassicDerivDirty = false; -} - } // namespace dynamics } // namespace dart diff --git a/dart/dynamics/ShapeNode.h b/dart/dynamics/ShapeNode.h index 48eb639ffcfcb..9199ffa2679d6 100644 --- a/dart/dynamics/ShapeNode.h +++ b/dart/dynamics/ShapeNode.h @@ -40,18 +40,21 @@ #include #include "dart/common/Signal.h" -#include "dart/dynamics/FixedFrame.h" #include "dart/dynamics/ShapeFrame.h" -#include "dart/dynamics/Node.h" -#include "dart/dynamics/TemplatedJacobianNode.h" +#include "dart/dynamics/FixedJacobianNode.h" +#include "dart/dynamics/CompositeNode.h" namespace dart { namespace dynamics { namespace detail { -using ShapeNodeCompositeBase = common::CompositeJoiner< - common::Virtual, ShapeFrame>; +using ShapeNodeCompositeBase = CompositeNode< + common::CompositeJoiner< + FixedJacobianNode, + ShapeFrame + > +>; } // namespace detail @@ -60,10 +63,7 @@ class CollisionAspect; class DynamicsAspect; class ShapeFrame; -class ShapeNode : - public detail::ShapeNodeCompositeBase, - public AccessoryNode, - public TemplatedJacobianNode +class ShapeNode : public detail::ShapeNodeCompositeBase { public: @@ -140,13 +140,6 @@ class ShapeNode : /// Same as copy(const ShapeNode&) ShapeNode& operator=(const ShapeNode& other); - /// Set name. If the name is already taken, this will return an altered - /// version which will be used by the Skeleton - const std::string& setName(const std::string& _name) override; - - // Documentation inherited - const std::string& getName() const override; - /// Set transformation of this shape node relative to the parent frame void setRelativeTransform(const Eigen::Isometry3d& transform) override; @@ -168,72 +161,6 @@ class ShapeNode : /// Same as getRelativeTranslation() Eigen::Vector3d getOffset() const; - // Documentation inherited - std::shared_ptr getSkeleton() override; - - // Documentation inherited - std::shared_ptr getSkeleton() const override; - - // Documentation inherited - bool dependsOn(size_t genCoordIndex) const override; - - // Documentation inherited - size_t getNumDependentGenCoords() const override; - - // Documentation inherited - size_t getDependentGenCoordIndex(size_t arrayIndex) const override; - - // Documentation inherited - const std::vector& getDependentGenCoordIndices() const override; - - // Documentation inherited - size_t getNumDependentDofs() const override; - - // Documentation inherited - DegreeOfFreedom* getDependentDof(size_t index) override; - - // Documentation inherited - const DegreeOfFreedom* getDependentDof(size_t index) const override; - - // Documentation inherited - const std::vector& getDependentDofs() override; - - // Documentation inherited - const std::vector& getDependentDofs() const override; - - // Documentation inherited - const std::vector getChainDofs() const override; - - //---------------------------------------------------------------------------- - /// \{ \name Jacobian Functions - //---------------------------------------------------------------------------- - - // Documentation inherited - const math::Jacobian& getJacobian() const override final; - - // Prevent the inherited getJacobian functions from being shadowed - using TemplatedJacobianNode::getJacobian; - - // Documentation inherited - const math::Jacobian& getWorldJacobian() const override final; - - // Prevent the inherited getWorldJacobian functions from being shadowed - using TemplatedJacobianNode::getWorldJacobian; - - // Documentation inherited - const math::Jacobian& getJacobianSpatialDeriv() const override final; - - // Prevent the inherited getJacobianSpatialDeriv functions from being shadowed - using TemplatedJacobianNode::getJacobianSpatialDeriv; - - // Documentation inherited - const math::Jacobian& getJacobianClassicDeriv() const override final; - - // Prevent the inherited getJacobianClassicDeriv functions from being shadowed - using TemplatedJacobianNode::getJacobianClassicDeriv; - - /// \} - protected: /// Constructor used by the Skeleton class @@ -247,49 +174,12 @@ class ShapeNode : /// class. Node* cloneNode(BodyNode* parent) const override; - /// Update the Jacobian of this ShapeNode. getJacobian() calls this function - /// if mIsShapeNodeJacobianDirty is true. - void updateShapeNodeJacobian() const; - - /// Update the World Jacobian cache. - void updateWorldJacobian() const; - - /// Update the spatial time derivative of the ShapeNode Jacobian. - /// getJacobianSpatialDeriv() calls this function if - /// mIsShapeNodeJacobianSpatialDerivDirty is true. - void updateShapeNodeJacobianSpatialDeriv() const; - - /// Update the classic time derivative of the ShapeNode Jacobian. - /// getJacobianClassicDeriv() calls this function if - /// mIsWorldJacobianClassicDerivDirty is true. - void updateWorldJacobianClassicDeriv() const; - protected: /// Properties of this ShapeNode DEPRECATED(6.0) UniqueProperties mShapeNodeP; - /// Cached Jacobian of this ShapeNode - /// - /// Do not use directly! Use getJacobian() to access this quantity - mutable math::Jacobian mShapeNodeJacobian; - - /// Cached World Jacobian of this ShapeNode - /// - /// Do not use directly! Use getWorldJacobian() to access this quantity - mutable math::Jacobian mWorldJacobian; - - /// Spatial time derivative of ShapeNode Jacobian - /// - /// Do not use directly! Use getJacobianSpatialDeriv() to access this quantity - mutable math::Jacobian mShapeNodeJacobianSpatialDeriv; - - /// Classic time derivative of the ShapeNode Jacobian - /// - /// Do not use directly! Use getJacobianClassicDeriv() to access this quantity - mutable math::Jacobian mWorldJacobianClassicDeriv; - /// Shape updated signal ShapeUpdatedSignal mShapeUpdatedSignal; diff --git a/dart/dynamics/detail/CompositeNode.h b/dart/dynamics/detail/CompositeNode.h new file mode 100644 index 0000000000000..3b215d6d5831d --- /dev/null +++ b/dart/dynamics/detail/CompositeNode.h @@ -0,0 +1,97 @@ +/* + * 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_COMPOSITENODE_H_ +#define DART_DYNAMICS_DETAIL_COMPOSITENODE_H_ + +#include "dart/dynamics/CompositeNode.h" + +namespace dart { +namespace dynamics { + +//============================================================================== +template +void CompositeStateNode::setNodeState(const Node::State& otherState) +{ + common::Composite::setCompositeState(static_cast(otherState)); +} + +//============================================================================== +template +std::unique_ptr CompositeStateNode::getNodeState() const +{ + return common::make_unique(common::Composite::getCompositeState()); +} + +//============================================================================== +template +void CompositeStateNode::copyNodeStateTo( + std::unique_ptr& outputState) const +{ + common::Composite::copyCompositeStateTo(static_cast(*outputState)); +} + +//============================================================================== +template +void CompositePropertiesNode::setNodeProperties( + const Node::Properties& otherProperties) +{ + common::Composite::setCompositeProperties( + static_cast(otherProperties)); +} + +//============================================================================== +template +std::unique_ptr +CompositePropertiesNode::getNodeProperties() const +{ + return common::make_unique( + common::Composite::getCompositeProperties()); +} + +//============================================================================== +template +void CompositePropertiesNode::copyNodePropertiesTo( + std::unique_ptr& outputProperties) const +{ + common::Composite::copyCompositePropertiesTo( + static_cast(*outputProperties)); +} + +} // namespace dynamics +} // namespace dart + +#endif // DART_DYNAMICS_DETAIL_COMPOSITENODE_H_ diff --git a/dart/dynamics/detail/EntityNode.h b/dart/dynamics/detail/EntityNode.h new file mode 100644 index 0000000000000..f563c232aece7 --- /dev/null +++ b/dart/dynamics/detail/EntityNode.h @@ -0,0 +1,85 @@ +/* + * 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_ENTITYNODE_H_ +#define DART_DYNAMICS_DETAIL_ENTITYNODE_H_ + +#include "dart/dynamics/EntityNode.h" + +namespace dart { +namespace dynamics { + +//============================================================================== +template +void EntityNode::setAspectProperties( + const typename NameAspect::Properties& properties) +{ + setName(properties.mName); +} + +//============================================================================== +template +const std::string& EntityNode::setName(const std::string& newName) +{ + using NameImpl = detail::EntityNodeAspectBase; + + if(NameImpl::mAspectProperties.mName == newName && !newName.empty()) + return NameImpl::mAspectProperties.mName; + + const std::string oldName = NameImpl::mAspectProperties.mName; + + NameImpl::mAspectProperties.mName = Node::registerNameChange(newName); + + Node::incrementVersion(); + Entity::mNameChangedSignal.raise( + this, oldName, NameImpl::mAspectProperties.mName); + + return NameImpl::mAspectProperties.mName; +} + +//============================================================================== +template +const std::string& EntityNode::getName() const +{ + using NameImpl = detail::EntityNodeAspectBase; + return NameImpl::mAspectProperties.mName; +} + +} // namespace dynamics +} // namespace dart + + +#endif // DART_DYNAMICS_DETAIL_ENTITYNODE_H_ diff --git a/dart/dynamics/detail/EntityNodeAspect.h b/dart/dynamics/detail/EntityNodeAspect.h new file mode 100644 index 0000000000000..9a84328eb8e2b --- /dev/null +++ b/dart/dynamics/detail/EntityNodeAspect.h @@ -0,0 +1,101 @@ +/* + * 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_ENTITYNODEASPECT_H_ +#define DART_DYNAMICS_DETAIL_ENTITYNODEASPECT_H_ + +#include "dart/dynamics/Node.h" + +namespace dart { +namespace dynamics { + +template +class EntityNode; + +namespace detail { + +//============================================================================== +struct EntityNodeProperties +{ + std::string mName; +}; + +//============================================================================== +template +using EntityNodeAspectBase = + common::EmbedProperties, EntityNodeProperties>; + +//============================================================================== +template +class EntityNodeBase : public Base, public EntityNodeAspectBase +{ +public: + + /// Forwarding constructor + template + EntityNodeBase(Args&&... args) + : Base(std::forward(args)...) + { + // Do nothing + } + + virtual ~EntityNodeBase() = default; +}; + +//============================================================================== +template +class EntityNodeBase : public common::CompositeJoiner< + EntityNodeAspectBase, Base > +{ +public: + + /// Forwarding constructor + template + EntityNodeBase(Args&&... args) + : common::CompositeJoiner>( + common::NoArg, std::forward(args)...) + { + // Do nothing + } + + virtual ~EntityNodeBase() = default; +}; + +} // namespace detail +} // namespace dart +} // namespace dynamics + +#endif // DART_DYNAMICS_DETAIL_ENTITYNODEASPECT_H_ diff --git a/unittests/testAspect.cpp b/unittests/testAspect.cpp index da800c9f9fe64..881386b8ef02d 100644 --- a/unittests/testAspect.cpp +++ b/unittests/testAspect.cpp @@ -119,7 +119,7 @@ class EmbeddedStateComposite : EmbeddedStateComposite(const EmbeddedStateData& state = EmbeddedStateData()) { - create(static_cast(state)); + createAspect(static_cast(state)); } void setAspectState(const AspectState& state) { mAspectState = state; } @@ -134,7 +134,7 @@ class EmbeddedPropertiesComposite : EmbeddedPropertiesComposite( const EmbeddedPropertiesData& properties = EmbeddedPropertiesData()) { - create(properties); + createAspect(properties); } void setAspectProperties(const AspectProperties& properties) @@ -153,7 +153,7 @@ class EmbeddedStateAndPropertiesComposite : EmbeddedStateAndPropertiesComposite() { - create(); + createAspect(); } void setAspectState(const AspectState& state) { mAspectState = state; } @@ -424,11 +424,11 @@ TEST(Aspect, Specialized) EXPECT_TRUE( mgr.get() == nullptr ); EXPECT_FALSE( usedSpecializedAspectAccess ); usedSpecializedAspectAccess = false; - sub_ptr spec = mgr.create(); + sub_ptr spec = mgr.createAspect(); EXPECT_TRUE( usedSpecializedAspectAccess ); usedSpecializedAspectAccess = false; SpecializedAspect* rawSpec = spec; - sub_ptr generic = mgr.create(); + sub_ptr generic = mgr.createAspect(); EXPECT_FALSE( usedSpecializedAspectAccess ); usedSpecializedAspectAccess = false; GenericAspect* rawGeneric = generic; @@ -440,10 +440,10 @@ TEST(Aspect, Specialized) EXPECT_FALSE( usedSpecializedAspectAccess ); usedSpecializedAspectAccess = false; // SpecializedAspect* newSpec = mgr.createSpecializedAspect(); - SpecializedAspect* newSpec = mgr.create(); + SpecializedAspect* newSpec = mgr.createAspect(); EXPECT_TRUE( usedSpecializedAspectAccess ); usedSpecializedAspectAccess = false; - GenericAspect* newGeneric = mgr.create(); + GenericAspect* newGeneric = mgr.createAspect(); EXPECT_FALSE( usedSpecializedAspectAccess ); usedSpecializedAspectAccess = false; EXPECT_TRUE( nullptr == spec ); @@ -486,7 +486,7 @@ TEST(Aspect, Releasing) EXPECT_TRUE( sender.get() == nullptr ); EXPECT_TRUE( receiver.get() == aspect ); - sender.set(receiver.release()); + sender.set(receiver.releaseAspect()); EXPECT_FALSE( nullptr == aspect ); @@ -519,7 +519,7 @@ TEST(Aspect, Releasing) // EXPECT_TRUE( receiver.getSpecializedAspect() == spec ); // sender.set(receiver.releaseSpecializedAspect()); - sender.set(receiver.release()); + sender.set(receiver.releaseAspect()); EXPECT_FALSE( nullptr == spec ); @@ -563,7 +563,7 @@ TEST(Aspect, Releasing) sub_ptr aspect2 = sender.get(); EXPECT_FALSE( nullptr == aspect2 ); - sender.set(receiver.release()); + sender.set(receiver.releaseAspect()); EXPECT_TRUE( nullptr == aspect2 ); EXPECT_FALSE( nullptr == rec_aspect ); EXPECT_TRUE( receiver.get() == nullptr ); diff --git a/unittests/testSkeleton.cpp b/unittests/testSkeleton.cpp index 91c3d8f89efcd..7022e9378564d 100644 --- a/unittests/testSkeleton.cpp +++ b/unittests/testSkeleton.cpp @@ -581,7 +581,7 @@ TEST(Skeleton, NodePersistence) EndEffector* manip = skel->getBodyNode(0)->createEndEffector("manip"); // Test both methods of adding a Support to an EndEffector - manip->create(); + manip->createAspect(); manip->createSupport(); EXPECT_EQ(skel->getEndEffector("manip"), manip); From 02d1ba527d56cad8ecb48c36c01c97f54d751fdb Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Wed, 6 Apr 2016 13:55:58 -0400 Subject: [PATCH 34/62] cleaned up implementations of Nodes --- dart/common/EmbeddedAspect.h | 6 +-- dart/dynamics/EndEffector.cpp | 63 ++++++++++++++-------- dart/dynamics/EndEffector.h | 63 +++++++++++----------- dart/dynamics/EulerJoint.cpp | 2 +- dart/dynamics/PlanarJoint.cpp | 2 +- dart/dynamics/PointMass.cpp | 2 +- dart/dynamics/PrismaticJoint.cpp | 2 +- dart/dynamics/RevoluteJoint.cpp | 2 +- dart/dynamics/ScrewJoint.cpp | 2 +- dart/dynamics/ShapeFrame.cpp | 21 +++----- dart/dynamics/ShapeFrame.h | 39 +++++++------- dart/dynamics/ShapeNode.cpp | 64 ++++++----------------- dart/dynamics/ShapeNode.h | 62 ++++------------------ dart/dynamics/Skeleton.cpp | 3 +- dart/dynamics/SoftBodyNode.cpp | 3 +- dart/dynamics/UniversalJoint.cpp | 2 +- dart/dynamics/detail/EntityNodeAspect.cpp | 58 ++++++++++++++++++++ dart/dynamics/detail/EntityNodeAspect.h | 7 +++ dart/dynamics/detail/SkeletonAspect.h | 3 +- osgDart/examples/osgHuboPuppet.cpp | 4 +- 20 files changed, 202 insertions(+), 208 deletions(-) create mode 100644 dart/dynamics/detail/EntityNodeAspect.cpp diff --git a/dart/common/EmbeddedAspect.h b/dart/common/EmbeddedAspect.h index ab2f9148dcacf..dc31b7163362d 100644 --- a/dart/common/EmbeddedAspect.h +++ b/dart/common/EmbeddedAspect.h @@ -142,7 +142,7 @@ class EmbedStateOnTopOf : public CompositeJoiner< // Forwarding constructor template EmbedStateOnTopOf(Args&&... args) - : Base(std::forward(args)...) + : Base(NoArg, std::forward(args)...) { // Do nothing } @@ -254,7 +254,7 @@ class EmbedPropertiesOnTopOf : public CompositeJoiner< // Forwarding constructor template EmbedPropertiesOnTopOf(Args&&... args) - : Base(std::forward(args)...) + : Base(NoArg, std::forward(args)...) { // Do nothing } @@ -454,7 +454,7 @@ class EmbedStateAndPropertiesOnTopOf : public CompositeJoiner< // Forwarding constructor template EmbedStateAndPropertiesOnTopOf(Args&&... args) - : Base(std::forward(args)...) + : Base(NoArg, std::forward(args)...) { // Do nothing } diff --git a/dart/dynamics/EndEffector.cpp b/dart/dynamics/EndEffector.cpp index 8cb38cea5909f..218cb9788d6b2 100644 --- a/dart/dynamics/EndEffector.cpp +++ b/dart/dynamics/EndEffector.cpp @@ -43,6 +43,14 @@ namespace dynamics { namespace detail { +//============================================================================== +EndEffectorProperties::EndEffectorProperties(const Eigen::Isometry3d& defaultTf) + : mDefaultTransform(defaultTf) +{ + // Do nothing +} + +//============================================================================== void SupportUpdate(Support* support) { if(EndEffector* ee = support->getComposite()) @@ -68,42 +76,49 @@ bool Support::isActive() const } //============================================================================== -EndEffector::UniqueProperties::UniqueProperties( - const Eigen::Isometry3d& _defaultTransform) - : mDefaultTransform(_defaultTransform) +EndEffector::Properties::Properties( + const FixedFrame::AspectProperties& fixedFrameProperties, + const UniqueProperties& effectorProperties, + const NameAspect::Properties& name, + const common::Composite::Properties& compositeProperties) + : FixedFrame::AspectProperties(fixedFrameProperties), + UniqueProperties(effectorProperties), + NameAspect::Properties(name), + mCompositeProperties(compositeProperties) { // Do nothing } //============================================================================== -EndEffector::PropertiesData::PropertiesData( - const UniqueProperties& _effectorProperties, - const common::Composite::Properties& _compositeProperties) - : UniqueProperties(_effectorProperties), - mCompositeProperties(_compositeProperties) +void EndEffector::setProperties(const Properties& properties) { - // Do nothing + FixedFrame::setAspectProperties(properties); + setAspectProperties(properties); + get()->setProperties(properties); + setCompositeProperties(properties.mCompositeProperties); } //============================================================================== -void EndEffector::setProperties(const PropertiesData& _properties, bool _useNow) +void EndEffector::setProperties(const UniqueProperties& properties, + bool useNow) { - setProperties(static_cast(_properties), _useNow); - setCompositeProperties(_properties.mCompositeProperties); + setDefaultRelativeTransform(properties.mDefaultTransform, useNow); } //============================================================================== -void EndEffector::setProperties(const UniqueProperties& _properties, - bool _useNow) +void EndEffector::setAspectProperties(const AspectProperties& properties) { - setName(_properties.mName); - setDefaultRelativeTransform(_properties.mDefaultTransform, _useNow); + setDefaultRelativeTransform(properties.mDefaultTransform); } //============================================================================== -EndEffector::PropertiesData EndEffector::getEndEffectorProperties() const +EndEffector::Properties EndEffector::getEndEffectorProperties() const { - return PropertiesData(mEndEffectorP, getCompositeProperties()); + return Properties( + FixedFrame::getAspectProperties(), + getAspectProperties(), + get()->getProperties(), + getCompositeProperties()); } //============================================================================== @@ -136,7 +151,7 @@ EndEffector& EndEffector::operator=(const EndEffector& _otherEndEffector) void EndEffector::setDefaultRelativeTransform( const Eigen::Isometry3d& _newDefaultTf, bool _useNow) { - mEndEffectorP.mDefaultTransform = _newDefaultTf; + mAspectProperties.mDefaultTransform = _newDefaultTf; if(_useNow) resetRelativeTransform(); @@ -145,7 +160,7 @@ void EndEffector::setDefaultRelativeTransform( //============================================================================== void EndEffector::resetRelativeTransform() { - setRelativeTransform(mEndEffectorP.mDefaultTransform); + setRelativeTransform(mAspectProperties.mDefaultTransform); } //============================================================================== @@ -162,11 +177,13 @@ void EndEffector::notifyTransformUpdate() } //============================================================================== -EndEffector::EndEffector(BodyNode* parent, const PropertiesData& properties) +EndEffector::EndEffector(BodyNode* parent, const Properties& properties) : Entity(ConstructFrame), Frame(parent), FixedFrame(parent, properties.mDefaultTransform), - detail::EndEffectorCompositeBase( + common::EmbedPropertiesOnTopOf< + EndEffector, detail::EndEffectorProperties, + detail::EndEffectorCompositeBase>( std::make_tuple(parent, properties.mDefaultTransform), common::NoArg) { setProperties(properties); @@ -175,7 +192,7 @@ EndEffector::EndEffector(BodyNode* parent, const PropertiesData& properties) //============================================================================== Node* EndEffector::cloneNode(BodyNode* _parent) const { - EndEffector* ee = new EndEffector(_parent, PropertiesData()); + EndEffector* ee = new EndEffector(_parent, Properties()); ee->duplicateAspects(this); ee->copy(this); diff --git a/dart/dynamics/EndEffector.h b/dart/dynamics/EndEffector.h index 130a409cd2b7e..c2a943b4e2edb 100644 --- a/dart/dynamics/EndEffector.h +++ b/dart/dynamics/EndEffector.h @@ -53,6 +53,15 @@ class Support; namespace detail { +//============================================================================== +struct EndEffectorProperties +{ + Eigen::Isometry3d mDefaultTransform; + + EndEffectorProperties( + const Eigen::Isometry3d& defaultTf = Eigen::Isometry3d::Identity()); +}; + //============================================================================== struct SupportStateData { @@ -89,7 +98,6 @@ class Support final : { public: -// DART_COMMON_ASPECT_STATE_PROPERTY_CONSTRUCTORS( Support, &detail::SupportUpdate, &detail::SupportUpdate ) DART_COMMON_ASPECT_STATE_PROPERTY_CONSTRUCTORS(Support) /// Set/Get the support geometry for this EndEffector. The SupportGeometry @@ -109,41 +117,35 @@ class Support final : }; //============================================================================== -class EndEffector final : public detail::EndEffectorCompositeBase +class EndEffector final : + public common::EmbedPropertiesOnTopOf< + EndEffector, detail::EndEffectorProperties, + detail::EndEffectorCompositeBase> { public: friend class Skeleton; friend class BodyNode; - struct UniqueProperties - { - /// Name of this EndEffector - std::string mName; - - /// The relative transform will be set to this whenever - /// resetRelativeTransform() is called - Eigen::Isometry3d mDefaultTransform; - - /// Default constructor - UniqueProperties( - const Eigen::Isometry3d& _defaultTransform = - Eigen::Isometry3d::Identity()); - }; + using UniqueProperties = detail::EndEffectorProperties; - struct PropertiesData : UniqueProperties + struct Properties : + FixedFrame::AspectProperties, + UniqueProperties, + NameAspect::Properties { - PropertiesData( - const UniqueProperties& _effectorProperties = UniqueProperties(), - const common::Composite::Properties& _compositeProperties = + Properties( + const FixedFrame::AspectProperties& fixedFrameProperties + = FixedFrame::AspectProperties(), + const UniqueProperties& effectorProperties = UniqueProperties(), + const NameAspect::Properties& name = "EndEffector", + const common::Composite::Properties& compositeProperties = common::Composite::Properties()); /// The properties of the EndEffector's Aspects common::Composite::Properties mCompositeProperties; }; - using Properties = Node::MakeProperties; - /// Destructor virtual ~EndEffector() = default; @@ -153,14 +155,17 @@ class EndEffector final : public detail::EndEffectorCompositeBase /// Set the Properties of this EndEffector. If _useNow is true, the current /// Transform will be set to the new default transform. - void setProperties(const PropertiesData& _properties, bool _useNow=false); + void setProperties(const Properties& _properties); /// Set the Properties of this EndEffector. If _useNow is true, the current /// Transform will be set to the new default transform. - void setProperties(const UniqueProperties& _properties, bool _useNow=false); + void setProperties(const UniqueProperties& properties, bool useNow=false); + + /// Set the AspectProperties of this EndEffector + void setAspectProperties(const AspectProperties& properties); /// Get the Properties of this EndEffector - PropertiesData getEndEffectorProperties() const; + Properties getEndEffectorProperties() const; /// Copy the State and Properties of another EndEffector void copy(const EndEffector& _otherEndEffector); @@ -176,7 +181,7 @@ class EndEffector final : public detail::EndEffectorCompositeBase /// resetRelativeTransform() is called. If _useNow is set to true, then /// resetRelativeTransform() will be called at the end of this function. void setDefaultRelativeTransform(const Eigen::Isometry3d& _newDefaultTf, - bool _useNow); + bool _useNow = false); /// Set the current relative transform of this EndEffector to the default /// relative transform of this EndEffector. The default relative transform can @@ -197,16 +202,12 @@ class EndEffector final : public detail::EndEffectorCompositeBase protected: /// Constructor used by the Skeleton class - explicit EndEffector(BodyNode* parent, const PropertiesData& properties); + explicit EndEffector(BodyNode* parent, const Properties& properties); /// Create a clone of this BodyNode. This may only be called by the Skeleton /// class. virtual Node* cloneNode(BodyNode* _parent) const override; - - /// Properties of this EndEffector - DEPRECATED(6.0) - UniqueProperties mEndEffectorP; }; } // namespace dynamics diff --git a/dart/dynamics/EulerJoint.cpp b/dart/dynamics/EulerJoint.cpp index 60b8ba3531c6e..2caeec4a75b81 100644 --- a/dart/dynamics/EulerJoint.cpp +++ b/dart/dynamics/EulerJoint.cpp @@ -290,7 +290,7 @@ Eigen::Matrix EulerJoint::getLocalJacobianStatic( //============================================================================== EulerJoint::EulerJoint(const Properties& properties) - : detail::EulerJointBase(common::NoArg, properties) + : detail::EulerJointBase(properties) { // Inherited Aspects must be created in the final joint class in reverse order // or else we get pure virtual function calls diff --git a/dart/dynamics/PlanarJoint.cpp b/dart/dynamics/PlanarJoint.cpp index 1d41091f54230..fe8ee9c1c4116 100644 --- a/dart/dynamics/PlanarJoint.cpp +++ b/dart/dynamics/PlanarJoint.cpp @@ -215,7 +215,7 @@ Eigen::Matrix PlanarJoint::getLocalJacobianStatic( //============================================================================== PlanarJoint::PlanarJoint(const Properties& properties) - : detail::PlanarJointBase(common::NoArg, properties) + : detail::PlanarJointBase(properties) { // Inherited Aspects must be created in the final joint class in reverse order // or else we get pure virtual function calls diff --git a/dart/dynamics/PointMass.cpp b/dart/dynamics/PointMass.cpp index 92d452e80c67a..a4a5a92d48130 100644 --- a/dart/dynamics/PointMass.cpp +++ b/dart/dynamics/PointMass.cpp @@ -1139,7 +1139,7 @@ PointMassNotifier::PointMassNotifier(SoftBodyNode* _parentSoftBody, mNeedPartialAccelerationUpdate(true), mParentSoftBodyNode(_parentSoftBody) { - // Do nothing + setName(_name); } //============================================================================== diff --git a/dart/dynamics/PrismaticJoint.cpp b/dart/dynamics/PrismaticJoint.cpp index 975422c44546d..a4f5eeafb67b4 100644 --- a/dart/dynamics/PrismaticJoint.cpp +++ b/dart/dynamics/PrismaticJoint.cpp @@ -141,7 +141,7 @@ const Eigen::Vector3d& PrismaticJoint::getAxis() const //============================================================================== PrismaticJoint::PrismaticJoint(const Properties& properties) - : detail::PrismaticJointBase(common::NoArg, properties) + : detail::PrismaticJointBase(properties) { // Inherited Aspects must be created in the final joint class in reverse order // or else we get pure virtual function calls diff --git a/dart/dynamics/RevoluteJoint.cpp b/dart/dynamics/RevoluteJoint.cpp index 6acfdf87012b3..ddd8192018db5 100644 --- a/dart/dynamics/RevoluteJoint.cpp +++ b/dart/dynamics/RevoluteJoint.cpp @@ -142,7 +142,7 @@ const Eigen::Vector3d& RevoluteJoint::getAxis() const //============================================================================== RevoluteJoint::RevoluteJoint(const Properties& properties) - : detail::RevoluteJointBase(common::NoArg, properties) + : detail::RevoluteJointBase(properties) { // Inherited Aspects must be created in the final joint class in reverse order // or else we get pure virtual function calls diff --git a/dart/dynamics/ScrewJoint.cpp b/dart/dynamics/ScrewJoint.cpp index fdf793b31c4fd..81f3a051d2176 100644 --- a/dart/dynamics/ScrewJoint.cpp +++ b/dart/dynamics/ScrewJoint.cpp @@ -160,7 +160,7 @@ double ScrewJoint::getPitch() const //============================================================================== ScrewJoint::ScrewJoint(const Properties& properties) - : detail::ScrewJointBase(common::NoArg, properties) + : detail::ScrewJointBase(properties) { // Inherited Aspects must be created in the final joint class in reverse order // or else we get pure virtual function calls diff --git a/dart/dynamics/ShapeFrame.cpp b/dart/dynamics/ShapeFrame.cpp index a4d6dc09d295f..1daad56ce6c4e 100644 --- a/dart/dynamics/ShapeFrame.cpp +++ b/dart/dynamics/ShapeFrame.cpp @@ -71,6 +71,13 @@ DynamicsAspectProperties::DynamicsAspectProperties( // Do nothing } +//============================================================================== +ShapeFrameProperties::ShapeFrameProperties(const ShapePtr& shape) + : mShape(shape) +{ + // Do nothing +} + } // namespace detail //============================================================================== @@ -182,20 +189,6 @@ DynamicsAspect::DynamicsAspect( // Do nothing } -//============================================================================== -ShapeFrame::UniqueProperties::UniqueProperties(const ShapePtr& shape) - : mShape(shape) -{ - // Do nothing -} - -//============================================================================== -ShapeFrame::UniqueProperties::UniqueProperties(ShapePtr&& shape) - : mShape(std::move(shape)) -{ - // Do nothing -} - //============================================================================== void ShapeFrame::setProperties(const ShapeFrame::UniqueProperties& properties) { diff --git a/dart/dynamics/ShapeFrame.h b/dart/dynamics/ShapeFrame.h index 52512112a7038..7eb12d63dbcf1 100644 --- a/dart/dynamics/ShapeFrame.h +++ b/dart/dynamics/ShapeFrame.h @@ -97,12 +97,24 @@ struct DynamicsAspectProperties /// Constructor DynamicsAspectProperties(const double frictionCoeff = 1.0, - const double restitutionCoeff = 0.0); + const double restitutionCoeff = 0.0); /// Destructor virtual ~DynamicsAspectProperties() = default; }; +struct ShapeFrameProperties +{ + /// Pointer to a shape + ShapePtr mShape; + + /// Constructor + ShapeFrameProperties(const ShapePtr& shape = nullptr); + + /// Virtual destructor + virtual ~ShapeFrameProperties() = default; +}; + } // namespace detail class VisualAspect final : @@ -214,8 +226,10 @@ class DynamicsAspect final : class ShapeFrame : public virtual common::VersionCounter, - public common::SpecializedForAspect< - VisualAspect, CollisionAspect, DynamicsAspect>, + public common::EmbedPropertiesOnTopOf< + ShapeFrame, detail::ShapeFrameProperties, + common::SpecializedForAspect< + VisualAspect, CollisionAspect, DynamicsAspect> >, public virtual Frame { public: @@ -232,21 +246,7 @@ class ShapeFrame : const Eigen::Isometry3d& oldTransform, const Eigen::Isometry3d& newTransform)>; - struct UniqueProperties - { - /// Shape pointer - ShapePtr mShape; - - /// Composed constructor - UniqueProperties(const ShapePtr& shape = nullptr); - - /// Composed move constructor - UniqueProperties(ShapePtr&& shape); - - virtual ~UniqueProperties() = default; - }; - - using AspectProperties = UniqueProperties; + using UniqueProperties = AspectProperties; using Properties = UniqueProperties; /// Destructor @@ -288,9 +288,6 @@ class ShapeFrame : /// Constructor ShapeFrame(Frame* parent, const ShapePtr& shape = nullptr); - /// ShapeFrame properties - Properties mAspectProperties; - /// Shape updated signal ShapeUpdatedSignal mShapeUpdatedSignal; diff --git a/dart/dynamics/ShapeNode.cpp b/dart/dynamics/ShapeNode.cpp index 87fd1c825785a..d811f5969e321 100644 --- a/dart/dynamics/ShapeNode.cpp +++ b/dart/dynamics/ShapeNode.cpp @@ -40,62 +40,36 @@ namespace dart { namespace dynamics { -//============================================================================== -ShapeNode::UniqueProperties::UniqueProperties( - const std::string& name, - const Eigen::Isometry3d& relativeTransform) - : mName(name), - mRelativeTransform(relativeTransform) -{ - // Do nothing -} - //============================================================================== ShapeNode::Properties::Properties( - const ShapeFrame::Properties& shapeFrameProperties, - const ShapeNode::UniqueProperties& shapeNodeProperties, + const ShapeFrame::AspectProperties& shapeFrameProperties, + const FixedFrame::AspectProperties& fixedFrameProperties, + const NameAspect::Properties& name, const CompositeProperties& compositeProperties) - : ShapeFrame::Properties(shapeFrameProperties), - ShapeNode::UniqueProperties(shapeNodeProperties), + : ShapeFrame::AspectProperties(shapeFrameProperties), + FixedFrame::AspectProperties(fixedFrameProperties), + NameAspect::Properties(name), mCompositeProperties(compositeProperties) { // Do nothing } -//============================================================================== -ShapeNode::Properties::Properties( - ShapeFrame::Properties&& shapeFrameProperties, - ShapeNode::UniqueProperties&& shapeNodeProperties, - CompositeProperties&& compositeProperties) - : ShapeFrame::Properties(std::move(shapeFrameProperties)), - ShapeNode::UniqueProperties(std::move(shapeNodeProperties)), - mCompositeProperties(std::move(compositeProperties)) -{ - // Do nothing -} - //============================================================================== void ShapeNode::setProperties(const Properties& properties) { - ShapeFrame::setProperties( - static_cast(properties)); - setProperties(static_cast(properties)); setCompositeProperties(properties.mCompositeProperties); -} - -//============================================================================== -void ShapeNode::setProperties(const ShapeNode::UniqueProperties& properties) -{ - if(!properties.mName.empty()) - setName(properties.mName); - - setRelativeTransform(properties.mRelativeTransform); + ShapeFrame::setAspectProperties(properties); + FixedFrame::setAspectProperties( + static_cast(properties)); + get()->setProperties(properties); } //============================================================================== const ShapeNode::Properties ShapeNode::getShapeNodeProperties() const { - return Properties(ShapeFrame::getAspectProperties(), mShapeNodeP, + return Properties(ShapeFrame::getAspectProperties(), + FixedFrame::getAspectProperties(), + getName(), getCompositeProperties()); } @@ -188,11 +162,7 @@ ShapeNode::ShapeNode(BodyNode* bodyNode, const Properties& properties) Frame(bodyNode), FixedFrame(bodyNode), detail::ShapeNodeCompositeBase( - std::make_tuple(bodyNode, properties.mRelativeTransform), bodyNode), - mShapeUpdatedSignal(ShapeUpdatedSignal()), - mRelativeTransformUpdatedSignal(RelativeTransformUpdatedSignal()), - onShapeUpdated(mShapeUpdatedSignal), - onRelativeTransformUpdated(mRelativeTransformUpdatedSignal) + std::make_tuple(bodyNode, properties.mRelativeTf), bodyNode) { setProperties(properties); } @@ -205,11 +175,7 @@ ShapeNode::ShapeNode(BodyNode* bodyNode, Frame(bodyNode), FixedFrame(bodyNode), detail::ShapeNodeCompositeBase( - std::make_tuple(bodyNode, Eigen::Isometry3d::Identity()), bodyNode), - mShapeUpdatedSignal(ShapeUpdatedSignal()), - mRelativeTransformUpdatedSignal(RelativeTransformUpdatedSignal()), - onShapeUpdated(mShapeUpdatedSignal), - onRelativeTransformUpdated(mRelativeTransformUpdatedSignal) + std::make_tuple(bodyNode, Eigen::Isometry3d::Identity()), bodyNode) { // TODO(MXG): Consider changing this to a delegating constructor instead Properties prop; diff --git a/dart/dynamics/ShapeNode.h b/dart/dynamics/ShapeNode.h index 9199ffa2679d6..8636767a8d3bd 100644 --- a/dart/dynamics/ShapeNode.h +++ b/dart/dynamics/ShapeNode.h @@ -81,40 +81,20 @@ class ShapeNode : public detail::ShapeNodeCompositeBase using CompositeProperties = common::Composite::Properties; - struct UniqueProperties - { - /// Name of ShapeNode - std::string mName; - - /// The current relative transform of the Shape in the ShapeNode - Eigen::Isometry3d mRelativeTransform; - // TODO(JS): Consider moving this to a FixedFrame::State - // (or FixedFrame::Properties) struct and the inheriting that struct. - // Endeffector has similar issue. - - /// Composed constructor - UniqueProperties( - const std::string& name = "", - const Eigen::Isometry3d& relativeTransform - = Eigen::Isometry3d::Identity()); - }; - - struct Properties : ShapeFrame::Properties, UniqueProperties + struct Properties : + ShapeFrame::AspectProperties, + FixedFrame::AspectProperties, + NameAspect::Properties { /// Composed constructor Properties( - const ShapeFrame::Properties& shapeFrameProperties - = ShapeFrame::Properties(), - const ShapeNode::UniqueProperties& shapeNodeProperties - = ShapeNode::UniqueProperties(), + const ShapeFrame::AspectProperties& shapeFrameProperties + = ShapeFrame::AspectProperties(), + const FixedFrame::AspectProperties& fixedFrameProperties + = FixedFrame::AspectProperties(), + const NameAspect::Properties& name = "ShapeNode", const CompositeProperties& compositeProperties = CompositeProperties()); - /// Composed move constructor - Properties( - ShapeFrame::Properties&& shapeFrameProperties, - ShapeNode::UniqueProperties&& shapeNodeProperties, - CompositeProperties&& compositeProperties); - /// The properties of the ShapeNode's Aspects CompositeProperties mCompositeProperties; }; @@ -125,9 +105,6 @@ class ShapeNode : public detail::ShapeNodeCompositeBase /// Set the Properties of this ShapeNode void setProperties(const Properties& properties); - /// Set the Properties of this ShapeNode - void setProperties(const UniqueProperties& properties); - /// Get the Properties of this ShapeNode const Properties getShapeNodeProperties() const; @@ -174,27 +151,6 @@ class ShapeNode : public detail::ShapeNodeCompositeBase /// class. Node* cloneNode(BodyNode* parent) const override; -protected: - - /// Properties of this ShapeNode - DEPRECATED(6.0) - UniqueProperties mShapeNodeP; - - /// Shape updated signal - ShapeUpdatedSignal mShapeUpdatedSignal; - - /// Relative transformation updated signal - RelativeTransformUpdatedSignal mRelativeTransformUpdatedSignal; - -public: - - /// Slot register for shape updated signal - common::SlotRegister onShapeUpdated; - - /// Slot register for relative transformation updated signal - common::SlotRegister - onRelativeTransformUpdated; - }; } // namespace dynamics diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index 69e045ef762ce..46b97fb67f477 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -174,8 +174,7 @@ SkeletonAspectProperties::SkeletonAspectProperties( const Eigen::Vector3d& _gravity, double _timeStep, bool _enabledSelfCollisionCheck, - bool _enableAdjacentBodyCheck, - size_t _version) + bool _enableAdjacentBodyCheck) : mName(_name), mIsMobile(_isMobile), mGravity(_gravity), diff --git a/dart/dynamics/SoftBodyNode.cpp b/dart/dynamics/SoftBodyNode.cpp index d6ad2d7c76cb0..a6c4568c1454f 100644 --- a/dart/dynamics/SoftBodyNode.cpp +++ b/dart/dynamics/SoftBodyNode.cpp @@ -233,8 +233,7 @@ SoftBodyNode::SoftBodyNode(BodyNode* _parentBodyNode, const Properties& _properties) : Entity(Frame::World(), false), Frame(Frame::World()), - Base(common::NoArg, - std::make_tuple(_parentBodyNode, _parentJoint, _properties)), + Base(std::make_tuple(_parentBodyNode, _parentJoint, _properties)), mSoftShapeNode(nullptr) { createSoftBodyAspect(); diff --git a/dart/dynamics/UniversalJoint.cpp b/dart/dynamics/UniversalJoint.cpp index 9468c36510bba..0e03160d94b1a 100644 --- a/dart/dynamics/UniversalJoint.cpp +++ b/dart/dynamics/UniversalJoint.cpp @@ -164,7 +164,7 @@ Eigen::Matrix UniversalJoint::getLocalJacobianStatic( //============================================================================== UniversalJoint::UniversalJoint(const Properties& properties) - : detail::UniversalJointBase(common::NoArg, properties) + : detail::UniversalJointBase(properties) { // Inherited Aspects must be created in the final joint class in reverse order // or else we get pure virtual function calls diff --git a/dart/dynamics/detail/EntityNodeAspect.cpp b/dart/dynamics/detail/EntityNodeAspect.cpp new file mode 100644 index 0000000000000..09de27d2c5571 --- /dev/null +++ b/dart/dynamics/detail/EntityNodeAspect.cpp @@ -0,0 +1,58 @@ +/* + * 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. + */ + +#include "dart/dynamics/detail/EntityNodeAspect.h" + +namespace dart { +namespace dynamics { +namespace detail { + +//============================================================================== +EntityNodeProperties::EntityNodeProperties(const std::string& name) + : mName(name) +{ + // Do nothing +} + +//============================================================================== +EntityNodeProperties::operator const std::string&() const +{ + return mName; +} + +} // namespace detail +} // namespace dynamics +} // namespace dart diff --git a/dart/dynamics/detail/EntityNodeAspect.h b/dart/dynamics/detail/EntityNodeAspect.h index 9a84328eb8e2b..a8619672aff7e 100644 --- a/dart/dynamics/detail/EntityNodeAspect.h +++ b/dart/dynamics/detail/EntityNodeAspect.h @@ -50,7 +50,14 @@ namespace detail { //============================================================================== struct EntityNodeProperties { + /// Name of the Entity/Node std::string mName; + + /// Default constructor + EntityNodeProperties(const std::string& name = ""); + + /// Implicit conversion to a string + operator const std::string&() const; }; //============================================================================== diff --git a/dart/dynamics/detail/SkeletonAspect.h b/dart/dynamics/detail/SkeletonAspect.h index f1c722150a529..1792f605355ed 100644 --- a/dart/dynamics/detail/SkeletonAspect.h +++ b/dart/dynamics/detail/SkeletonAspect.h @@ -86,8 +86,7 @@ struct SkeletonAspectProperties const Eigen::Vector3d& _gravity = Eigen::Vector3d(0.0, 0.0, -9.81), double _timeStep = 0.001, bool _enabledSelfCollisionCheck = false, - bool _enableAdjacentBodyCheck = false, - size_t _version = 0); + bool _enableAdjacentBodyCheck = false); }; //============================================================================== diff --git a/osgDart/examples/osgHuboPuppet.cpp b/osgDart/examples/osgHuboPuppet.cpp index e5306abe06ab5..32e01cdd5e0ea 100644 --- a/osgDart/examples/osgHuboPuppet.cpp +++ b/osgDart/examples/osgHuboPuppet.cpp @@ -1391,6 +1391,8 @@ int main() setupEndEffectors(hubo); Eigen::VectorXd positions = hubo->getPositions(); + // We make a clone to test whether the cloned version behaves the exact same + // as the original version. hubo = hubo->clone("hubo_copy"); hubo->setPositions(positions); @@ -1418,7 +1420,7 @@ int main() std::cout << "Alt + Click: Try to translate a body without changing its orientation\n" << "Ctrl + Click: Try to rotate a body without changing its translation\n" << "Shift + Click: Move a body using only its parent joint\n" - << "1 -> 4: Toggle the interactive target of an EndEffector\n" + << "1 -> 6: Toggle the interactive target of an EndEffector\n" << "W A S D: Move the robot around the scene\n" << "Q E: Rotate the robot counter-clockwise and clockwise\n" << "F Z: Shift the robot's elevation up and down\n" From c5d937582fd0c33a984d43179875dd49224747f1 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Wed, 6 Apr 2016 15:59:31 -0400 Subject: [PATCH 35/62] embedded state aspect into MultiDofJoint --- dart/dynamics/MultiDofJoint.h | 47 +------ dart/dynamics/detail/MultiDofJoint.h | 132 +++++++++--------- .../dynamics/detail/MultiDofJointProperties.h | 55 +++++++- 3 files changed, 125 insertions(+), 109 deletions(-) diff --git a/dart/dynamics/MultiDofJoint.h b/dart/dynamics/MultiDofJoint.h index fdcee1fa56420..19ad20b0a2116 100644 --- a/dart/dynamics/MultiDofJoint.h +++ b/dart/dynamics/MultiDofJoint.h @@ -67,6 +67,7 @@ class MultiDofJoint : public detail::MultiDofJointBase< MultiDofJoint, DOF using Base = detail::MultiDofJointBase, DOF>; using UniqueProperties = detail::MultiDofJointUniqueProperties; using Properties = detail::MultiDofJointProperties; + using AspectState = typename Base::AspectState; using AspectProperties = typename Base::AspectProperties; DART_BAKE_SPECIALIZED_ASPECT_IRREGULAR( typename MultiDofJoint::Aspect, MultiDofJointAspect ) @@ -82,6 +83,9 @@ class MultiDofJoint : public detail::MultiDofJointBase< MultiDofJoint, DOF /// Set the Properties of this MultiDofJoint void setProperties(const UniqueProperties& _properties); + /// Set the AspectState of this MultiDofJoint + void setAspectState(const AspectState& state); + /// Set the AspectProperties of this MultiDofJoint void setAspectProperties(const AspectProperties& properties); @@ -599,49 +603,6 @@ class MultiDofJoint : public detail::MultiDofJointBase< MultiDofJoint, DOF /// Array of DegreeOfFreedom objects std::array mDofs; - /// Command - Vector mCommands; - - //---------------------------------------------------------------------------- - // Configuration - //---------------------------------------------------------------------------- - - /// Position - Vector mPositions; - - /// Derivatives w.r.t. an arbitrary scalr variable - Vector mPositionDeriv; - - //---------------------------------------------------------------------------- - // Velocity - //---------------------------------------------------------------------------- - - /// Generalized velocity - Vector mVelocities; - - /// Derivatives w.r.t. an arbitrary scalr variable - Vector mVelocitiesDeriv; - - //---------------------------------------------------------------------------- - // Acceleration - //---------------------------------------------------------------------------- - - /// Generalized acceleration - Vector mAccelerations; - - /// Derivatives w.r.t. an arbitrary scalr variable - Vector mAccelerationsDeriv; - - //---------------------------------------------------------------------------- - // Force - //---------------------------------------------------------------------------- - - /// Generalized force - Vector mForces; - - /// Derivatives w.r.t. an arbitrary scalr variable - Vector mForcesDeriv; - //---------------------------------------------------------------------------- // Impulse //---------------------------------------------------------------------------- diff --git a/dart/dynamics/detail/MultiDofJoint.h b/dart/dynamics/detail/MultiDofJoint.h index 8d07b9b6a1cda..b3cde99a6185a 100644 --- a/dart/dynamics/detail/MultiDofJoint.h +++ b/dart/dynamics/detail/MultiDofJoint.h @@ -100,6 +100,17 @@ void MultiDofJoint::setProperties(const UniqueProperties& _properties) setAspectProperties(_properties); } +//============================================================================== +template +void MultiDofJoint::setAspectState(const AspectState& state) +{ + setCommands(state.mCommands); + setPositionsStatic(state.mPositions); + setVelocitiesStatic(state.mVelocities); + setAccelerationsStatic(state.mAccelerations); + setForces(state.mForces); +} + //============================================================================== template void MultiDofJoint::setAspectProperties(const AspectProperties& properties) @@ -306,7 +317,7 @@ void MultiDofJoint::setCommand(size_t _index, double command) switch (Joint::mAspectProperties.mActuatorType) { case Joint::FORCE: - mCommands[_index] = math::clip(command, + this->mAspectState.mCommands[_index] = math::clip(command, Base::mAspectProperties.mForceLowerLimits[_index], Base::mAspectProperties.mForceUpperLimits[_index]); break; @@ -317,20 +328,20 @@ void MultiDofJoint::setCommand(size_t _index, double command) << command << ") command for a PASSIVE joint [" << this->getName() << "].\n"; } - mCommands[_index] = command; + this->mAspectState.mCommands[_index] = command; break; case Joint::SERVO: - mCommands[_index] = math::clip(command, + this->mAspectState.mCommands[_index] = math::clip(command, Base::mAspectProperties.mVelocityLowerLimits[_index], Base::mAspectProperties.mVelocityUpperLimits[_index]); break; case Joint::ACCELERATION: - mCommands[_index] = math::clip(command, + this->mAspectState.mCommands[_index] = math::clip(command, Base::mAspectProperties.mAccelerationLowerLimits[_index], Base::mAspectProperties.mAccelerationUpperLimits[_index]); break; case Joint::VELOCITY: - mCommands[_index] = math::clip(command, + this->mAspectState.mCommands[_index] = math::clip(command, Base::mAspectProperties.mVelocityLowerLimits[_index], Base::mAspectProperties.mVelocityUpperLimits[_index]); // TODO: This possibly makes the acceleration to exceed the limits. @@ -342,7 +353,7 @@ void MultiDofJoint::setCommand(size_t _index, double command) << command << ") command for a LOCKED joint [" << this->getName() << "].\n"; } - mCommands[_index] = command; + this->mAspectState.mCommands[_index] = command; break; default: assert(false); @@ -360,7 +371,7 @@ double MultiDofJoint::getCommand(size_t _index) const return 0.0; } - return mCommands[_index]; + return this->mAspectState.mCommands[_index]; } //============================================================================== @@ -376,7 +387,7 @@ void MultiDofJoint::setCommands(const Eigen::VectorXd& _commands) switch (Joint::mAspectProperties.mActuatorType) { case Joint::FORCE: - mCommands = math::clip(_commands, + this->mAspectState.mCommands = math::clip(_commands, Base::mAspectProperties.mForceLowerLimits, Base::mAspectProperties.mForceUpperLimits); break; @@ -387,20 +398,20 @@ void MultiDofJoint::setCommands(const Eigen::VectorXd& _commands) << _commands.transpose() << ") command for a PASSIVE joint [" << this->getName() << "].\n"; } - mCommands = _commands; + this->mAspectState.mCommands = _commands; break; case Joint::SERVO: - mCommands = math::clip(_commands, + this->mAspectState.mCommands = math::clip(_commands, Base::mAspectProperties.mVelocityLowerLimits, Base::mAspectProperties.mVelocityUpperLimits); break; case Joint::ACCELERATION: - mCommands = math::clip(_commands, + this->mAspectState.mCommands = math::clip(_commands, Base::mAspectProperties.mAccelerationLowerLimits, Base::mAspectProperties.mAccelerationUpperLimits); break; case Joint::VELOCITY: - mCommands = math::clip(_commands, + this->mAspectState.mCommands = math::clip(_commands, Base::mAspectProperties.mVelocityLowerLimits, Base::mAspectProperties.mVelocityUpperLimits); // TODO: This possibly makes the acceleration to exceed the limits. @@ -412,7 +423,7 @@ void MultiDofJoint::setCommands(const Eigen::VectorXd& _commands) << _commands.transpose() << ") command for a LOCKED joint [" << this->getName() << "].\n"; } - mCommands = _commands; + this->mAspectState.mCommands = _commands; break; default: assert(false); @@ -424,14 +435,14 @@ void MultiDofJoint::setCommands(const Eigen::VectorXd& _commands) template Eigen::VectorXd MultiDofJoint::getCommands() const { - return mCommands; + return this->mAspectState.mCommands; } //============================================================================== template void MultiDofJoint::resetCommands() { - mCommands.setZero(); + this->mAspectState.mCommands.setZero(); } //============================================================================== @@ -444,11 +455,11 @@ void MultiDofJoint::setPosition(size_t _index, double _position) return; } - if(mPositions[_index] == _position) + if(this->mAspectState.mPositions[_index] == _position) return; // Note: It would not make much sense to use setPositionsStatic() here - mPositions[_index] = _position; + this->mAspectState.mPositions[_index] = _position; this->notifyPositionUpdate(); } @@ -627,16 +638,16 @@ void MultiDofJoint::setVelocity(size_t _index, double _velocity) return; } - if(mVelocities[_index] == _velocity) + if(this->mAspectState.mVelocities[_index] == _velocity) return; // Note: It would not make much sense to use setVelocitiesStatic() here - mVelocities[_index] = _velocity; + this->mAspectState.mVelocities[_index] = _velocity; this->notifyVelocityUpdate(); #if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) if (Joint::mAspectProperties.mActuatorType == Joint::VELOCITY) - mCommands[_index] = this->getVelocitiesStatic()[_index]; + this->mAspectState.mCommands[_index] = this->getVelocitiesStatic()[_index]; // TODO: Remove at DART 5.1. #endif } @@ -668,7 +679,7 @@ void MultiDofJoint::setVelocities(const Eigen::VectorXd& _velocities) #if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) if (Joint::mAspectProperties.mActuatorType == Joint::VELOCITY) - mCommands = this->getVelocitiesStatic(); + this->mAspectState.mCommands = this->getVelocitiesStatic(); // TODO: Remove at DART 5.1. #endif } @@ -808,16 +819,16 @@ void MultiDofJoint::setAcceleration(size_t _index, double _acceleration) return; } - if(mAccelerations[_index] == _acceleration) + if(this->mAspectState.mAccelerations[_index] == _acceleration) return; // Note: It would not make much sense to use setAccelerationsStatic() here - mAccelerations[_index] = _acceleration; + this->mAspectState.mAccelerations[_index] = _acceleration; this->notifyAccelerationUpdate(); #if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) if (Joint::mAspectProperties.mActuatorType == Joint::ACCELERATION) - mCommands[_index] = this->getAccelerationsStatic()[_index]; + this->mAspectState.mCommands[_index] = this->getAccelerationsStatic()[_index]; // TODO: Remove at DART 5.1. #endif } @@ -849,7 +860,7 @@ void MultiDofJoint::setAccelerations(const Eigen::VectorXd& _accelerations) #if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) if (Joint::mAspectProperties.mActuatorType == Joint::ACCELERATION) - mCommands = this->getAccelerationsStatic(); + this->mAspectState.mCommands = this->getAccelerationsStatic(); // TODO: Remove at DART 5.1. #endif } @@ -928,10 +939,10 @@ double MultiDofJoint::getAccelerationUpperLimit(size_t _index) const template void MultiDofJoint::setPositionsStatic(const Vector& _positions) { - if(mPositions == _positions) + if(this->mAspectState.mPositions == _positions) return; - mPositions = _positions; + this->mAspectState.mPositions = _positions; this->notifyPositionUpdate(); } @@ -940,17 +951,17 @@ template const typename MultiDofJoint::Vector& MultiDofJoint::getPositionsStatic() const { - return mPositions; + return this->mAspectState.mPositions; } //============================================================================== template void MultiDofJoint::setVelocitiesStatic(const Vector& _velocities) { - if(mVelocities == _velocities) + if(this->mAspectState.mVelocities == _velocities) return; - mVelocities = _velocities; + this->mAspectState.mVelocities = _velocities; this->notifyVelocityUpdate(); } @@ -959,17 +970,17 @@ template const typename MultiDofJoint::Vector& MultiDofJoint::getVelocitiesStatic() const { - return mVelocities; + return this->mAspectState.mVelocities; } //============================================================================== template void MultiDofJoint::setAccelerationsStatic(const Vector& _accels) { - if(mAccelerations == _accels) + if(this->mAspectState.mAccelerations == _accels) return; - mAccelerations = _accels; + this->mAspectState.mAccelerations = _accels; this->notifyAccelerationUpdate(); } @@ -978,7 +989,7 @@ template const typename MultiDofJoint::Vector& MultiDofJoint::getAccelerationsStatic() const { - return mAccelerations; + return this->mAspectState.mAccelerations; } //============================================================================== @@ -991,11 +1002,11 @@ void MultiDofJoint::setForce(size_t _index, double _force) return; } - mForces[_index] = _force; + this->mAspectState.mForces[_index] = _force; #if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) if (Joint::mAspectProperties.mActuatorType == Joint::FORCE) - mCommands[_index] = mForces[_index]; + this->mAspectState.mCommands[_index] = this->mAspectState.mForces[_index]; // TODO: Remove at DART 5.1. #endif } @@ -1010,7 +1021,7 @@ double MultiDofJoint::getForce(size_t _index) return 0.0; } - return mForces[_index]; + return this->mAspectState.mForces[_index]; } //============================================================================== @@ -1023,11 +1034,11 @@ void MultiDofJoint::setForces(const Eigen::VectorXd& _forces) return; } - mForces = _forces; + this->mAspectState.mForces = _forces; #if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) if (Joint::mAspectProperties.mActuatorType == Joint::FORCE) - mCommands = mForces; + this->mAspectState.mCommands = this->mAspectState.mForces; // TODO: Remove at DART 5.1. #endif } @@ -1036,18 +1047,18 @@ void MultiDofJoint::setForces(const Eigen::VectorXd& _forces) template Eigen::VectorXd MultiDofJoint::getForces() const { - return mForces; + return this->mAspectState.mForces; } //============================================================================== template void MultiDofJoint::resetForces() { - mForces.setZero(); + this->mAspectState.mForces.setZero(); #if DART_MAJOR_MINOR_VERSION_AT_MOST(5,1) if (Joint::mAspectProperties.mActuatorType == Joint::FORCE) - mCommands = mForces; + this->mAspectState.mCommands = this->mAspectState.mForces; // TODO: Remove at DART 5.1. #endif } @@ -1353,22 +1364,13 @@ Eigen::Vector6d MultiDofJoint::getBodyConstraintWrench() const { assert(this->mChildBodyNode); return this->mChildBodyNode->getBodyForce() - - this->getLocalJacobianStatic() * mForces; + - this->getLocalJacobianStatic() * this->mAspectState.mForces; } //============================================================================== template MultiDofJoint::MultiDofJoint(const Properties& properties) - : mCommands(Vector::Zero()), - mPositions(properties.mInitialPositions), - mPositionDeriv(Vector::Zero()), - mVelocities(properties.mInitialVelocities), - mVelocitiesDeriv(Vector::Zero()), - mAccelerations(Vector::Zero()), - mAccelerationsDeriv(Vector::Zero()), - mForces(Vector::Zero()), - mForcesDeriv(Vector::Zero()), - mVelocityChanges(Vector::Zero()), + : mVelocityChanges(Vector::Zero()), mImpulses(Vector::Zero()), mConstraintImpulses(Vector::Zero()), mJacobian(Eigen::Matrix::Zero()), @@ -1382,6 +1384,8 @@ MultiDofJoint::MultiDofJoint(const Properties& properties) mDofs[i] = this->createDofPointer(i); // Joint and MultiDofJoint Aspects must be created by the most derived class. + this->mAspectState.mPositions = properties.mInitialPositions; + this->mAspectState.mVelocities = properties.mInitialVelocities; } //============================================================================== @@ -1924,20 +1928,20 @@ void MultiDofJoint::updateTotalForce( switch (Joint::mAspectProperties.mActuatorType) { case Joint::FORCE: - mForces = mCommands; + this->mAspectState.mForces = this->mAspectState.mCommands; updateTotalForceDynamic(_bodyForce, _timeStep); break; case Joint::PASSIVE: case Joint::SERVO: - mForces.setZero(); + this->mAspectState.mForces.setZero(); updateTotalForceDynamic(_bodyForce, _timeStep); break; case Joint::ACCELERATION: - setAccelerationsStatic(mCommands); + setAccelerationsStatic(this->mAspectState.mCommands); updateTotalForceKinematic(_bodyForce, _timeStep); break; case Joint::VELOCITY: - setAccelerationsStatic( (mCommands - getVelocitiesStatic()) / _timeStep ); + setAccelerationsStatic( (this->mAspectState.mCommands - getVelocitiesStatic()) / _timeStep ); updateTotalForceKinematic(_bodyForce, _timeStep); break; case Joint::LOCKED: @@ -1969,7 +1973,7 @@ void MultiDofJoint::updateTotalForceDynamic( getVelocitiesStatic(); // - mTotalForce = mForces + springForce + dampingForce; + mTotalForce = this->mAspectState.mForces + springForce + dampingForce; mTotalForce.noalias() -= getLocalJacobianStatic().transpose()*_bodyForce; } @@ -2134,7 +2138,7 @@ void MultiDofJoint::updateForceID(const Eigen::Vector6d& _bodyForce, bool _withDampingForces, bool _withSpringForces) { - mForces = getLocalJacobianStatic().transpose()*_bodyForce; + this->mAspectState.mForces = getLocalJacobianStatic().transpose()*_bodyForce; // Damping force if (_withDampingForces) @@ -2142,7 +2146,7 @@ void MultiDofJoint::updateForceID(const Eigen::Vector6d& _bodyForce, const Eigen::Matrix dampingForces = (-Base::mAspectProperties.mDampingCoefficients).asDiagonal() *getVelocitiesStatic(); - mForces -= dampingForces; + this->mAspectState.mForces -= dampingForces; } // Spring force @@ -2152,7 +2156,7 @@ void MultiDofJoint::updateForceID(const Eigen::Vector6d& _bodyForce, = (-Base::mAspectProperties.mSpringStiffnesses).asDiagonal() *(getPositionsStatic() - Base::mAspectProperties.mRestPositions + getVelocitiesStatic()*_timeStep); - mForces -= springForces; + this->mAspectState.mForces -= springForces; } } @@ -2240,7 +2244,7 @@ void MultiDofJoint::updateConstrainedTermsDynamic(double _timeStep) setVelocitiesStatic(getVelocitiesStatic() + mVelocityChanges); setAccelerationsStatic(getAccelerationsStatic() + mVelocityChanges*invTimeStep); - mForces.noalias() += mImpulses*invTimeStep; + this->mAspectState.mForces.noalias() += mImpulses*invTimeStep; // Note: As long as this is only called from BodyNode::updateConstrainedTerms } @@ -2249,7 +2253,7 @@ template void MultiDofJoint::updateConstrainedTermsKinematic( double _timeStep) { - mForces.noalias() += mImpulses / _timeStep; + this->mAspectState.mForces.noalias() += mImpulses / _timeStep; } //============================================================================== @@ -2298,7 +2302,7 @@ void MultiDofJoint::updateTotalForceForInvMassMatrix( const Eigen::Vector6d& _bodyForce) { // Compute alpha - mInvM_a = mForces; + mInvM_a = this->mAspectState.mForces; mInvM_a.noalias() -= getLocalJacobianStatic().transpose() * _bodyForce; } diff --git a/dart/dynamics/detail/MultiDofJointProperties.h b/dart/dynamics/detail/MultiDofJointProperties.h index e08e2fbdf6b29..885384e0afc6f 100644 --- a/dart/dynamics/detail/MultiDofJointProperties.h +++ b/dart/dynamics/detail/MultiDofJointProperties.h @@ -48,6 +48,37 @@ template class MultiDofJoint; namespace detail { +//============================================================================== +template +struct MultiDofJointState +{ + constexpr static size_t NumDofs = DOF; + using Vector = Eigen::Matrix; + + /// Position + Vector mPositions; + + /// Generalized velocity + Vector mVelocities; + + /// Generalized acceleration + Vector mAccelerations; + + /// Generalized force + Vector mForces; + + /// Command + Vector mCommands; + + MultiDofJointState( + const Vector& positions = Vector::Zero(), + const Vector& velocities = Vector::Zero(), + const Vector& accelerations = Vector::Zero(), + const Vector& forces = Vector::Zero(), + const Vector& commands = Vector::Zero()); +}; + +//============================================================================== template struct MultiDofJointUniqueProperties { @@ -161,9 +192,29 @@ struct MultiDofJointProperties : // // See this StackOverflow answer: http://stackoverflow.com/a/14396189/111426 // +template +constexpr size_t MultiDofJointState::NumDofs; + template constexpr size_t MultiDofJointUniqueProperties::NumDofs; +//============================================================================== +template +MultiDofJointState::MultiDofJointState( + const Vector& positions, + const Vector& velocities, + const Vector& accelerations, + const Vector& forces, + const Vector& commands) + : mPositions(positions), + mVelocities(velocities), + mAccelerations(accelerations), + mForces(forces), + mCommands(commands) +{ + // Do nothing +} + //============================================================================== template MultiDofJointUniqueProperties::MultiDofJointUniqueProperties( @@ -239,8 +290,8 @@ MultiDofJointProperties::MultiDofJointProperties( } template -using MultiDofJointBase = common::EmbedPropertiesOnTopOf< - Derived, MultiDofJointUniqueProperties, Joint>; +using MultiDofJointBase = common::EmbedStateAndPropertiesOnTopOf< + Derived, MultiDofJointState, MultiDofJointUniqueProperties, Joint>; } // namespace detail } // namespace dynamics From 62c2a35f79bd473998aa0d787aea82f060cd7876 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Wed, 6 Apr 2016 16:15:32 -0400 Subject: [PATCH 36/62] renamed headers for consistency --- dart/dynamics/BodyNode.h | 2 +- dart/dynamics/EulerJoint.h | 2 +- dart/dynamics/MultiDofJoint.h | 2 +- dart/dynamics/PlanarJoint.h | 2 +- dart/dynamics/PrismaticJoint.h | 2 +- dart/dynamics/RevoluteJoint.h | 2 +- dart/dynamics/ScrewJoint.h | 2 +- dart/dynamics/SingleDofJoint.h | 2 +- dart/dynamics/Skeleton.h | 2 +- dart/dynamics/SoftBodyNode.h | 2 +- dart/dynamics/UniversalJoint.h | 2 +- dart/dynamics/detail/{BodyNodeProperties.h => BodyNodeAspect.h} | 0 .../detail/{EulerJointProperties.cpp => EulerJointAspect.cpp} | 0 .../detail/{EulerJointProperties.h => EulerJointAspect.h} | 0 .../detail/{MultiDofJointProperties.h => MultiDofJointAspect.h} | 0 .../detail/{PlanarJointProperties.cpp => PlanarJointAspect.cpp} | 0 .../detail/{PlanarJointProperties.h => PlanarJointAspect.h} | 0 .../{PrismaticJointProperties.cpp => PrismaticJointAspect.cpp} | 0 .../{PrismaticJointProperties.h => PrismaticJointAspect.h} | 0 .../{RevoluteJointProperties.cpp => RevoluteJointAspect.cpp} | 0 .../detail/{RevoluteJointProperties.h => RevoluteJointAspect.h} | 0 .../detail/{ScrewJointProperties.cpp => ScrewJointAspect.cpp} | 0 .../detail/{ScrewJointProperties.h => ScrewJointAspect.h} | 0 .../{SingleDofJointProperties.cpp => SingleDofJointAspect.cpp} | 0 .../{SingleDofJointProperties.h => SingleDofJointAspect.h} | 0 .../detail/{SoftBodyNodeProperties.h => SoftBodyNodeAspect.h} | 0 .../{UniversalJointProperties.cpp => UniversalJointAspect.cpp} | 0 .../{UniversalJointProperties.h => UniversalJointAspect.h} | 0 28 files changed, 11 insertions(+), 11 deletions(-) rename dart/dynamics/detail/{BodyNodeProperties.h => BodyNodeAspect.h} (100%) rename dart/dynamics/detail/{EulerJointProperties.cpp => EulerJointAspect.cpp} (100%) rename dart/dynamics/detail/{EulerJointProperties.h => EulerJointAspect.h} (100%) rename dart/dynamics/detail/{MultiDofJointProperties.h => MultiDofJointAspect.h} (100%) rename dart/dynamics/detail/{PlanarJointProperties.cpp => PlanarJointAspect.cpp} (100%) rename dart/dynamics/detail/{PlanarJointProperties.h => PlanarJointAspect.h} (100%) rename dart/dynamics/detail/{PrismaticJointProperties.cpp => PrismaticJointAspect.cpp} (100%) rename dart/dynamics/detail/{PrismaticJointProperties.h => PrismaticJointAspect.h} (100%) rename dart/dynamics/detail/{RevoluteJointProperties.cpp => RevoluteJointAspect.cpp} (100%) rename dart/dynamics/detail/{RevoluteJointProperties.h => RevoluteJointAspect.h} (100%) rename dart/dynamics/detail/{ScrewJointProperties.cpp => ScrewJointAspect.cpp} (100%) rename dart/dynamics/detail/{ScrewJointProperties.h => ScrewJointAspect.h} (100%) rename dart/dynamics/detail/{SingleDofJointProperties.cpp => SingleDofJointAspect.cpp} (100%) rename dart/dynamics/detail/{SingleDofJointProperties.h => SingleDofJointAspect.h} (100%) rename dart/dynamics/detail/{SoftBodyNodeProperties.h => SoftBodyNodeAspect.h} (100%) rename dart/dynamics/detail/{UniversalJointProperties.cpp => UniversalJointAspect.cpp} (100%) rename dart/dynamics/detail/{UniversalJointProperties.h => UniversalJointAspect.h} (100%) diff --git a/dart/dynamics/BodyNode.h b/dart/dynamics/BodyNode.h index 20ee72e09591f..89e08daa8b5a9 100644 --- a/dart/dynamics/BodyNode.h +++ b/dart/dynamics/BodyNode.h @@ -53,7 +53,7 @@ #include "dart/dynamics/SmartPointer.h" #include "dart/dynamics/TemplatedJacobianNode.h" #include "dart/dynamics/SpecializedNodeManager.h" -#include "dart/dynamics/detail/BodyNodeProperties.h" +#include "dart/dynamics/detail/BodyNodeAspect.h" #include "dart/dynamics/Skeleton.h" namespace dart { diff --git a/dart/dynamics/EulerJoint.h b/dart/dynamics/EulerJoint.h index ea4d5b52f2df5..30d355a7917ac 100644 --- a/dart/dynamics/EulerJoint.h +++ b/dart/dynamics/EulerJoint.h @@ -37,7 +37,7 @@ #ifndef DART_DYNAMICS_EULERJOINT_H_ #define DART_DYNAMICS_EULERJOINT_H_ -#include "dart/dynamics/detail/EulerJointProperties.h" +#include "dart/dynamics/detail/EulerJointAspect.h" namespace dart { namespace dynamics { diff --git a/dart/dynamics/MultiDofJoint.h b/dart/dynamics/MultiDofJoint.h index 19ad20b0a2116..05d43b0af4fc0 100644 --- a/dart/dynamics/MultiDofJoint.h +++ b/dart/dynamics/MultiDofJoint.h @@ -48,7 +48,7 @@ #include "dart/dynamics/Skeleton.h" #include "dart/dynamics/DegreeOfFreedom.h" #include "dart/common/RequiresAspect.h" -#include "dart/dynamics/detail/MultiDofJointProperties.h" +#include "dart/dynamics/detail/MultiDofJointAspect.h" namespace dart { namespace dynamics { diff --git a/dart/dynamics/PlanarJoint.h b/dart/dynamics/PlanarJoint.h index bc5476a0ed2f5..f5c049307e260 100644 --- a/dart/dynamics/PlanarJoint.h +++ b/dart/dynamics/PlanarJoint.h @@ -37,7 +37,7 @@ #ifndef DART_DYNAMICS_PLANARRJOINT_H_ #define DART_DYNAMICS_PLANARRJOINT_H_ -#include "dart/dynamics/detail/PlanarJointProperties.h" +#include "dart/dynamics/detail/PlanarJointAspect.h" namespace dart { namespace dynamics { diff --git a/dart/dynamics/PrismaticJoint.h b/dart/dynamics/PrismaticJoint.h index 1e417f3d0d0c5..b4d041a507c11 100644 --- a/dart/dynamics/PrismaticJoint.h +++ b/dart/dynamics/PrismaticJoint.h @@ -37,7 +37,7 @@ #ifndef DART_DYNAMICS_PRISMATICJOINT_H_ #define DART_DYNAMICS_PRISMATICJOINT_H_ -#include "dart/dynamics/detail/PrismaticJointProperties.h" +#include "dart/dynamics/detail/PrismaticJointAspect.h" namespace dart { namespace dynamics { diff --git a/dart/dynamics/RevoluteJoint.h b/dart/dynamics/RevoluteJoint.h index 34310fd412884..a916069bdbc71 100644 --- a/dart/dynamics/RevoluteJoint.h +++ b/dart/dynamics/RevoluteJoint.h @@ -37,7 +37,7 @@ #ifndef DART_DYNAMICS_REVOLUTEJOINT_H_ #define DART_DYNAMICS_REVOLUTEJOINT_H_ -#include "dart/dynamics/detail/RevoluteJointProperties.h" +#include "dart/dynamics/detail/RevoluteJointAspect.h" namespace dart { namespace dynamics { diff --git a/dart/dynamics/ScrewJoint.h b/dart/dynamics/ScrewJoint.h index 20500c8e8aa6b..ec84e69a03e8d 100644 --- a/dart/dynamics/ScrewJoint.h +++ b/dart/dynamics/ScrewJoint.h @@ -37,7 +37,7 @@ #ifndef DART_DYNAMICS_SCREWJOINT_H_ #define DART_DYNAMICS_SCREWJOINT_H_ -#include "dart/dynamics/detail/ScrewJointProperties.h" +#include "dart/dynamics/detail/ScrewJointAspect.h" namespace dart { namespace dynamics { diff --git a/dart/dynamics/SingleDofJoint.h b/dart/dynamics/SingleDofJoint.h index 4d297d2c58ae7..a9f2207765bdd 100644 --- a/dart/dynamics/SingleDofJoint.h +++ b/dart/dynamics/SingleDofJoint.h @@ -40,7 +40,7 @@ #include #include "dart/dynamics/Joint.h" -#include "dart/dynamics/detail/SingleDofJointProperties.h" +#include "dart/dynamics/detail/SingleDofJointAspect.h" namespace dart { namespace dynamics { diff --git a/dart/dynamics/Skeleton.h b/dart/dynamics/Skeleton.h index e4b4032048e1e..c6d635b0d1fa6 100644 --- a/dart/dynamics/Skeleton.h +++ b/dart/dynamics/Skeleton.h @@ -47,7 +47,7 @@ #include "dart/dynamics/Joint.h" #include "dart/dynamics/ShapeNode.h" #include "dart/dynamics/EndEffector.h" -#include "dart/dynamics/detail/BodyNodeProperties.h" +#include "dart/dynamics/detail/BodyNodeAspect.h" #include "dart/dynamics/SpecializedNodeManager.h" #include "dart/dynamics/detail/SkeletonAspect.h" diff --git a/dart/dynamics/SoftBodyNode.h b/dart/dynamics/SoftBodyNode.h index 4047c63c3ac81..1e9a6c3e9d60b 100644 --- a/dart/dynamics/SoftBodyNode.h +++ b/dart/dynamics/SoftBodyNode.h @@ -37,7 +37,7 @@ #ifndef DART_DYNAMICS_SOFTBODYNODE_H_ #define DART_DYNAMICS_SOFTBODYNODE_H_ -#include "dart/dynamics/detail/SoftBodyNodeProperties.h" +#include "dart/dynamics/detail/SoftBodyNodeAspect.h" namespace dart { namespace dynamics { diff --git a/dart/dynamics/UniversalJoint.h b/dart/dynamics/UniversalJoint.h index e4af7096845b4..2f00622f93c90 100644 --- a/dart/dynamics/UniversalJoint.h +++ b/dart/dynamics/UniversalJoint.h @@ -37,7 +37,7 @@ #ifndef DART_DYNAMICS_UNIVERSALJOINT_H_ #define DART_DYNAMICS_UNIVERSALJOINT_H_ -#include "dart/dynamics/detail/UniversalJointProperties.h" +#include "dart/dynamics/detail/UniversalJointAspect.h" namespace dart { namespace dynamics { diff --git a/dart/dynamics/detail/BodyNodeProperties.h b/dart/dynamics/detail/BodyNodeAspect.h similarity index 100% rename from dart/dynamics/detail/BodyNodeProperties.h rename to dart/dynamics/detail/BodyNodeAspect.h diff --git a/dart/dynamics/detail/EulerJointProperties.cpp b/dart/dynamics/detail/EulerJointAspect.cpp similarity index 100% rename from dart/dynamics/detail/EulerJointProperties.cpp rename to dart/dynamics/detail/EulerJointAspect.cpp diff --git a/dart/dynamics/detail/EulerJointProperties.h b/dart/dynamics/detail/EulerJointAspect.h similarity index 100% rename from dart/dynamics/detail/EulerJointProperties.h rename to dart/dynamics/detail/EulerJointAspect.h diff --git a/dart/dynamics/detail/MultiDofJointProperties.h b/dart/dynamics/detail/MultiDofJointAspect.h similarity index 100% rename from dart/dynamics/detail/MultiDofJointProperties.h rename to dart/dynamics/detail/MultiDofJointAspect.h diff --git a/dart/dynamics/detail/PlanarJointProperties.cpp b/dart/dynamics/detail/PlanarJointAspect.cpp similarity index 100% rename from dart/dynamics/detail/PlanarJointProperties.cpp rename to dart/dynamics/detail/PlanarJointAspect.cpp diff --git a/dart/dynamics/detail/PlanarJointProperties.h b/dart/dynamics/detail/PlanarJointAspect.h similarity index 100% rename from dart/dynamics/detail/PlanarJointProperties.h rename to dart/dynamics/detail/PlanarJointAspect.h diff --git a/dart/dynamics/detail/PrismaticJointProperties.cpp b/dart/dynamics/detail/PrismaticJointAspect.cpp similarity index 100% rename from dart/dynamics/detail/PrismaticJointProperties.cpp rename to dart/dynamics/detail/PrismaticJointAspect.cpp diff --git a/dart/dynamics/detail/PrismaticJointProperties.h b/dart/dynamics/detail/PrismaticJointAspect.h similarity index 100% rename from dart/dynamics/detail/PrismaticJointProperties.h rename to dart/dynamics/detail/PrismaticJointAspect.h diff --git a/dart/dynamics/detail/RevoluteJointProperties.cpp b/dart/dynamics/detail/RevoluteJointAspect.cpp similarity index 100% rename from dart/dynamics/detail/RevoluteJointProperties.cpp rename to dart/dynamics/detail/RevoluteJointAspect.cpp diff --git a/dart/dynamics/detail/RevoluteJointProperties.h b/dart/dynamics/detail/RevoluteJointAspect.h similarity index 100% rename from dart/dynamics/detail/RevoluteJointProperties.h rename to dart/dynamics/detail/RevoluteJointAspect.h diff --git a/dart/dynamics/detail/ScrewJointProperties.cpp b/dart/dynamics/detail/ScrewJointAspect.cpp similarity index 100% rename from dart/dynamics/detail/ScrewJointProperties.cpp rename to dart/dynamics/detail/ScrewJointAspect.cpp diff --git a/dart/dynamics/detail/ScrewJointProperties.h b/dart/dynamics/detail/ScrewJointAspect.h similarity index 100% rename from dart/dynamics/detail/ScrewJointProperties.h rename to dart/dynamics/detail/ScrewJointAspect.h diff --git a/dart/dynamics/detail/SingleDofJointProperties.cpp b/dart/dynamics/detail/SingleDofJointAspect.cpp similarity index 100% rename from dart/dynamics/detail/SingleDofJointProperties.cpp rename to dart/dynamics/detail/SingleDofJointAspect.cpp diff --git a/dart/dynamics/detail/SingleDofJointProperties.h b/dart/dynamics/detail/SingleDofJointAspect.h similarity index 100% rename from dart/dynamics/detail/SingleDofJointProperties.h rename to dart/dynamics/detail/SingleDofJointAspect.h diff --git a/dart/dynamics/detail/SoftBodyNodeProperties.h b/dart/dynamics/detail/SoftBodyNodeAspect.h similarity index 100% rename from dart/dynamics/detail/SoftBodyNodeProperties.h rename to dart/dynamics/detail/SoftBodyNodeAspect.h diff --git a/dart/dynamics/detail/UniversalJointProperties.cpp b/dart/dynamics/detail/UniversalJointAspect.cpp similarity index 100% rename from dart/dynamics/detail/UniversalJointProperties.cpp rename to dart/dynamics/detail/UniversalJointAspect.cpp diff --git a/dart/dynamics/detail/UniversalJointProperties.h b/dart/dynamics/detail/UniversalJointAspect.h similarity index 100% rename from dart/dynamics/detail/UniversalJointProperties.h rename to dart/dynamics/detail/UniversalJointAspect.h From 14c89d5a816e67e93a633f94cc72ddbfabca5a35 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Wed, 6 Apr 2016 17:51:09 -0400 Subject: [PATCH 37/62] enhancing features of Composite State and Properties --- dart/common/Composite.cpp | 4 +- dart/common/Composite.h | 14 +-- .../detail/CompositeStateAndProperties.h | 100 ++++++++++++++++++ 3 files changed, 105 insertions(+), 13 deletions(-) create mode 100644 dart/common/detail/CompositeStateAndProperties.h diff --git a/dart/common/Composite.cpp b/dart/common/Composite.cpp index 2de96424b8dad..c55f02b4d9cff 100644 --- a/dart/common/Composite.cpp +++ b/dart/common/Composite.cpp @@ -166,7 +166,7 @@ Composite::State Composite::getCompositeState() const //============================================================================== void Composite::copyCompositeStateTo(State& outgoingStates) const { - StateMap& states = outgoingStates.getMap(); + auto& states = outgoingStates.getMap(); extractDataFromObjectTypeMap( states, mAspectMap); } @@ -192,7 +192,7 @@ Composite::Properties Composite::getCompositeProperties() const void Composite::copyCompositePropertiesTo( Properties& outgoingProperties) const { - PropertiesMap& properties = outgoingProperties.getMap(); + auto& properties = outgoingProperties.getMap(); extractDataFromObjectTypeMap( properties, mAspectMap); } diff --git a/dart/common/Composite.h b/dart/common/Composite.h index f101ede6a2a45..6b17eac242f5c 100644 --- a/dart/common/Composite.h +++ b/dart/common/Composite.h @@ -37,12 +37,7 @@ #ifndef DART_COMMON_COMPOSITE_H_ #define DART_COMMON_COMPOSITE_H_ -#include -#include -#include -#include - -#include "dart/common/Aspect.h" +#include "dart/common/detail/CompositeStateAndProperties.h" namespace dart { namespace common { @@ -61,11 +56,8 @@ class Composite { public: - using StateMap = std::map< std::type_index, std::unique_ptr >; - using State = CloneableMap; - - using PropertiesMap = std::map< std::type_index, std::unique_ptr >; - using Properties = CloneableMap; + using State = detail::CompositeState; + using Properties = detail::CompositeProperties; using AspectMap = std::map< std::type_index, std::unique_ptr >; using RequiredAspectSet = std::unordered_set; diff --git a/dart/common/detail/CompositeStateAndProperties.h b/dart/common/detail/CompositeStateAndProperties.h new file mode 100644 index 0000000000000..ea63be59be0f1 --- /dev/null +++ b/dart/common/detail/CompositeStateAndProperties.h @@ -0,0 +1,100 @@ +/* + * 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_COMMON_DETAIL_COMPOSITESTATEANDPROPERTIES_H_ +#define DART_COMMON_DETAIL_COMPOSITESTATEANDPROPERTIES_H_ + +#include +#include +#include +#include + +#include "dart/common/Aspect.h" + +namespace dart { +namespace common { +namespace detail { + +//============================================================================== +template +struct GetState +{ + using Type = typename AspectT::State; +}; + +//============================================================================== +template +struct GetProperties +{ + using Type = typename AspectT::Properties; +}; + +//============================================================================== +using CompositeStateMap = std::map< std::type_index, std::unique_ptr >; +using CompositeState = CloneableMap; + +using CompositePropertiesMap = std::map< std::type_index, std::unique_ptr >; +using CompositeProperties = CloneableMap; + +//============================================================================== +template GetData> +class CompositeData : public CloneableMap +{ +public: + + /// Forwarding constructor + template + CompositeData(Args&&... args) + : CloneableMap(std::forward(args)...) + { + // Do nothing + } + + template + GetState::Type& make(Args&&... args) + { + using Data = GetState::Type; + std::unique_ptr data = make_unique(std::forward(args)...); + + } + +}; + +} // namespace detail +} // namespace common +} // namespace dart + +#endif // DART_COMMON_DETAIL_COMPOSITESTATEANDPROPERTIES_H_ From 6be41240831bb429aa393d379bcc2f2846c45240 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Wed, 6 Apr 2016 23:19:20 -0400 Subject: [PATCH 38/62] made Composite States and Properties more accessible to the user --- dart/common/Cloneable.h | 2 +- dart/common/Composite.h | 6 + .../detail/CompositeStateAndProperties.h | 187 +++++++++++++++++- unittests/testAspect.cpp | 67 +++---- 4 files changed, 214 insertions(+), 48 deletions(-) diff --git a/dart/common/Cloneable.h b/dart/common/Cloneable.h index 638f517430826..882ce80d4dbe6 100644 --- a/dart/common/Cloneable.h +++ b/dart/common/Cloneable.h @@ -260,7 +260,7 @@ class CloneableMap /// Get the map that is being held const MapType& getMap() const; -private: +protected: /// A map containing the collection of States for the Aspect MapType mMap; diff --git a/dart/common/Composite.h b/dart/common/Composite.h index 6b17eac242f5c..34c8e9eb4e751 100644 --- a/dart/common/Composite.h +++ b/dart/common/Composite.h @@ -62,6 +62,12 @@ class Composite using AspectMap = std::map< std::type_index, std::unique_ptr >; using RequiredAspectSet = std::unordered_set; + template + using MakeState = detail::MakeCompositeState; + + template + using MakeProperties = detail::MakeCompositeProperties; + /// Virtual destructor virtual ~Composite() = default; diff --git a/dart/common/detail/CompositeStateAndProperties.h b/dart/common/detail/CompositeStateAndProperties.h index ea63be59be0f1..28579818f0370 100644 --- a/dart/common/detail/CompositeStateAndProperties.h +++ b/dart/common/detail/CompositeStateAndProperties.h @@ -48,29 +48,35 @@ namespace dart { namespace common { namespace detail { +//============================================================================== +template +struct GetAspect +{ + using Type = typename static_if_else< + std::is_base_of::value, + AspectT, typename AspectT::Aspect>::type; +}; + //============================================================================== template struct GetState { - using Type = typename AspectT::State; + using Type = typename GetAspect::Type::State; }; //============================================================================== template struct GetProperties { - using Type = typename AspectT::Properties; + using Type = typename GetAspect::Type::Properties; }; //============================================================================== using CompositeStateMap = std::map< std::type_index, std::unique_ptr >; -using CompositeState = CloneableMap; - using CompositePropertiesMap = std::map< std::type_index, std::unique_ptr >; -using CompositeProperties = CloneableMap; //============================================================================== -template GetData> +template class GetData> class CompositeData : public CloneableMap { public: @@ -83,16 +89,179 @@ class CompositeData : public CloneableMap // Do nothing } + virtual ~CompositeData() = default; + + /// Create (or replace) a piece of data in this + template + typename GetData::Type& create(Args&&... args) + { + using Data = typename GetData::Type; + using AspectType = typename GetAspect::Type; + using DataType = typename GetData::Type; + + std::unique_ptr& data = this->mMap[typeid(AspectType)] + = make_unique(std::forward(args)...); + return static_cast(*data); + } + + template + typename GetData::Type* get() + { + using Data = typename GetData::Type; + using AspectType = typename GetAspect::Type; + + typename MapType::iterator it = this->mMap.find(typeid(AspectType)); + if(this->mMap.end() == it) + return nullptr; + + return static_cast(it->second.get()); + } + + template + const typename GetData::Type* get() const + { + return const_cast*>(this)->get(); + } + template - GetState::Type& make(Args&&... args) + typename GetData::Type& getOrCreate(Args&&... args) + { + using Data = typename GetData::Type; + using AspectType = typename GetAspect::Type; + + auto& it = this->mMap.insert( + std::make_pair>( + typeid(AspectType), nullptr)); + + const bool exists = !it.second; + if(!exists) + it.first = make_unique(std::forward(args)...); + + return static_cast(*it.first); + } + +}; + +//============================================================================== +using CompositeState = CompositeData; +using CompositeProperties = CompositeData; + +//============================================================================== +template class GetData, typename... Args> +class ComposeData +{ +public: + virtual ~ComposeData() = default; + + void setFrom(const CompositeType&) + { + // Do nothing + } + +protected: + + void _addData(CompositeType&) const + { + // Do nothing + } +}; + +//============================================================================== +template class GetData, class AspectT, + typename... Remainder> +struct ComposeData : + public GetData::Type, + public ComposeData +{ +public: + + enum Delegate_t { Delegate }; + + using Base = typename GetData::Type; + using AspectType = typename GetAspect::Type; + + template + struct ConvertIfData + { + using Type = typename static_if_else< + std::is_base_of::value, + typename Base::Data, Arg>::type; + }; + + ComposeData() = default; + + virtual ~ComposeData() = default; + + template + ComposeData(Args&&... args) + : ComposeData(std::forward(args)...) + { + // Do nothing + } + + operator CompositeType() const { - using Data = GetState::Type; - std::unique_ptr data = make_unique(std::forward(args)...); + CompositeType composite; + _addData(composite); + return composite; } + void setFrom(const CompositeType& composite) + { + const Base* data = composite.template get(); + if(data) + static_cast(*this) = *data; + + ComposeData::setFrom(composite); + } + + ComposeData& operator =(const CompositeType& composite) + { + setFrom(composite); + return *this; + } + +protected: + + void _addData(CompositeType& composite) const + { + composite.template create(static_cast(*this)); + ComposeData::_addData(composite); + } + + void _findData() + { + // Do nothing + } + + template + void _findData(Arg1 arg1, Args&&... args) + { + _useIfData(static_cast::Type&>(arg1)); + _findData(std::forward(args)...); + } + + template + void _useIfData(Arg) + { + // Do nothing + } + + void _useIfData(const typename Base::Data& data) + { + static_cast(*this) = data; + } }; +//============================================================================== +template +using MakeCompositeState = ComposeData; + +template +using MakeCompositeProperties = + ComposeData; + } // namespace detail } // namespace common } // namespace dart diff --git a/unittests/testAspect.cpp b/unittests/testAspect.cpp index 881386b8ef02d..b37d6e74ddb00 100644 --- a/unittests/testAspect.cpp +++ b/unittests/testAspect.cpp @@ -280,51 +280,19 @@ class StatefulAspect : public Aspect, public Subject { public: - class State : public Aspect::State + struct Data { - public: - - State() : val(static_cast(dart::math::random(0, 100))) { } - - State(const State& other) : val(other.val) { } - - State(const T& otherVal) : val(otherVal) { } - T val; - std::unique_ptr clone() const override - { - return dart::common::make_unique(*this); - } - - void copy(const Aspect::State& anotherState) override + Data(const T& someVal = 0) + : val(someVal) { - val = static_cast(anotherState).val; + // Do nothing } }; - class Properties : public Aspect::Properties - { - public: - - Properties() : val(static_cast(dart::math::random(0, 100))) { } - - Properties(const Properties& other) : val(other.val) { } - - Properties(const T& otherVal) : val(otherVal) { } - - T val; - - std::unique_ptr clone() const override - { - return dart::common::make_unique(*this); - } - - void copy(const Aspect::Properties& otherProperties) override - { - val = static_cast(otherProperties).val; - } - }; + using State = Aspect::MakeState; + using Properties = Aspect::MakeProperties; StatefulAspect(Composite* mgr) : Aspect(mgr) @@ -783,6 +751,29 @@ TEST(Aspect, Duplication) EXPECT_FALSE(mgr2.get() == nullptr); EXPECT_FALSE(mgr2.get() == nullptr); EXPECT_FALSE(mgr2.get() == nullptr); + + Composite::MakeState state; + state.DoubleAspect::State::val = 1e-6; + state.FloatAspect::State::val = 1.5; + state.IntAspect::State::val = 456; + + mgr1.setCompositeState(state); + + EXPECT_EQ(mgr1.get()->mState.val, 1e-6); + EXPECT_EQ(mgr1.get()->mState.val, 1.5); + EXPECT_EQ(mgr1.get()->mState.val, 456); + + state = mgr2.getCompositeState(); + + EXPECT_EQ(state.DoubleAspect::State::val, 0); + EXPECT_EQ(state.FloatAspect::State::val, 0); + EXPECT_EQ(state.IntAspect::State::val, 0); + + state = mgr1.getCompositeState(); + + EXPECT_EQ(state.DoubleAspect::State::val, 1e-6); + EXPECT_EQ(state.FloatAspect::State::val, 1.5); + EXPECT_EQ(state.IntAspect::State::val, 456); } TEST(Aspect, Embedded) From b44d428fe8d1fd212e701655b29aad5edc34e6a6 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Fri, 8 Apr 2016 03:22:04 -0400 Subject: [PATCH 39/62] final structural changes for major release --- apps/atlasSimbicon/Controller.cpp | 5 +- apps/atlasSimbicon/Controller.h | 2 +- dart/common/Composite.h | 10 +- ...teStateAndProperties.h => CompositeData.h} | 111 +++++++++++++++--- dart/dynamics/BodyNode.cpp | 8 +- dart/dynamics/BodyNode.h | 2 +- dart/dynamics/EndEffector.cpp | 29 +---- dart/dynamics/EndEffector.h | 26 ++-- dart/dynamics/ShapeFrame.cpp | 9 ++ dart/dynamics/ShapeFrame.h | 3 + dart/dynamics/ShapeNode.cpp | 39 ++---- dart/dynamics/ShapeNode.h | 28 ++--- dart/dynamics/Skeleton.cpp | 47 ++------ dart/dynamics/Skeleton.h | 26 ++-- dart/dynamics/detail/BodyNodeAspect.h | 2 - docs/readthedocs/tutorials/collisions.md | 7 +- docs/readthedocs/tutorials/multi-pendulum.md | 8 +- tutorials/tutorialCollisions-Finished.cpp | 4 +- tutorials/tutorialMultiPendulum-Finished.cpp | 4 +- tutorials/tutorialMultiPendulum.cpp | 4 +- unittests/TestHelpers.h | 20 ++-- unittests/testAspect.cpp | 37 +++++- unittests/testDynamics.cpp | 12 +- unittests/testInverseKinematics.cpp | 4 +- unittests/testSoftDynamics.cpp | 11 +- 25 files changed, 253 insertions(+), 205 deletions(-) rename dart/common/detail/{CompositeStateAndProperties.h => CompositeData.h} (73%) diff --git a/apps/atlasSimbicon/Controller.cpp b/apps/atlasSimbicon/Controller.cpp index 79b279431c626..463aa4a4f8eed 100644 --- a/apps/atlasSimbicon/Controller.cpp +++ b/apps/atlasSimbicon/Controller.cpp @@ -73,7 +73,8 @@ Controller::Controller(SkeletonPtr _atlasRobot, // harnessLeftFoot(); // harnessRightFoot(); - mInitialState = mAtlasRobot->getState(); + mInitialState = mAtlasRobot->getConfiguration( + Skeleton::CONFIG_POSITIONS | Skeleton::CONFIG_VELOCITIES); } //============================================================================== @@ -303,7 +304,7 @@ void Controller::unharnessRightFoot() //============================================================================== void Controller::resetRobot() { - mAtlasRobot->setState(mInitialState); + mAtlasRobot->setConfiguration(mInitialState); dtmsg << "Robot is reset." << std::endl; } diff --git a/apps/atlasSimbicon/Controller.h b/apps/atlasSimbicon/Controller.h index 4f30135a70b83..6855b4414881c 100644 --- a/apps/atlasSimbicon/Controller.h +++ b/apps/atlasSimbicon/Controller.h @@ -181,7 +181,7 @@ class Controller dart::constraint::WeldJointConstraintPtr mWeldJointConstraintRightFoot; /// \brief Initial state of the robot - Eigen::VectorXd mInitialState; + dart::dynamics::Skeleton::Configuration mInitialState; }; #endif // APPS_ATLASROBOT_CONTROLLER_H_ diff --git a/dart/common/Composite.h b/dart/common/Composite.h index 34c8e9eb4e751..5dec327e66aee 100644 --- a/dart/common/Composite.h +++ b/dart/common/Composite.h @@ -37,7 +37,7 @@ #ifndef DART_COMMON_COMPOSITE_H_ #define DART_COMMON_COMPOSITE_H_ -#include "dart/common/detail/CompositeStateAndProperties.h" +#include "dart/common/detail/CompositeData.h" namespace dart { namespace common { @@ -62,11 +62,11 @@ class Composite using AspectMap = std::map< std::type_index, std::unique_ptr >; using RequiredAspectSet = std::unordered_set; - template - using MakeState = detail::MakeCompositeState; + template + using MakeState = detail::MakeCompositeState; - template - using MakeProperties = detail::MakeCompositeProperties; + template + using MakeProperties = detail::MakeCompositeProperties; /// Virtual destructor virtual ~Composite() = default; diff --git a/dart/common/detail/CompositeStateAndProperties.h b/dart/common/detail/CompositeData.h similarity index 73% rename from dart/common/detail/CompositeStateAndProperties.h rename to dart/common/detail/CompositeData.h index 28579818f0370..8bccbcbf6b19a 100644 --- a/dart/common/detail/CompositeStateAndProperties.h +++ b/dart/common/detail/CompositeData.h @@ -147,10 +147,18 @@ using CompositeState = CompositeData; using CompositeProperties = CompositeData; //============================================================================== -template class GetData, typename... Args> +template class GetData, typename... Aspects> class ComposeData { public: + + ComposeData() = default; + + ComposeData(const CompositeType&) + { + // Do nothing + } + virtual ~ComposeData() = default; void setFrom(const CompositeType&) @@ -178,6 +186,7 @@ struct ComposeData : enum Delegate_t { Delegate }; using Base = typename GetData::Type; + using Data = typename Base::Data; using AspectType = typename GetAspect::Type; template @@ -188,15 +197,51 @@ struct ComposeData : typename Base::Data, Arg>::type; }; + template + struct ConvertIfComposite + { + using Type = typename static_if_else< + std::is_base_of::value, + CompositeType, Arg>::type; + }; + ComposeData() = default; virtual ~ComposeData() = default; - template - ComposeData(Args&&... args) - : ComposeData(std::forward(args)...) + template + ComposeData(const Arg1& arg1, const Args&... args) + : ComposeData( + Delegate, + static_cast::Type&>(arg1), + args...) { - // Do nothing + // This constructor delegates + } + + /// Grab relevant data out of a composite object + ComposeData(const CompositeType& composite) + : ComposeData(composite) + { + _setBaseFrom(composite); + } + + template + ComposeData(const ComposeData& composite) + : ComposeData(static_cast(composite)) + { + // This is a delegating constructor. If we get passed another ComposeData + // object, then we convert it into a composite to ensure that we grab all + // of its aspects. + } + + // Dev Note: We must not use the argument 'composite' as a temporary, or else + // it will get deleted when it reaches the last base constructor, preventing + // any higher level constructors from calling _setBaseFrom(~) on it. + ComposeData(CompositeType&& composite) + : ComposeData(static_cast(composite)) + { + // This is a delegating constructor } operator CompositeType() const @@ -209,10 +254,7 @@ struct ComposeData : void setFrom(const CompositeType& composite) { - const Base* data = composite.template get(); - if(data) - static_cast(*this) = *data; - + _setBaseFrom(composite); ComposeData::setFrom(composite); } @@ -222,8 +264,40 @@ struct ComposeData : return *this; } + /// Grab any relevant data and copy it into this composite. Note that there + /// will be NO compilation error, even if there is no relevant data in any of + /// the arguments that get passed in. It will simply ignore all the arguments + /// silently. + template + void copy(const Args&... args) + { + _findData(args...); + } + protected: + template + ComposeData(Delegate_t, const Args&... args) + : ComposeData(args...) + { + // Pass all the arguments along to the next base class + } + + template + ComposeData(Delegate_t, const Data& arg1, const Args&... args) + : Base(arg1), + ComposeData(args...) + { + // Peel off the first argument and then pass along the rest + } + + void _setBaseFrom(const CompositeType& composite) + { + const Base* data = composite.template get(); + if(data) + static_cast(*this) = *data; + } + void _addData(CompositeType& composite) const { composite.template create(static_cast(*this)); @@ -236,27 +310,32 @@ struct ComposeData : } template - void _findData(Arg1 arg1, Args&&... args) + void _findData(const Arg1& arg1, const Args&... args) { - _useIfData(static_cast::Type&>(arg1)); - _findData(std::forward(args)...); + _attemptToUse(static_cast::Type&>(arg1)); + _findData(args...); } template - void _useIfData(Arg) + void _attemptToUse(const Arg&) { // Do nothing } - void _useIfData(const typename Base::Data& data) + void _attemptToUse(const typename Base::Data& data) { static_cast(*this) = data; } + + void _attemptToUse(const CompositeType& composite) + { + _setBaseFrom(composite); + } }; //============================================================================== -template -using MakeCompositeState = ComposeData; +template +using MakeCompositeState = ComposeData; template using MakeCompositeProperties = diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index a10463b9b36cc..ae7dc619c9d1c 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -338,7 +338,7 @@ void BodyNode::setAspectProperties(const AspectProperties& properties) //============================================================================== BodyNode::Properties BodyNode::getBodyNodeProperties() const { - return mAspectProperties; + return getCompositeProperties(); } //============================================================================== @@ -895,7 +895,7 @@ const Joint* BodyNode::getChildJoint(size_t _index) const //============================================================================== ShapeNode* BodyNode::createShapeNode(const ShapePtr& shape) { - ShapeNode::Properties properties; + ShapeNode::BasicProperties properties; properties.mShape = shape; return createShapeNode(properties, true); @@ -905,7 +905,7 @@ ShapeNode* BodyNode::createShapeNode(const ShapePtr& shape) ShapeNode* BodyNode::createShapeNode(const ShapePtr& shape, const std::string& name) { - ShapeNode::Properties properties; + ShapeNode::BasicProperties properties; properties.mShape = shape; properties.mName = name; @@ -973,7 +973,7 @@ void BodyNode::removeAllShapeNodes() //============================================================================== EndEffector* BodyNode::createEndEffector(const std::string& _name) { - EndEffector::Properties properties; + EndEffector::BasicProperties properties; properties.mName = _name; return createNode(properties); diff --git a/dart/dynamics/BodyNode.h b/dart/dynamics/BodyNode.h index 89e08daa8b5a9..c43d60a299ae5 100644 --- a/dart/dynamics/BodyNode.h +++ b/dart/dynamics/BodyNode.h @@ -104,7 +104,7 @@ class BodyNode : using NodePropertiesMap = detail::NodePropertiesMap; using AspectProperties = detail::BodyNodeAspectProperties; - using Properties = detail::BodyNodeProperties; + using Properties = common::Composite::MakeProperties; BodyNode(const BodyNode&) = delete; diff --git a/dart/dynamics/EndEffector.cpp b/dart/dynamics/EndEffector.cpp index 218cb9788d6b2..6f2201ceec819 100644 --- a/dart/dynamics/EndEffector.cpp +++ b/dart/dynamics/EndEffector.cpp @@ -76,26 +76,9 @@ bool Support::isActive() const } //============================================================================== -EndEffector::Properties::Properties( - const FixedFrame::AspectProperties& fixedFrameProperties, - const UniqueProperties& effectorProperties, - const NameAspect::Properties& name, - const common::Composite::Properties& compositeProperties) - : FixedFrame::AspectProperties(fixedFrameProperties), - UniqueProperties(effectorProperties), - NameAspect::Properties(name), - mCompositeProperties(compositeProperties) +void EndEffector::setProperties(const BasicProperties& properties) { - // Do nothing -} - -//============================================================================== -void EndEffector::setProperties(const Properties& properties) -{ - FixedFrame::setAspectProperties(properties); - setAspectProperties(properties); - get()->setProperties(properties); - setCompositeProperties(properties.mCompositeProperties); + setCompositeProperties(properties); } //============================================================================== @@ -114,11 +97,7 @@ void EndEffector::setAspectProperties(const AspectProperties& properties) //============================================================================== EndEffector::Properties EndEffector::getEndEffectorProperties() const { - return Properties( - FixedFrame::getAspectProperties(), - getAspectProperties(), - get()->getProperties(), - getCompositeProperties()); + return getCompositeProperties(); } //============================================================================== @@ -177,7 +156,7 @@ void EndEffector::notifyTransformUpdate() } //============================================================================== -EndEffector::EndEffector(BodyNode* parent, const Properties& properties) +EndEffector::EndEffector(BodyNode* parent, const BasicProperties& properties) : Entity(ConstructFrame), Frame(parent), FixedFrame(parent, properties.mDefaultTransform), diff --git a/dart/dynamics/EndEffector.h b/dart/dynamics/EndEffector.h index c2a943b4e2edb..3fdf1d70cae24 100644 --- a/dart/dynamics/EndEffector.h +++ b/dart/dynamics/EndEffector.h @@ -129,22 +129,12 @@ class EndEffector final : using UniqueProperties = detail::EndEffectorProperties; - struct Properties : - FixedFrame::AspectProperties, - UniqueProperties, - NameAspect::Properties - { - Properties( - const FixedFrame::AspectProperties& fixedFrameProperties - = FixedFrame::AspectProperties(), - const UniqueProperties& effectorProperties = UniqueProperties(), - const NameAspect::Properties& name = "EndEffector", - const common::Composite::Properties& compositeProperties = - common::Composite::Properties()); - - /// The properties of the EndEffector's Aspects - common::Composite::Properties mCompositeProperties; - }; + using BasicProperties = common::Composite::MakeProperties< + NameAspect, + FixedFrame, + EndEffector>; + + using Properties = common::Composite::Properties; /// Destructor virtual ~EndEffector() = default; @@ -155,7 +145,7 @@ class EndEffector final : /// Set the Properties of this EndEffector. If _useNow is true, the current /// Transform will be set to the new default transform. - void setProperties(const Properties& _properties); + void setProperties(const BasicProperties& _properties); /// Set the Properties of this EndEffector. If _useNow is true, the current /// Transform will be set to the new default transform. @@ -202,7 +192,7 @@ class EndEffector final : protected: /// Constructor used by the Skeleton class - explicit EndEffector(BodyNode* parent, const Properties& properties); + explicit EndEffector(BodyNode* parent, const BasicProperties& properties); /// Create a clone of this BodyNode. This may only be called by the Skeleton /// class. diff --git a/dart/dynamics/ShapeFrame.cpp b/dart/dynamics/ShapeFrame.cpp index 1daad56ce6c4e..47a7241e28dbc 100644 --- a/dart/dynamics/ShapeFrame.cpp +++ b/dart/dynamics/ShapeFrame.cpp @@ -263,6 +263,7 @@ ShapeFrame::ShapeFrame(Frame* parent, const Properties& properties) onShapeUpdated(mShapeUpdatedSignal), onRelativeTransformUpdated(mRelativeTransformUpdatedSignal) { + createAspect(); mAmShapeFrame = true; setProperties(properties); } @@ -278,10 +279,18 @@ ShapeFrame::ShapeFrame(Frame* parent, onShapeUpdated(mShapeUpdatedSignal), onRelativeTransformUpdated(mRelativeTransformUpdatedSignal) { + createAspect(); mAmShapeFrame = true; setShape(shape); } +//============================================================================== +ShapeFrame::ShapeFrame(const std::tuple& args) + : ShapeFrame(std::get<0>(args), std::get<1>(args)) +{ + // Delegating constructor +} + } // namespace dynamics } // namespace dart diff --git a/dart/dynamics/ShapeFrame.h b/dart/dynamics/ShapeFrame.h index 7eb12d63dbcf1..fa2a8f7f4dd14 100644 --- a/dart/dynamics/ShapeFrame.h +++ b/dart/dynamics/ShapeFrame.h @@ -288,6 +288,9 @@ class ShapeFrame : /// Constructor ShapeFrame(Frame* parent, const ShapePtr& shape = nullptr); + /// Delegating constructor + ShapeFrame(const std::tuple& args); + /// Shape updated signal ShapeUpdatedSignal mShapeUpdatedSignal; diff --git a/dart/dynamics/ShapeNode.cpp b/dart/dynamics/ShapeNode.cpp index d811f5969e321..c9904479624eb 100644 --- a/dart/dynamics/ShapeNode.cpp +++ b/dart/dynamics/ShapeNode.cpp @@ -40,37 +40,16 @@ namespace dart { namespace dynamics { -//============================================================================== -ShapeNode::Properties::Properties( - const ShapeFrame::AspectProperties& shapeFrameProperties, - const FixedFrame::AspectProperties& fixedFrameProperties, - const NameAspect::Properties& name, - const CompositeProperties& compositeProperties) - : ShapeFrame::AspectProperties(shapeFrameProperties), - FixedFrame::AspectProperties(fixedFrameProperties), - NameAspect::Properties(name), - mCompositeProperties(compositeProperties) -{ - // Do nothing -} - //============================================================================== void ShapeNode::setProperties(const Properties& properties) { - setCompositeProperties(properties.mCompositeProperties); - ShapeFrame::setAspectProperties(properties); - FixedFrame::setAspectProperties( - static_cast(properties)); - get()->setProperties(properties); + setCompositeProperties(properties); } //============================================================================== const ShapeNode::Properties ShapeNode::getShapeNodeProperties() const { - return Properties(ShapeFrame::getAspectProperties(), - FixedFrame::getAspectProperties(), - getName(), - getCompositeProperties()); + return getCompositeProperties(); } //============================================================================== @@ -157,12 +136,13 @@ Eigen::Vector3d ShapeNode::getOffset() const } //============================================================================== -ShapeNode::ShapeNode(BodyNode* bodyNode, const Properties& properties) +ShapeNode::ShapeNode(BodyNode* bodyNode, const BasicProperties& properties) : Entity(ConstructFrame), Frame(bodyNode), FixedFrame(bodyNode), detail::ShapeNodeCompositeBase( - std::make_tuple(bodyNode, properties.mRelativeTf), bodyNode) + std::make_tuple(bodyNode, properties.mRelativeTf), + bodyNode, properties) { setProperties(properties); } @@ -175,14 +155,11 @@ ShapeNode::ShapeNode(BodyNode* bodyNode, Frame(bodyNode), FixedFrame(bodyNode), detail::ShapeNodeCompositeBase( - std::make_tuple(bodyNode, Eigen::Isometry3d::Identity()), bodyNode) + std::make_tuple(bodyNode, Eigen::Isometry3d::Identity()), + std::make_tuple(bodyNode, ShapeFrame::Properties(shape))) { // TODO(MXG): Consider changing this to a delegating constructor instead - Properties prop; - prop.mShape = shape; - prop.mName = name; - - setProperties(prop); + setName(name); } //============================================================================== diff --git a/dart/dynamics/ShapeNode.h b/dart/dynamics/ShapeNode.h index 8636767a8d3bd..04a23c03bb5cf 100644 --- a/dart/dynamics/ShapeNode.h +++ b/dart/dynamics/ShapeNode.h @@ -79,25 +79,13 @@ class ShapeNode : public detail::ShapeNodeCompositeBase const Eigen::Isometry3d& oldTransform, const Eigen::Isometry3d& newTransform)>; - using CompositeProperties = common::Composite::Properties; - - struct Properties : - ShapeFrame::AspectProperties, - FixedFrame::AspectProperties, - NameAspect::Properties - { - /// Composed constructor - Properties( - const ShapeFrame::AspectProperties& shapeFrameProperties - = ShapeFrame::AspectProperties(), - const FixedFrame::AspectProperties& fixedFrameProperties - = FixedFrame::AspectProperties(), - const NameAspect::Properties& name = "ShapeNode", - const CompositeProperties& compositeProperties = CompositeProperties()); - - /// The properties of the ShapeNode's Aspects - CompositeProperties mCompositeProperties; - }; + using BasicProperties = common::Composite::MakeProperties< + NameAspect, + FixedFrame, + ShapeFrame>; + + using Properties = common::Composite::Properties; + /// Destructor virtual ~ShapeNode() = default; @@ -141,7 +129,7 @@ class ShapeNode : public detail::ShapeNodeCompositeBase protected: /// Constructor used by the Skeleton class - ShapeNode(BodyNode* bodyNode, const Properties& properties); + ShapeNode(BodyNode* bodyNode, const BasicProperties& properties); /// Constructor used by the Skeleton class ShapeNode(BodyNode* bodyNode, const ShapePtr& shape, diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index 46b97fb67f477..98b842b8c06e0 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -518,6 +518,18 @@ Skeleton::Configuration Skeleton::getConfiguration( return config; } +//============================================================================== +void Skeleton::setState(const State& state) +{ + setCompositeState(state); +} + +//============================================================================== +Skeleton::State Skeleton::getState() const +{ + return getCompositeState(); +} + //============================================================================== void Skeleton::setProperties(const Properties& properties) { @@ -1319,41 +1331,6 @@ DART_BAKE_SPECIALIZED_NODE_SKEL_DEFINITIONS( Skeleton, ShapeNode ) //============================================================================== DART_BAKE_SPECIALIZED_NODE_SKEL_DEFINITIONS( Skeleton, EndEffector ) -//============================================================================== -void Skeleton::setState(const Eigen::VectorXd& _state) -{ - assert(_state.size() % 2 == 0); - - size_t index = 0; - size_t halfSize = _state.size() / 2; - Joint* joint; - - for (size_t i = 0; i < mSkelCache.mBodyNodes.size(); ++i) - { - joint = mSkelCache.mBodyNodes[i]->getParentJoint(); - - const size_t dof = joint->getNumDofs(); - - if (dof) - { - joint->setPositions(_state.segment(index, dof)); - joint->setVelocities(_state.segment(index + halfSize, dof)); - - index += dof; - } - } -} - -//============================================================================== -Eigen::VectorXd Skeleton::getState() const -{ - Eigen::VectorXd state(2 * getNumDofs()); - - state << getPositions(), getVelocities(); - - return state; -} - //============================================================================== void Skeleton::integratePositions(double _dt) { diff --git a/dart/dynamics/Skeleton.h b/dart/dynamics/Skeleton.h index c6d635b0d1fa6..e622735853bb1 100644 --- a/dart/dynamics/Skeleton.h +++ b/dart/dynamics/Skeleton.h @@ -71,6 +71,8 @@ class Skeleton : using AspectPropertiesData = detail::SkeletonAspectProperties; using AspectProperties = common::Aspect::MakeProperties; + + using State = common::Composite::State; using Properties = common::Composite::Properties; enum ConfigFlag_t @@ -195,6 +197,20 @@ class Skeleton : Configuration getConfiguration(const std::vector& indices, int flags = CONFIG_ALL) const; + /// \} + + //---------------------------------------------------------------------------- + /// \{ \name State + //---------------------------------------------------------------------------- + + /// Set the State of this Skeleton [alias for setCompositeState(~)] + void setState(const State& state); + + /// Get the State of this Skeleton [alias for getCompositeState()] + State getState() const; + + /// \} + //---------------------------------------------------------------------------- /// \{ \name Properties //---------------------------------------------------------------------------- @@ -471,16 +487,6 @@ class Skeleton : Eigen::VectorXd getVelocityDifferences( const Eigen::VectorXd& _dq2, const Eigen::VectorXd& _dq1) const; - //---------------------------------------------------------------------------- - // State - //---------------------------------------------------------------------------- - - /// Set the state of this skeleton described in generalized coordinates - void setState(const Eigen::VectorXd& _state); - - /// Get the state of this skeleton described in generalized coordinates - Eigen::VectorXd getState() const; - //---------------------------------------------------------------------------- /// \{ \name Support Polygon //---------------------------------------------------------------------------- diff --git a/dart/dynamics/detail/BodyNodeAspect.h b/dart/dynamics/detail/BodyNodeAspect.h index d751645025a28..50302efddab38 100644 --- a/dart/dynamics/detail/BodyNodeAspect.h +++ b/dart/dynamics/detail/BodyNodeAspect.h @@ -106,8 +106,6 @@ struct BodyNodeAspectProperties EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -using BodyNodeProperties = BodyNodeAspectProperties; - //============================================================================== using NodeTypeStateVector = common::CloneableVector< std::unique_ptr >; using NodeStateMap = std::map< std::type_index, std::unique_ptr >; diff --git a/docs/readthedocs/tutorials/collisions.md b/docs/readthedocs/tutorials/collisions.md index 118c0882ef169..e1d7bce3a8aae 100644 --- a/docs/readthedocs/tutorials/collisions.md +++ b/docs/readthedocs/tutorials/collisions.md @@ -127,7 +127,7 @@ that new BodyNode: ```cpp BodyNode* bn = chain->createJointAndBodyNodePair( - parent, properties, BodyNode::Properties(name)).second; + parent, properties, BodyNode::AspectProperties(name)).second; ``` There's a lot going on in this function, so let's break it down for a moment: @@ -145,7 +145,7 @@ type that we want to create, but the default argument is a standard rigid BodyNode, so we can leave the second argument blank. ```cpp -(parent, properties, BodyNode::Properties(name)) +(parent, properties, BodyNode::AspectProperties(name)) ``` Now for the function arguments: The first specifies the parent BodyNode. In the @@ -153,7 +153,8 @@ event that you want to create a root BodyNode, you can simply pass in a nullptr as the parent. The second argument is a ``JointType::Properties`` struct, so we pass in the ``properties`` object that we created earlier. The third argument is a ``BodyNode::Properties`` struct, but we're going to set the BodyNode properties -later, so we'll just toss the name in and leave the rest as default values. +later, so we'll just toss the name in by wrapping it up in a +``BodyNode::AspectProperties`` object and leave the rest as default values. Now notice the very last thing on this line of code: diff --git a/docs/readthedocs/tutorials/multi-pendulum.md b/docs/readthedocs/tutorials/multi-pendulum.md index c96ea9fc33def..ae2257e4cab3b 100644 --- a/docs/readthedocs/tutorials/multi-pendulum.md +++ b/docs/readthedocs/tutorials/multi-pendulum.md @@ -38,8 +38,8 @@ World. In the function ``makeRootBody``, we create a pair of a empty pendulum skeleton. ```cpp -BodyNodePtr bn = - pendulum->createJointAndBodyNodePair(nullptr, properties, BodyNode::Properties(name)).second; +BodyNodePtr bn = pendulum->createJointAndBodyNodePair( + nullptr, properties, BodyNode::AspectProperties(name)).second; ``` Note that the first parameters is a nullptr, which indicates that @@ -50,8 +50,8 @@ the first parameter. In fact, this is how we add more BodyNodes to the pendulum in the function ``addBody``: ```cpp -BodyNodePtr bn = - pendulum->createJointAndBodyNodePair(parent, properties, BodyNode::Properties(name)).second; +BodyNodePtr bn = pendulum->createJointAndBodyNodePair( + parent, properties, BodyNode::AspectProperties(name)).second; ``` The simplest way to set up a simulation program in DART is to use ``SimWindow`` class. A SimWindow owns an instance of ``World`` and diff --git a/tutorials/tutorialCollisions-Finished.cpp b/tutorials/tutorialCollisions-Finished.cpp index e799c149e132a..17a99a68ee66e 100644 --- a/tutorials/tutorialCollisions-Finished.cpp +++ b/tutorials/tutorialCollisions-Finished.cpp @@ -372,7 +372,7 @@ BodyNode* addRigidBody(const SkeletonPtr& chain, const std::string& name, // Create the Joint and Body pair BodyNode* bn = chain->createJointAndBodyNodePair( - parent, properties, BodyNode::Properties(name)).second; + parent, properties, BodyNode::AspectProperties(name)).second; // Make the shape based on the requested Shape type ShapePtr shape; @@ -484,7 +484,7 @@ BodyNode* addSoftBody(const SkeletonPtr& chain, const std::string& name, soft_properties.mDampCoeff = default_soft_damping; // Create the Joint and Body pair - SoftBodyNode::Properties body_properties(BodyNode::Properties(name), + SoftBodyNode::Properties body_properties(BodyNode::AspectProperties(name), soft_properties); SoftBodyNode* bn = chain->createJointAndBodyNodePair( parent, joint_properties, body_properties).second; diff --git a/tutorials/tutorialMultiPendulum-Finished.cpp b/tutorials/tutorialMultiPendulum-Finished.cpp index 46ccad4545700..a80eeef89a4c5 100644 --- a/tutorials/tutorialMultiPendulum-Finished.cpp +++ b/tutorials/tutorialMultiPendulum-Finished.cpp @@ -372,7 +372,7 @@ BodyNode* makeRootBody(const SkeletonPtr& pendulum, const std::string& name) properties.mDampingCoefficients = Eigen::Vector3d::Constant(default_damping); BodyNodePtr bn = pendulum->createJointAndBodyNodePair( - nullptr, properties, BodyNode::Properties(name)).second; + nullptr, properties, BodyNode::AspectProperties(name)).second; // Make a shape for the Joint const double& R = default_width; @@ -402,7 +402,7 @@ BodyNode* addBody(const SkeletonPtr& pendulum, BodyNode* parent, // Create a new BodyNode, attached to its parent by a RevoluteJoint BodyNodePtr bn = pendulum->createJointAndBodyNodePair( - parent, properties, BodyNode::Properties(name)).second; + parent, properties, BodyNode::AspectProperties(name)).second; // Make a shape for the Joint const double R = default_width / 2.0; diff --git a/tutorials/tutorialMultiPendulum.cpp b/tutorials/tutorialMultiPendulum.cpp index e4a35c69d2621..1e41a4a3af084 100644 --- a/tutorials/tutorialMultiPendulum.cpp +++ b/tutorials/tutorialMultiPendulum.cpp @@ -301,7 +301,7 @@ BodyNode* makeRootBody(const SkeletonPtr& pendulum, const std::string& name) properties.mDampingCoefficients = Eigen::Vector3d::Constant(default_damping); BodyNodePtr bn = pendulum->createJointAndBodyNodePair( - nullptr, properties, BodyNode::Properties(name)).second; + nullptr, properties, BodyNode::AspectProperties(name)).second; // Make a shape for the Joint const double& R = default_width; @@ -331,7 +331,7 @@ BodyNode* addBody(const SkeletonPtr& pendulum, BodyNode* parent, // Create a new BodyNode, attached to its parent by a RevoluteJoint BodyNodePtr bn = pendulum->createJointAndBodyNodePair( - parent, properties, BodyNode::Properties(name)).second; + parent, properties, BodyNode::AspectProperties(name)).second; // Make a shape for the Joint const double R = default_width / 2.0; diff --git a/unittests/TestHelpers.h b/unittests/TestHelpers.h index 708d2fda9df5e..07a97c2b36ed3 100644 --- a/unittests/TestHelpers.h +++ b/unittests/TestHelpers.h @@ -107,7 +107,7 @@ bool equals(const Eigen::DenseBase& _expected, void addEndEffector(SkeletonPtr robot, BodyNode* parent_node, Vector3d dim) { // Create the end-effector node with a random dimension - BodyNode::Properties node(std::string("ee")); + BodyNode::Properties node(BodyNode::AspectProperties("ee")); std::shared_ptr shape(new BoxShape(Vector3d(0.2, 0.2, 0.2))); Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); @@ -167,7 +167,7 @@ SkeletonPtr createThreeLinkRobot(Vector3d dim1, TypeOfDOF type1, Vector3d dimEE = dim1; // Create the first link - BodyNode::Properties node(std::string("link1")); + BodyNode::Properties node(BodyNode::AspectProperties("link1")); node.mInertia.setLocalCOM(Vector3d(0.0, 0.0, dim1(2)/2.0)); std::shared_ptr shape(new BoxShape(dim1)); @@ -186,7 +186,7 @@ SkeletonPtr createThreeLinkRobot(Vector3d dim1, TypeOfDOF type1, if(stopAfter > 1) { // Create the second link - node = BodyNode::Properties(std::string("link2")); + node = BodyNode::Properties(BodyNode::AspectProperties("link2")); node.mInertia.setLocalCOM(Vector3d(0.0, 0.0, dim2(2)/2.0)); shape = std::shared_ptr(new BoxShape(dim2)); @@ -212,7 +212,7 @@ SkeletonPtr createThreeLinkRobot(Vector3d dim1, TypeOfDOF type1, if(stopAfter > 2) { // Create the third link - node = BodyNode::Properties(std::string("link3")); + node = BodyNode::Properties(BodyNode::AspectProperties("link3")); node.mInertia.setLocalCOM(Vector3d(0.0, 0.0, dim3(2)/2.0)); shape = std::shared_ptr(new BoxShape(dim3)); std::pair pair3 = add1DofJoint( @@ -265,7 +265,7 @@ SkeletonPtr createNLinkRobot(int _n, Vector3d dim, TypeOfDOF type, robot->disableSelfCollision(); // Create the first link, the joint with the ground and its shape - BodyNode::Properties node(std::string("link1")); + BodyNode::Properties node(BodyNode::AspectProperties("link1")); node.mInertia.setLocalCOM(Vector3d(0.0, 0.0, dim(2)/2.0)); std::shared_ptr shape(new BoxShape(dim)); @@ -289,7 +289,7 @@ SkeletonPtr createNLinkRobot(int _n, Vector3d dim, TypeOfDOF type, ssLink << "link" << i; ssJoint << "joint" << i; - node = BodyNode::Properties(ssLink.str()); + node = BodyNode::Properties(BodyNode::AspectProperties(ssLink.str())); node.mInertia.setLocalCOM(Vector3d(0.0, 0.0, dim(2)/2.0)); shape = std::shared_ptr(new BoxShape(dim)); @@ -332,7 +332,7 @@ SkeletonPtr createNLinkPendulum(size_t numBodyNodes, robot->disableSelfCollision(); // Create the first link, the joint with the ground and its shape - BodyNode::Properties node(std::string("link1")); + BodyNode::Properties node(BodyNode::AspectProperties("link1")); node.mInertia.setLocalCOM(Vector3d(0.0, 0.0, dim(2)/2.0)); std::shared_ptr shape(new BoxShape(dim)); @@ -359,7 +359,7 @@ SkeletonPtr createNLinkPendulum(size_t numBodyNodes, ssLink << "link" << i; ssJoint << "joint" << i; - node = BodyNode::Properties(ssLink.str()); + node = BodyNode::Properties(BodyNode::AspectProperties(ssLink.str())); node.mInertia.setLocalCOM(Vector3d(0.0, 0.0, dim(2)/2.0)); shape = std::shared_ptr(new BoxShape(dim)); @@ -399,7 +399,7 @@ SkeletonPtr createGround( T.linear() = eulerXYZToMatrix(_orientation); Joint::Properties joint("joint1", T); - BodyNode::Properties node(std::string("link")); + BodyNode::Properties node(BodyNode::AspectProperties(std::string("link"))); std::shared_ptr shape(new BoxShape(_size)); node.mInertia.setMass(mass); @@ -423,7 +423,7 @@ SkeletonPtr createObject( MultiDofJoint<6>::Properties joint(std::string("joint1")); - BodyNode::Properties node(std::string("link1")); + BodyNode::Properties node(BodyNode::AspectProperties(std::string("link1"))); node.mInertia.setMass(mass); SkeletonPtr skeleton = Skeleton::create(); diff --git a/unittests/testAspect.cpp b/unittests/testAspect.cpp index b37d6e74ddb00..fb024bcc26cca 100644 --- a/unittests/testAspect.cpp +++ b/unittests/testAspect.cpp @@ -587,7 +587,7 @@ TEST(Aspect, StateAndProperties) mgr1.createAspect(); - // ---- Test state transfer ---- + // ---- Test State Transfer ---- mgr2.setCompositeState(mgr1.getCompositeState()); @@ -618,7 +618,7 @@ TEST(Aspect, StateAndProperties) EXPECT_TRUE( nullptr == mgr2.get() ); - // ---- Test property transfer ---- + // ---- Test Property Transfer ---- mgr2.setCompositeProperties(mgr1.getCompositeProperties()); @@ -647,6 +647,39 @@ TEST(Aspect, StateAndProperties) EXPECT_TRUE( nullptr == mgr2.get() ); EXPECT_TRUE( nullptr == mgr2.get() ); + + + // ---- Test Data Containers ---- + Composite::MakeState state( + mgr1.getCompositeState()); + + EXPECT_EQ(mgr1.get()->mState.val, + state.DoubleAspect::State::val); + EXPECT_EQ(mgr1.get()->mState.val, + state.FloatAspect::State::val); + EXPECT_EQ(mgr1.get()->mState.val, + state.IntAspect::State::val); + + + Composite::MakeProperties properties( + mgr2.getCompositeProperties()); + + EXPECT_EQ(mgr1.get()->mProperties.val, + properties.DoubleAspect::Properties::val); + EXPECT_EQ(mgr1.get()->mProperties.val, + properties.CharAspect::Properties::val); + EXPECT_EQ(mgr1.get()->mProperties.val, + properties.FloatAspect::Properties::val); + + + DoubleAspect::State doubleState(2.5); + FloatAspect::State floatState(4.7); + IntAspect::State intState(7); + CharAspect::State charState('h'); + + // The constructor arguments should match the type order + Composite::MakeState( + doubleState, intState, charState, floatState); } TEST(Aspect, Construction) diff --git a/unittests/testDynamics.cpp b/unittests/testDynamics.cpp index 80d52ce08b88d..8cdca2eb45a37 100644 --- a/unittests/testDynamics.cpp +++ b/unittests/testDynamics.cpp @@ -1250,10 +1250,14 @@ void DynamicsTest::compareEquationsOfMotion(const std::string& _fileName) } // Set random states - VectorXd x = skel->getState(); - for (int k = 0; k < x.size(); ++k) - x[k] = random(lb, ub); - skel->setState(x); + Skeleton::Configuration x = skel->getConfiguration( + Skeleton::CONFIG_POSITIONS | Skeleton::CONFIG_VELOCITIES); + for (int k = 0; k < skel->getNumDofs(); ++k) + { + x.mPositions[k] = random(lb, ub); + x.mVelocities[k] = random(lb, ub); + } + skel->setConfiguration(x); //------------------------ Mass Matrix Test ---------------------------- // Get matrices diff --git a/unittests/testInverseKinematics.cpp b/unittests/testInverseKinematics.cpp index 21a647f5906f5..ce19b87c54168 100644 --- a/unittests/testInverseKinematics.cpp +++ b/unittests/testInverseKinematics.cpp @@ -52,7 +52,7 @@ SkeletonPtr createFreeFloatingTwoLinkRobot(Vector3d dim1, SkeletonPtr robot = Skeleton::create(); // Create the first link - BodyNode::Properties node(std::string("link1")); + BodyNode::Properties node(BodyNode::AspectProperties("link1")); node.mInertia.setLocalCOM(Vector3d(0.0, 0.0, dim1(2)/2.0)); std::shared_ptr shape(new BoxShape(dim1)); @@ -62,7 +62,7 @@ SkeletonPtr createFreeFloatingTwoLinkRobot(Vector3d dim1, shape); // Create the second link - node = BodyNode::Properties(std::string("link2")); + node = BodyNode::Properties(BodyNode::AspectProperties("link2")); node.mInertia.setLocalCOM(Vector3d(0.0, 0.0, dim2(2)/2.0)); shape = std::shared_ptr(new BoxShape(dim2)); diff --git a/unittests/testSoftDynamics.cpp b/unittests/testSoftDynamics.cpp index e27d4fd463993..5923904d0fdfd 100644 --- a/unittests/testSoftDynamics.cpp +++ b/unittests/testSoftDynamics.cpp @@ -311,10 +311,13 @@ void SoftDynamicsTest::compareEquationsOfMotion(const std::string& _fileName) } // Set random states - VectorXd x = softSkel->getState(); - for (int k = 0; k < x.size(); ++k) - x[k] = random(lb, ub); - softSkel->setState(x); + Skeleton::Configuration x = softSkel->getConfiguration(); + for (int k = 0; k < softSkel->getNumDofs(); ++k) + { + x.mPositions[k] = random(lb, ub); + x.mVelocities[k] = random(lb, ub); + } + softSkel->setConfiguration(x); //------------------------ Mass Matrix Test ---------------------------- // Get matrices From 16eec9608b8c4132708d118406a06b3347642e82 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Sun, 10 Apr 2016 11:31:16 -0400 Subject: [PATCH 40/62] add Eigen alignment and virtual destructors wherever they are missing --- dart/common/detail/CompositeData.h | 6 +++++ dart/dynamics/EndEffector.h | 28 +++++++++++++++++++++- dart/dynamics/FixedFrame.h | 8 +++++++ dart/dynamics/FixedJacobianNode.h | 7 ++++++ dart/dynamics/HierarchicalIK.h | 12 ++++++++++ dart/dynamics/PointMass.h | 4 ++++ dart/dynamics/ShapeFrame.h | 3 +++ dart/dynamics/SimpleFrame.h | 4 ++++ dart/dynamics/SingleDofJoint.h | 4 ++++ dart/dynamics/detail/BodyNodeAspect.h | 5 ++++ dart/dynamics/detail/MultiDofJointAspect.h | 6 ++++- dart/dynamics/detail/SkeletonAspect.h | 2 ++ dart/dynamics/detail/SoftBodyNodeAspect.h | 2 ++ 13 files changed, 89 insertions(+), 2 deletions(-) diff --git a/dart/common/detail/CompositeData.h b/dart/common/detail/CompositeData.h index 8bccbcbf6b19a..27bf953e2bcaf 100644 --- a/dart/common/detail/CompositeData.h +++ b/dart/common/detail/CompositeData.h @@ -37,6 +37,8 @@ #ifndef DART_COMMON_DETAIL_COMPOSITESTATEANDPROPERTIES_H_ #define DART_COMMON_DETAIL_COMPOSITESTATEANDPROPERTIES_H_ +#include + #include #include #include @@ -331,6 +333,10 @@ struct ComposeData : { _setBaseFrom(composite); } + +public: + // To get byte-aligned Eigen vectors + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; //============================================================================== diff --git a/dart/dynamics/EndEffector.h b/dart/dynamics/EndEffector.h index 3fdf1d70cae24..d0eaca61f3ab5 100644 --- a/dart/dynamics/EndEffector.h +++ b/dart/dynamics/EndEffector.h @@ -56,24 +56,45 @@ namespace detail { //============================================================================== struct EndEffectorProperties { + /// The default relative transform for the EndEffector. If the relative + /// transform of the EndEffector is ever changed, you can call + /// resetRelativeTransform() to return the relative transform to this one. Eigen::Isometry3d mDefaultTransform; EndEffectorProperties( const Eigen::Isometry3d& defaultTf = Eigen::Isometry3d::Identity()); + + // To get byte-aligned Eigen vectors + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; //============================================================================== struct SupportStateData { + /// Whether or not this EndEffector is currently being used to support the + /// weight of the robot. + bool mActive; + inline SupportStateData(bool active = false) : mActive(active) { } - bool mActive; + // To get byte-aligned Eigen vectors + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; //============================================================================== struct SupportPropertiesData { + /// A set of points representing the support polygon that can be provided by + /// the EndEffector. These points must be defined relative to the EndEffector + /// frame. math::SupportGeometry mGeometry; + + inline SupportPropertiesData( + const math::SupportGeometry& geometry = math::SupportGeometry()) + : mGeometry(geometry) { } + + // To get byte-aligned Eigen vectors + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; void SupportUpdate(Support* support); @@ -198,6 +219,11 @@ class EndEffector final : /// class. virtual Node* cloneNode(BodyNode* _parent) const override; +public: + + // To get byte-aligned Eigen vectors + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; } // namespace dynamics diff --git a/dart/dynamics/FixedFrame.h b/dart/dynamics/FixedFrame.h index dbe4e918ee593..b6eec983ddc9f 100644 --- a/dart/dynamics/FixedFrame.h +++ b/dart/dynamics/FixedFrame.h @@ -47,10 +47,14 @@ namespace dynamics { namespace detail { struct FixedFrameProperties { + /// The relative transform of the FixedFrame Eigen::Isometry3d mRelativeTf; FixedFrameProperties( const Eigen::Isometry3d& relativeTf = Eigen::Isometry3d::Identity()); + + // To get byte-aligned Eigen vectors + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace detail @@ -103,6 +107,10 @@ class FixedFrame : /// Used for Relative Velocity and Relative Acceleration of this Frame static const Eigen::Vector6d mZero; + +public: + // To get byte-aligned Eigen vectors + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace dynamics diff --git a/dart/dynamics/FixedJacobianNode.h b/dart/dynamics/FixedJacobianNode.h index 7bd3af2f14e09..62fa30036ade9 100644 --- a/dart/dynamics/FixedJacobianNode.h +++ b/dart/dynamics/FixedJacobianNode.h @@ -172,10 +172,17 @@ class FixedJacobianNode : /// /// Do not use directly! Use getJacobianClassicDeriv() to access this quantity math::Jacobian mWorldJacobianClassicDeriv; + + // To get byte-aligned Eigen vectors + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; mutable Cache mCache; +public: + // To get byte-aligned Eigen vectors + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; } // namespace dynamics diff --git a/dart/dynamics/HierarchicalIK.h b/dart/dynamics/HierarchicalIK.h index d1ecce1f5c997..4b2bb6a8e144a 100644 --- a/dart/dynamics/HierarchicalIK.h +++ b/dart/dynamics/HierarchicalIK.h @@ -304,6 +304,10 @@ class HierarchicalIK : public common::Subject /// Cache for Jacobians mutable math::Jacobian mJacCache; + +public: + // To get byte-aligned Eigen vectors + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; /// The CompositeIK class allows you to specify an arbitrary hierarchy of @@ -348,6 +352,10 @@ class CompositeIK : public HierarchicalIK /// The set of modules being used by this CompositeIK std::unordered_set< std::shared_ptr > mModuleSet; + +public: + // To get byte-aligned Eigen vectors + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; /// The WholeBodyIK class provides an interface for simultaneously solving all @@ -375,6 +383,10 @@ class WholeBodyIK : public HierarchicalIK /// Constructor WholeBodyIK(const SkeletonPtr& _skel); + +public: + // To get byte-aligned Eigen vectors + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace dynamics diff --git a/dart/dynamics/PointMass.h b/dart/dynamics/PointMass.h index 4aae235f7b81e..86bed4ada1e78 100644 --- a/dart/dynamics/PointMass.h +++ b/dart/dynamics/PointMass.h @@ -77,6 +77,8 @@ class PointMass : public common::Subject const Eigen::Vector3d& forces = Eigen::Vector3d::Zero()); bool operator==(const State& other) const; + + virtual ~State() = default; }; /// Properties for each PointMass @@ -142,6 +144,8 @@ class PointMass : public common::Subject bool operator==(const Properties& other) const; bool operator!=(const Properties& other) const; + + virtual ~Properties() = default; }; //-------------------------------------------------------------------------- diff --git a/dart/dynamics/ShapeFrame.h b/dart/dynamics/ShapeFrame.h index 2cae153690cb1..3da71de19588d 100644 --- a/dart/dynamics/ShapeFrame.h +++ b/dart/dynamics/ShapeFrame.h @@ -73,6 +73,9 @@ struct VisualAspectProperties /// Destructor virtual ~VisualAspectProperties() = default; + + // To get byte-aligned Eigen vectors + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; struct CollisionAspectProperties diff --git a/dart/dynamics/SimpleFrame.h b/dart/dynamics/SimpleFrame.h index 3d8b5e6a6365f..9b486fa54b67d 100644 --- a/dart/dynamics/SimpleFrame.h +++ b/dart/dynamics/SimpleFrame.h @@ -211,6 +211,10 @@ class SimpleFrame : public Detachable, public ShapeFrame /// Partial Acceleration of this Frame mutable Eigen::Vector6d mPartialAcceleration; +public: + // To get byte-aligned Eigen vectors + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; } // namespace dart diff --git a/dart/dynamics/SingleDofJoint.h b/dart/dynamics/SingleDofJoint.h index a9f2207765bdd..63436be44da1c 100644 --- a/dart/dynamics/SingleDofJoint.h +++ b/dart/dynamics/SingleDofJoint.h @@ -736,6 +736,10 @@ class SingleDofJoint : public detail::SingleDofJointBase void updateConstrainedTermsKinematic(double _timeStep); /// \} + +public: + // To get byte-aligned Eigen vectors + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace dynamics diff --git a/dart/dynamics/detail/BodyNodeAspect.h b/dart/dynamics/detail/BodyNodeAspect.h index 50302efddab38..df1cc5817d050 100644 --- a/dart/dynamics/detail/BodyNodeAspect.h +++ b/dart/dynamics/detail/BodyNodeAspect.h @@ -65,6 +65,11 @@ struct BodyNodeState BodyNodeState(bool isColliding = false, const Eigen::Vector6d& Fext = Eigen::Vector6d::Zero()); + + virtual ~BodyNodeState() = default; + + // To get byte-aligned Eigen vectors + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; //============================================================================== diff --git a/dart/dynamics/detail/MultiDofJointAspect.h b/dart/dynamics/detail/MultiDofJointAspect.h index 885384e0afc6f..83fdf9ceb9071 100644 --- a/dart/dynamics/detail/MultiDofJointAspect.h +++ b/dart/dynamics/detail/MultiDofJointAspect.h @@ -76,6 +76,11 @@ struct MultiDofJointState const Vector& accelerations = Vector::Zero(), const Vector& forces = Vector::Zero(), const Vector& commands = Vector::Zero()); + + virtual ~MultiDofJointState() = default; + + // To get byte-aligned Eigen vectors + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; //============================================================================== @@ -164,7 +169,6 @@ struct MultiDofJointUniqueProperties public: // To get byte-aligned Eigen vectors EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; //============================================================================== diff --git a/dart/dynamics/detail/SkeletonAspect.h b/dart/dynamics/detail/SkeletonAspect.h index 1792f605355ed..a0369420c3297 100644 --- a/dart/dynamics/detail/SkeletonAspect.h +++ b/dart/dynamics/detail/SkeletonAspect.h @@ -87,6 +87,8 @@ struct SkeletonAspectProperties double _timeStep = 0.001, bool _enabledSelfCollisionCheck = false, bool _enableAdjacentBodyCheck = false); + + virtual ~SkeletonAspectProperties() = default; }; //============================================================================== diff --git a/dart/dynamics/detail/SoftBodyNodeAspect.h b/dart/dynamics/detail/SoftBodyNodeAspect.h index aaf5fd7bcee1a..668630e2a8823 100644 --- a/dart/dynamics/detail/SoftBodyNodeAspect.h +++ b/dart/dynamics/detail/SoftBodyNodeAspect.h @@ -61,6 +61,8 @@ struct SoftBodyNodeUniqueState { /// Array of States for PointMasses std::vector mPointStates; + + virtual ~SoftBodyNodeUniqueState() = default; }; //============================================================================== From 11b85b388f7c0357423328d5652e5c29fbf2fc8b Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Sun, 10 Apr 2016 12:06:15 -0400 Subject: [PATCH 41/62] rename header file for consistency --- dart/dynamics/Joint.h | 2 +- dart/dynamics/detail/{Joint.h => JointAspect.h} | 0 2 files changed, 1 insertion(+), 1 deletion(-) rename dart/dynamics/detail/{Joint.h => JointAspect.h} (100%) diff --git a/dart/dynamics/Joint.h b/dart/dynamics/Joint.h index 0b93e723ea31b..26250cab6e9d5 100644 --- a/dart/dynamics/Joint.h +++ b/dart/dynamics/Joint.h @@ -48,7 +48,7 @@ #include "dart/common/EmbeddedAspect.h" #include "dart/math/MathTypes.h" #include "dart/dynamics/SmartPointer.h" -#include "dart/dynamics/detail/Joint.h" +#include "dart/dynamics/detail/JointAspect.h" namespace dart { namespace dynamics { diff --git a/dart/dynamics/detail/Joint.h b/dart/dynamics/detail/JointAspect.h similarity index 100% rename from dart/dynamics/detail/Joint.h rename to dart/dynamics/detail/JointAspect.h From 118a723c10085c738f3caac2b28c64eac95661ae Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 11 Apr 2016 11:22:58 -0400 Subject: [PATCH 42/62] Add Eigen alignment to VisualAspect --- dart/dynamics/ShapeFrame.h | 3 +++ dart/dynamics/detail/EntityNode.h | 2 ++ 2 files changed, 5 insertions(+) diff --git a/dart/dynamics/ShapeFrame.h b/dart/dynamics/ShapeFrame.h index 3da71de19588d..79840d4821365 100644 --- a/dart/dynamics/ShapeFrame.h +++ b/dart/dynamics/ShapeFrame.h @@ -179,6 +179,9 @@ class VisualAspect final : /// setting bool isHidden() const; + // To get byte-aligned Eigen vectors + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; class CollisionAspect final : diff --git a/dart/dynamics/detail/EntityNode.h b/dart/dynamics/detail/EntityNode.h index f563c232aece7..ca638dbf693fc 100644 --- a/dart/dynamics/detail/EntityNode.h +++ b/dart/dynamics/detail/EntityNode.h @@ -39,6 +39,8 @@ #include "dart/dynamics/EntityNode.h" +#include "dart/dynamics/Entity.h" + namespace dart { namespace dynamics { From e174114d05597cdb949c3808e398d7edf6d0c026 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Wed, 13 Apr 2016 16:48:05 -0400 Subject: [PATCH 43/62] rename osgDart --> gui::osg --- dart/gui/osg/examples/osgSoftBodies.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/dart/gui/osg/examples/osgSoftBodies.cpp b/dart/gui/osg/examples/osgSoftBodies.cpp index fd8051f431eb2..49fef7bd9fb80 100644 --- a/dart/gui/osg/examples/osgSoftBodies.cpp +++ b/dart/gui/osg/examples/osgSoftBodies.cpp @@ -43,12 +43,12 @@ using namespace dart::dynamics; -class RecordingWorld : public osgDart::WorldNode +class RecordingWorld : public dart::gui::osg::WorldNode { public: RecordingWorld(const dart::simulation::WorldPtr& world) - : osgDart::WorldNode(world) + : dart::gui::osg::WorldNode(world) { grabTimeSlice(); mCurrentIndex = 0; From b3c131fabe2bf3925a5cb1aa0dbfbd8ffd117e6c Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 14 Apr 2016 18:09:38 -0400 Subject: [PATCH 44/62] Change manager --> composite --- dart/common/Aspect.cpp | 4 ++-- dart/common/Aspect.h | 4 ++-- dart/common/Composite.h | 6 +++--- dart/common/RequiresAspect.h | 2 +- dart/common/detail/Aspect.h | 4 ++-- unittests/testAspect.cpp | 26 +++++++++++++------------- 6 files changed, 23 insertions(+), 23 deletions(-) diff --git a/dart/common/Aspect.cpp b/dart/common/Aspect.cpp index 53419e00ef170..113349a730ca0 100644 --- a/dart/common/Aspect.cpp +++ b/dart/common/Aspect.cpp @@ -69,9 +69,9 @@ const Aspect::Properties* Aspect::getAspectProperties() const } //============================================================================== -Aspect::Aspect(Composite* manager) +Aspect::Aspect(Composite* composite) { - if(nullptr == manager) + if(nullptr == composite) { dterr << "[Aspect::constructor] You are not allowed to construct an Aspect " << "outside of an Composite!\n"; diff --git a/dart/common/Aspect.h b/dart/common/Aspect.h index 2942fe38afb42..790d5e58c1b32 100644 --- a/dart/common/Aspect.h +++ b/dart/common/Aspect.h @@ -94,7 +94,7 @@ class Aspect /// Virtual destructor virtual ~Aspect() = default; - /// Clone this Aspect into a new manager + /// Clone this Aspect into a new composite virtual std::unique_ptr cloneAspect(Composite* newComposite) const = 0; /// Set the State of this Aspect. By default, this does nothing. @@ -118,7 +118,7 @@ class Aspect /// We require the Composite argument in this constructor to make it clear /// to extensions that they must have a Composite argument in their /// constructors. - Aspect(Composite* manager); + Aspect(Composite* composite); /// This function will be triggered (1) after the Aspect has been created /// [transfer will be false] and (2) after the Aspect has been transferred diff --git a/dart/common/Composite.h b/dart/common/Composite.h index 5dec327e66aee..dc05b4f47c755 100644 --- a/dart/common/Composite.h +++ b/dart/common/Composite.h @@ -137,7 +137,7 @@ class Composite /// Set the states of the aspects in this Composite based on the given /// Composite::State. The states of any Aspect types that do not exist - /// within this manager will be ignored. + /// within this composite will be ignored. void setCompositeState(const State& newStates); /// Get the states of the aspects inside of this Composite @@ -148,7 +148,7 @@ class Composite /// Set the properties of the aspects in this Composite based on the given /// Composite::Properties. The properties of any Aspect types that do not - /// exist within this manager will be ignored. + /// exist within this composite will be ignored. void setCompositeProperties(const Properties& newProperties); /// Get the properties of the aspects inside of this Composite @@ -188,7 +188,7 @@ class Composite AspectMap mAspectMap; /// A set containing type information for Aspects which are not allowed to - /// leave this manager. + /// leave this composite. RequiredAspectSet mRequiredAspects; }; diff --git a/dart/common/RequiresAspect.h b/dart/common/RequiresAspect.h index c7282b1cec5d3..30a7e122070bb 100644 --- a/dart/common/RequiresAspect.h +++ b/dart/common/RequiresAspect.h @@ -45,7 +45,7 @@ namespace common { //============================================================================== /// RequiresAspect allows classes that inherit Composite to know which Aspects /// are required for their operation. This guarantees that there is no way for -/// a required Aspect do not get unexpectedly removed from their manager. +/// a required Aspect do not get unexpectedly removed from their composite. /// /// Required Aspects are also automatically specialized for. template diff --git a/dart/common/detail/Aspect.h b/dart/common/detail/Aspect.h index 30aa5ae996f7d..193d707d2a255 100644 --- a/dart/common/detail/Aspect.h +++ b/dart/common/detail/Aspect.h @@ -76,9 +76,9 @@ void CompositeTrackingAspect::setComposite(Composite* newComposit if(nullptr == mComposite) { dterr << "[" << typeid(*this).name() << "::setComposite] Attempting to use a " - << "[" << typeid(newComposite).name() << "] type manager, but this " + << "[" << typeid(*newComposite).name() << "] type composite, but this " << "Aspect is only designed to be attached to a [" - << typeid(CompositeType).name() << "] type manager. This may cause " + << typeid(CompositeType).name() << "] type composite. This may cause " << "undefined behavior!\n"; assert(false); } diff --git a/unittests/testAspect.cpp b/unittests/testAspect.cpp index fb024bcc26cca..a60747b10fe72 100644 --- a/unittests/testAspect.cpp +++ b/unittests/testAspect.cpp @@ -246,15 +246,15 @@ class GenericAspect : public Aspect, public Subject { public: - GenericAspect(Composite* manager) - : Aspect(manager) + GenericAspect(Composite* composite) + : Aspect(composite) { // Do nothing } - std::unique_ptr cloneAspect(Composite* newManager) const override final + std::unique_ptr cloneAspect(Composite* newComposite) const override final { - return dart::common::make_unique(newManager); + return dart::common::make_unique(newComposite); } }; @@ -263,15 +263,15 @@ class SpecializedAspect : public Aspect, public Subject { public: - SpecializedAspect(Composite* manager) - : Aspect(manager) + SpecializedAspect(Composite* composite) + : Aspect(composite) { // Do nothing } - std::unique_ptr cloneAspect(Composite* newManager) const override final + std::unique_ptr cloneAspect(Composite* newComposite) const override final { - return dart::common::make_unique(newManager); + return dart::common::make_unique(newComposite); } }; @@ -320,9 +320,9 @@ class StatefulAspect : public Aspect, public Subject // Do nothing } - std::unique_ptr cloneAspect(Composite* newManager) const override final + std::unique_ptr cloneAspect(Composite* newComposite) const override final { - return dart::common::make_unique(newManager, *this); + return dart::common::make_unique(newComposite, *this); } void setAspectState(const Aspect::State& otherState) override @@ -362,7 +362,7 @@ typedef StatefulAspect FloatAspect; typedef StatefulAspect CharAspect; typedef StatefulAspect IntAspect; -class CustomSpecializedManager : public SpecializedForAspect { }; +class CustomSpecializedComposite : public SpecializedForAspect { }; TEST(Aspect, Generic) { @@ -384,7 +384,7 @@ TEST(Aspect, Generic) TEST(Aspect, Specialized) { usedSpecializedAspectAccess = false; - CustomSpecializedManager mgr; + CustomSpecializedComposite mgr; EXPECT_TRUE( mgr.get() == nullptr ); EXPECT_TRUE( usedSpecializedAspectAccess ); usedSpecializedAspectAccess = false; @@ -435,7 +435,7 @@ TEST(Aspect, Specialized) TEST(Aspect, Releasing) { Composite sender; - CustomSpecializedManager receiver; + CustomSpecializedComposite receiver; // ---- Test generic releases ---- { From e621f51b4424311bc494949a50fae1576ef1d461 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 14 Apr 2016 20:30:33 -0400 Subject: [PATCH 45/62] Change extensible --> cloneable --- dart/common/Cloneable.h | 8 ++++---- dart/common/detail/Cloneable.h | 6 +++--- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/dart/common/Cloneable.h b/dart/common/Cloneable.h index 882ce80d4dbe6..b330db55fb8a3 100644 --- a/dart/common/Cloneable.h +++ b/dart/common/Cloneable.h @@ -34,8 +34,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_COMMON_EXTENSIBLE_H_ -#define DART_COMMON_EXTENSIBLE_H_ +#ifndef DART_COMMON_CLONEABLE_H_ +#define DART_COMMON_CLONEABLE_H_ #include #include @@ -292,7 +292,7 @@ class CloneableVector /// Create a copy of this CloneableVector's contents std::unique_ptr< CloneableVector > clone() const; - /// Copy the contents of another extensible vector into this one. + /// Copy the contents of another cloneable vector into this one. void copy(const CloneableVector& anotherVector); /// Get a reference to the std::vector that this class is wrapping @@ -312,4 +312,4 @@ class CloneableVector #include "dart/common/detail/Cloneable.h" -#endif // DART_COMMON_EXTENSIBLE_H_ +#endif // DART_COMMON_CLONEABLE_H_ diff --git a/dart/common/detail/Cloneable.h b/dart/common/detail/Cloneable.h index 8a9ce1110a345..9236ce7c27e68 100644 --- a/dart/common/detail/Cloneable.h +++ b/dart/common/detail/Cloneable.h @@ -34,8 +34,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_COMMON_DETAIL_EXTENSIBLE_H_ -#define DART_COMMON_DETAIL_EXTENSIBLE_H_ +#ifndef DART_COMMON_DETAIL_CLONEABLE_H_ +#define DART_COMMON_DETAIL_CLONEABLE_H_ #include "dart/common/Cloneable.h" #include "dart/common/StlHelpers.h" @@ -571,4 +571,4 @@ const std::vector& CloneableVector::getVector() const } // namespace common } // namespace dart -#endif // DART_COMMON_DETAIL_EXTENSIBLE_H_ +#endif // DART_COMMON_DETAIL_CLONEABLE_H_ From fe60a44935a6435e366a48b12dfcaacb5c672bd5 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 14 Apr 2016 21:43:08 -0400 Subject: [PATCH 46/62] Fix compilation error due to the last commit --- dart/common/detail/Aspect.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dart/common/detail/Aspect.h b/dart/common/detail/Aspect.h index 193d707d2a255..747253fa8c649 100644 --- a/dart/common/detail/Aspect.h +++ b/dart/common/detail/Aspect.h @@ -76,7 +76,7 @@ void CompositeTrackingAspect::setComposite(Composite* newComposit if(nullptr == mComposite) { dterr << "[" << typeid(*this).name() << "::setComposite] Attempting to use a " - << "[" << typeid(*newComposite).name() << "] type composite, but this " + << "[" << typeid(newComposite).name() << "] type composite, but this " << "Aspect is only designed to be attached to a [" << typeid(CompositeType).name() << "] type composite. This may cause " << "undefined behavior!\n"; From a4af75c411d3d5ac48f7b7266bbe44a3e0b8c6bc Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Thu, 14 Apr 2016 23:53:18 -0400 Subject: [PATCH 47/62] renamed eraseAspect --> removeAspect and moved type to SpecializedForAspect --- dart/common/Composite.h | 4 +--- dart/common/CompositeJoiner.h | 2 +- dart/common/SpecializedForAspect.h | 8 +++++--- dart/common/detail/Composite.h | 6 +++--- dart/common/detail/CompositeJoiner.h | 2 +- dart/common/detail/SpecializedForAspect.h | 10 +++++----- 6 files changed, 16 insertions(+), 16 deletions(-) diff --git a/dart/common/Composite.h b/dart/common/Composite.h index dc05b4f47c755..d2f0e2c1f7a4c 100644 --- a/dart/common/Composite.h +++ b/dart/common/Composite.h @@ -117,7 +117,7 @@ class Composite /// Remove an Aspect from this Composite. template - void eraseAspect(); + void removeAspect(); /// Remove an Aspect from this Composite, but return its unique_ptr instead /// of letting it be deleted. This allows you to safely use move semantics to @@ -168,8 +168,6 @@ class Composite protected: - template struct type { }; - /// Add this Aspect to the Composite. This allows derived Composite types to /// call the protected Aspect::setComposite function. void addToComposite(Aspect* aspect); diff --git a/dart/common/CompositeJoiner.h b/dart/common/CompositeJoiner.h index 53ea40a84a349..03cc407ceec23 100644 --- a/dart/common/CompositeJoiner.h +++ b/dart/common/CompositeJoiner.h @@ -120,7 +120,7 @@ class CompositeJoiner : public Base1, public Base2 // Documentation inherited template - void eraseAspect(); + void removeAspect(); // Documentation inherited template diff --git a/dart/common/SpecializedForAspect.h b/dart/common/SpecializedForAspect.h index 828ba15410ca6..c4dddf451a207 100644 --- a/dart/common/SpecializedForAspect.h +++ b/dart/common/SpecializedForAspect.h @@ -95,7 +95,7 @@ class SpecializedForAspect : public virtual Composite /// Remove an Aspect from this Composite template - void eraseAspect(); + void removeAspect(); /// Remove an Aspect from this Composite, but return its unique_ptr instead /// of letting it be deleted. This allows you to safely use move semantics to @@ -109,6 +109,8 @@ class SpecializedForAspect : public virtual Composite protected: + template struct type { }; + /// Redirect to Composite::has() template bool _has(type) const; @@ -160,10 +162,10 @@ class SpecializedForAspect : public virtual Composite /// Redirect to Composite::erase() template - void _eraseAspect(type); + void _removeAspect(type); /// Specialized implementation of erase() - void _eraseAspect(type); + void _removeAspect(type); /// Redirect to Composite::release() template diff --git a/dart/common/detail/Composite.h b/dart/common/detail/Composite.h index 68fe1d9262da7..74ce29cf9331d 100644 --- a/dart/common/detail/Composite.h +++ b/dart/common/detail/Composite.h @@ -103,10 +103,10 @@ T* Composite::createAspect(Args&&... args) //============================================================================== template -void Composite::eraseAspect() +void Composite::removeAspect() { AspectMap::iterator it = mAspectMap.find( typeid(T) ); - DART_COMMON_CHECK_ILLEGAL_ASPECT_ERASE(eraseAspect, T, DART_BLANK) + DART_COMMON_CHECK_ILLEGAL_ASPECT_ERASE(removeAspect, T, DART_BLANK) if(mAspectMap.end() != it) { removeFromComposite(it->second.get()); @@ -209,7 +209,7 @@ void createAspects(T* mgr) \ inline void erase ## AspectName ()\ {\ - this->template eraseAspect();\ + this->template removeAspect();\ }\ \ inline std::unique_ptr< TypeName > release ## AspectName ()\ diff --git a/dart/common/detail/CompositeJoiner.h b/dart/common/detail/CompositeJoiner.h index 08f3b1499e7eb..161c256f0e543 100644 --- a/dart/common/detail/CompositeJoiner.h +++ b/dart/common/detail/CompositeJoiner.h @@ -82,7 +82,7 @@ DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(T*, CompositeJoiner, get, (), ()) DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(const T*, CompositeJoiner, get, () const, ()) DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(void, CompositeJoiner, set, (const T* aspect), (aspect)) DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(void, CompositeJoiner, set, (std::unique_ptr&& aspect), (std::move(aspect))) -DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(void, CompositeJoiner, eraseAspect, (), ()) +DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(void, CompositeJoiner, removeAspect, (), ()) DETAIL_DART_COMMON_TEMPLATEJOINERDISPATCH_IMPL(std::unique_ptr, CompositeJoiner, releaseAspect, (), ()) //============================================================================== diff --git a/dart/common/detail/SpecializedForAspect.h b/dart/common/detail/SpecializedForAspect.h index 8443d45b458b8..675e7aaa1b108 100644 --- a/dart/common/detail/SpecializedForAspect.h +++ b/dart/common/detail/SpecializedForAspect.h @@ -108,9 +108,9 @@ T* SpecializedForAspect::createAspect(Args&&... args) //============================================================================== template template -void SpecializedForAspect::eraseAspect() +void SpecializedForAspect::removeAspect() { - _eraseAspect(type()); + _removeAspect(type()); } //============================================================================== @@ -267,14 +267,14 @@ SpecAspect* SpecializedForAspect::_createAspect( //============================================================================== template template -void SpecializedForAspect::_eraseAspect(type) +void SpecializedForAspect::_removeAspect(type) { - Composite::eraseAspect(); + Composite::removeAspect(); } //============================================================================== template -void SpecializedForAspect::_eraseAspect(type) +void SpecializedForAspect::_removeAspect(type) { #ifdef DART_UNITTEST_SPECIALIZED_ASPECT_ACCESS usedSpecializedAspectAccess = true; From ec1b27aa5e6f33cc6445f778b0829bb1ac2c0b33 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Fri, 15 Apr 2016 18:20:52 -0400 Subject: [PATCH 48/62] making mIsColliding deprecated again --- dart/dynamics/BodyNode.cpp | 13 +++++-------- dart/dynamics/BodyNode.h | 4 ++++ dart/dynamics/detail/BodyNodeAspect.h | 9 ++------- 3 files changed, 11 insertions(+), 15 deletions(-) diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index a78c67c62aea8..c91cd39fda42e 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -212,11 +212,8 @@ AllNodeProperties getAllNodeProperties(const BodyNode* bodyNode) } //============================================================================== -BodyNodeState::BodyNodeState( - bool isColliding, - const Eigen::Vector6d& Fext) - : mIsColliding(isColliding), - mFext(Fext) +BodyNodeState::BodyNodeState(const Eigen::Vector6d& Fext) + : mFext(Fext) { // Do nothing } @@ -317,7 +314,6 @@ void BodyNode::setProperties(const AspectProperties& _properties) //============================================================================== void BodyNode::setAspectState(const AspectState& state) { - setColliding(state.mIsColliding); if(mAspectState.mFext != state.mFext) { mAspectState.mFext = state.mFext; @@ -1183,13 +1179,13 @@ const Eigen::Vector6d& BodyNode::getBodyVelocityChange() const //============================================================================== void BodyNode::setColliding(bool _isColliding) { - mAspectState.mIsColliding = _isColliding; + mIsColliding = _isColliding; } //============================================================================== bool BodyNode::isColliding() { - return mAspectState.mIsColliding; + return mIsColliding; } //============================================================================== @@ -1270,6 +1266,7 @@ BodyNode::BodyNode(BodyNode* _parentBodyNode, Joint* _parentJoint, Frame(Frame::World()), TemplatedJacobianNode(this), mID(BodyNode::msBodyNodeCount++), + mIsColliding(false), mParentJoint(_parentJoint), mParentBodyNode(nullptr), mPartialAcceleration(Eigen::Vector6d::Zero()), diff --git a/dart/dynamics/BodyNode.h b/dart/dynamics/BodyNode.h index bb226cb79f583..4c043b3be1fec 100644 --- a/dart/dynamics/BodyNode.h +++ b/dart/dynamics/BodyNode.h @@ -1041,6 +1041,10 @@ class BodyNode : /// Counts the number of nodes globally. static size_t msBodyNodeCount; + /// Whether the node is currently in collision with another node. + DEPRECATED(6.0) + bool mIsColliding; + //-------------------------------------------------------------------------- // Structural Properties //-------------------------------------------------------------------------- diff --git a/dart/dynamics/detail/BodyNodeAspect.h b/dart/dynamics/detail/BodyNodeAspect.h index df1cc5817d050..462efc9cd6138 100644 --- a/dart/dynamics/detail/BodyNodeAspect.h +++ b/dart/dynamics/detail/BodyNodeAspect.h @@ -56,15 +56,10 @@ namespace detail { //============================================================================== struct BodyNodeState -{ - /// Whether the node is currently in collision with another node. - bool mIsColliding; - - /// External spatial force +{/// External spatial force Eigen::Vector6d mFext; - BodyNodeState(bool isColliding = false, - const Eigen::Vector6d& Fext = Eigen::Vector6d::Zero()); + BodyNodeState(const Eigen::Vector6d& Fext = Eigen::Vector6d::Zero()); virtual ~BodyNodeState() = default; From 33db6c2e2732bc1dc21399bfd47cc932074808e1 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Fri, 15 Apr 2016 19:40:57 -0400 Subject: [PATCH 49/62] removed outdated header inclusions --- dart/dynamics/SoftBodyNode.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/dart/dynamics/SoftBodyNode.cpp b/dart/dynamics/SoftBodyNode.cpp index 33c690179e355..ba0b7f33835a4 100644 --- a/dart/dynamics/SoftBodyNode.cpp +++ b/dart/dynamics/SoftBodyNode.cpp @@ -45,8 +45,6 @@ #include "dart/dynamics/Joint.h" #include "dart/dynamics/Shape.h" #include "dart/dynamics/Skeleton.h" -#include "dart/renderer/LoadOpengl.h" -#include "dart/renderer/RenderInterface.h" #include "dart/dynamics/PointMass.h" #include "dart/dynamics/SoftMeshShape.h" From eb189a88724eb564cc3ddfb282cd5072850c358e Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Mon, 18 Apr 2016 11:55:43 -0400 Subject: [PATCH 50/62] Replace static_if_else with std::conditional --- dart/common/StlHelpers.h | 15 --------------- dart/common/detail/CompositeData.h | 6 +++--- dart/common/detail/EmbeddedAspect.h | 4 ++-- 3 files changed, 5 insertions(+), 20 deletions(-) diff --git a/dart/common/StlHelpers.h b/dart/common/StlHelpers.h index a34a133632fa5..dc7b4901bd6ee 100644 --- a/dart/common/StlHelpers.h +++ b/dart/common/StlHelpers.h @@ -71,21 +71,6 @@ static T getVectorObjectIfAvailable(size_t index, const std::vector& vec) return nullptr; } -//============================================================================== -/// static_if_else allows the compiler to choose between two different possible -/// types at runtime based on a runtime boolean. -template -struct static_if_else -{ - using type = T_else; -}; - -template -struct static_if_else -{ - using type = T_if; -}; - } // namespace common } // namespace dart diff --git a/dart/common/detail/CompositeData.h b/dart/common/detail/CompositeData.h index 27bf953e2bcaf..18db480f06400 100644 --- a/dart/common/detail/CompositeData.h +++ b/dart/common/detail/CompositeData.h @@ -54,7 +54,7 @@ namespace detail { template struct GetAspect { - using Type = typename static_if_else< + using Type = typename std::conditional< std::is_base_of::value, AspectT, typename AspectT::Aspect>::type; }; @@ -194,7 +194,7 @@ struct ComposeData : template struct ConvertIfData { - using Type = typename static_if_else< + using Type = typename std::conditional< std::is_base_of::value, typename Base::Data, Arg>::type; }; @@ -202,7 +202,7 @@ struct ComposeData : template struct ConvertIfComposite { - using Type = typename static_if_else< + using Type = typename std::conditional< std::is_base_of::value, CompositeType, Arg>::type; }; diff --git a/dart/common/detail/EmbeddedAspect.h b/dart/common/detail/EmbeddedAspect.h index 04331f4e5508a..678ef83724c43 100644 --- a/dart/common/detail/EmbeddedAspect.h +++ b/dart/common/detail/EmbeddedAspect.h @@ -101,7 +101,7 @@ class EmbeddedStateAspect : public BaseT template struct ConvertIfState { - using type = typename static_if_else< + using type = typename std::conditional< std::is_base_of::value, StateData, T>::type; }; @@ -272,7 +272,7 @@ class EmbeddedPropertiesAspect : public BaseT template struct ConvertIfProperties { - using type = typename static_if_else< + using type = typename std::conditional< std::is_base_of::value, PropertiesData, T>::type; }; From 401bd26c80b51759ffdfb18f519fbd05d99eb01d Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Mon, 18 Apr 2016 12:12:33 -0400 Subject: [PATCH 51/62] Changed enumeration tag convention from *_t to *Tag --- dart/common/CompositeJoiner.h | 4 ++-- dart/common/Empty.h | 2 +- dart/common/detail/CompositeData.h | 6 +++--- dart/common/detail/CompositeJoiner.h | 4 ++-- dart/common/detail/EmbeddedAspect.h | 12 ++++++------ dart/dynamics/Chain.cpp | 4 ++-- dart/dynamics/Chain.h | 6 +++--- dart/dynamics/Entity.cpp | 4 ++-- dart/dynamics/Entity.h | 8 ++++---- dart/dynamics/FixedFrame.cpp | 2 +- dart/dynamics/FixedFrame.h | 2 +- dart/dynamics/Frame.cpp | 4 ++-- dart/dynamics/Frame.h | 8 ++++---- dart/dynamics/NodeManagerJoiner.h | 4 ++-- dart/dynamics/detail/NodeManagerJoiner.h | 4 ++-- dart/math/Geometry.cpp | 4 ++-- dart/math/Geometry.h | 13 +++++++------ 17 files changed, 46 insertions(+), 45 deletions(-) diff --git a/dart/common/CompositeJoiner.h b/dart/common/CompositeJoiner.h index 03cc407ceec23..45d6f828f30a2 100644 --- a/dart/common/CompositeJoiner.h +++ b/dart/common/CompositeJoiner.h @@ -87,12 +87,12 @@ class CompositeJoiner : public Base1, public Base2 /// This constructor passes one argument to the Base1 constructor and no /// arguments to the Base2 constructor. template - CompositeJoiner(Base1Arg&& arg1, NoArg_t); + CompositeJoiner(Base1Arg&& arg1, NoArgTag); /// This constructor passes no arguments to the Base1 constructor and /// arbitrarily many arguments to the Base2 constructor. template - CompositeJoiner(NoArg_t, Base2Args&&... args2); + CompositeJoiner(NoArgTag, Base2Args&&... args2); // Documentation inherited template diff --git a/dart/common/Empty.h b/dart/common/Empty.h index b7192b845ebdb..8c9eedf20573a 100644 --- a/dart/common/Empty.h +++ b/dart/common/Empty.h @@ -46,7 +46,7 @@ struct Empty { }; /// Used to tag arguments as blank for in variadic joiner classes such as /// common::CompositeJoiner and dynamics::NodeManagerJoiner -enum NoArg_t { NoArg }; +enum NoArgTag { NoArg }; } // namespace common } // namespace dart diff --git a/dart/common/detail/CompositeData.h b/dart/common/detail/CompositeData.h index 18db480f06400..ced433a600720 100644 --- a/dart/common/detail/CompositeData.h +++ b/dart/common/detail/CompositeData.h @@ -185,7 +185,7 @@ struct ComposeData : { public: - enum Delegate_t { Delegate }; + enum DelegateTag { Delegate }; using Base = typename GetData::Type; using Data = typename Base::Data; @@ -279,14 +279,14 @@ struct ComposeData : protected: template - ComposeData(Delegate_t, const Args&... args) + ComposeData(DelegateTag, const Args&... args) : ComposeData(args...) { // Pass all the arguments along to the next base class } template - ComposeData(Delegate_t, const Data& arg1, const Args&... args) + ComposeData(DelegateTag, const Data& arg1, const Args&... args) : Base(arg1), ComposeData(args...) { diff --git a/dart/common/detail/CompositeJoiner.h b/dart/common/detail/CompositeJoiner.h index 161c256f0e543..dd9face3f8872 100644 --- a/dart/common/detail/CompositeJoiner.h +++ b/dart/common/detail/CompositeJoiner.h @@ -58,7 +58,7 @@ CompositeJoiner::CompositeJoiner( template template CompositeJoiner::CompositeJoiner( - Base1Arg&& arg1, NoArg_t) + Base1Arg&& arg1, NoArgTag) : Base1(std::forward(arg1)), Base2() { @@ -69,7 +69,7 @@ CompositeJoiner::CompositeJoiner( template template CompositeJoiner::CompositeJoiner( - NoArg_t, Base2Args&&... args2) + NoArgTag, Base2Args&&... args2) : Base1(), Base2(std::forward(args2)...) { diff --git a/dart/common/detail/EmbeddedAspect.h b/dart/common/detail/EmbeddedAspect.h index 678ef83724c43..89ba62bbd5cfd 100644 --- a/dart/common/detail/EmbeddedAspect.h +++ b/dart/common/detail/EmbeddedAspect.h @@ -90,7 +90,7 @@ class EmbeddedStateAspect : public BaseT constexpr static void (*SetEmbeddedState)(Derived*, const State&) = setEmbeddedState; constexpr static const State& (*GetEmbeddedState)(const Derived*) = getEmbeddedState; - enum Delegate_t { Delegate }; + enum DelegateTag { Delegate }; EmbeddedStateAspect() = delete; EmbeddedStateAspect(const EmbeddedStateAspect&) = delete; @@ -198,7 +198,7 @@ class EmbeddedStateAspect : public BaseT /// arguments into the constructor of the Base class. template EmbeddedStateAspect( - Delegate_t, Composite* comp, const StateData& state, + DelegateTag, Composite* comp, const StateData& state, RemainingArgs&&... remainingArgs) : Base(comp, std::forward(remainingArgs)...), mTemporaryState(make_unique(state)) @@ -210,7 +210,7 @@ class EmbeddedStateAspect : public BaseT /// arguments into the constructor of the Base class. template EmbeddedStateAspect( - Delegate_t, Composite* comp, BaseArgs&&... args) + DelegateTag, Composite* comp, BaseArgs&&... args) : Base(comp, std::forward(args)...) { // Do nothing @@ -252,7 +252,7 @@ class EmbeddedPropertiesAspect : public BaseT { protected: - enum Delegate_t { Delegate }; + enum DelegateTag { Delegate }; public: @@ -368,7 +368,7 @@ class EmbeddedPropertiesAspect : public BaseT /// arguments into the constructor of the Base class. template EmbeddedPropertiesAspect( - Delegate_t, Composite* comp, const PropertiesData& properties, + DelegateTag, Composite* comp, const PropertiesData& properties, RemainingArgs&&... remainingArgs) : Base(comp, std::forward(remainingArgs)...), mTemporaryProperties(make_unique(properties)) @@ -380,7 +380,7 @@ class EmbeddedPropertiesAspect : public BaseT /// arguments into the constructor of the Base class. template EmbeddedPropertiesAspect( - Delegate_t, Composite* comp, BaseArgs&&... args) + DelegateTag, Composite* comp, BaseArgs&&... args) : Base(comp, std::forward(args)...) { // Do nothing diff --git a/dart/dynamics/Chain.cpp b/dart/dynamics/Chain.cpp index 97093f9e4fee9..928e4170c2dbb 100644 --- a/dart/dynamics/Chain.cpp +++ b/dart/dynamics/Chain.cpp @@ -114,7 +114,7 @@ ChainPtr Chain::create(BodyNode* _start, BodyNode* _target, //============================================================================== ChainPtr Chain::create(BodyNode* _start, BodyNode* _target, - IncludeBoth_t, const std::string& _name) + IncludeBothTag, const std::string& _name) { ChainPtr chain(new Chain(_start, _target, IncludeBoth, _name)); chain->mPtr = chain; @@ -165,7 +165,7 @@ Chain::Chain(BodyNode* _start, BodyNode* _target, const std::string& _name) //============================================================================== Chain::Chain(BodyNode* _start, BodyNode* _target, - IncludeBoth_t, const std::string& _name) + IncludeBothTag, const std::string& _name) : Linkage(Chain::Criteria(_start, _target, true), _name) { // Do nothing diff --git a/dart/dynamics/Chain.h b/dart/dynamics/Chain.h index 5828e755d1358..7a13cf951fa7a 100644 --- a/dart/dynamics/Chain.h +++ b/dart/dynamics/Chain.h @@ -81,7 +81,7 @@ class Chain : public Linkage /// This enum is used to specify to the create() function that both the start /// and the target BodyNodes should be included in the Chain that gets /// generated. - enum IncludeBoth_t { IncludeBoth }; + enum IncludeBothTag { IncludeBoth }; /// Create a Chain given some Chain::Criteria static ChainPtr create(const Chain::Criteria& _criteria, @@ -95,7 +95,7 @@ class Chain : public Linkage /// Create a Chain given a start and a target BodyNode. In this version, both /// BodyNodes will be included in the Chain that gets created. static ChainPtr create(BodyNode* _start, BodyNode* _target, - IncludeBoth_t, const std::string& _name = "Chain"); + IncludeBothTag, const std::string& _name = "Chain"); /// Returns false if this Chain has been broken, or some new Branching has /// been added. @@ -112,7 +112,7 @@ class Chain : public Linkage /// Alternative constructor for the Chain class Chain(BodyNode* _start, BodyNode* _target, - IncludeBoth_t, const std::string& _name = "Chain"); + IncludeBothTag, const std::string& _name = "Chain"); }; diff --git a/dart/dynamics/Entity.cpp b/dart/dynamics/Entity.cpp index 29a18e977f2dd..aba230ca6957a 100644 --- a/dart/dynamics/Entity.cpp +++ b/dart/dynamics/Entity.cpp @@ -181,7 +181,7 @@ bool Entity::needsAccelerationUpdate() const } //============================================================================== -Entity::Entity(ConstructFrame_t) +Entity::Entity(ConstructFrameTag) : mParentFrame(nullptr), mNeedTransformUpdate(true), mNeedVelocityUpdate(true), @@ -203,7 +203,7 @@ Entity::Entity(ConstructFrame_t) } //============================================================================== -Entity::Entity(ConstructAbstract_t) +Entity::Entity(ConstructAbstractTag) : onFrameChanged(mFrameChangedSignal), onNameChanged(mNameChangedSignal), onTransformUpdated(mTransformUpdatedSignal), diff --git a/dart/dynamics/Entity.h b/dart/dynamics/Entity.h index 2da6ffaa3abe6..9179a7658cf17 100644 --- a/dart/dynamics/Entity.h +++ b/dart/dynamics/Entity.h @@ -140,15 +140,15 @@ class Entity : public virtual common::Subject /// Used when constructing a Frame class, because the Frame constructor will /// take care of setting up the parameters you pass into it - enum ConstructFrame_t { ConstructFrame }; + enum ConstructFrameTag { ConstructFrame }; - explicit Entity(ConstructFrame_t); + explicit Entity(ConstructFrameTag); /// Used when constructing a pure abstract class, because calling the Entity /// constructor is just a formality - enum ConstructAbstract_t { ConstructAbstract }; + enum ConstructAbstractTag { ConstructAbstract }; - explicit Entity(ConstructAbstract_t); + explicit Entity(ConstructAbstractTag); /// Used by derived classes to change their parent frames virtual void changeParentFrame(Frame* _newParentFrame); diff --git a/dart/dynamics/FixedFrame.cpp b/dart/dynamics/FixedFrame.cpp index 087231477700e..9d558950a8f78 100644 --- a/dart/dynamics/FixedFrame.cpp +++ b/dart/dynamics/FixedFrame.cpp @@ -123,7 +123,7 @@ FixedFrame::FixedFrame() } //============================================================================== -FixedFrame::FixedFrame(ConstructAbstract_t) +FixedFrame::FixedFrame(ConstructAbstractTag) { dterr << "[FixedFrame::FixedFrame] Attempting to construct a pure abstract " << "FixedFrame object. This is not allowed!\n"; diff --git a/dart/dynamics/FixedFrame.h b/dart/dynamics/FixedFrame.h index b6eec983ddc9f..65ad9c33eecf0 100644 --- a/dart/dynamics/FixedFrame.h +++ b/dart/dynamics/FixedFrame.h @@ -103,7 +103,7 @@ class FixedFrame : FixedFrame(); /// Abstract constructor - explicit FixedFrame(ConstructAbstract_t); + explicit FixedFrame(ConstructAbstractTag); /// Used for Relative Velocity and Relative Acceleration of this Frame static const Eigen::Vector6d mZero; diff --git a/dart/dynamics/Frame.cpp b/dart/dynamics/Frame.cpp index 815f2965898d0..5978b2e4d527d 100644 --- a/dart/dynamics/Frame.cpp +++ b/dart/dynamics/Frame.cpp @@ -527,7 +527,7 @@ Frame::Frame() } //============================================================================== -Frame::Frame(ConstructAbstract_t) +Frame::Frame(ConstructAbstractTag) : Entity(Entity::ConstructAbstract), mAmWorld(false), mAmShapeFrame(false) @@ -592,7 +592,7 @@ void Frame::processRemovedEntity(Entity*) } //============================================================================== -Frame::Frame(ConstructWorld_t) +Frame::Frame(ConstructWorldTag) : Entity(this, true), mWorldTransform(Eigen::Isometry3d::Identity()), mVelocity(Eigen::Vector6d::Zero()), diff --git a/dart/dynamics/Frame.h b/dart/dynamics/Frame.h index dbaf0c711093e..f3f2738a2bb17 100644 --- a/dart/dynamics/Frame.h +++ b/dart/dynamics/Frame.h @@ -256,7 +256,7 @@ class Frame : public virtual Entity /// Used when constructing a pure abstract class, because calling the Frame /// constructor is just a formality - enum ConstructAbstract_t { ConstructAbstract }; + enum ConstructAbstractTag { ConstructAbstract }; /// Constructor for typical usage explicit Frame(Frame* _refFrame); @@ -265,7 +265,7 @@ class Frame : public virtual Entity Frame(); /// Constructor for use by pure abstract classes - explicit Frame(ConstructAbstract_t); + explicit Frame(ConstructAbstractTag); // Documentation inherited virtual void changeParentFrame(Frame* _newParentFrame) override; @@ -282,10 +282,10 @@ class Frame : public virtual Entity private: /// Used when constructing the World - enum ConstructWorld_t { ConstructWorld }; + enum ConstructWorldTag { ConstructWorld }; /// Constructor only to be used by the WorldFrame class - explicit Frame(ConstructWorld_t); + explicit Frame(ConstructWorldTag); protected: /// World transform of this Frame. This object is mutable to enable diff --git a/dart/dynamics/NodeManagerJoiner.h b/dart/dynamics/NodeManagerJoiner.h index 46b532e3b9fc2..6a3c46d7f03ce 100644 --- a/dart/dynamics/NodeManagerJoiner.h +++ b/dart/dynamics/NodeManagerJoiner.h @@ -75,12 +75,12 @@ class NodeManagerJoinerForBodyNode : public Base1, public Base2 /// This constructor passes one argument to the Base1 constructor and no /// arguments to the Base2 constructor. template - NodeManagerJoinerForBodyNode(Base1Arg&& arg1, common::NoArg_t); + NodeManagerJoinerForBodyNode(Base1Arg&& arg1, common::NoArgTag); /// This constructor passes no arguments to the Base1 constructor and /// arbitrarily many arguments to the Base2 constructor. template - NodeManagerJoinerForBodyNode(common::NoArg_t, Base2Args&&... args2); + NodeManagerJoinerForBodyNode(common::NoArgTag, Base2Args&&... args2); template size_t getNumNodes() const; diff --git a/dart/dynamics/detail/NodeManagerJoiner.h b/dart/dynamics/detail/NodeManagerJoiner.h index 7b60f0ad6dd89..c25c591ee5a85 100644 --- a/dart/dynamics/detail/NodeManagerJoiner.h +++ b/dart/dynamics/detail/NodeManagerJoiner.h @@ -58,7 +58,7 @@ NodeManagerJoinerForBodyNode::NodeManagerJoinerForBodyNode( template template NodeManagerJoinerForBodyNode::NodeManagerJoinerForBodyNode( - Base1Arg&& arg1, common::NoArg_t) + Base1Arg&& arg1, common::NoArgTag) : Base1(std::forward(arg1)), Base2() { @@ -69,7 +69,7 @@ NodeManagerJoinerForBodyNode::NodeManagerJoinerForBodyNode( template template NodeManagerJoinerForBodyNode::NodeManagerJoinerForBodyNode( - common::NoArg_t, Base2Args&&... args2) + common::NoArgTag, Base2Args&&... args2) : Base1(), Base2(std::forward(args2)...) { diff --git a/dart/math/Geometry.cpp b/dart/math/Geometry.cpp index 7453994c577c0..98d3066cd3a5d 100644 --- a/dart/math/Geometry.cpp +++ b/dart/math/Geometry.cpp @@ -1591,7 +1591,7 @@ Eigen::Vector2d computeCentroidOfHull(const SupportPolygon& _convexHull) midp12 = 0.5 * (p1+p2); midp01 = 0.5 * (p0+p1); - Intersection_t result = + IntersectionResult result = computeIntersection(intersect, p0, midp12, p2, midp01); if(BEYOND_ENDPOINTS == result) @@ -1763,7 +1763,7 @@ SupportPolygon computeConvexHull(std::vector& _originalIndices, } //============================================================================== -Intersection_t computeIntersection(Eigen::Vector2d& _intersectionPoint, +IntersectionResult computeIntersection(Eigen::Vector2d& _intersectionPoint, const Eigen::Vector2d& a1, const Eigen::Vector2d& a2, const Eigen::Vector2d& b1, diff --git a/dart/math/Geometry.h b/dart/math/Geometry.h index 42b89dcaf9271..4857568622324 100644 --- a/dart/math/Geometry.h +++ b/dart/math/Geometry.h @@ -518,7 +518,7 @@ Eigen::Vector2d computeCentroidOfHull(const SupportPolygon& _convexHull); /// Intersection_t is returned by the computeIntersection() function to indicate /// whether there was a valid intersection between the two line segments -enum Intersection_t { +enum IntersectionResult { INTERSECTING = 0, ///< An intersection was found PARALLEL, ///< The line segments are parallel @@ -528,11 +528,12 @@ enum Intersection_t { /// Compute the intersection between a line segment that goes from a1 -> a2 and /// a line segment that goes from b1 -> b2. -Intersection_t computeIntersection(Eigen::Vector2d& _intersectionPoint, - const Eigen::Vector2d& a1, - const Eigen::Vector2d& a2, - const Eigen::Vector2d& b1, - const Eigen::Vector2d& b2); +IntersectionResult computeIntersection( + Eigen::Vector2d& _intersectionPoint, + const Eigen::Vector2d& a1, + const Eigen::Vector2d& a2, + const Eigen::Vector2d& b1, + const Eigen::Vector2d& b2); /// Compute a 2D cross product double cross(const Eigen::Vector2d& _v1, const Eigen::Vector2d& _v2); From 93436e82b7ba2aca518e98276071d20cc90a1585 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 20 Apr 2016 20:00:35 -0400 Subject: [PATCH 52/62] Update testAspect.cpp * adding minor additional checks * renaming variable name: mgr --> comp --- unittests/testAspect.cpp | 236 ++++++++++++++++++++++----------------- 1 file changed, 135 insertions(+), 101 deletions(-) diff --git a/unittests/testAspect.cpp b/unittests/testAspect.cpp index a60747b10fe72..9a6ef70bf4e72 100644 --- a/unittests/testAspect.cpp +++ b/unittests/testAspect.cpp @@ -366,69 +366,77 @@ class CustomSpecializedComposite : public SpecializedForAspect() == nullptr ); + EXPECT_TRUE( comp.get() == nullptr ); + EXPECT_FALSE( comp.has() ); - sub_ptr aspect = mgr.createAspect(); + comp.set(nullptr); + EXPECT_TRUE( comp.get() == nullptr ); + EXPECT_FALSE( comp.has() ); + + sub_ptr aspect = comp.createAspect(); GenericAspect* rawAspect = aspect; EXPECT_FALSE( nullptr == aspect ); - EXPECT_TRUE( mgr.get() == aspect ); + EXPECT_FALSE( nullptr == rawAspect ); + EXPECT_TRUE( comp.get() == aspect ); + EXPECT_TRUE( comp.get() == rawAspect); - GenericAspect* newAspect = mgr.createAspect(); + GenericAspect* newAspect = comp.createAspect(); EXPECT_FALSE( nullptr == newAspect ); - EXPECT_TRUE( nullptr == aspect ); + EXPECT_FALSE( nullptr == rawAspect ); EXPECT_FALSE( rawAspect == newAspect ); + EXPECT_TRUE( nullptr == aspect ); } TEST(Aspect, Specialized) { usedSpecializedAspectAccess = false; - CustomSpecializedComposite mgr; + CustomSpecializedComposite comp; - EXPECT_TRUE( mgr.get() == nullptr ); + EXPECT_TRUE( comp.get() == nullptr ); EXPECT_TRUE( usedSpecializedAspectAccess ); usedSpecializedAspectAccess = false; -// EXPECT_TRUE( mgr.getSpecializedAspect() == nullptr ); - EXPECT_TRUE( mgr.get() == nullptr ); +// EXPECT_TRUE( comp.getSpecializedAspect() == nullptr ); + EXPECT_TRUE( comp.get() == nullptr ); EXPECT_FALSE( usedSpecializedAspectAccess ); usedSpecializedAspectAccess = false; - sub_ptr spec = mgr.createAspect(); + sub_ptr spec = comp.createAspect(); EXPECT_TRUE( usedSpecializedAspectAccess ); usedSpecializedAspectAccess = false; SpecializedAspect* rawSpec = spec; - sub_ptr generic = mgr.createAspect(); + sub_ptr generic = comp.createAspect(); EXPECT_FALSE( usedSpecializedAspectAccess ); usedSpecializedAspectAccess = false; GenericAspect* rawGeneric = generic; - EXPECT_TRUE( mgr.get() == spec ); + EXPECT_TRUE( comp.get() == spec ); EXPECT_TRUE( usedSpecializedAspectAccess ); usedSpecializedAspectAccess = false; -// EXPECT_TRUE( mgr.getSpecializedAspect() == spec ); +// EXPECT_TRUE( comp.getSpecializedAspect() == spec ); - EXPECT_TRUE( mgr.get() == generic ); + EXPECT_TRUE( comp.get() == generic ); EXPECT_FALSE( usedSpecializedAspectAccess ); usedSpecializedAspectAccess = false; -// SpecializedAspect* newSpec = mgr.createSpecializedAspect(); - SpecializedAspect* newSpec = mgr.createAspect(); +// SpecializedAspect* newSpec = comp.createSpecializedAspect(); + SpecializedAspect* newSpec = comp.createAspect(); EXPECT_TRUE( usedSpecializedAspectAccess ); usedSpecializedAspectAccess = false; - GenericAspect* newGeneric = mgr.createAspect(); + GenericAspect* newGeneric = comp.createAspect(); EXPECT_FALSE( usedSpecializedAspectAccess ); usedSpecializedAspectAccess = false; EXPECT_TRUE( nullptr == spec ); EXPECT_TRUE( nullptr == generic ); - EXPECT_FALSE( mgr.get() == rawSpec ); + EXPECT_FALSE( comp.get() == rawSpec ); EXPECT_TRUE( usedSpecializedAspectAccess ); usedSpecializedAspectAccess = false; -// EXPECT_FALSE( mgr.getSpecializedAspect() == rawSpec ); +// EXPECT_FALSE( comp.getSpecializedAspect() == rawSpec ); - EXPECT_FALSE( mgr.get() == rawGeneric ); + EXPECT_FALSE( comp.get() == rawGeneric ); EXPECT_FALSE( usedSpecializedAspectAccess ); usedSpecializedAspectAccess = false; - EXPECT_TRUE( mgr.get() == newSpec ); + EXPECT_TRUE( comp.get() == newSpec ); EXPECT_TRUE( usedSpecializedAspectAccess ); usedSpecializedAspectAccess = false; -// EXPECT_TRUE( mgr.getSpecializedAspect() == newSpec ); +// EXPECT_TRUE( comp.getSpecializedAspect() == newSpec ); - EXPECT_TRUE( mgr.get() == newGeneric ); + EXPECT_TRUE( comp.get() == newGeneric ); EXPECT_FALSE( usedSpecializedAspectAccess ); usedSpecializedAspectAccess = false; } @@ -454,6 +462,20 @@ TEST(Aspect, Releasing) EXPECT_TRUE( sender.get() == nullptr ); EXPECT_TRUE( receiver.get() == aspect ); + sender.set(sender.releaseAspect()); + + EXPECT_FALSE( nullptr == aspect ); + + EXPECT_TRUE( sender.get() == nullptr ); + EXPECT_TRUE( receiver.get() == aspect ); + + receiver.set(receiver.releaseAspect()); + + EXPECT_FALSE( nullptr == aspect ); + + EXPECT_TRUE( sender.get() == nullptr ); + EXPECT_TRUE( receiver.get() == aspect ); + sender.set(receiver.releaseAspect()); EXPECT_FALSE( nullptr == aspect ); @@ -575,100 +597,100 @@ void makePropertiesDifferent(AspectT* aspect, const AspectT* differentFrom) TEST(Aspect, StateAndProperties) { - Composite mgr1; - mgr1.createAspect(); - mgr1.createAspect(); - mgr1.createAspect(); - mgr1.createAspect(); + Composite comp1; + comp1.createAspect(); + comp1.createAspect(); + comp1.createAspect(); + comp1.createAspect(); - Composite mgr2; - mgr2.createAspect(); - mgr2.createAspect(); + Composite comp2; + comp2.createAspect(); + comp2.createAspect(); - mgr1.createAspect(); + comp1.createAspect(); // ---- Test State Transfer ---- - mgr2.setCompositeState(mgr1.getCompositeState()); + comp2.setCompositeState(comp1.getCompositeState()); - EXPECT_EQ( mgr1.get()->mState.val, - mgr2.get()->mState.val ); + EXPECT_EQ( comp1.get()->mState.val, + comp2.get()->mState.val ); - EXPECT_EQ( mgr1.get()->mState.val, - mgr2.get()->mState.val ); + EXPECT_EQ( comp1.get()->mState.val, + comp2.get()->mState.val ); - makeStatesDifferent( mgr2.get(), mgr1.get() ); - makeStatesDifferent( mgr2.get(), mgr1.get() ); + makeStatesDifferent( comp2.get(), comp1.get() ); + makeStatesDifferent( comp2.get(), comp1.get() ); - EXPECT_NE( mgr1.get()->mState.val, - mgr2.get()->mState.val ); + EXPECT_NE( comp1.get()->mState.val, + comp2.get()->mState.val ); - EXPECT_NE( mgr1.get()->mState.val, - mgr2.get()->mState.val ); + EXPECT_NE( comp1.get()->mState.val, + comp2.get()->mState.val ); - mgr1.setCompositeState( mgr2.getCompositeState() ); + comp1.setCompositeState( comp2.getCompositeState() ); - EXPECT_EQ( mgr1.get()->mState.val, - mgr2.get()->mState.val ); + EXPECT_EQ( comp1.get()->mState.val, + comp2.get()->mState.val ); - EXPECT_EQ( mgr1.get()->mState.val, - mgr2.get()->mState.val ); + EXPECT_EQ( comp1.get()->mState.val, + comp2.get()->mState.val ); - EXPECT_TRUE( nullptr == mgr2.get() ); - EXPECT_TRUE( nullptr == mgr2.get() ); + EXPECT_TRUE( nullptr == comp2.get() ); + EXPECT_TRUE( nullptr == comp2.get() ); // ---- Test Property Transfer ---- - mgr2.setCompositeProperties(mgr1.getCompositeProperties()); + comp2.setCompositeProperties(comp1.getCompositeProperties()); - EXPECT_EQ( mgr1.get()->mProperties.val, - mgr2.get()->mProperties.val ); + EXPECT_EQ( comp1.get()->mProperties.val, + comp2.get()->mProperties.val ); - EXPECT_EQ( mgr1.get()->mProperties.val, - mgr2.get()->mProperties.val ); + EXPECT_EQ( comp1.get()->mProperties.val, + comp2.get()->mProperties.val ); - makePropertiesDifferent( mgr2.get(), mgr1.get() ); - makePropertiesDifferent( mgr2.get(), mgr1.get() ); + makePropertiesDifferent( comp2.get(), comp1.get() ); + makePropertiesDifferent( comp2.get(), comp1.get() ); - EXPECT_NE( mgr1.get()->mProperties.val, - mgr2.get()->mProperties.val ); + EXPECT_NE( comp1.get()->mProperties.val, + comp2.get()->mProperties.val ); - EXPECT_NE( mgr1.get()->mProperties.val, - mgr2.get()->mProperties.val ); + EXPECT_NE( comp1.get()->mProperties.val, + comp2.get()->mProperties.val ); - mgr1.setCompositeProperties( mgr2.getCompositeProperties() ); + comp1.setCompositeProperties( comp2.getCompositeProperties() ); - EXPECT_EQ( mgr1.get()->mProperties.val, - mgr2.get()->mProperties.val ); + EXPECT_EQ( comp1.get()->mProperties.val, + comp2.get()->mProperties.val ); - EXPECT_EQ( mgr1.get()->mProperties.val, - mgr2.get()->mProperties.val ); + EXPECT_EQ( comp1.get()->mProperties.val, + comp2.get()->mProperties.val ); - EXPECT_TRUE( nullptr == mgr2.get() ); - EXPECT_TRUE( nullptr == mgr2.get() ); + EXPECT_TRUE( nullptr == comp2.get() ); + EXPECT_TRUE( nullptr == comp2.get() ); // ---- Test Data Containers ---- Composite::MakeState state( - mgr1.getCompositeState()); + comp1.getCompositeState()); - EXPECT_EQ(mgr1.get()->mState.val, + EXPECT_EQ(comp1.get()->mState.val, state.DoubleAspect::State::val); - EXPECT_EQ(mgr1.get()->mState.val, + EXPECT_EQ(comp1.get()->mState.val, state.FloatAspect::State::val); - EXPECT_EQ(mgr1.get()->mState.val, + EXPECT_EQ(comp1.get()->mState.val, state.IntAspect::State::val); Composite::MakeProperties properties( - mgr2.getCompositeProperties()); + comp2.getCompositeProperties()); - EXPECT_EQ(mgr1.get()->mProperties.val, + EXPECT_EQ(comp1.get()->mProperties.val, properties.DoubleAspect::Properties::val); - EXPECT_EQ(mgr1.get()->mProperties.val, + EXPECT_EQ(comp1.get()->mProperties.val, properties.CharAspect::Properties::val); - EXPECT_EQ(mgr1.get()->mProperties.val, + EXPECT_EQ(comp1.get()->mProperties.val, properties.FloatAspect::Properties::val); @@ -684,17 +706,20 @@ TEST(Aspect, StateAndProperties) TEST(Aspect, Construction) { - Composite mgr; + Composite comp; - mgr.createAspect(); + comp.createAspect(); - double s = dart::math::random(0, 100); - mgr.createAspect(s); - EXPECT_EQ(mgr.get()->mState.val, s); + double s1 = dart::math::random(0, 100); + comp.createAspect(s1); + EXPECT_EQ(comp.get()->mState.val, s1); + double s2 = dart::math::random(0, 100); double p = dart::math::random(0, 100); - mgr.createAspect(dart::math::random(0, 100), p); - EXPECT_EQ(mgr.get()->mProperties.val, p); + comp.createAspect(s2, p); + EXPECT_NE(comp.get()->mState.val, s1); + EXPECT_EQ(comp.get()->mState.val, s2); + EXPECT_EQ(comp.get()->mProperties.val, p); } TEST(Aspect, Joints) @@ -762,6 +787,7 @@ TEST(Aspect, BodyNodes) dart::dynamics::DynamicsAspect>( std::make_shared(Eigen::Vector3d::Ones())); + EXPECT_EQ(bn->getNumShapeNodes(), 1u); EXPECT_EQ(bn->getNumShapeNodesWith(), 1u); EXPECT_EQ(bn->getNumShapeNodesWith(), 1u); EXPECT_EQ(bn->getNumShapeNodesWith(), 1u); @@ -769,40 +795,40 @@ TEST(Aspect, BodyNodes) TEST(Aspect, Duplication) { - Composite mgr1, mgr2; + Composite comp1, comp2; - mgr1.createAspect(); - mgr1.createAspect(); - mgr1.createAspect(); - mgr1.createAspect(); + comp1.createAspect(); + comp1.createAspect(); + comp1.createAspect(); + comp1.createAspect(); - mgr2.createAspect(); + comp2.createAspect(); - mgr2.duplicateAspects(&mgr1); + comp2.duplicateAspects(&comp1); - EXPECT_FALSE(mgr2.get() == nullptr); - EXPECT_FALSE(mgr2.get() == nullptr); - EXPECT_FALSE(mgr2.get() == nullptr); - EXPECT_FALSE(mgr2.get() == nullptr); + EXPECT_FALSE(comp2.get() == nullptr); + EXPECT_FALSE(comp2.get() == nullptr); + EXPECT_FALSE(comp2.get() == nullptr); + EXPECT_FALSE(comp2.get() == nullptr); Composite::MakeState state; state.DoubleAspect::State::val = 1e-6; state.FloatAspect::State::val = 1.5; state.IntAspect::State::val = 456; - mgr1.setCompositeState(state); + comp1.setCompositeState(state); - EXPECT_EQ(mgr1.get()->mState.val, 1e-6); - EXPECT_EQ(mgr1.get()->mState.val, 1.5); - EXPECT_EQ(mgr1.get()->mState.val, 456); + EXPECT_EQ(comp1.get()->mState.val, 1e-6); + EXPECT_EQ(comp1.get()->mState.val, 1.5); + EXPECT_EQ(comp1.get()->mState.val, 456); - state = mgr2.getCompositeState(); + state = comp2.getCompositeState(); EXPECT_EQ(state.DoubleAspect::State::val, 0); EXPECT_EQ(state.FloatAspect::State::val, 0); EXPECT_EQ(state.IntAspect::State::val, 0); - state = mgr1.getCompositeState(); + state = comp1.getCompositeState(); EXPECT_EQ(state.DoubleAspect::State::val, 1e-6); EXPECT_EQ(state.FloatAspect::State::val, 1.5); @@ -812,8 +838,16 @@ TEST(Aspect, Duplication) TEST(Aspect, Embedded) { EmbeddedStateComposite s; + EXPECT_TRUE(s.has()); + EXPECT_TRUE(s.get() != nullptr); + EmbeddedPropertiesComposite p; + EXPECT_TRUE(p.has()); + EXPECT_TRUE(p.get() != nullptr); + EmbeddedStateAndPropertiesComposite sp; + EXPECT_TRUE(sp.has()); + EXPECT_TRUE(sp.get() != nullptr); // --------- Test Embedded State ----------- EmbeddedStateComposite::AspectState state = s.getAspectState(); From 5189938acefcab207de00c37c6e7e10ebccdb6cc Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Thu, 21 Apr 2016 13:02:48 -0400 Subject: [PATCH 53/62] Made changes based on Reviewable discussion --- dart/common/Aspect.h | 3 +++ dart/common/Composite.h | 4 ++-- dart/common/EmbeddedAspect.h | 9 +++++++++ dart/common/StlHelpers.h | 1 - dart/common/detail/Aspect.h | 13 ++++++++++++- dart/common/detail/Composite.h | 8 ++++---- dart/common/detail/EmbeddedAspect.h | 18 ++++++++++++------ dart/common/detail/SpecializedForAspect.h | 5 +++-- dart/dynamics/BodyNode.h | 2 +- dart/dynamics/Skeleton.h | 2 +- dart/dynamics/detail/BodyNodeAspect.h | 2 +- 11 files changed, 48 insertions(+), 19 deletions(-) diff --git a/dart/common/Aspect.h b/dart/common/Aspect.h index 790d5e58c1b32..712e2ddc9dd6b 100644 --- a/dart/common/Aspect.h +++ b/dart/common/Aspect.h @@ -149,6 +149,9 @@ class CompositeTrackingAspect : public Aspect /// Get the Composite of this Aspect const CompositeType* getComposite() const; + /// Returns true if this Aspect has a Composite that matches CompositeType + bool hasComposite() const; + protected: /// Grab the new Composite diff --git a/dart/common/Composite.h b/dart/common/Composite.h index d2f0e2c1f7a4c..7116a8c7a83ca 100644 --- a/dart/common/Composite.h +++ b/dart/common/Composite.h @@ -196,11 +196,11 @@ class Composite // itself. Also consider allowing the user to specify arguments for the // constructors of the Aspects. template -void createAspects(T* /*mgr*/); +void createAspects(T* /*comp*/); //============================================================================== template -void createAspects(T* mgr); +void createAspects(T* comp); } // namespace common } // namespace dart diff --git a/dart/common/EmbeddedAspect.h b/dart/common/EmbeddedAspect.h index dc31b7163362d..be504d697a3b1 100644 --- a/dart/common/EmbeddedAspect.h +++ b/dart/common/EmbeddedAspect.h @@ -87,6 +87,10 @@ class EmbeddedStateAspect : public detail::EmbeddedStateAspect< /// \endcode /// /// To embed both state and properties information, use EmbedStateAndProperties. +/// +/// It is possible to customize the way an EmbeddedStateAspect interacts with +/// your Composite by using the dart::common::detail::EmbeddedStateAspect class +/// directly instead of inheriting this class. template class EmbedState : public virtual common::RequiresAspect< common::EmbeddedStateAspect > @@ -199,6 +203,11 @@ class EmbeddedPropertiesAspect : public detail::EmbeddedPropertiesAspect< /// \endcode /// /// To embed both state and properties information, use EmbedStateAndProperties. +/// +/// It is possible to customize the way an EmbeddedPropertiesAspect interacts +/// with your Composite by using the +/// dart::common::detail::EmbeddedPropertiesAspect class directly instead of +/// inheriting this class. template class EmbedProperties : public virtual common::RequiresAspect< common::EmbeddedPropertiesAspect > diff --git a/dart/common/StlHelpers.h b/dart/common/StlHelpers.h index dc7b4901bd6ee..ae1f276b3af85 100644 --- a/dart/common/StlHelpers.h +++ b/dart/common/StlHelpers.h @@ -41,7 +41,6 @@ #include #include #include -#include "dart/common/Console.h" // Macro to suppress -Wunused-parameter and -Wunused-variable warnings in // release mode when a variable is only used in assertions. diff --git a/dart/common/detail/Aspect.h b/dart/common/detail/Aspect.h index 747253fa8c649..43fd718e5e473 100644 --- a/dart/common/detail/Aspect.h +++ b/dart/common/detail/Aspect.h @@ -68,10 +68,19 @@ const CompositeType* CompositeTrackingAspect::getComposite() cons return mComposite; } +//============================================================================== +template +bool CompositeTrackingAspect::hasComposite() const +{ + return (nullptr != mComposite); +} + //============================================================================== template void CompositeTrackingAspect::setComposite(Composite* newComposite) { + assert(nullptr == mComposite); + mComposite = dynamic_cast(newComposite); if(nullptr == mComposite) { @@ -86,8 +95,10 @@ void CompositeTrackingAspect::setComposite(Composite* newComposit //============================================================================== template -void CompositeTrackingAspect::loseComposite(Composite* /*oldComposite*/) +void CompositeTrackingAspect::loseComposite( + Composite* oldComposite) { + assert(oldComposite == mComposite); mComposite = nullptr; } diff --git a/dart/common/detail/Composite.h b/dart/common/detail/Composite.h index 74ce29cf9331d..f34b527e0fd9f 100644 --- a/dart/common/detail/Composite.h +++ b/dart/common/detail/Composite.h @@ -146,18 +146,18 @@ bool Composite::requiresAspect() const //============================================================================== template -void createAspects(T* /*mgr*/) +void createAspects(T* /*comp*/) { // Do nothing } //============================================================================== template -void createAspects(T* mgr) +void createAspects(T* comp) { - mgr->template createAspect(); + comp->template createAspect(); - createAspects(mgr); + createAspects(comp); } } // namespace common diff --git a/dart/common/detail/EmbeddedAspect.h b/dart/common/detail/EmbeddedAspect.h index 89ba62bbd5cfd..7f6a7b1d6a1ac 100644 --- a/dart/common/detail/EmbeddedAspect.h +++ b/dart/common/detail/EmbeddedAspect.h @@ -219,6 +219,8 @@ class EmbeddedStateAspect : public BaseT /// Pass the temporary State of this Aspect into the new Composite void setComposite(Composite* newComposite) override { + assert(nullptr == this->getComposite()); + Base::setComposite(newComposite); if(mTemporaryState) SetEmbeddedState(static_cast(this), *mTemporaryState); @@ -234,9 +236,10 @@ class EmbeddedStateAspect : public BaseT Base::loseComposite(oldComposite); } - /// During transitions between Composite objects, this will hold the State of - /// the Aspect. Once the Aspect has been moved into a new Composite, this - /// State will be pushed into the Composite and cleared. + /// After this Aspect is constructed and during transitions between Composite + /// objects, this will hold the State of the Aspect. Once the Aspect has been + /// moved into a new Composite, this State will be pushed into the Composite + /// and cleared. std::unique_ptr mTemporaryState; }; @@ -389,6 +392,8 @@ class EmbeddedPropertiesAspect : public BaseT /// Pass the temporary Properties of this Aspect into the new Composite void setComposite(Composite* newComposite) override { + assert(nullptr == this->getComposite()); + Base::setComposite(newComposite); if(mTemporaryProperties) SetEmbeddedProperties(static_cast(this), *mTemporaryProperties); @@ -405,9 +410,10 @@ class EmbeddedPropertiesAspect : public BaseT Base::loseComposite(oldComposite); } - /// During transitions between Composite objects, this will hold the Properties - /// of the Aspect. Once the Aspect has been moved into a new Composite, these - /// Properties will be pushed into the Composite and cleared. + /// After this Aspect is constructed and during transitions between Composite + /// objects, this will hold the Properties of the Aspect. Once the Aspect has + /// been moved into a new Composite, these Properties will be pushed into the + /// Composite and cleared. std::unique_ptr mTemporaryProperties; }; diff --git a/dart/common/detail/SpecializedForAspect.h b/dart/common/detail/SpecializedForAspect.h index 675e7aaa1b108..bd0c7fdf2a5d4 100644 --- a/dart/common/detail/SpecializedForAspect.h +++ b/dart/common/detail/SpecializedForAspect.h @@ -53,8 +53,9 @@ namespace common { template SpecializedForAspect::SpecializedForAspect() { - mAspectMap[typeid( SpecAspect )] = nullptr; - mSpecAspectIterator = mAspectMap.find(typeid( SpecAspect )); + mSpecAspectIterator = mAspectMap.insert( + std::make_pair>( + typeid(SpecAspect), nullptr)).first; } //============================================================================== diff --git a/dart/dynamics/BodyNode.h b/dart/dynamics/BodyNode.h index f802d28b6c945..b3f770b6b0fc8 100644 --- a/dart/dynamics/BodyNode.h +++ b/dart/dynamics/BodyNode.h @@ -75,7 +75,7 @@ class Marker; /// BodyNode inherits Frame, and a parent Frame of a BodyNode is the parent /// BodyNode of the BodyNode. class BodyNode : - public detail::BodyNodeAspects, + public detail::BodyNodeCompositeBase, public virtual BodyNodeSpecializedFor, public SkeletonRefCountingBase, public TemplatedJacobianNode diff --git a/dart/dynamics/Skeleton.h b/dart/dynamics/Skeleton.h index 19046b5f69651..a007f3864147a 100644 --- a/dart/dynamics/Skeleton.h +++ b/dart/dynamics/Skeleton.h @@ -69,7 +69,7 @@ class Skeleton : using State = common::Composite::State; using Properties = common::Composite::Properties; - enum ConfigFlag_t + enum ConfigFlags { CONFIG_NOTHING = 0, CONFIG_POSITIONS = 1 << 1, diff --git a/dart/dynamics/detail/BodyNodeAspect.h b/dart/dynamics/detail/BodyNodeAspect.h index 462efc9cd6138..d35e20a01e6e4 100644 --- a/dart/dynamics/detail/BodyNodeAspect.h +++ b/dart/dynamics/detail/BodyNodeAspect.h @@ -144,7 +144,7 @@ using NodeVectorProxyAspect = common::ProxyStateAndPropertiesAspect; //============================================================================== -using BodyNodeAspects = common::EmbedStateAndPropertiesOnTopOf< +using BodyNodeCompositeBase = common::EmbedStateAndPropertiesOnTopOf< BodyNode, BodyNodeState, BodyNodeAspectProperties, common::RequiresAspect >; From 2e5f422bb4f9c61b23ea24eb0682dea6f8d5c20c Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Thu, 21 Apr 2016 13:16:04 -0400 Subject: [PATCH 54/62] Mark oldComposite as unused to suppress warnings --- dart/common/detail/Aspect.h | 1 + 1 file changed, 1 insertion(+) diff --git a/dart/common/detail/Aspect.h b/dart/common/detail/Aspect.h index 43fd718e5e473..a0381b99086b6 100644 --- a/dart/common/detail/Aspect.h +++ b/dart/common/detail/Aspect.h @@ -98,6 +98,7 @@ template void CompositeTrackingAspect::loseComposite( Composite* oldComposite) { + DART_UNUSED(oldComposite); assert(oldComposite == mComposite); mComposite = nullptr; } From e00ffecdb7dcd7ddf46e5543cc982af6ac19b4fa Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Thu, 21 Apr 2016 15:01:28 -0400 Subject: [PATCH 55/62] Making header guards match file names --- dart/common/detail/CompositeData.h | 6 +++--- dart/dynamics/detail/BodyNodeAspect.h | 6 +++--- dart/dynamics/detail/EulerJointAspect.h | 6 +++--- dart/dynamics/detail/MultiDofJointAspect.h | 6 +++--- dart/dynamics/detail/PlanarJointAspect.h | 6 +++--- dart/dynamics/detail/PrismaticJointAspect.h | 6 +++--- dart/dynamics/detail/RevoluteJointAspect.h | 6 +++--- dart/dynamics/detail/ScrewJointAspect.h | 6 +++--- dart/dynamics/detail/SingleDofJointAspect.h | 6 +++--- dart/dynamics/detail/SoftBodyNodeAspect.h | 6 +++--- dart/dynamics/detail/UniversalJointAspect.h | 6 +++--- 11 files changed, 33 insertions(+), 33 deletions(-) diff --git a/dart/common/detail/CompositeData.h b/dart/common/detail/CompositeData.h index ced433a600720..6aad81de0f0c3 100644 --- a/dart/common/detail/CompositeData.h +++ b/dart/common/detail/CompositeData.h @@ -34,8 +34,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_COMMON_DETAIL_COMPOSITESTATEANDPROPERTIES_H_ -#define DART_COMMON_DETAIL_COMPOSITESTATEANDPROPERTIES_H_ +#ifndef DART_COMMON_DETAIL_COMPOSITEDATA_H_ +#define DART_COMMON_DETAIL_COMPOSITEDATA_H_ #include @@ -351,4 +351,4 @@ using MakeCompositeProperties = } // namespace common } // namespace dart -#endif // DART_COMMON_DETAIL_COMPOSITESTATEANDPROPERTIES_H_ +#endif // DART_COMMON_DETAIL_COMPOSITEDATA_H_ diff --git a/dart/dynamics/detail/BodyNodeAspect.h b/dart/dynamics/detail/BodyNodeAspect.h index d35e20a01e6e4..a0676a16ec1bb 100644 --- a/dart/dynamics/detail/BodyNodeAspect.h +++ b/dart/dynamics/detail/BodyNodeAspect.h @@ -34,8 +34,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_DYNAMICS_DETAIL_BODYNODEPROPERTIES_H_ -#define DART_DYNAMICS_DETAIL_BODYNODEPROPERTIES_H_ +#ifndef DART_DYNAMICS_DETAIL_BODYNODEASPECT_H_ +#define DART_DYNAMICS_DETAIL_BODYNODEASPECT_H_ #include "dart/dynamics/Entity.h" #include "dart/dynamics/Inertia.h" @@ -152,4 +152,4 @@ using BodyNodeCompositeBase = common::EmbedStateAndPropertiesOnTopOf< } // namespace dynamics } // namespace dart -#endif // DART_DYNAMICS_DETAIL_BODYNODEPROPERTIES_H_ +#endif // DART_DYNAMICS_DETAIL_BODYNODEASPECT_H_ diff --git a/dart/dynamics/detail/EulerJointAspect.h b/dart/dynamics/detail/EulerJointAspect.h index 6cfd9eb3dbdb1..cb5c5f6379664 100644 --- a/dart/dynamics/detail/EulerJointAspect.h +++ b/dart/dynamics/detail/EulerJointAspect.h @@ -34,8 +34,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_DYNAMICS_DETAIL_EULERJOINTPROPERTIES_H_ -#define DART_DYNAMICS_DETAIL_EULERJOINTPROPERTIES_H_ +#ifndef DART_DYNAMICS_DETAIL_EULERJOINTASPECT_H_ +#define DART_DYNAMICS_DETAIL_EULERJOINTASPECT_H_ #include @@ -92,4 +92,4 @@ using EulerJointBase = common::EmbedPropertiesOnTopOf< } // namespace dart -#endif // DART_DYNAMICS_DETAIL_EULERJOINTPROPERTIES_H_ +#endif // DART_DYNAMICS_DETAIL_EULERJOINTASPECT_H_ diff --git a/dart/dynamics/detail/MultiDofJointAspect.h b/dart/dynamics/detail/MultiDofJointAspect.h index 4e358b64c952c..44d1962283054 100644 --- a/dart/dynamics/detail/MultiDofJointAspect.h +++ b/dart/dynamics/detail/MultiDofJointAspect.h @@ -34,8 +34,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_DYNAMICS_DETAIL_MULTIDOFJOINTPROPERTIES_H_ -#define DART_DYNAMICS_DETAIL_MULTIDOFJOINTPROPERTIES_H_ +#ifndef DART_DYNAMICS_DETAIL_MULTIDOFJOINTASPECT_H_ +#define DART_DYNAMICS_DETAIL_MULTIDOFJOINTASPECT_H_ #include "dart/math/Helpers.h" #include "dart/dynamics/Joint.h" @@ -302,5 +302,5 @@ using MultiDofJointBase = common::EmbedStateAndPropertiesOnTopOf< } // namespace dynamics } // namespace dart -#endif // DART_DYNAMICS_DETAIL_MULTIDOFJOINTPROPERTIES_H_ +#endif // DART_DYNAMICS_DETAIL_MULTIDOFJOINTASPECT_H_ diff --git a/dart/dynamics/detail/PlanarJointAspect.h b/dart/dynamics/detail/PlanarJointAspect.h index 263d8356faf82..ce067f865c1bc 100644 --- a/dart/dynamics/detail/PlanarJointAspect.h +++ b/dart/dynamics/detail/PlanarJointAspect.h @@ -34,8 +34,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_DYNAMICS_DETAIL_PLANARJOINTPROPERTIES_H_ -#define DART_DYNAMICS_DETAIL_PLANARJOINTPROPERTIES_H_ +#ifndef DART_DYNAMICS_DETAIL_PLANARJOINTASPECT_H_ +#define DART_DYNAMICS_DETAIL_PLANARJOINTASPECT_H_ #include @@ -130,4 +130,4 @@ using PlanarJointBase = common::EmbedPropertiesOnTopOf< } // namespace dynamics } // namespace dart -#endif // DART_DYNAMICS_DETAIL_PLANARJOINTPROPERTIES_H_ +#endif // DART_DYNAMICS_DETAIL_PLANARJOINTASPECT_H_ diff --git a/dart/dynamics/detail/PrismaticJointAspect.h b/dart/dynamics/detail/PrismaticJointAspect.h index a3646d2c705f8..292a1eb6800d7 100644 --- a/dart/dynamics/detail/PrismaticJointAspect.h +++ b/dart/dynamics/detail/PrismaticJointAspect.h @@ -34,8 +34,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_DYNAMICS_DETAIL_PRISMATICJOINTPROPERTIES_H_ -#define DART_DYNAMICS_DETAIL_PRISMATICJOINTPROPERTIES_H_ +#ifndef DART_DYNAMICS_DETAIL_PRISMATICJOINTASPECT_H_ +#define DART_DYNAMICS_DETAIL_PRISMATICJOINTASPECT_H_ #include @@ -83,4 +83,4 @@ using PrismaticJointBase = common::EmbedPropertiesOnTopOf< } // namespace dynamics } // namespace dart -#endif // DART_DYNAMICS_DETAIL_PRISMATICJOINTPROPERTIES_H_ +#endif // DART_DYNAMICS_DETAIL_PRISMATICJOINTASPECT_H_ diff --git a/dart/dynamics/detail/RevoluteJointAspect.h b/dart/dynamics/detail/RevoluteJointAspect.h index 8e1aa62fe8b04..9daa10482bf2d 100644 --- a/dart/dynamics/detail/RevoluteJointAspect.h +++ b/dart/dynamics/detail/RevoluteJointAspect.h @@ -34,8 +34,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_DYNAMICS_DETAIL_REVOLUTEJOINTPROPERTIES_H_ -#define DART_DYNAMICS_DETAIL_REVOLUTEJOINTPROPERTIES_H_ +#ifndef DART_DYNAMICS_DETAIL_REVOLUTEJOINTASPECT_H_ +#define DART_DYNAMICS_DETAIL_REVOLUTEJOINTASPECT_H_ #include @@ -85,4 +85,4 @@ using RevoluteJointBase = common::EmbedPropertiesOnTopOf< } // namespace dart -#endif // DART_DYNAMICS_DETAIL_REVOLUTEJOINTPROPERTIES_H_ +#endif // DART_DYNAMICS_DETAIL_REVOLUTEJOINTASPECT_H_ diff --git a/dart/dynamics/detail/ScrewJointAspect.h b/dart/dynamics/detail/ScrewJointAspect.h index 592ef0b38255d..4158d03ae0705 100644 --- a/dart/dynamics/detail/ScrewJointAspect.h +++ b/dart/dynamics/detail/ScrewJointAspect.h @@ -34,8 +34,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_DYNAMICS_DETAIL_SCREWJOINTPROPERTIES_H_ -#define DART_DYNAMICS_DETAIL_SCREWJOINTPROPERTIES_H_ +#ifndef DART_DYNAMICS_DETAIL_SCREWJOINTASPECT_H_ +#define DART_DYNAMICS_DETAIL_SCREWJOINTASPECT_H_ #include @@ -87,4 +87,4 @@ using ScrewJointBase = common::EmbedPropertiesOnTopOf< } // namespace dynamics } // namespace dart -#endif // DART_DYNAMICS_DETAIL_SCREWJOINTPROPERTIES_H_ +#endif // DART_DYNAMICS_DETAIL_SCREWJOINTASPECT_H_ diff --git a/dart/dynamics/detail/SingleDofJointAspect.h b/dart/dynamics/detail/SingleDofJointAspect.h index d685569620bb1..8e0935a7a193c 100644 --- a/dart/dynamics/detail/SingleDofJointAspect.h +++ b/dart/dynamics/detail/SingleDofJointAspect.h @@ -34,8 +34,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_DYNAMICS_DETAIL_SINGLEDOFJOINTPROPERTIES_H_ -#define DART_DYNAMICS_DETAIL_SINGLEDOFJOINTPROPERTIES_H_ +#ifndef DART_DYNAMICS_DETAIL_SINGLEDOFJOINTASPECT_H_ +#define DART_DYNAMICS_DETAIL_SINGLEDOFJOINTASPECT_H_ #include "dart/common/RequiresAspect.h" @@ -170,4 +170,4 @@ using SingleDofJointBase = common::EmbedStateAndPropertiesOnTopOf< } // namespace dynamics } // namespace dart -#endif // DART_DYNAMICS_DETAIL_SINGLEDOFJOINTPROPERTIES_H_ +#endif // DART_DYNAMICS_DETAIL_SINGLEDOFJOINTASPECT_H_ diff --git a/dart/dynamics/detail/SoftBodyNodeAspect.h b/dart/dynamics/detail/SoftBodyNodeAspect.h index 668630e2a8823..c8bc5ba28cc71 100644 --- a/dart/dynamics/detail/SoftBodyNodeAspect.h +++ b/dart/dynamics/detail/SoftBodyNodeAspect.h @@ -34,8 +34,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_DYNAMICS_DETAIL_SOFTBODYNODEPROPERTIES_H_ -#define DART_DYNAMICS_DETAIL_SOFTBODYNODEPROPERTIES_H_ +#ifndef DART_DYNAMICS_DETAIL_SOFTBODYNODEASPECT_H_ +#define DART_DYNAMICS_DETAIL_SOFTBODYNODEASPECT_H_ #include "dart/dynamics/BodyNode.h" #include "dart/dynamics/PointMass.h" @@ -128,4 +128,4 @@ using SoftBodyNodeBase = common::EmbedStateAndPropertiesOnTopOf< } // namespace dynamics } // namespace dart -#endif // DART_DYNAMICS_DETAIL_SOFTBODYNODEPROPERTIES_H_ +#endif // DART_DYNAMICS_DETAIL_SOFTBODYNODEASPECT_H_ diff --git a/dart/dynamics/detail/UniversalJointAspect.h b/dart/dynamics/detail/UniversalJointAspect.h index 304eafa1c5a87..df986a3951662 100644 --- a/dart/dynamics/detail/UniversalJointAspect.h +++ b/dart/dynamics/detail/UniversalJointAspect.h @@ -34,8 +34,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_DYNAMICS_DETAIL_UNIVERSALJOINTPROPERTIES_H_ -#define DART_DYNAMICS_DETAIL_UNIVERSALJOINTPROPERTIES_H_ +#ifndef DART_DYNAMICS_DETAIL_UNIVERSALJOINTASPECT_H_ +#define DART_DYNAMICS_DETAIL_UNIVERSALJOINTASPECT_H_ #include @@ -84,4 +84,4 @@ using UniversalJointBase = common::EmbedPropertiesOnTopOf< } // namespace dynamics } // namespace dart -#endif // DART_DYNAMICS_DETAIL_UNIVERSALJOINTPROPERTIES_H_ +#endif // DART_DYNAMICS_DETAIL_UNIVERSALJOINTASPECT_H_ From a15b9c36a07553f7b644c2166da58871cc39898e Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Thu, 21 Apr 2016 15:07:12 -0400 Subject: [PATCH 56/62] Bring back transform verification --- dart/dynamics/ScrewJoint.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/dart/dynamics/ScrewJoint.cpp b/dart/dynamics/ScrewJoint.cpp index 38c758bc76413..de75db5664cbc 100644 --- a/dart/dynamics/ScrewJoint.cpp +++ b/dart/dynamics/ScrewJoint.cpp @@ -186,6 +186,7 @@ void ScrewJoint::updateLocalTransform() const mT = Joint::mAspectProperties.mT_ParentBodyToJoint * math::expMap(S * getPositionStatic()) * Joint::mAspectProperties.mT_ChildBodyToJoint.inverse(); + assert(math::verifyTransform(mT)); } //============================================================================== From 6acb6934f8e2757cf16398a084dbc70f1386e638 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Thu, 21 Apr 2016 15:36:03 -0400 Subject: [PATCH 57/62] Moved detail-classes into the detail directory --- dart/dynamics/EndEffector.h | 59 +---------- dart/dynamics/FixedFrame.h | 15 +-- dart/dynamics/ShapeFrame.h | 81 ++------------- dart/dynamics/detail/EndEffectorAspect.h | 109 ++++++++++++++++++++ dart/dynamics/detail/FixedFrameAspect.h | 62 +++++++++++ dart/dynamics/detail/ShapeFrameAspect.h | 126 +++++++++++++++++++++++ 6 files changed, 305 insertions(+), 147 deletions(-) create mode 100644 dart/dynamics/detail/EndEffectorAspect.h create mode 100644 dart/dynamics/detail/FixedFrameAspect.h create mode 100644 dart/dynamics/detail/ShapeFrameAspect.h diff --git a/dart/dynamics/EndEffector.h b/dart/dynamics/EndEffector.h index d0eaca61f3ab5..be94396ddeaf2 100644 --- a/dart/dynamics/EndEffector.h +++ b/dart/dynamics/EndEffector.h @@ -42,6 +42,7 @@ #include "dart/common/AspectWithVersion.h" #include "dart/dynamics/FixedJacobianNode.h" #include "dart/dynamics/CompositeNode.h" +#include "dart/dynamics/detail/EndEffectorAspect.h" namespace dart { namespace dynamics { @@ -49,64 +50,6 @@ namespace dynamics { class BodyNode; class Skeleton; class EndEffector; -class Support; - -namespace detail { - -//============================================================================== -struct EndEffectorProperties -{ - /// The default relative transform for the EndEffector. If the relative - /// transform of the EndEffector is ever changed, you can call - /// resetRelativeTransform() to return the relative transform to this one. - Eigen::Isometry3d mDefaultTransform; - - EndEffectorProperties( - const Eigen::Isometry3d& defaultTf = Eigen::Isometry3d::Identity()); - - // To get byte-aligned Eigen vectors - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -//============================================================================== -struct SupportStateData -{ - /// Whether or not this EndEffector is currently being used to support the - /// weight of the robot. - bool mActive; - - inline SupportStateData(bool active = false) : mActive(active) { } - - // To get byte-aligned Eigen vectors - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -//============================================================================== -struct SupportPropertiesData -{ - /// A set of points representing the support polygon that can be provided by - /// the EndEffector. These points must be defined relative to the EndEffector - /// frame. - math::SupportGeometry mGeometry; - - inline SupportPropertiesData( - const math::SupportGeometry& geometry = math::SupportGeometry()) - : mGeometry(geometry) { } - - // To get byte-aligned Eigen vectors - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -void SupportUpdate(Support* support); - -using EndEffectorCompositeBase = CompositeNode< - common::CompositeJoiner< - FixedJacobianNode, - common::SpecializedForAspect - > ->; - -} // namespace detail //============================================================================== class Support final : diff --git a/dart/dynamics/FixedFrame.h b/dart/dynamics/FixedFrame.h index 65ad9c33eecf0..9a8a582df99d9 100644 --- a/dart/dynamics/FixedFrame.h +++ b/dart/dynamics/FixedFrame.h @@ -40,24 +40,11 @@ #include "dart/dynamics/Frame.h" #include "dart/common/EmbeddedAspect.h" #include "dart/common/VersionCounter.h" +#include "dart/dynamics/detail/FixedFrameAspect.h" namespace dart { namespace dynamics { -namespace detail { -struct FixedFrameProperties -{ - /// The relative transform of the FixedFrame - Eigen::Isometry3d mRelativeTf; - - FixedFrameProperties( - const Eigen::Isometry3d& relativeTf = Eigen::Isometry3d::Identity()); - - // To get byte-aligned Eigen vectors - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; -} // namespace detail - /// The FixedFrame class represents a Frame with zero relative velocity and /// zero relative acceleration. It does not move within its parent Frame after /// its relative transform is set. However, classes that inherit the FixedFrame diff --git a/dart/dynamics/ShapeFrame.h b/dart/dynamics/ShapeFrame.h index 62d2176c12582..793476da21af4 100644 --- a/dart/dynamics/ShapeFrame.h +++ b/dart/dynamics/ShapeFrame.h @@ -45,81 +45,12 @@ #include "dart/dynamics/FixedFrame.h" #include "dart/dynamics/TemplatedJacobianNode.h" #include "dart/dynamics/EllipsoidShape.h" +#include "dart/dynamics/detail/ShapeFrameAspect.h" namespace dart { namespace dynamics { -class VisualAspect; -class CollisionAspect; -class DynamicsAspect; -class ShapeFrame; - -namespace detail { - -struct VisualAspectProperties -{ - /// Color for the primitive shape - Eigen::Vector4d mRGBA; - - bool mUseDefaultColor; - - /// True if this shape node should be kept from rendering - bool mHidden; - - /// Constructor - VisualAspectProperties( - const Eigen::Vector4d& color = Eigen::Vector4d(0.5, 0.5, 1.0, 1.0), - const bool hidden = false); - - /// Destructor - virtual ~VisualAspectProperties() = default; - - // To get byte-aligned Eigen vectors - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -struct CollisionAspectProperties -{ - /// This object is collidable if true - bool mCollidable; - - /// Constructor - CollisionAspectProperties(const bool collidable = true); - - /// Destructor - virtual ~CollisionAspectProperties() = default; -}; - -struct DynamicsAspectProperties -{ - /// Coefficient of friction - double mFrictionCoeff; - - /// Coefficient of restitution - double mRestitutionCoeff; - - /// Constructor - DynamicsAspectProperties(const double frictionCoeff = 1.0, - const double restitutionCoeff = 0.0); - - /// Destructor - virtual ~DynamicsAspectProperties() = default; -}; - -struct ShapeFrameProperties -{ - /// Pointer to a shape - ShapePtr mShape; - - /// Constructor - ShapeFrameProperties(const ShapePtr& shape = nullptr); - - /// Virtual destructor - virtual ~ShapeFrameProperties() = default; -}; - -} // namespace detail - +//============================================================================== class VisualAspect final : public common::AspectWithVersionedProperties< VisualAspect, @@ -184,6 +115,7 @@ class VisualAspect final : }; +//============================================================================== class CollisionAspect final : public common::AspectWithVersionedProperties< CollisionAspect, @@ -205,6 +137,7 @@ class CollisionAspect final : }; +//============================================================================== class DynamicsAspect final : public common::AspectWithVersionedProperties< DynamicsAspect, @@ -230,12 +163,10 @@ class DynamicsAspect final : }; +//============================================================================== class ShapeFrame : public virtual common::VersionCounter, - public common::EmbedPropertiesOnTopOf< - ShapeFrame, detail::ShapeFrameProperties, - common::SpecializedForAspect< - VisualAspect, CollisionAspect, DynamicsAspect> >, + public detail::ShapeFrameCompositeBase, public virtual Frame { public: diff --git a/dart/dynamics/detail/EndEffectorAspect.h b/dart/dynamics/detail/EndEffectorAspect.h new file mode 100644 index 0000000000000..3c88ea7d4688e --- /dev/null +++ b/dart/dynamics/detail/EndEffectorAspect.h @@ -0,0 +1,109 @@ +/* + * 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_ENDEFFECTORASPECT_H_ +#define DART_DYNAMICS_DETAIL_ENDEFFECTORASPECT_H_ + +#include +#include "dart/dynamics/CompositeNode.h" +#include "dart/common/SpecializedForAspect.h" + +namespace dart { +namespace dynamics { + +class FixedJacobianNode; +class Support; + +namespace detail { + +//============================================================================== +struct EndEffectorProperties +{ + /// The default relative transform for the EndEffector. If the relative + /// transform of the EndEffector is ever changed, you can call + /// resetRelativeTransform() to return the relative transform to this one. + Eigen::Isometry3d mDefaultTransform; + + EndEffectorProperties( + const Eigen::Isometry3d& defaultTf = Eigen::Isometry3d::Identity()); + + // To get byte-aligned Eigen vectors + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +//============================================================================== +struct SupportStateData +{ + /// Whether or not this EndEffector is currently being used to support the + /// weight of the robot. + bool mActive; + + inline SupportStateData(bool active = false) : mActive(active) { } + + // To get byte-aligned Eigen vectors + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +//============================================================================== +struct SupportPropertiesData +{ + /// A set of points representing the support polygon that can be provided by + /// the EndEffector. These points must be defined relative to the EndEffector + /// frame. + math::SupportGeometry mGeometry; + + inline SupportPropertiesData( + const math::SupportGeometry& geometry = math::SupportGeometry()) + : mGeometry(geometry) { } + + // To get byte-aligned Eigen vectors + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +void SupportUpdate(Support* support); + +using EndEffectorCompositeBase = CompositeNode< + common::CompositeJoiner< + FixedJacobianNode, + common::SpecializedForAspect + > +>; + +} // namespace detail +} // namespace dynamics +} // namespace dart + +#endif // DART_DYNAMICS_DETAIL_ENDEFFECTORASPECT_H_ diff --git a/dart/dynamics/detail/FixedFrameAspect.h b/dart/dynamics/detail/FixedFrameAspect.h new file mode 100644 index 0000000000000..08ccd67bdf821 --- /dev/null +++ b/dart/dynamics/detail/FixedFrameAspect.h @@ -0,0 +1,62 @@ +/* + * 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_FIXEDFRAMEASPECT_H_ +#define DART_DYNAMICS_DETAIL_FIXEDFRAMEASPECT_H_ + +#include + +namespace dart { +namespace dynamics { +namespace detail { + +struct FixedFrameProperties +{ + /// The relative transform of the FixedFrame + Eigen::Isometry3d mRelativeTf; + + FixedFrameProperties( + const Eigen::Isometry3d& relativeTf = Eigen::Isometry3d::Identity()); + + // To get byte-aligned Eigen vectors + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // namespace detail +} // namespace dynamics +} // namespace dart + +#endif // DART_DYNAMICS_DETAIL_FIXEDFRAMEASPECT_H_ diff --git a/dart/dynamics/detail/ShapeFrameAspect.h b/dart/dynamics/detail/ShapeFrameAspect.h new file mode 100644 index 0000000000000..9cdd5e603107a --- /dev/null +++ b/dart/dynamics/detail/ShapeFrameAspect.h @@ -0,0 +1,126 @@ +/* + * 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_SHAPEFRAMEASPECT_H_ +#define DART_DYNAMICS_DETAIL_SHAPEFRAMEASPECT_H_ + +#include + +#include "dart/common/EmbeddedAspect.h" +#include "dart/dynamics/SmartPointer.h" + +namespace dart { +namespace dynamics { + +class VisualAspect; +class CollisionAspect; +class DynamicsAspect; +class ShapeFrame; + +namespace detail { + +struct VisualAspectProperties +{ + /// Color for the primitive shape + Eigen::Vector4d mRGBA; + + bool mUseDefaultColor; + + /// True if this shape node should be kept from rendering + bool mHidden; + + /// Constructor + VisualAspectProperties( + const Eigen::Vector4d& color = Eigen::Vector4d(0.5, 0.5, 1.0, 1.0), + const bool hidden = false); + + /// Destructor + virtual ~VisualAspectProperties() = default; + + // To get byte-aligned Eigen vectors + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +struct CollisionAspectProperties +{ + /// This object is collidable if true + bool mCollidable; + + /// Constructor + CollisionAspectProperties(const bool collidable = true); + + /// Destructor + virtual ~CollisionAspectProperties() = default; +}; + +struct DynamicsAspectProperties +{ + /// Coefficient of friction + double mFrictionCoeff; + + /// Coefficient of restitution + double mRestitutionCoeff; + + /// Constructor + DynamicsAspectProperties(const double frictionCoeff = 1.0, + const double restitutionCoeff = 0.0); + + /// Destructor + virtual ~DynamicsAspectProperties() = default; +}; + +struct ShapeFrameProperties +{ + /// Pointer to a shape + ShapePtr mShape; + + /// Constructor + ShapeFrameProperties(const ShapePtr& shape = nullptr); + + /// Virtual destructor + virtual ~ShapeFrameProperties() = default; +}; + +using ShapeFrameCompositeBase = common::EmbedPropertiesOnTopOf< + ShapeFrame, detail::ShapeFrameProperties, + common::SpecializedForAspect< + VisualAspect, CollisionAspect, DynamicsAspect> >; + +} // namespace detail +} // namespace dynamics +} // namespace dart + +#endif // DART_DYNAMICS_DETAIL_SHAPEFRAMEASPECT_H_ From e5e808ae790252f399a64eccea6053ccff05d1f4 Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Thu, 21 Apr 2016 15:41:08 -0400 Subject: [PATCH 58/62] Code quality cleanup based on Reviewable comments --- dart/common/detail/EmbeddedAspect.h | 8 ++++---- dart/dynamics/FixedJacobianNode.cpp | 1 - dart/dynamics/SimpleFrame.h | 10 +++++----- dart/dynamics/SoftBodyNode.h | 3 --- dart/dynamics/SoftMeshShape.cpp | 2 +- dart/dynamics/detail/BodyNodeAspect.h | 3 ++- 6 files changed, 12 insertions(+), 15 deletions(-) diff --git a/dart/common/detail/EmbeddedAspect.h b/dart/common/detail/EmbeddedAspect.h index 7f6a7b1d6a1ac..351f5f915ad78 100644 --- a/dart/common/detail/EmbeddedAspect.h +++ b/dart/common/detail/EmbeddedAspect.h @@ -149,7 +149,7 @@ class EmbeddedStateAspect : public BaseT /// Set the State of this Aspect void setState(const State& state) { - if(this->getComposite()) + if(this->hasComposite()) { SetEmbeddedState(static_cast(this), state); return; @@ -169,7 +169,7 @@ class EmbeddedStateAspect : public BaseT /// Get the State of this Aspect const State& getState() const { - if(this->getComposite()) + if(this->hasComposite()) { return GetEmbeddedState(static_cast(this)); } @@ -323,7 +323,7 @@ class EmbeddedPropertiesAspect : public BaseT // Documentation inherited void setProperties(const Properties& properties) { - if(this->getComposite()) + if(this->hasComposite()) { SetEmbeddedProperties(static_cast(this), properties); return; @@ -343,7 +343,7 @@ class EmbeddedPropertiesAspect : public BaseT // Documentation inherited const Properties& getProperties() const { - if(this->getComposite()) + if(this->hasComposite()) { return GetEmbeddedProperties(static_cast(this)); } diff --git a/dart/dynamics/FixedJacobianNode.cpp b/dart/dynamics/FixedJacobianNode.cpp index 6faa614255549..1dd0ed1c92a80 100644 --- a/dart/dynamics/FixedJacobianNode.cpp +++ b/dart/dynamics/FixedJacobianNode.cpp @@ -213,6 +213,5 @@ void FixedJacobianNode::updateWorldJacobianClassicDeriv() const mIsWorldJacobianClassicDerivDirty = false; } - } // namespace dynamics } // namespace dart diff --git a/dart/dynamics/SimpleFrame.h b/dart/dynamics/SimpleFrame.h index fe9ad9c97d916..9338092bf4417 100644 --- a/dart/dynamics/SimpleFrame.h +++ b/dart/dynamics/SimpleFrame.h @@ -77,7 +77,7 @@ class SimpleFrame : public Detachable, public ShapeFrame /// Create a new SimpleFrame with the same world transform, velocity, and /// acceleration as this one. _refFrame will be used as the reference Frame /// of the new SimpleFrame. - virtual std::shared_ptr clone(Frame* _refFrame = Frame::World()) const; + std::shared_ptr clone(Frame* _refFrame = Frame::World()) const; /// Make the world transform, world velocity, and world acceleration of this /// SimpleFrame match another Frame. The _refFrame argument will be the new @@ -160,7 +160,7 @@ class SimpleFrame : public Detachable, public ShapeFrame const Frame* _inCoordinatesOf); // Documentation inherited - virtual const Eigen::Vector6d& getRelativeSpatialVelocity() const override; + const Eigen::Vector6d& getRelativeSpatialVelocity() const override; //-------------------------------------------------------------------------- // Acceleration @@ -181,13 +181,13 @@ class SimpleFrame : public Detachable, public ShapeFrame const Frame* _inCoordinatesOf); // Documentation inherited - virtual const Eigen::Vector6d& getRelativeSpatialAcceleration() const override; + const Eigen::Vector6d& getRelativeSpatialAcceleration() const override; // Documentation inherited - virtual const Eigen::Vector6d& getPrimaryRelativeAcceleration() const override; + const Eigen::Vector6d& getPrimaryRelativeAcceleration() const override; // Documentation inherited - virtual const Eigen::Vector6d& getPartialAcceleration() const override; + const Eigen::Vector6d& getPartialAcceleration() const override; //-------------------------------------------------------------------------- // Classic Method diff --git a/dart/dynamics/SoftBodyNode.h b/dart/dynamics/SoftBodyNode.h index a97666248baa2..40fca20374fa6 100644 --- a/dart/dynamics/SoftBodyNode.h +++ b/dart/dynamics/SoftBodyNode.h @@ -306,9 +306,6 @@ class SoftBodyNode : public detail::SoftBodyNodeBase /// An Entity which tracks when the point masses need to be updated PointMassNotifier* mNotifier; - /// SoftBodyNode Properties -// UniqueProperties mSoftP; - /// \brief Soft mesh shape belonging to this node. WeakShapeNodePtr mSoftShapeNode; diff --git a/dart/dynamics/SoftMeshShape.cpp b/dart/dynamics/SoftMeshShape.cpp index 5cb3beb0f2e85..9728a9b0453cf 100644 --- a/dart/dynamics/SoftMeshShape.cpp +++ b/dart/dynamics/SoftMeshShape.cpp @@ -91,7 +91,7 @@ void SoftMeshShape::_buildMesh() int nFaces = mSoftBodyNode->getNumFaces(); // Create new aiMesh - mAssimpMesh = std::unique_ptr(new aiMesh()); + mAssimpMesh = common::make_unique(); // Set vertices and normals mAssimpMesh->mNumVertices = nVertices; diff --git a/dart/dynamics/detail/BodyNodeAspect.h b/dart/dynamics/detail/BodyNodeAspect.h index a0676a16ec1bb..d0fbed27ca942 100644 --- a/dart/dynamics/detail/BodyNodeAspect.h +++ b/dart/dynamics/detail/BodyNodeAspect.h @@ -56,7 +56,8 @@ namespace detail { //============================================================================== struct BodyNodeState -{/// External spatial force +{ + /// External spatial force Eigen::Vector6d mFext; BodyNodeState(const Eigen::Vector6d& Fext = Eigen::Vector6d::Zero()); From 1f3ddc281d3beea8a299d5515a573226b549735f Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 21 Apr 2016 18:30:41 -0400 Subject: [PATCH 59/62] Fix headerguard of JointAspect.h --- dart/dynamics/detail/JointAspect.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/dart/dynamics/detail/JointAspect.h b/dart/dynamics/detail/JointAspect.h index 5ac56c2be5669..ddc8d6fe40619 100644 --- a/dart/dynamics/detail/JointAspect.h +++ b/dart/dynamics/detail/JointAspect.h @@ -34,8 +34,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef DART_DYNAMICS_DETAIL_JOINT_H_ -#define DART_DYNAMICS_DETAIL_JOINT_H_ +#ifndef DART_DYNAMICS_DETAIL_JOINTASPECT_H_ +#define DART_DYNAMICS_DETAIL_JOINTASPECT_H_ #include #include @@ -141,4 +141,4 @@ struct JointProperties } // namespace dynamics } // namespace dart -#endif // DART_DYNAMICS_DETAIL_JOINT_H_ +#endif // DART_DYNAMICS_DETAIL_JOINTASPECT_H_ From eb63515e04e5a7312f6f3c1a4f7edbf34be35c22 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 21 Apr 2016 18:50:34 -0400 Subject: [PATCH 60/62] Move detail-typedef into the detail directory --- dart/dynamics/FixedJacobianNode.h | 15 +----- dart/dynamics/ShapeNode.h | 16 +------ dart/dynamics/detail/FixedJacobianNode.h | 60 ++++++++++++++++++++++++ dart/dynamics/detail/ShapeNode.h | 60 ++++++++++++++++++++++++ 4 files changed, 122 insertions(+), 29 deletions(-) create mode 100644 dart/dynamics/detail/FixedJacobianNode.h create mode 100644 dart/dynamics/detail/ShapeNode.h diff --git a/dart/dynamics/FixedJacobianNode.h b/dart/dynamics/FixedJacobianNode.h index 62fa30036ade9..839254002ace1 100644 --- a/dart/dynamics/FixedJacobianNode.h +++ b/dart/dynamics/FixedJacobianNode.h @@ -37,24 +37,11 @@ #ifndef DART_DYNAMICS_FIXEDJACOBIANNODE_H_ #define DART_DYNAMICS_FIXEDJACOBIANNODE_H_ -#include "dart/dynamics/TemplatedJacobianNode.h" -#include "dart/dynamics/FixedFrame.h" -#include "dart/dynamics/EntityNode.h" +#include "dart/dynamics/detail/FixedJacobianNode.h" namespace dart { namespace dynamics { -class FixedJacobianNode; - -namespace detail { - -using FixedJacobianNodeCompositeBase = common::CompositeJoiner< - EntityNode< TemplatedJacobianNode >, - common::Virtual ->; - -} // namespace detail - class FixedJacobianNode : public detail::FixedJacobianNodeCompositeBase, public AccessoryNode diff --git a/dart/dynamics/ShapeNode.h b/dart/dynamics/ShapeNode.h index 3b74b682101d4..0d74486c6722f 100644 --- a/dart/dynamics/ShapeNode.h +++ b/dart/dynamics/ShapeNode.h @@ -40,28 +40,14 @@ #include #include "dart/common/Signal.h" -#include "dart/dynamics/ShapeFrame.h" -#include "dart/dynamics/FixedJacobianNode.h" -#include "dart/dynamics/CompositeNode.h" +#include "dart/dynamics/detail/ShapeNode.h" namespace dart { namespace dynamics { -namespace detail { - -using ShapeNodeCompositeBase = CompositeNode< - common::CompositeJoiner< - FixedJacobianNode, - ShapeFrame - > ->; - -} // namespace detail - class VisualAspect; class CollisionAspect; class DynamicsAspect; -class ShapeFrame; class ShapeNode : public detail::ShapeNodeCompositeBase { diff --git a/dart/dynamics/detail/FixedJacobianNode.h b/dart/dynamics/detail/FixedJacobianNode.h new file mode 100644 index 0000000000000..f693fa395c14c --- /dev/null +++ b/dart/dynamics/detail/FixedJacobianNode.h @@ -0,0 +1,60 @@ +/* + * 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_FIXEDJACOBIANNODE_H_ +#define DART_DYNAMICS_DETAIL_FIXEDJACOBIANNODE_H_ + +#include "dart/dynamics/TemplatedJacobianNode.h" +#include "dart/dynamics/FixedFrame.h" +#include "dart/dynamics/EntityNode.h" + +namespace dart { +namespace dynamics { + +class FixedJacobianNode; + +namespace detail { + +using FixedJacobianNodeCompositeBase = common::CompositeJoiner< + EntityNode< TemplatedJacobianNode >, + common::Virtual +>; + +} // namespace detail +} // namespace dynamics +} // namespace dart + +#endif // DART_DYNAMICS_DETAIL_FIXEDJACOBIANNODE_H_ diff --git a/dart/dynamics/detail/ShapeNode.h b/dart/dynamics/detail/ShapeNode.h new file mode 100644 index 0000000000000..ead279ecf2e43 --- /dev/null +++ b/dart/dynamics/detail/ShapeNode.h @@ -0,0 +1,60 @@ +/* + * Copyright (c) 2016, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * 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_SHAPENODE_H_ +#define DART_DYNAMICS_DETAIL_SHAPENODE_H_ + +#include "dart/dynamics/ShapeFrame.h" +#include "dart/dynamics/FixedJacobianNode.h" +#include "dart/dynamics/CompositeNode.h" + +namespace dart { +namespace dynamics { + +namespace detail { + +using ShapeNodeCompositeBase = CompositeNode< + common::CompositeJoiner< + FixedJacobianNode, + ShapeFrame + > +>; + +} // namespace detail +} // namespace dynamics +} // namespace dart + +#endif // DART_DYNAMICS_DETAIL_SHAPENODE_H_ From 628a273b72a2b1e22c6eb81bd484449ed25c66e8 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 21 Apr 2016 18:52:27 -0400 Subject: [PATCH 61/62] Remove unnecessary empty line --- dart/dynamics/detail/ShapeNode.h | 1 - 1 file changed, 1 deletion(-) diff --git a/dart/dynamics/detail/ShapeNode.h b/dart/dynamics/detail/ShapeNode.h index ead279ecf2e43..359e774a05c5c 100644 --- a/dart/dynamics/detail/ShapeNode.h +++ b/dart/dynamics/detail/ShapeNode.h @@ -43,7 +43,6 @@ namespace dart { namespace dynamics { - namespace detail { using ShapeNodeCompositeBase = CompositeNode< From 3f8c8adba256bf6203d1827602421192d578001f Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Thu, 21 Apr 2016 20:14:43 -0400 Subject: [PATCH 62/62] Removing embarrassing hardcoded path names --- dart/gui/osg/examples/osgHuboPuppet.cpp | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/dart/gui/osg/examples/osgHuboPuppet.cpp b/dart/gui/osg/examples/osgHuboPuppet.cpp index 07db569a1228a..fe9819f1765f7 100644 --- a/dart/gui/osg/examples/osgHuboPuppet.cpp +++ b/dart/gui/osg/examples/osgHuboPuppet.cpp @@ -893,16 +893,6 @@ class InputHandler : public ::osgGA::GUIEventHandler if( ::osgGA::GUIEventAdapter::KEYDOWN == ea.getEventType() ) { - if( ea.getKey() == ::osgGA::GUIEventAdapter::KEY_Tab ) - { - if(mViewer->isRecording()) - mViewer->pauseRecording(); - else - mViewer->record("/home/grey/dump"); - - mViewer->captureScreen("/home/grey/dump/capture.png"); - } - if( ea.getKey() == 'p' ) { for(size_t i=0; i < mHubo->getNumDofs(); ++i)