Support DART 6.1 (#113)

Resolves #113
personalrobotics · Nov 24, 2016 · d5a3730 · d5a3730
1 parent b658c17
commit d5a3730
Show file tree

Hide file tree

Showing 15 changed files with 28 additions and 15 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -102,7 +102,7 @@ find_package(Boost COMPONENTS filesystem)
 include_directories(${Boost_INCLUDE_DIRS})
 
 # TODO: utils-urdf should only be required for aikido::perception.
-find_package(DART REQUIRED COMPONENTS optimizer-nlopt utils-urdf CONFIG)
+find_package(DART 6.1.0 REQUIRED COMPONENTS optimizer-nlopt utils-urdf CONFIG)
 include_directories(${DART_INCLUDE_DIRS})
 
 #==============================================================================

diff --git a/include/aikido/constraint/NonColliding.hpp b/include/aikido/constraint/NonColliding.hpp
@@ -6,7 +6,7 @@
 #include <tuple>
 #include "../statespace/dart/MetaSkeletonStateSpace.hpp"
 #include <dart/collision/CollisionDetector.hpp>
-#include <dart/collision/Option.hpp>
+#include <dart/collision/CollisionOption.hpp>
 #include <dart/collision/CollisionGroup.hpp>
 
 namespace aikido {

diff --git a/include/aikido/control/BarrettFingerPositionCommandExecutor.hpp b/include/aikido/control/BarrettFingerPositionCommandExecutor.hpp
@@ -1,7 +1,7 @@
 #ifndef AIKIDO_CONTROL_BARRETFINGERPOSITIONCOMMANDEXECUTOR_HPP_
 #define AIKIDO_CONTROL_BARRETFINGERPOSITIONCOMMANDEXECUTOR_HPP_
 #include <dart/collision/CollisionDetector.hpp>
-#include <dart/collision/Option.hpp>
+#include <dart/collision/CollisionOption.hpp>
 #include <dart/collision/CollisionGroup.hpp>
 #include <dart/collision/CollisionFilter.hpp>
 #include <dart/dynamics/dynamics.hpp>

diff --git a/include/aikido/control/BarrettFingerSpreadCommandExecutor.hpp b/include/aikido/control/BarrettFingerSpreadCommandExecutor.hpp
@@ -1,7 +1,7 @@
 #ifndef AIKIDO_CONTROL_BARRETFINGERSPREADCOMMANDEXECUTOR_HPP_
 #define AIKIDO_CONTROL_BARRETFINGERSPREADCOMMANDEXECUTOR_HPP_
 #include <dart/collision/CollisionDetector.hpp>
-#include <dart/collision/Option.hpp>
+#include <dart/collision/CollisionOption.hpp>
 #include <dart/collision/CollisionGroup.hpp>
 #include <dart/collision/CollisionFilter.hpp>
 #include <dart/dynamics/dynamics.hpp>

diff --git a/include/aikido/control/BarrettHandPositionCommandExecutor.hpp b/include/aikido/control/BarrettHandPositionCommandExecutor.hpp
@@ -3,7 +3,7 @@
 #include "BarrettFingerPositionCommandExecutor.hpp"
 #include "BarrettFingerSpreadCommandExecutor.hpp"
 #include <dart/collision/CollisionDetector.hpp>
-#include <dart/collision/Option.hpp>
+#include <dart/collision/CollisionOption.hpp>
 #include <dart/collision/CollisionGroup.hpp>
 #include <dart/collision/CollisionFilter.hpp>
 #include <dart/dynamics/dynamics.hpp>

diff --git a/include/aikido/statespace/dart/SO2Joint.hpp b/include/aikido/statespace/dart/SO2Joint.hpp
@@ -21,7 +21,7 @@ class SO2Joint
   /// position limits.
   ///
   /// \param _joint joint to create a state space for
-  explicit SO2Joint(::dart::dynamics::SingleDofJoint* _joint);
+  explicit SO2Joint(::dart::dynamics::GenericJoint<::dart::math::R1Space>* _joint);
 
   // Documentation inherited.
   void convertPositionsToState(

diff --git a/src/constraint/TSR.cpp b/src/constraint/TSR.cpp
@@ -72,6 +72,9 @@ class TSRSampleGenerator : public SampleGenerator
 
 
   friend class TSR;
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 //=============================================================================

diff --git a/src/control/BarrettFingerPositionCommandExecutor.cpp b/src/control/BarrettFingerPositionCommandExecutor.cpp
@@ -10,6 +10,8 @@ BarrettFingerPositionCommandExecutor::BarrettFingerPositionCommandExecutor(
   ::dart::collision::CollisionDetectorPtr _collisionDetector,
   ::dart::collision::CollisionOption _collisionOptions)
 : mFinger(std::move(_finger))
+, mProximalDof(nullptr)
+, mDistalDof(nullptr)
 , mCollisionDetector(std::move(_collisionDetector))
 , mCollisionOptions(std::move(_collisionOptions))
 , mInExecution(false)
@@ -20,8 +22,13 @@ BarrettFingerPositionCommandExecutor::BarrettFingerPositionCommandExecutor(
   if (_proximal == _distal)
     throw std::invalid_argument("proximal and distal dofs should be different.");
 
-  mProximalDof = mFinger->getDof(_proximal);
-  mDistalDof = mFinger->getDof(_distal);
+  const auto numDofs = mFinger->getNumDofs();
+
+  if (_proximal < numDofs)
+    mProximalDof = mFinger->getDof(_proximal);
+
+  if (_distal < numDofs)
+    mDistalDof = mFinger->getDof(_distal);
 
   if (!mProximalDof)
     throw std::invalid_argument("Finger does not have proximal dof.");

diff --git a/src/control/BarrettFingerSpreadCommandExecutor.cpp b/src/control/BarrettFingerSpreadCommandExecutor.cpp
@@ -33,6 +33,10 @@ BarrettFingerSpreadCommandExecutor::BarrettFingerSpreadCommandExecutor(
       throw std::invalid_argument(msg.str());
     }
 
+    const auto numDofs = mFingers[i]->getNumDofs();
+    if (_spread >= numDofs)
+      throw std::invalid_argument("Finger does not have spread dof.");
+
     mSpreadDofs.push_back(mFingers[i]->getDof(_spread));
     if (!mSpreadDofs[i]){
       std::stringstream msg;

diff --git a/src/statespace/dart/MetaSkeletonStateSpace.cpp b/src/statespace/dart/MetaSkeletonStateSpace.cpp
@@ -16,7 +16,6 @@ using ::dart::dynamics::RevoluteJoint;
 using ::dart::dynamics::Joint;
 using ::dart::dynamics::ScrewJoint;
 using ::dart::dynamics::TranslationalJoint;
-using ::dart::dynamics::SingleDofJoint;
 using ::dart::dynamics::WeldJoint;
 using ::dart::dynamics::INVALID_INDEX;
 

diff --git a/src/statespace/dart/SO2Joint.cpp b/src/statespace/dart/SO2Joint.cpp
@@ -5,8 +5,7 @@ namespace statespace {
 namespace dart {
 
 //=============================================================================
-SO2Joint::SO2Joint(
-      ::dart::dynamics::SingleDofJoint* _joint)
+SO2Joint::SO2Joint(::dart::dynamics::GenericJoint<::dart::math::R1Space> *_joint)
   : JointStateSpace(_joint)
   , SO2()
 {

diff --git a/tests/constraint/test_DifferentiableIntersection.cpp b/tests/constraint/test_DifferentiableIntersection.cpp
@@ -31,7 +31,7 @@ TEST(DifferentiableIntersection, InvalidConstructor)
   // constraints have different space
   constraints.push_back(std::make_shared<PolynomialConstraint>(
     Eigen::Vector3d(1, 2, 3), rvss));
-  constraints.push_back(std::make_shared<TSR>());
+  constraints.push_back(Eigen::make_aligned_shared<TSR>());
   EXPECT_THROW(DifferentiableIntersection(constraints, rvss), std::invalid_argument);
 }
 

diff --git a/tests/constraint/test_FramePairDifferentiable.cpp b/tests/constraint/test_FramePairDifferentiable.cpp
@@ -28,7 +28,7 @@ class FramePairDifferentiableTest : public ::testing::Test {
   protected:
     virtual void SetUp() {
 
-      tsr = std::make_shared<TSR>(
+      tsr = Eigen::make_aligned_shared<TSR>(
         std::unique_ptr<RNG>(new RNGWrapper<std::default_random_engine>(0)));
 
       Eigen::MatrixXd Bw = Eigen::Matrix<double, 6, 2>::Zero();

diff --git a/tests/constraint/test_NonColliding.cpp b/tests/constraint/test_NonColliding.cpp
@@ -62,7 +62,8 @@ class NonCollidingTest : public ::testing::Test
     group->addDofs(mBox->getDofs());
 
     mStateSpace = std::make_shared<MetaSkeletonStateSpace>(group);
-    mManipulator->enableSelfCollision(true);
+    mManipulator->enableSelfCollisionCheck();
+    mManipulator->enableAdjacentBodyCheck();
 
   }
 

diff --git a/tests/constraint/test_RnBoxConstraint.cpp b/tests/constraint/test_RnBoxConstraint.cpp
@@ -84,7 +84,7 @@ class RnBoxConstraintTests : public ::testing::Test
   std::vector<Rn::ScopedState> mTargets;
 
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 TEST_F(RnBoxConstraintTests, constructor_StateSpaceIsNull_Throws)