API for utilizing Analytical IK

dart/dynamics/detail/InverseKinematics.h

@@ -37,13 +37,18 @@
 #ifndef DART_DYNAMICS_DETAIL_INVERSEKINEMATICS_H_
 #define DART_DYNAMICS_DETAIL_INVERSEKINEMATICS_H_
 
+#include <type_traits>
+
+namespace dart {
+namespace dynamics {
+
 //==============================================================================
 template <class IKErrorMethod, typename... Args>
 IKErrorMethod& InverseKinematics::setErrorMethod(Args&&... args)
 {
   IKErrorMethod* newMethod =
       new IKErrorMethod(this, std::forward<Args>(args)...);
-  mErrorMethod = std::unique_ptr<IKErrorMethod>(newMethod);
+  mErrorMethod = std::unique_ptr<ErrorMethod>(newMethod);
   return *newMethod;
 }
 
@@ -53,7 +58,12 @@ IKGradientMethod& InverseKinematics::setGradientMethod(Args&&... args)
 {
   IKGradientMethod* newMethod =
       new IKGradientMethod(this, std::forward<Args>(args)...);
-  mGradientMethod = std::unique_ptr<IKGradientMethod>(newMethod);
+  mGradientMethod = std::unique_ptr<GradientMethod>(newMethod);
+
+  mAnalytical = dynamic_cast<Analytical*>(mGradientMethod.get());
+  if(nullptr != mAnalytical)
+    mAnalytical->constructDofMap();
+
   return *newMethod;
 }
 
@@ -69,4 +79,7 @@ void InverseKinematics::setDofs(const std::vector<DegreeOfFreedomT*>& _dofs)
   setDofs(indices);
 }
 
+} // namespace dart
+} // namespace dynamics
+
 #endif // DART_DYNAMICS_DETAIL_INVERSEKINEMATICS_H_

dart/dynamics/detail/NodePtr.h

@@ -222,7 +222,7 @@ class TemplateWeakNodePtr
     }
 
     mWeakBodyNodePtr = _ptr->getBodyNodePtr();
-    mWeakDestructor = _ptr->mDestructor;
+    mWeakDestructor = _ptr->getOrCreateDestructor();
     mNode = _ptr;
   }
 

dart/math/Geometry.cpp

@@ -1567,7 +1567,7 @@ Eigen::Vector2d computeCentroidOfHull(const SupportPolygon& _convexHull)
     Intersection_t result =
         computeIntersection(intersect, p0, midp12, p2, midp01);
 
-    if(INTERSECTING != result)
+    if(BEYOND_ENDPOINTS == result)
     {
       double a1 = atan2( (p1-p0)[1], (p1-p0)[0] )*180.0/M_PI;
       double a2 = atan2( (p2-p0)[1], (p2-p0)[0] )*180.0/M_PI;

dart/math/Geometry.h

@@ -428,6 +428,22 @@ bool verifyRotation(const Eigen::Matrix3d& _R);
 /// all the elements are not NaN values.
 bool verifyTransform(const Eigen::Isometry3d& _T);
 
+/// Get the remainder of dividing x by y
+inline double mod(double x, double y)
+{
+  if( 0.0 == y )
+    return x;
+
+  return x - y * floor(x/y);
+}
+
+/// Compute the angle (in the range of -pi to +pi) which ignores any full
+/// rotations
+inline double wrapToPi(double angle)
+{
+  return mod(angle+M_PI, 2*M_PI) - M_PI;
+}
+
 template <typename MatrixType, typename ReturnType>
 void extractNullSpace(const Eigen::JacobiSVD<MatrixType>& _SVD, ReturnType& _NS)
 {

dart/optimizer/GradientDescentSolver.cpp

@@ -124,6 +124,13 @@ bool GradientDescentSolver::solve()
   double gamma = mGradientP.mStepSize;
   size_t dim = problem->getDimension();
 
+  if(dim == 0)
+  {
+    problem->setOptimalSolution(Eigen::VectorXd());
+    problem->setOptimumValue(0.0);
+    return true;
+  }
+
   Eigen::VectorXd x = problem->getInitialGuess();
   assert(x.size() == static_cast<int>(dim));
 

data/urdf/drchubo/CMakeLists.txt

@@ -0,0 +1,5 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(drchubo)
+find_package(catkin REQUIRED)
+catkin_package()
+