Updating the constraint namespace to C++11

apps/atlasSimbicon/Controller.cpp

@@ -231,7 +231,7 @@ void Controller::harnessPelvis()
     return;
 
   BodyNode* bd = mAtlasRobot->getBodyNode("pelvis");
-  mWeldJointConstraintPelvis = new WeldJointConstraint(bd);
+  mWeldJointConstraintPelvis = std::make_shared<WeldJointConstraint>(bd);
   mConstratinSolver->addConstraint(mWeldJointConstraintPelvis);
   mPelvisHarnessOn = true;
 
@@ -257,7 +257,7 @@ void Controller::harnessLeftFoot()
     return;
 
   BodyNode* bd = mAtlasRobot->getBodyNode("l_foot");
-  mWeldJointConstraintLeftFoot = new WeldJointConstraint(bd);
+  mWeldJointConstraintLeftFoot = std::make_shared<WeldJointConstraint>(bd);
   mLeftFootHarnessOn = true;
 
   dtmsg << "Left foot is harnessed." << std::endl;
@@ -282,7 +282,7 @@ void Controller::harnessRightFoot()
     return;
 
   BodyNode* bd = mAtlasRobot->getBodyNode("r_foot");
-  mWeldJointConstraintRightFoot = new WeldJointConstraint(bd);
+  mWeldJointConstraintRightFoot = std::make_shared<WeldJointConstraint>(bd);
   mRightFootHarnessOn = true;
 
   dtmsg << "Right foot is harnessed." << std::endl;

apps/atlasSimbicon/Controller.h

@@ -172,13 +172,13 @@ class Controller
   dart::dynamics::BodyNode* _getRightFoot() const;
 
   /// \brief Weld joint constraint for pelvis harnessing
-  dart::constraint::WeldJointConstraint* mWeldJointConstraintPelvis;
+  dart::constraint::WeldJointConstraintPtr mWeldJointConstraintPelvis;
 
   /// \brief Weld joint constraint for left foot harnessing
-  dart::constraint::WeldJointConstraint* mWeldJointConstraintLeftFoot;
+  dart::constraint::WeldJointConstraintPtr mWeldJointConstraintLeftFoot;
 
   /// \brief Weld joint constraint for right foot harnessing
-  dart::constraint::WeldJointConstraint* mWeldJointConstraintRightFoot;
+  dart::constraint::WeldJointConstraintPtr mWeldJointConstraintRightFoot;
 
   /// \brief Initial state of the robot
   Eigen::VectorXd mInitialState;

apps/jointConstraints/MyWindow.cpp

@@ -142,7 +142,7 @@ void MyWindow::keyboard(unsigned char key, int x, int y)
       if (mHarnessOn)
       {
         BodyNode* bd = mWorld->getSkeleton(1)->getBodyNode("h_pelvis");
-        mWeldJoint = new WeldJointConstraint(bd);
+        mWeldJoint = std::make_shared<WeldJointConstraint>(bd);
         mWorld->getConstraintSolver()->addConstraint(mWeldJoint);
       }
       else

apps/jointConstraints/MyWindow.h

@@ -72,7 +72,7 @@ class MyWindow : public dart::gui::SimWindow
 private:
   Eigen::Vector3d mForce;
   Controller* mController;
-  dart::constraint::WeldJointConstraint* mWeldJoint;
+  dart::constraint::WeldJointConstraintPtr mWeldJoint;
   int mImpulseDuration;
   bool mHarnessOn;
 

apps/rigidLoop/Main.cpp

@@ -74,7 +74,8 @@ int main(int argc, char* argv[])
   bd2->getVisualizationShape(0)->setColor(Eigen::Vector3d(1.0, 0.0, 0.0));
   Eigen::Vector3d offset(0.0, 0.025, 0.0);
   Eigen::Vector3d jointPos = bd1->getTransform() * offset;
-  BallJointConstraint *cl = new BallJointConstraint( bd1, bd2, jointPos);
+  BallJointConstraintPtr cl =
+      std::make_shared<BallJointConstraint>( bd1, bd2, jointPos);
   //WeldJointConstraint *cl = new WeldJointConstraint(bd1, bd2);
   myWorld->getConstraintSolver()->addConstraint(cl);
 

dart/common/SmartPointer.h

@@ -0,0 +1,52 @@
+/*
+ * Copyright (c) 2016, Georgia Tech Research Corporation
+ * All rights reserved.
+ *
+ * Author(s): Michael X. Grey <mxgrey@gatech.edu>
+ *
+ * Georgia Tech Graphics Lab and Humanoid Robotics Lab
+ *
+ * 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
+ *   without modification, are permitted provided that the following
+ *   conditions are met:
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+ *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
+ *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *   POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef DART_COMMON_SMARTPOINTER_H_
+#define DART_COMMON_SMARTPOINTER_H_
+
+#include <memory>
+
+// -- Standard shared/weak pointers --
+// Define a typedef for const and non-const version of shared_ptr and weak_ptr
+// for the class X
+#define DART_COMMON_MAKE_SHARED_WEAK( X )\
+  class X ;\
+  typedef std::shared_ptr< X >       X ## Ptr;\
+  typedef std::shared_ptr< const X > Const ## X ## Ptr;\
+  typedef std::weak_ptr< X >         Weak ## X ## Ptr;\
+  typedef std::weak_ptr< const X >   WeakConst ## X ## Ptr;
+
+#endif // DART_COMMON_SMARTPOINTER_H_

dart/constraint/ConstrainedGroup.cpp

@@ -57,7 +57,7 @@ ConstrainedGroup::~ConstrainedGroup()
 }
 
 //==============================================================================
-void ConstrainedGroup::addConstraint(ConstraintBase* _constraint)
+void ConstrainedGroup::addConstraint(const ConstraintBasePtr& _constraint)
 {
   assert(_constraint != nullptr && "Null constraint pointer is now allowed.");
   assert(containConstraint(_constraint) == false
@@ -74,14 +74,14 @@ size_t ConstrainedGroup::getNumConstraints() const
 }
 
 //==============================================================================
-ConstraintBase* ConstrainedGroup::getConstraint(size_t _index) const
+ConstraintBasePtr ConstrainedGroup::getConstraint(size_t _index) const
 {
   assert(_index < mConstraints.size());
   return mConstraints[_index];
 }
 
 //==============================================================================
-void ConstrainedGroup::removeConstraint(ConstraintBase* _constraint)
+void ConstrainedGroup::removeConstraint(const ConstraintBasePtr& _constraint)
 {
   assert(_constraint != nullptr && "Null constraint pointer is now allowed.");
   assert(containConstraint(_constraint) == true
@@ -108,16 +108,20 @@ void ConstrainedGroup::removeAllConstraints()
 }
 
 //==============================================================================
-bool ConstrainedGroup::containConstraint(ConstraintBase* _constraint) const
+bool ConstrainedGroup::containConstraint(
+    const ConstConstraintBasePtr& _constraint) const
 {
 //  std::cout << "CommunityTEST::_containConstraint(): Not implemented."
 //            << std::endl;
 
+  // TODO(MXG): Is there any reason these functions are not implemented yet?
+
   return false;
 }
 
 //==============================================================================
-bool ConstrainedGroup::checkAndAddConstraint(ConstraintBase* _constraint)
+bool ConstrainedGroup::checkAndAddConstraint(
+    const ConstraintBasePtr& _constraint)
 {
   std::cout << "CommunityTEST::_checkAndAddConstraint(): Not implemented."
             << std::endl;

dart/constraint/ConstrainedGroup.h

@@ -41,6 +41,8 @@
 #include <memory>
 #include <Eigen/Dense>
 
+#include "dart/constraint/SmartPointer.h"
+
 namespace dart {
 
 namespace dynamics {
@@ -74,16 +76,16 @@ class ConstrainedGroup
   //----------------------------------------------------------------------------
 
   /// Add constraint
-  void addConstraint(ConstraintBase* _constraint);
+  void addConstraint(const ConstraintBasePtr& _constraint);
 
   /// Return number of constraints in this constrained group
   size_t getNumConstraints() const;
 
   /// Return a constraint
-  ConstraintBase* getConstraint(size_t _index) const;
+  ConstraintBasePtr getConstraint(size_t _index) const;
 
   /// Remove constraint
-  void removeConstraint(ConstraintBase* _constraint);
+  void removeConstraint(const ConstraintBasePtr& _constraint);
 
   /// Remove all constraints
   void removeAllConstraints();
@@ -99,13 +101,13 @@ class ConstrainedGroup
 
 private:
   /// Return true if _constraint is contained
-  bool containConstraint(ConstraintBase* _constraint) const;
+  bool containConstraint(const ConstConstraintBasePtr& _constraint) const;
 
   /// Return true and add the constraint if _constraint is contained
-  bool checkAndAddConstraint(ConstraintBase* _constraint);
+  bool checkAndAddConstraint(const ConstraintBasePtr& _constraint);
 
   /// List of constraints
-  std::vector<ConstraintBase*> mConstraints;
+  std::vector<ConstraintBasePtr> mConstraints;
 
   ///
   std::shared_ptr<dynamics::Skeleton> mRootSkeleton;

dart/constraint/ConstraintSolver.cpp

@@ -182,7 +182,7 @@ void ConstraintSolver::removeAllSkeletons()
 }
 
 //==============================================================================
-void ConstraintSolver::addConstraint(ConstraintBase* _constraint)
+void ConstraintSolver::addConstraint(const ConstraintBasePtr& _constraint)
 {
   assert(_constraint);
 
@@ -197,7 +197,7 @@ void ConstraintSolver::addConstraint(ConstraintBase* _constraint)
 }
 
 //==============================================================================
-void ConstraintSolver::removeConstraint(ConstraintBase* _constraint)
+void ConstraintSolver::removeConstraint(const ConstraintBasePtr& _constraint)
 {
   assert(_constraint);
 
@@ -306,15 +306,17 @@ bool ConstraintSolver::checkAndAddSkeleton(const SkeletonPtr& _skeleton)
 }
 
 //==============================================================================
-bool ConstraintSolver::containConstraint(const ConstraintBase* _constraint) const
+bool ConstraintSolver::containConstraint(
+    const ConstConstraintBasePtr& _constraint) const
 {
   return std::find(mManualConstraints.begin(),
                    mManualConstraints.end(),
                    _constraint) != mManualConstraints.end();
 }
 
 //==============================================================================
-bool ConstraintSolver::checkAndAddConstraint(ConstraintBase* _constraint)
+bool ConstraintSolver::checkAndAddConstraint(
+    const ConstraintBasePtr& _constraint)
 {
   if (!containConstraint(_constraint))
   {
@@ -352,13 +354,9 @@ void ConstraintSolver::updateConstraints()
   mCollisionDetector->detectCollision(true, true);
 
   // Destroy previous contact constraints
-  for (const auto& contactConstraint : mContactConstraints)
-    delete contactConstraint;
   mContactConstraints.clear();
 
   // Destroy previous soft contact constraints
-  for (const auto& softContactConstraint : mSoftContactConstraints)
-    delete softContactConstraint;
   mSoftContactConstraints.clear();
 
   // Create new contact constraints
@@ -369,11 +367,12 @@ void ConstraintSolver::updateConstraints()
     if (isSoftContact(ct))
     {
       mSoftContactConstraints.push_back(
-            new SoftContactConstraint(ct, mTimeStep));
+            std::make_shared<SoftContactConstraint>(ct, mTimeStep));
     }
     else
     {
-      mContactConstraints.push_back(new ContactConstraint(ct, mTimeStep));
+      mContactConstraints.push_back(
+            std::make_shared<ContactConstraint>(ct, mTimeStep));
     }
   }
 
@@ -399,12 +398,6 @@ void ConstraintSolver::updateConstraints()
   // Update automatic constraints: joint constraints
   //----------------------------------------------------------------------------
   // Destroy previous joint constraints
-  for (const auto& jointLimitConstraint : mJointLimitConstraints)
-    delete jointLimitConstraint;
-  for (const auto& servoMotorConstraint : mServoMotorConstraints)
-    delete servoMotorConstraint;
-  for (const auto& jointFrictionConstraint : mJointCoulombFrictionConstraints)
-    delete jointFrictionConstraint;
   mJointLimitConstraints.clear();
   mServoMotorConstraints.clear();
   mJointCoulombFrictionConstraints.clear();
@@ -426,16 +419,18 @@ void ConstraintSolver::updateConstraints()
         if (joint->getCoulombFriction(j) != 0.0)
         {
           mJointCoulombFrictionConstraints.push_back(
-                new JointCoulombFrictionConstraint(joint));
+                std::make_shared<JointCoulombFrictionConstraint>(joint));
           break;
         }
       }
 
       if (joint->isPositionLimitEnforced())
-        mJointLimitConstraints.push_back(new JointLimitConstraint(joint));
+        mJointLimitConstraints.push_back(
+              std::make_shared<JointLimitConstraint>(joint));
 
       if (joint->getActuatorType() == dynamics::Joint::SERVO)
-        mServoMotorConstraints.push_back(new ServoMotorConstraint(joint));
+        mServoMotorConstraints.push_back(
+              std::make_shared<ServoMotorConstraint>(joint));
     }
   }
 
@@ -478,7 +473,7 @@ void ConstraintSolver::buildConstrainedGroups()
   //----------------------------------------------------------------------------
   // Unite skeletons according to constraints's relationships
   //----------------------------------------------------------------------------
-  for (std::vector<ConstraintBase*>::iterator it = mActiveConstraints.begin();
+  for (std::vector<ConstraintBasePtr>::iterator it = mActiveConstraints.begin();
        it != mActiveConstraints.end(); ++it)
   {
     (*it)->uniteSkeletons();
@@ -487,7 +482,7 @@ void ConstraintSolver::buildConstrainedGroups()
   //----------------------------------------------------------------------------
   // Build constraint groups
   //----------------------------------------------------------------------------
-  for (std::vector<ConstraintBase*>::const_iterator it = mActiveConstraints.begin();
+  for (std::vector<ConstraintBasePtr>::const_iterator it = mActiveConstraints.begin();
        it != mActiveConstraints.end(); ++it)
   {
     bool found = false;
@@ -514,7 +509,7 @@ void ConstraintSolver::buildConstrainedGroups()
   }
 
   // Add active constraints to constrained groups
-  for (std::vector<ConstraintBase*>::const_iterator it = mActiveConstraints.begin();
+  for (std::vector<ConstraintBasePtr>::const_iterator it = mActiveConstraints.begin();
        it != mActiveConstraints.end(); ++it)
   {
     dynamics::SkeletonPtr skel = (*it)->getRootSkeleton();