Merge pull request #449 from dartsim/grey/initial_pos_vel

Added initial position and initial velocity properties

dart/dynamics/DegreeOfFreedom.cpp

@@ -120,12 +120,6 @@ double DegreeOfFreedom::getPosition() const
   return mJoint->getPosition(mIndexInJoint);
 }
 
-//==============================================================================
-void DegreeOfFreedom::resetPosition()
-{
-  setPosition(0.0);
-}
-
 //==============================================================================
 void DegreeOfFreedom::setPositionLimits(double _lowerLimit, double _upperLimit)
 {
@@ -183,6 +177,24 @@ bool DegreeOfFreedom::isCyclic() const
   return mJoint->isCyclic(mIndexInJoint);
 }
 
+//==============================================================================
+void DegreeOfFreedom::resetPosition()
+{
+  mJoint->resetPosition(mIndexInJoint);
+}
+
+//==============================================================================
+void DegreeOfFreedom::setInitialPosition(double _initial)
+{
+  mJoint->setInitialPosition(mIndexInJoint, _initial);
+}
+
+//==============================================================================
+double DegreeOfFreedom::getInitialPosition() const
+{
+  return mJoint->getInitialPosition(mIndexInJoint);
+}
+
 //==============================================================================
 void DegreeOfFreedom::setVelocity(double _velocity)
 {
@@ -198,7 +210,7 @@ double DegreeOfFreedom::getVelocity() const
 //==============================================================================
 void DegreeOfFreedom::resetVelocity()
 {
-  setVelocity(0.0);
+  mJoint->resetVelocity(mIndexInJoint);
 }
 
 //==============================================================================
@@ -245,6 +257,18 @@ double DegreeOfFreedom::getVelocityUpperLimit() const
   return mJoint->getVelocityUpperLimit(mIndexInJoint);
 }
 
+//==============================================================================
+void DegreeOfFreedom::setInitialVelocity(double _initial)
+{
+  mJoint->setInitialVelocity(mIndexInJoint, _initial);
+}
+
+//==============================================================================
+double DegreeOfFreedom::getInitialVelocity() const
+{
+  return mJoint->getInitialVelocity(mIndexInJoint);
+}
+
 //==============================================================================
 void DegreeOfFreedom::setAcceleration(double _acceleration)
 {

dart/dynamics/DegreeOfFreedom.h

@@ -135,9 +135,6 @@ class DegreeOfFreedom : public virtual common::Subject
   /// Get the position of this DegreeOfFreedom
   double getPosition() const;
 
-  /// Set the position of this DegreeOfFreedom to zero
-  void resetPosition();
-
   /// Set the position limits of this DegreeOfFreedom
   void setPositionLimits(double _lowerLimit, double _upperLimit);
 
@@ -168,6 +165,15 @@ class DegreeOfFreedom : public virtual common::Subject
   /// Get whether the position of this DegreeOfFreedom has limits.
   bool hasPositionLimit() const;
 
+  /// Set the position of this DegreeOfFreedom to zero
+  void resetPosition();
+
+  /// Change the position that resetPosition() will give to this DegreeOfFreedom
+  void setInitialPosition(double _initial);
+
+  /// Get the position that resetPosition() will give to this DegreeOfFreedom
+  double getInitialPosition() const;
+
   /// \}
 
   //----------------------------------------------------------------------------
@@ -180,9 +186,6 @@ class DegreeOfFreedom : public virtual common::Subject
   /// Get the velocity of this DegreeOfFreedom
   double getVelocity() const;
 
-  /// Set the velocity of this DegreeOfFreedom to zero
-  void resetVelocity();
-
   /// Set the velocity limits of this DegreeOfFreedom
   void setVelocityLimits(double _lowerLimit, double _upperLimit);
 
@@ -204,6 +207,15 @@ class DegreeOfFreedom : public virtual common::Subject
   /// Get the upper Velocity limit of this DegreeOfFreedom
   double getVelocityUpperLimit() const;
 
+  /// Set the velocity of this DegreeOfFreedom to zero
+  void resetVelocity();
+
+  /// Change the velocity that resetVelocity() will give to this DegreeOfFreedom
+  void setInitialVelocity(double _initial);
+
+  /// Get the velocity that resetVelocity() will give to this DegreeOfFreedom
+  double getInitialVelocity() const;
+
   /// \}
 
   //----------------------------------------------------------------------------

dart/dynamics/Joint.cpp

@@ -64,7 +64,7 @@ Joint::Properties::Properties(const std::string& _name,
     mIsPositionLimited(_isPositionLimited),
     mActuatorType(_actuatorType)
 {
-
+  // Do nothing
 }
 
 //==============================================================================
@@ -318,6 +318,56 @@ size_t Joint::getTreeIndex() const
   return mChildBodyNode->getTreeIndex();
 }
 
+//==============================================================================
+bool Joint::checkSanity(bool _printWarnings) const
+{
+  bool sane = true;
+  for(size_t i=0; i < getNumDofs(); ++i)
+  {
+    if(getInitialPosition(i) < getPositionLowerLimit(i)
+       || getPositionUpperLimit(i) < getInitialPosition(i))
+    {
+      if(_printWarnings)
+      {
+        dtwarn << "[Joint::checkSanity] Initial position of index " << i << " ["
+               << getDofName(i) << "] in Joint [" << getName() << "] is "
+               << "outside of its position limits\n"
+               << " -- Initial Position: " << getInitialPosition(i) << "\n"
+               << " -- Limits: [" << getPositionLowerLimit(i) << ", "
+               << getPositionUpperLimit(i) << "]\n";
+      }
+      else
+      {
+        return false;
+      }
+
+      sane = false;
+    }
+
+    if(getInitialVelocity(i) < getVelocityLowerLimit(i)
+       || getVelocityUpperLimit(i) < getInitialVelocity(i))
+    {
+      if(_printWarnings)
+      {
+        dtwarn << "[Joint::checkSanity] Initial velocity of index " << i << " ["
+               << getDofName(i) << "] is Joint [" << getName() << "] is "
+               << "outside of its velocity limits\n"
+               << " -- Initial Velocity: " << getInitialVelocity(i) << "\n"
+               << " -- Limits: [" << getVelocityLowerLimit(i) << ", "
+               << getVelocityUpperLimit(i) << "]\n";
+      }
+      else
+      {
+        return false;
+      }
+
+      sane = false;
+    }
+  }
+
+  return sane;
+}
+
 //==============================================================================
 void Joint::setTransformFromParentBodyNode(const Eigen::Isometry3d& _T)
 {

dart/dynamics/Joint.h

@@ -339,9 +339,6 @@ class Joint : public virtual common::Subject
   /// Get the positions of all generalized coordinates in this Joint
   virtual Eigen::VectorXd getPositions() const = 0;
 
-  /// Set the positions of all generalized coordinates in this Joint to zero
-  virtual void resetPositions() = 0;
-
   /// Set lower limit for position
   virtual void setPositionLowerLimit(size_t _index, double _position) = 0;
 
@@ -364,6 +361,27 @@ class Joint : public virtual common::Subject
   /// Get whether the position of a generalized coordinate has limits.
   virtual bool hasPositionLimit(size_t _index) const = 0;
 
+  /// Set the position of this generalized coordinate to its initial position
+  virtual void resetPosition(size_t _index) = 0;
+
+  /// Set the positions of all generalized coordinates in this Joint to their
+  /// initial positions
+  virtual void resetPositions() = 0;
+
+  /// Change the position that resetPositions() will give to this coordinate
+  virtual void setInitialPosition(size_t _index, double _initial) = 0;
+
+  /// Get the position that resetPosition() will give to this coordinate
+  virtual double getInitialPosition(size_t _index) const = 0;
+
+  /// Change the positions that resetPositions() will give to this Joint's
+  /// coordinates
+  virtual void setInitialPositions(const Eigen::VectorXd& _initial) = 0;
+
+  /// Get the positions that resetPositions() will give to this Joint's
+  /// coordinates
+  virtual Eigen::VectorXd getInitialPositions() const = 0;
+
   /// \}
 
   //----------------------------------------------------------------------------
@@ -382,9 +400,6 @@ class Joint : public virtual common::Subject
   /// Get the velocities of all generalized coordinates in this Joint
   virtual Eigen::VectorXd getVelocities() const = 0;
 
-  /// Set the velocities of all generalized coordinates in this Joint to zero
-  virtual void resetVelocities() = 0;
-
   /// Set lower limit for velocity
   virtual void setVelocityLowerLimit(size_t _index, double _velocity) = 0;
 
@@ -397,6 +412,28 @@ class Joint : public virtual common::Subject
   /// Get upper limit for velocity
   virtual double getVelocityUpperLimit(size_t _index) const = 0;
 
+  /// Set the velocity of a generalized coordinate in this Joint to its initial
+  /// velocity
+  virtual void resetVelocity(size_t _index) = 0;
+
+  /// Set the velocities of all generalized coordinates in this Joint to their
+  /// initial velocities
+  virtual void resetVelocities() = 0;
+
+  /// Change the velocity that resetVelocity() will give to this coordinate
+  virtual void setInitialVelocity(size_t _index, double _initial) = 0;
+
+  /// Get the velocity that resetVelocity() will give to this coordinate
+  virtual double getInitialVelocity(size_t _index) const = 0;
+
+  /// Change the velocities that resetVelocities() will give to this Joint's
+  /// coordinates
+  virtual void setInitialVelocities(const Eigen::VectorXd& _initial) = 0;
+
+  /// Get the velocities that resetVelocities() will give to this Joint's
+  /// coordinates
+  virtual Eigen::VectorXd getInitialVelocities() const = 0;
+
   /// \}
 
   //----------------------------------------------------------------------------
@@ -465,6 +502,15 @@ class Joint : public virtual common::Subject
 
   /// \}
 
+  //----------------------------------------------------------------------------
+  /// \{ \name Sanity Check
+  //----------------------------------------------------------------------------
+
+  /// Returns false if the initial position or initial velocity are outside of
+  /// limits
+  // TODO: Consider extending this to a more comprehensive sanity check
+  bool checkSanity(bool _printWarnings = true) const;
+
   //----------------------------------------------------------------------------
   /// \{ \name Velocity change
   //----------------------------------------------------------------------------

dart/dynamics/MultiDofJoint.h

@@ -71,12 +71,18 @@ class MultiDofJoint : public Joint
     /// Upper limit of position
     Vector mPositionUpperLimits;
 
+    /// Initial positions
+    Vector mInitialPositions;
+
     /// Min value allowed.
     Vector mVelocityLowerLimits;
 
     /// Max value allowed.
     Vector mVelocityUpperLimits;
 
+    /// Initial velocities
+    Vector mInitialVelocities;
+
     /// Min value allowed.
     Vector mAccelerationLowerLimits;
 
@@ -122,6 +128,9 @@ class MultiDofJoint : public Joint
       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
@@ -237,9 +246,6 @@ class MultiDofJoint : public Joint
   // Documentation inherited
   Eigen::VectorXd getPositions() const override;
 
-  // Documentation inherited
-  void resetPositions() override;
-
   // Documentation inherited
   void setPositionLowerLimit(size_t _index, double _position) override;
 
@@ -255,6 +261,24 @@ class MultiDofJoint : public Joint
   // Documentation inherited
   virtual bool hasPositionLimit(size_t _index) const override;
 
+  // Documentation inherited
+  void resetPosition(size_t _index) override;
+
+  // Documentation inherited
+  void resetPositions() override;
+
+  // Documentation inherited
+  void setInitialPosition(size_t _index, double _initial) override;
+
+  // Documentation inherited
+  double getInitialPosition(size_t _index) const override;
+
+  // Documentation inherited
+  void setInitialPositions(const Eigen::VectorXd& _initial) override;
+
+  // Documentation inherited
+  Eigen::VectorXd getInitialPositions() const override;
+
   //----------------------------------------------------------------------------
   // Velocity
   //----------------------------------------------------------------------------
@@ -271,9 +295,6 @@ class MultiDofJoint : public Joint
   // Documentation inherited
   Eigen::VectorXd getVelocities() const override;
 
-  // Documentation inherited
-  void resetVelocities() override;
-
   // Documentation inherited
   void setVelocityLowerLimit(size_t _index, double _velocity) override;
 
@@ -286,6 +307,24 @@ class MultiDofJoint : public Joint
   // Documentation inherited
   double getVelocityUpperLimit(size_t _index) const override;
 
+  // Documentation inherited
+  void resetVelocity(size_t _index) override;
+
+  // Documentation inherited
+  void resetVelocities() override;
+
+  // Documentation inherited
+  void setInitialVelocity(size_t _index, double _initial) override;
+
+  // Documentation inherited
+  double getInitialVelocity(size_t _index) const override;
+
+  // Documentation inherited
+  void setInitialVelocities(const Eigen::VectorXd& _initial) override;
+
+  // Documentation inherited
+  Eigen::VectorXd getInitialVelocities() const override;
+
   //----------------------------------------------------------------------------
   // Acceleration
   //----------------------------------------------------------------------------