Merge pull request #691 from dartsim/grey/todos

Small updates for 6.0
dartsim · Apr 24, 2016 · 346c1bd · 346c1bd
2 parents b91ca79 + 366e614
commit 346c1bd
Show file tree

Hide file tree

Showing 7 changed files with 29 additions and 64 deletions.
diff --git a/dart/dynamics/PointMass.cpp b/dart/dynamics/PointMass.cpp
@@ -722,23 +722,6 @@ Eigen::Vector3d PointMass::getWorldAcceleration() const
 void PointMass::init()
 {
   mDependentGenCoordIndices = mParentSoftBodyNode->getDependentGenCoordIndices();
-
-  // 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];
 }
 
 //==============================================================================

diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp
@@ -453,6 +453,7 @@ SkeletonPtr Skeleton::clone(const std::string& cloneName) const
 
   skelClone->setProperties(getAspectProperties());
   skelClone->setName(cloneName);
+  skelClone->setState(getState());
 
   return skelClone;
 }

diff --git a/dart/dynamics/Skeleton.h b/dart/dynamics/Skeleton.h
@@ -164,15 +164,9 @@ class Skeleton :
   Skeleton& operator=(const Skeleton& _other) = delete;
 
   /// Create an identical clone of this Skeleton.
-  ///
-  /// Note: the state of the Skeleton will NOT be cloned, only the structure and
-  /// properties will be [TODO(MXG): copy the state as well]
   SkeletonPtr clone() const;
 
   /// Create an identical clone of this Skeleton, except that it has a new name.
-  ///
-  /// Note: the state of the Skeleton will NOT be cloned, only the structure and
-  /// properties will be [TODO(MXG): copy the state as well]
   SkeletonPtr clone(const std::string& cloneName) const;
 
   /// \}

diff --git a/dart/dynamics/detail/MultiDofJointAspect.h b/dart/dynamics/detail/MultiDofJointAspect.h
@@ -146,8 +146,10 @@ struct MultiDofJointUniqueProperties
   MultiDofJointUniqueProperties(
       const Vector& _positionLowerLimits = Vector::Constant(-math::constantsd::inf()),
       const Vector& _positionUpperLimits = Vector::Constant( math::constantsd::inf()),
+      const Vector& _initialPositions = Vector::Constant(0.0),
       const Vector& _velocityLowerLimits = Vector::Constant(-math::constantsd::inf()),
       const Vector& _velocityUpperLimits = Vector::Constant( math::constantsd::inf()),
+      const Vector& _initialVelocities = Vector::Constant(0.0),
       const Vector& _accelerationLowerLimits = Vector::Constant(-math::constantsd::inf()),
       const Vector& _accelerationUpperLimits = Vector::Constant( math::constantsd::inf()),
       const Vector& _forceLowerLimits = Vector::Constant(-math::constantsd::inf()),
@@ -156,9 +158,6 @@ struct MultiDofJointUniqueProperties
       const Vector& _restPosition = Vector::Constant(0.0),
       const Vector& _dampingCoefficient = Vector::Constant(0.0),
       const Vector& _coulombFrictions = Vector::Constant(0.0));
-  // TODO(MXG): In version 6.0, we should add mInitialPositions and
-  // mInitialVelocities to the constructor arguments. For now we must wait in
-  // order to avoid breaking the API.
 
   /// Copy constructor
   // Note: we only need this because VS2013 lacks full support for std::array
@@ -225,8 +224,10 @@ template <size_t DOF>
 MultiDofJointUniqueProperties<DOF>::MultiDofJointUniqueProperties(
     const Vector& _positionLowerLimits,
     const Vector& _positionUpperLimits,
+    const Vector& _initialPositions,
     const Vector& _velocityLowerLimits,
     const Vector& _velocityUpperLimits,
+    const Vector& _initialVelocities,
     const Vector& _accelerationLowerLimits,
     const Vector& _accelerationUpperLimits,
     const Vector& _forceLowerLimits,
@@ -237,10 +238,10 @@ MultiDofJointUniqueProperties<DOF>::MultiDofJointUniqueProperties(
     const Vector& _coulombFrictions)
   : mPositionLowerLimits(_positionLowerLimits),
     mPositionUpperLimits(_positionUpperLimits),
-    mInitialPositions(Vector::Zero()),
+    mInitialPositions(_initialPositions),
     mVelocityLowerLimits(_velocityLowerLimits),
     mVelocityUpperLimits(_velocityUpperLimits),
-    mInitialVelocities(Vector::Zero()),
+    mInitialVelocities(_initialVelocities),
     mAccelerationLowerLimits(_accelerationLowerLimits),
     mAccelerationUpperLimits(_accelerationUpperLimits),
     mForceLowerLimits(_forceLowerLimits),

diff --git a/dart/dynamics/detail/SingleDofJointAspect.cpp b/dart/dynamics/detail/SingleDofJointAspect.cpp
@@ -60,8 +60,10 @@ SingleDofJointState::SingleDofJointState(
 SingleDofJointUniqueProperties::SingleDofJointUniqueProperties(
     double _positionLowerLimit,
     double _positionUpperLimit,
+    double _initialPosition,
     double _velocityLowerLimit,
     double _velocityUpperLimit,
+    double _initialVelocity,
     double _accelerationLowerLimit,
     double _accelerationUpperLimit,
     double _forceLowerLimit,
@@ -71,13 +73,13 @@ SingleDofJointUniqueProperties::SingleDofJointUniqueProperties(
     double _dampingCoefficient,
     double _coulombFriction,
     bool _preserveDofName,
-    std::string _dofName)
+    const std::string& _dofName)
   : mPositionLowerLimit(_positionLowerLimit),
     mPositionUpperLimit(_positionUpperLimit),
-    mInitialPosition(0.0),
+    mInitialPosition(_initialPosition),
     mVelocityLowerLimit(_velocityLowerLimit),
     mVelocityUpperLimit(_velocityUpperLimit),
-    mInitialVelocity(0.0),
+    mInitialVelocity(_initialVelocity),
     mAccelerationLowerLimit(_accelerationLowerLimit),
     mAccelerationUpperLimit(_accelerationUpperLimit),
     mForceLowerLimit(_forceLowerLimit),

diff --git a/dart/dynamics/detail/SingleDofJointAspect.h b/dart/dynamics/detail/SingleDofJointAspect.h
@@ -128,23 +128,23 @@ struct SingleDofJointUniqueProperties
   std::string mDofName;
 
   /// Constructor
-  SingleDofJointUniqueProperties(double _positionLowerLimit = -math::constantsd::inf(),
-                   double _positionUpperLimit =  math::constantsd::inf(),
-                   double _velocityLowerLimit = -math::constantsd::inf(),
-                   double _velocityUpperLimit =  math::constantsd::inf(),
-                   double _accelerationLowerLimit = -math::constantsd::inf(),
-                   double _accelerationUpperLimit =  math::constantsd::inf(),
-                   double _forceLowerLimit = -math::constantsd::inf(),
-                   double _forceUpperLimit =  math::constantsd::inf(),
-                   double _springStiffness = 0.0,
-                   double _restPosition = 0.0,
-                   double _dampingCoefficient = 0.0,
-                   double _coulombFriction = 0.0,
-                   bool _preserveDofName = false,
-                   std::string _dofName = "");
-  // TODO(MXG): In version 6.0, we should add mInitialPosition and
-  // mInitialVelocity to the constructor arguments. For now we must wait in
-  // order to avoid breaking the API
+  SingleDofJointUniqueProperties(
+      double _positionLowerLimit = -math::constantsd::inf(),
+      double _positionUpperLimit =  math::constantsd::inf(),
+      double _initialPosition = 0.0,
+      double _velocityLowerLimit = -math::constantsd::inf(),
+      double _velocityUpperLimit =  math::constantsd::inf(),
+      double _initialVelocity = 0.0,
+      double _accelerationLowerLimit = -math::constantsd::inf(),
+      double _accelerationUpperLimit =  math::constantsd::inf(),
+      double _forceLowerLimit = -math::constantsd::inf(),
+      double _forceUpperLimit =  math::constantsd::inf(),
+      double _springStiffness = 0.0,
+      double _restPosition = 0.0,
+      double _dampingCoefficient = 0.0,
+      double _coulombFriction = 0.0,
+      bool _preserveDofName = false,
+      const std::string& _dofName = "");
 
   virtual ~SingleDofJointUniqueProperties() = default;
 };

diff --git a/unittests/testWorld.cpp b/unittests/testWorld.cpp
@@ -198,22 +198,6 @@ TEST(World, Cloning)
     for(size_t j=1; j<5; ++j)
       clones.push_back(clones[j-1]->clone());
 
-    // Make sure all the Skeleton states match at the start
-    // TODO(MXG): This should be removed once state also gets copied over during a cloning
-    for(size_t j=1; j<clones.size(); ++j)
-    {
-      for(size_t k=0; k<original->getNumSkeletons(); ++k)
-      {
-        dart::dynamics::SkeletonPtr skel = original->getSkeleton(k);
-        dart::dynamics::SkeletonPtr clone = clones[j]->getSkeleton(k);
-
-        clone->setPositions(skel->getPositions());
-        clone->setVelocities(skel->getVelocities());
-        clone->setAccelerations(skel->getAccelerations());
-        clone->setForces(skel->getForces());
-      }
-    }
-
 #ifndef NDEBUG // Debug mode
     size_t numIterations = 3;
 #else