Eradicating memory leaks & making classes copy-safe and clonable

apps/addDeleteSkels/Main.cpp

@@ -43,7 +43,7 @@
 
 int main(int argc, char* argv[]) {
   // create and initialize the world
-  dart::simulation::World *myWorld
+  dart::simulation::WorldPtr myWorld
       = dart::utils::SkelParser::readWorld(
           DART_DATA_PATH"/skel/ground.skel");
   assert(myWorld != NULL);

apps/addDeleteSkels/MyWindow.cpp

@@ -86,25 +86,25 @@ void MyWindow::keyboard(unsigned char _key, int _x, int _y) {
 
 void MyWindow::spawnCube(const Eigen::Vector3d& _position,
                          const Eigen::Vector3d& _size,
-                         double _mass) {
-  dart::dynamics::Skeleton*  newCubeSkeleton =
-      new dart::dynamics::Skeleton();
-  dart::dynamics::BodyNode*  newBodyNode     =
-      new dart::dynamics::BodyNode("cube_link");
-  dart::dynamics::FreeJoint* newFreeJoint    =
-      new dart::dynamics::FreeJoint("cube_joint");
-  dart::dynamics::BoxShape*  newBoxShape     =
-      new dart::dynamics::BoxShape(_size);
+                         double _mass)
+{
+  dart::dynamics::SkeletonPtr newCubeSkeleton =
+      dart::dynamics::Skeleton::create();
+
+  dart::dynamics::BodyNode::Properties body;
+  body.mName = "cube_link";
+  body.mInertia.setMass(_mass);
+  dart::dynamics::ShapePtr newBoxShape(new dart::dynamics::BoxShape(_size));
+  body.mVizShapes.push_back(newBoxShape);
+  body.mColShapes.push_back(newBoxShape);
+  newBoxShape->setColor(dart::math::randomVector<3>(0.0, 1.0));
+
+  dart::dynamics::FreeJoint::Properties joint;
+  joint.mName = "cube_joint";
+  joint.mT_ParentBodyToJoint = Eigen::Translation3d(_position);
+
+  newCubeSkeleton->createJointAndBodyNodePair<dart::dynamics::FreeJoint>(
+        nullptr, joint, body);
 
-  newBodyNode->addVisualizationShape(newBoxShape);
-  newBodyNode->addCollisionShape(newBoxShape);
-  newBodyNode->setMass(_mass);
-  newBodyNode->setParentJoint(newFreeJoint);
-  newFreeJoint->setTransformFromParentBodyNode(
-        Eigen::Isometry3d(Eigen::Translation3d(_position)));
-  newBoxShape->setColor(Eigen::Vector3d(dart::math::random(0.0, 1.0),
-                                        dart::math::random(0.0, 1.0),
-                                        dart::math::random(0.0, 1.0)));
-  newCubeSkeleton->addBodyNode(newBodyNode);
   mWorld->addSkeleton(newCubeSkeleton);
 }

apps/atlasSimbicon/Controller.cpp

@@ -49,7 +49,7 @@ using namespace constraint;
 using namespace dynamics;
 
 //==============================================================================
-Controller::Controller(Skeleton* _atlasRobot,
+Controller::Controller(SkeletonPtr _atlasRobot,
                        ConstraintSolver* _collisionSolver)
   : mAtlasRobot(_atlasRobot),
     mConstratinSolver(_collisionSolver),
@@ -89,7 +89,7 @@ void Controller::update(double _currentTime)
 }
 
 //==============================================================================
-Skeleton* Controller::getAtlasRobot()
+SkeletonPtr Controller::getAtlasRobot()
 {
   return mAtlasRobot;
 }

apps/atlasSimbicon/Controller.h

@@ -50,7 +50,7 @@ class Controller
 {
 public:
   /// \brief Constructor
-  Controller(dart::dynamics::Skeleton* _atlasRobot,
+  Controller(dart::dynamics::SkeletonPtr _atlasRobot,
              dart::constraint::ConstraintSolver* _collisionSolver);
 
   /// \brief Destructor
@@ -61,7 +61,7 @@ class Controller
   virtual void update(double _currentTime);
 
   /// \brief
-  dart::dynamics::Skeleton* getAtlasRobot();
+  dart::dynamics::SkeletonPtr getAtlasRobot();
 
   /// \brief Get current state machine
   StateMachine* getCurrentState();
@@ -104,7 +104,7 @@ class Controller
 
 protected:
   /// \brief Atlas robot skeleton
-  dart::dynamics::Skeleton* mAtlasRobot;
+  dart::dynamics::SkeletonPtr mAtlasRobot;
 
   /// \brief Conllision detector
   dart::constraint::ConstraintSolver* mConstratinSolver;

apps/atlasSimbicon/Main.cpp

@@ -51,15 +51,15 @@ using namespace dart::utils;
 int main(int argc, char* argv[])
 {
   // Create empty soft world
-  World* myWorld = new World;
+  WorldPtr myWorld(new World);
 
   // Load ground and Atlas robot and add them to the world
   DartLoader urdfLoader;
-  Skeleton* ground = urdfLoader.parseSkeleton(
+  SkeletonPtr ground = urdfLoader.parseSkeleton(
         DART_DATA_PATH"sdf/atlas/ground.urdf");
-//  Skeleton* atlas = SoftSdfParser::readSkeleton(
+//  SkeletonPtr atlas = SoftSdfParser::readSkeleton(
 //        DART_DATA_PATH"sdf/atlas/atlas_v3_no_head.sdf");
-  Skeleton* atlas
+  SkeletonPtr atlas
       = SoftSdfParser::readSkeleton(
           DART_DATA_PATH"sdf/atlas/atlas_v3_no_head_soft_feet.sdf");
   myWorld->addSkeleton(atlas);
@@ -69,6 +69,7 @@ int main(int argc, char* argv[])
   VectorXd q = atlas->getPositions();
   q[0] = -0.5 * DART_PI;
   atlas->setPositions(q);
+
   atlas->computeForwardKinematics(true, true, false);
 
   // Set gravity of the world

apps/atlasSimbicon/State.cpp

@@ -51,7 +51,44 @@ using namespace dart::dynamics;
 using namespace dart::constraint;
 
 //==============================================================================
-State::State(Skeleton* _skeleton, const std::string& _name)
+void createIndexing(std::vector<size_t>&)
+{
+  // Do nothing
+}
+
+//==============================================================================
+template <typename ...Args>
+void createIndexing(std::vector<size_t>& _indexing, size_t _name,
+                    Args... args)
+{
+  _indexing.push_back(_name);
+  createIndexing(_indexing, args...);
+}
+
+//==============================================================================
+static std::vector<size_t> createIndexing()
+{
+  // This app was made with dof indices hardcoded, but some internal DART
+  // development have changed the underlying indexing for the robot, so this
+  // map converts the old indexing into the new indexing as a temporary (or
+  // maybe permanent) workaround.
+
+  std::vector<size_t> indexing;
+  //                        0   1   2   3   4   5   6
+  createIndexing(indexing,  0,  1,  2,  3,  4,  5,  6,
+  //                        7   8   9  10  11  12  13
+                           14, 26,  7, 15, 27,  8, 16,
+  //                       14  15  16  17  18  19  20
+                           28,  9, 21, 17, 29, 10, 22,
+  //                       21  22  23  24  25  26  27
+                           18, 30, 11, 23, 19, 31, 12,
+  //                       28  29  30  31  32
+                           24, 13, 25, 20, 32);
+  return indexing;
+}
+
+//==============================================================================
+State::State(SkeletonPtr _skeleton, const std::string& _name)
   : mName(_name),
     mSkeleton(_skeleton),
     mNextState(this),
@@ -62,7 +99,8 @@ State::State(Skeleton* _skeleton, const std::string& _name)
     mDesiredGlobalSwingLegAngleOnSagital(0.0),
     mDesiredGlobalSwingLegAngleOnCoronal(0.0),
     mDesiredGlobalPelvisAngleOnSagital(0.0),
-    mDesiredGlobalPelvisAngleOnCoronal(0.0)
+    mDesiredGlobalPelvisAngleOnCoronal(0.0),
+    mDofMapping(createIndexing())
 {
   int dof = mSkeleton->getNumDofs();
 
@@ -145,8 +183,8 @@ void State::computeControlForce(double _timestep)
   assert(mNextState != NULL && "Next state should be set.");
 
   int dof = mSkeleton->getNumDofs();
-  VectorXd q = mSkeleton->getPositions();
-  VectorXd dq = mSkeleton->getVelocities();
+  VectorXd q = mSkeleton->getPositions(mDofMapping);
+  VectorXd dq = mSkeleton->getVelocities(mDofMapping);
 
   // Compute relative joint angles from desired global angles of the pelvis and
   // the swing leg
@@ -197,7 +235,7 @@ void State::computeControlForce(double _timestep)
   _updateTorqueForStanceLeg();
 
   // Apply control torque to the skeleton
-  mSkeleton->setForces(mTorque);
+  mSkeleton->setForces(mDofMapping, mTorque);
 
   mElapsedTime += _timestep;
   mFrame++;

apps/atlasSimbicon/State.h

@@ -73,7 +73,7 @@ class State
 {
 public:
   /// \brief Constructor
-  explicit State(dart::dynamics::Skeleton* _skeleton, const std::string& _name);
+  explicit State(dart::dynamics::SkeletonPtr _skeleton, const std::string& _name);
 
   /// \brief Destructor
   virtual ~State();
@@ -238,7 +238,7 @@ class State
   std::string mName;
 
   /// \brief Target skeleton for control
-  dart::dynamics::Skeleton* mSkeleton;
+  dart::dynamics::SkeletonPtr mSkeleton;
 
   /// \brief Next state. Default is myself.
   State* mNextState;
@@ -297,6 +297,10 @@ class State
   /// \brief Joint map
   std::map<const std::string, int> mJointMap;
 
+  /// \brief Maps the dofs from the indexing expected by the controller to the
+  /// Skeleton's actual indexing
+  std::vector<size_t> mDofMapping;
+
 private:
   /// \brief Build joint map
   void _buildJointMap();

apps/bipedStand/Controller.cpp

@@ -36,22 +36,71 @@
  */
 
 #include "apps/bipedStand/Controller.h"
+#include "dart/dynamics/Group.h"
 
-Controller::Controller(dart::dynamics::Skeleton* _skel,
+void createIndexing(std::vector<size_t>&)
+{
+  // Do nothing
+}
+
+template <typename ...Args>
+void createIndexing(std::vector<size_t>& _indexing, size_t _name,
+                    Args... args)
+{
+  _indexing.push_back(_name);
+  createIndexing(_indexing, args...);
+}
+
+static std::vector<size_t> createIndexing()
+{
+  // This app was made with dof indices hardcoded, but some internal DART
+  // development have changed the underlying indexing for the robot, so this
+  // map converts the old indexing into the new indexing as a temporary (or
+  // maybe permanent) workaround.
+
+  std::vector<size_t> indexing;
+  //                        0   1   2   3   4   5   6
+  createIndexing(indexing,  0,  1,  2,  3,  4,  5,  6,
+  //                        7   8   9  10  11  12  13
+                            7,  8, 13, 14, 15, 20, 21,
+  //                       14  15  16  17  18  19  20
+                            9, 16, 22, 10, 11, 17, 18,
+  //                       21  22  23  24  25  26  27
+                           23, 24, 25, 31, 12, 19, 26,
+  //                       28  29  30  31  32  33  34
+                           27, 28, 32, 33, 34, 29, 35,
+  //                       35  36
+                           30, 36);
+  return indexing;
+}
+
+static std::vector<dart::dynamics::DegreeOfFreedom*> getDofs(
+    const dart::dynamics::SkeletonPtr& _skel)
+{
+  std::vector<dart::dynamics::DegreeOfFreedom*> dofs;
+  const std::vector<size_t>& indexing = createIndexing();
+  dofs.reserve(indexing.size());
+
+  for(size_t index : indexing)
+    dofs.push_back(_skel->getDof(index));
+
+  return dofs;
+}
+
+Controller::Controller(dart::dynamics::SkeletonPtr _skel,
                        double _t) {
-  mSkel = _skel;
+  mSkel = dart::dynamics::Group::create("Group", getDofs(_skel));
+  mLeftHeel = _skel->getBodyNode("h_heel_left");
   mTimestep = _t;
   mFrame = 0;
   int nDof = mSkel->getNumDofs();
   mKp = Eigen::MatrixXd::Identity(nDof, nDof);
   mKd = Eigen::MatrixXd::Identity(nDof, nDof);
 
   mTorques.resize(nDof);
-  mDesiredDofs.resize(nDof);
-  for (int i = 0; i < nDof; i++) {
-    mTorques[i] = 0.0;
-    mDesiredDofs[i] = mSkel->getPosition(i);
-  }
+  mTorques.setZero();
+
+  resetDesiredDofs();
 
   // using SPD results in simple Kp coefficients
   for (int i = 0; i < 6; i++) {
@@ -81,8 +130,13 @@ void Controller::setDesiredDof(int _index, double _val) {
   mDesiredDofs[_index] = _val;
 }
 
-void Controller::computeTorques(const Eigen::VectorXd& _dof,
-                                const Eigen::VectorXd& _dofVel) {
+void Controller::resetDesiredDofs() {
+  mDesiredDofs = mSkel->getPositions();
+}
+
+void Controller::computeTorques() {
+  Eigen::VectorXd _dof = mSkel->getPositions();
+  Eigen::VectorXd _dofVel = mSkel->getVelocities();
   Eigen::VectorXd constrForces = mSkel->getConstraintForces();
 
   // SPD tracking
@@ -97,7 +151,7 @@ void Controller::computeTorques(const Eigen::VectorXd& _dof,
 
   // ankle strategy for sagital plane
   Eigen::Vector3d com = mSkel->getCOM();
-  Eigen::Vector3d cop = mSkel->getBodyNode("h_heel_left")->getTransform()
+  Eigen::Vector3d cop = mLeftHeel->getTransform()
                         * Eigen::Vector3d(0.05, 0, 0);
   Eigen::Vector2d diff(com[0] - cop[0], com[2] - cop[2]);
   double offset = com[0] - cop[0];
@@ -128,7 +182,7 @@ void Controller::computeTorques(const Eigen::VectorXd& _dof,
   mFrame++;
 }
 
-dart::dynamics::Skeleton*Controller::getSkel() {
+dart::dynamics::MetaSkeletonPtr Controller::getSkel() {
   return mSkel;
 }
 
@@ -143,4 +197,3 @@ Eigen::MatrixXd Controller::getKp() {
 Eigen::MatrixXd Controller::getKd() {
   return mKd;
 }
-

apps/bipedStand/Controller.h

@@ -46,22 +46,23 @@
 
 class Controller {
 public:
-  Controller(dart::dynamics::Skeleton* _skel,
+  Controller(dart::dynamics::SkeletonPtr _skel,
              double _t);
   virtual ~Controller();
 
   Eigen::VectorXd getTorques();
   double getTorque(int _index);
   void setDesiredDof(int _index, double _val);
-  void computeTorques(const Eigen::VectorXd& _dof,
-                      const Eigen::VectorXd& _dofVel);
-  dart::dynamics::Skeleton* getSkel();
+  void resetDesiredDofs();
+  void computeTorques();
+  dart::dynamics::MetaSkeletonPtr getSkel();
   Eigen::VectorXd getDesiredDofs();
   Eigen::MatrixXd getKp();
   Eigen::MatrixXd getKd();
 
 protected:
-  dart::dynamics::Skeleton* mSkel;
+  dart::dynamics::MetaSkeletonPtr mSkel;
+  dart::dynamics::BodyNodePtr mLeftHeel;
   Eigen::VectorXd mTorques;
   Eigen::VectorXd mDesiredDofs;
   Eigen::MatrixXd mKp;