Merge branch 'grey/aspects' into grey/debug_ik

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 <Eigen/Core>
 
@@ -351,4 +351,4 @@ using MakeCompositeProperties =
 } // namespace common
 } // namespace dart
 
-#endif // DART_COMMON_DETAIL_COMPOSITESTATEANDPROPERTIES_H_
+#endif // DART_COMMON_DETAIL_COMPOSITEDATA_H_

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<Derived*>(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<const Derived*>(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<Derived*>(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<const Derived*>(this));
     }

dart/dynamics/EndEffector.h

@@ -42,71 +42,14 @@
 #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 {
 
 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<Support>
-    >
->;
-
-} // namespace detail
 
 //==============================================================================
 class Support final :

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

dart/dynamics/FixedJacobianNode.cpp

@@ -214,6 +214,5 @@ void FixedJacobianNode::updateWorldJacobianClassicDeriv() const
   mIsWorldJacobianClassicDerivDirty = false;
 }
 
-
 } // namespace dynamics
 } // namespace dart

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));
 }
 
 //==============================================================================

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:

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<SimpleFrame> clone(Frame* _refFrame = Frame::World()) const;
+  std::shared_ptr<SimpleFrame> 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

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;
 

dart/dynamics/SoftMeshShape.cpp

@@ -91,7 +91,7 @@ void SoftMeshShape::_buildMesh()
   int nFaces    = mSoftBodyNode->getNumFaces();
 
   // Create new aiMesh
-  mAssimpMesh = std::unique_ptr<aiMesh>(new aiMesh());
+  mAssimpMesh = common::make_unique<aiMesh>();
 
   // Set vertices and normals
   mAssimpMesh->mNumVertices = nVertices;

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"
@@ -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());
@@ -152,4 +153,4 @@ using BodyNodeCompositeBase = common::EmbedStateAndPropertiesOnTopOf<
 } // namespace dynamics
 } // namespace dart
 
-#endif // DART_DYNAMICS_DETAIL_BODYNODEPROPERTIES_H_
+#endif // DART_DYNAMICS_DETAIL_BODYNODEASPECT_H_