Polishing up for DART 5.0

LICENSE

@@ -1,10 +1,10 @@
-Copyright (c) 2011, Georgia Tech Research Corporation
+Copyright (c) 2011-2015, Georgia Tech Research Corporation
 All rights reserved.
 
 Georgia Tech Graphics Lab and Humanoid Robotics Lab
-Directed by:
-    - Prof. C. Karen Liu <karenliu@cc.gatech.edu>
-    - Prof. Mike Stilman <mstilman@cc.gatech.edu>
+
+Directed by Prof. C. Karen Liu and Prof. Mike Stilman
+<karenliu@cc.gatech.edu> <mstilman@cc.gatech.edu>
 
 This file is provided under the following "BSD-style" License:
   Redistribution and use in source and binary forms, with or

apps/addDeleteSkels/Main.cpp

@@ -46,7 +46,7 @@ int main(int argc, char* argv[]) {
   dart::simulation::WorldPtr myWorld
       = dart::utils::SkelParser::readWorld(
           DART_DATA_PATH"/skel/ground.skel");
-  assert(myWorld != NULL);
+  assert(myWorld != nullptr);
   Eigen::Vector3d gravity(0.0, -9.81, 0.0);
   myWorld->setGravity(gravity);
 

apps/addDeleteSkels/MyWindow.h

@@ -60,10 +60,6 @@ class MyWindow : public dart::gui::SimWindow {
       const Eigen::Vector3d& _position = Eigen::Vector3d(0.0, 1.0, 0.0),
       const Eigen::Vector3d& _size     = Eigen::Vector3d(0.1, 0.1, 0.1),
       double _mass = 0.1);
-
-
-  /// \brief Number of frames for applying external force
-  int mImpulseDuration;
 };
 
 #endif  // APPS_ADDDELETESKELS_MYWINDOW_H_

apps/atlasSimbicon/Controller.cpp

@@ -53,13 +53,13 @@ Controller::Controller(SkeletonPtr _atlasRobot,
                        ConstraintSolver* _collisionSolver)
   : mAtlasRobot(_atlasRobot),
     mConstratinSolver(_collisionSolver),
-    mCurrentStateMachine(NULL),
+    mCurrentStateMachine(nullptr),
     mPelvisHarnessOn(false),
     mLeftFootHarnessOn(false),
     mRightFootHarnessOn(false),
-    mWeldJointConstraintPelvis(NULL),
-    mWeldJointConstraintLeftFoot(NULL),
-    mWeldJointConstraintRightFoot(NULL)
+    mWeldJointConstraintPelvis(nullptr),
+    mWeldJointConstraintLeftFoot(nullptr),
+    mWeldJointConstraintRightFoot(nullptr)
 {
   _buildStateMachines();
   _setJointDamping();
@@ -131,16 +131,15 @@ void Controller::changeStateMachine(const string& _name, double _currentTime)
   // _state should be in mStates
   StateMachine* stateMachine = _findStateMachine(_name);
 
-  assert(stateMachine != NULL && "Invaild state machine.");
+  assert(stateMachine != nullptr && "Invaild state machine.");
 
   changeStateMachine(stateMachine, _currentTime);
 }
 
 //==============================================================================
 void Controller::changeStateMachine(size_t _idx, double _currentTime)
 {
-  assert(0 <= _idx && _idx <= mStateMachines.size()
-         && "Invalid index of StateMachine.");
+  assert(_idx <= mStateMachines.size() && "Invalid index of StateMachine.");
 
   changeStateMachine(mStateMachines[_idx], _currentTime);
 }
@@ -211,7 +210,7 @@ void Controller::printDebugInfo() const
               << joint->getName()
               << " (" << joint->getNumDofs() << ")"
               << std::endl;
-    if (parentBody != NULL)
+    if (parentBody != nullptr)
     {
       std::cout << "    Parent body: " << parentBody->getName() << std::endl;
     }
@@ -300,7 +299,6 @@ void Controller::unharnessRightFoot()
 void Controller::resetRobot()
 {
   mAtlasRobot->setState(mInitialState);
-  mAtlasRobot->computeForwardKinematics(true, true, false);
 
   dtmsg << "Robot is reset." << std::endl;
 }
@@ -828,9 +826,9 @@ BodyNode* Controller::_getRightFoot() const
 }
 
 //==============================================================================
-bool Controller::_containStateMachine(StateMachine* _stateMachine)
+bool Controller::_containStateMachine(const StateMachine* _stateMachine) const
 {
-  for (vector<StateMachine*>::iterator it = mStateMachines.begin();
+  for (vector<StateMachine*>::const_iterator it = mStateMachines.begin();
        it != mStateMachines.end(); ++it)
   {
     if (*it == _stateMachine)
@@ -841,17 +839,17 @@ bool Controller::_containStateMachine(StateMachine* _stateMachine)
 }
 
 //==============================================================================
-bool Controller::_containStateMachine(const string& _name)
+bool Controller::_containStateMachine(const string& _name) const
 {
   return _containStateMachine(_findStateMachine(_name));
 }
 
 //==============================================================================
-StateMachine* Controller::_findStateMachine(const string& _name)
+StateMachine* Controller::_findStateMachine(const string& _name) const
 {
-  StateMachine* stateMachine = NULL;
+  StateMachine* stateMachine = nullptr;
 
-  for (vector<StateMachine*>::iterator it = mStateMachines.begin();
+  for (vector<StateMachine*>::const_iterator it = mStateMachines.begin();
        it != mStateMachines.end(); ++it)
   {
     if ((*it)->getName() == _name)

apps/atlasSimbicon/Controller.h

@@ -126,14 +126,14 @@ class Controller
 
 private:
   /// \brief Check if this controller contains _stateMachine
-  bool _containStateMachine(StateMachine* _stateMachine);
+  bool _containStateMachine(const StateMachine* _stateMachine) const;
 
   /// \brief Check if this controller contains a state machine whose name is
   ///        _name
-  bool _containStateMachine(const std::string& _name);
+  bool _containStateMachine(const std::string& _name) const;
 
   /// \brief Find a state machine whose name is _name
-  StateMachine* _findStateMachine(const std::string& _name);
+  StateMachine* _findStateMachine(const std::string& _name) const;
 
   /// \brief Build state machines
   void _buildStateMachines();

apps/atlasSimbicon/Humanoid.cpp

@@ -52,12 +52,12 @@ using namespace dart::constraint;
 
 //==============================================================================
 Humanoid::Humanoid(Skeleton* _skeleton)
-  : mSkeleton(NULL),
-    mPelvis(NULL),
-    mLeftThigh(NULL),
-    mRightThigh(NULL),
-    mLeftFoot(NULL),
-    mRightFoot(NULL)
+  : mSkeleton(nullptr),
+    mPelvis(nullptr),
+    mLeftThigh(nullptr),
+    mRightThigh(nullptr),
+    mLeftFoot(nullptr),
+    mRightFoot(nullptr)
 {
 
 }
@@ -120,9 +120,9 @@ AtlasRobot::~AtlasRobot()
   mLeftThigh  = mSkeleton->getBodyNode("l_uleg");
   mRightThigh = mSkeleton->getBodyNode("r_uleg");
 
-  assert(mPelvis     != NULL);
-  assert(mLeftFoot   != NULL);
-  assert(mRightFoot  != NULL);
-  assert(mLeftThigh  != NULL);
-  assert(mRightThigh != NULL);
+  assert(mPelvis     != nullptr);
+  assert(mLeftFoot   != nullptr);
+  assert(mRightFoot  != nullptr);
+  assert(mLeftThigh  != nullptr);
+  assert(mRightThigh != nullptr);
 }

apps/atlasSimbicon/Main.cpp

@@ -70,8 +70,6 @@ int main(int argc, char* argv[])
   q[0] = -0.5 * DART_PI;
   atlas->setPositions(q);
 
-  atlas->computeForwardKinematics(true, true, false);
-
   // Set gravity of the world
   myWorld->setGravity(Vector3d(0.0, -9.81, 0.0));
 

apps/atlasSimbicon/State.cpp

@@ -127,14 +127,14 @@ State::State(SkeletonPtr _skeleton, const std::string& _name)
   mRightFoot  = mSkeleton->getBodyNode("r_foot");
   mLeftThigh  = mSkeleton->getBodyNode("l_uleg");
   mRightThigh = mSkeleton->getBodyNode("r_uleg");
-  mStanceFoot = NULL;
+  mStanceFoot = nullptr;
 
-  assert(mPelvis     != NULL);
-  assert(mLeftFoot   != NULL);
-  assert(mRightFoot  != NULL);
-  assert(mLeftThigh  != NULL);
-  assert(mRightThigh != NULL);
-//  assert(mStanceFoot != NULL);
+  assert(mPelvis     != nullptr);
+  assert(mLeftFoot   != nullptr);
+  assert(mRightFoot  != nullptr);
+  assert(mLeftThigh  != nullptr);
+  assert(mRightThigh != nullptr);
+//  assert(mStanceFoot != nullptr);
 }
 
 //==============================================================================
@@ -164,7 +164,7 @@ void State::setNextState(State* _nextState)
 //==============================================================================
 void State::setTerminalCondition(TerminalCondition* _condition)
 {
-  assert(_condition != NULL);
+  assert(_condition != nullptr);
 
   mTerminalCondition = _condition;
 }
@@ -180,7 +180,7 @@ void State::begin(double _currentTime)
 //==============================================================================
 void State::computeControlForce(double _timestep)
 {
-  assert(mNextState != NULL && "Next state should be set.");
+  assert(mNextState != nullptr && "Next state should be set.");
 
   int dof = mSkeleton->getNumDofs();
   VectorXd q = mSkeleton->getPositions(mDofMapping);
@@ -244,7 +244,7 @@ void State::computeControlForce(double _timestep)
 //==============================================================================
 bool State::isTerminalConditionSatisfied() const
 {
-  assert(mTerminalCondition != NULL && "Invalid terminal condition.");
+  assert(mTerminalCondition != nullptr && "Invalid terminal condition.");
 
   return mTerminalCondition->isSatisfied();
 }
@@ -273,17 +273,16 @@ Eigen::Isometry3d State::getCOMFrame() const
   Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
 
   // Y-axis
-  Eigen::Vector3d yAxis = Eigen::Vector3d::UnitY();
+  const Eigen::Vector3d yAxis = Eigen::Vector3d::UnitY();
 
   // X-axis
-  Eigen::Vector3d xAxis = mPelvis->getTransform().linear().col(0);
   Eigen::Vector3d pelvisXAxis = mPelvis->getTransform().linear().col(0);
-  double mag = yAxis.dot(pelvisXAxis);
+  const double mag = yAxis.dot(pelvisXAxis);
   pelvisXAxis -= mag * yAxis;
-  xAxis = pelvisXAxis.normalized();
+  const Eigen::Vector3d xAxis = pelvisXAxis.normalized();
 
   // Z-axis
-  Eigen::Vector3d zAxis = xAxis.cross(yAxis);
+  const Eigen::Vector3d zAxis = xAxis.cross(yAxis);
 
   T.translation() = getCOM();
 
@@ -333,7 +332,7 @@ double State::getCoronalCOMVelocity()
 //==============================================================================
 Eigen::Vector3d State::getStanceAnklePosition() const
 {
-  if (mStanceFoot == NULL)
+  if (mStanceFoot == nullptr)
     return getCOM();
   else
     return _getJointPosition(mStanceFoot);
@@ -590,7 +589,7 @@ void State::setDesiredJointPosition(const string& _jointName, double _val)
   // TODO(JS)
   NOT_YET(State::setDesiredJointPosition());
 
-  assert(mSkeleton->getJoint(_jointName) != NULL);
+  assert(mSkeleton->getJoint(_jointName) != nullptr);
 
   mDesiredJointPositions[mJointMap[_jointName]] = _val;
 }
@@ -610,7 +609,7 @@ double State::getDesiredJointPosition(const string& _jointName) const
   // TODO(JS)
   NOT_YET(State::getDesiredJointPosition());
 
-  assert(mSkeleton->getJoint(_jointName) != NULL);
+  assert(mSkeleton->getJoint(_jointName) != nullptr);
 
   return mDesiredJointPositions[mJointMap.at(_jointName)];
 }
@@ -668,7 +667,7 @@ double State::getProportionalGain(const string& _jointName) const
   // TODO(JS)
   NOT_YET(State::getProportionalGain());
 
-  assert(mSkeleton->getJoint(_jointName) != NULL);
+  assert(mSkeleton->getJoint(_jointName) != nullptr);
 
   return mKp[mJointMap.at(_jointName)];
 }

apps/atlasSimbicon/State.h

@@ -60,9 +60,6 @@ class BodyNode;
 class Joint;
 class Skeleton;
 }  // namespace dynamics
-namespace constraint {
-class OldConstraintDynamics;
-}  // namespace constraint
 }  // namespace dart
 
 class TerminalCondition;

apps/atlasSimbicon/StateMachine.cpp

@@ -51,7 +51,7 @@ using namespace dart::constraint;
 //==============================================================================
 StateMachine::StateMachine(const std::string& _name)
   : mName(_name),
-    mCurrentState(NULL),
+    mCurrentState(nullptr),
     mBeginTime(0.0),
     mEndTime(0.0),
     mFrame(0),
@@ -85,7 +85,7 @@ const std::string& StateMachine::getName() const
 //==============================================================================
 void StateMachine::addState(State* _state)
 {
-  assert(_state != NULL && "Invalid state");
+  assert(_state != nullptr && "Invalid state");
   assert(!_containState(_state) && "_state shouldn't be in mStates");
 
   mStates.push_back(_state);
@@ -94,7 +94,7 @@ void StateMachine::addState(State* _state)
 //==============================================================================
 void StateMachine::setInitialState(State* _state)
 {
-  assert(_state != NULL);
+  assert(_state != nullptr);
   assert(_containState(_state));
 
   mCurrentState = _state;
@@ -113,7 +113,7 @@ void StateMachine::begin(double _currentTime)
 //==============================================================================
 void StateMachine::computeControlForce(double _dt)
 {
-  assert(mCurrentState != NULL && "Invaild current state.");
+  assert(mCurrentState != nullptr && "Invaild current state.");
 
   // Check transition is needed from current state
   if (mCurrentState->isTerminalConditionSatisfied())
@@ -172,23 +172,23 @@ void StateMachine::transiteTo(string& _stateName, double _currentTime)
   // _state should be in mStates
   State* state = _findState(_stateName);
 
-  assert(state != NULL && "Invaild state.");
+  assert(state != nullptr && "Invaild state.");
 
   transiteTo(state, _currentTime);
 }
 
 //==============================================================================
 void StateMachine::transiteTo(size_t _idx, double _currentTime)
 {
-  assert(0 <= _idx && _idx <= mStates.size() && "Invalid index of State.");
+  assert(_idx <= mStates.size() && "Invalid index of State.");
 
   transiteTo(mStates[_idx], _currentTime);
 }
 
 //==============================================================================
-bool StateMachine::_containState(State* _state)
+bool StateMachine::_containState(const State* _state) const
 {
-  for (vector<State*>::iterator it = mStates.begin();
+  for (vector<State*>::const_iterator it = mStates.begin();
        it != mStates.end(); ++it)
   {
     if (*it == _state)
@@ -199,17 +199,17 @@ bool StateMachine::_containState(State* _state)
 }
 
 //==============================================================================
-bool StateMachine::_containState(const string& _name)
+bool StateMachine::_containState(const string& _name) const
 {
   return _containState(_findState(_name));
 }
 
 //==============================================================================
-State* StateMachine::_findState(const string& _name)
+State* StateMachine::_findState(const string& _name) const
 {
-  State* state = NULL;
+  State* state = nullptr;
 
-  for (vector<State*>::iterator it = mStates.begin();
+  for (vector<State*>::const_iterator it = mStates.begin();
        it != mStates.end(); ++it)
   {
     if ((*it)->getName() == _name)