pinocchio_v2

humanoid-path-planner · Feb 18, 2019 · 197ad25 · 197ad25
1 parent 2e5e2f4
commit 197ad25
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 6 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -66,7 +66,7 @@ if (OCTOMAP_INCLUDE_DIRS AND OCTOMAP_LIBRARY_DIRS)
   list(GET VERSION_LIST 2 OCTOMAP_PATCH_VERSION)
   add_definitions (-DOCTOMAP_MAJOR_VERSION=${OCTOMAP_MAJOR_VERSION}
                    -DOCTOMAP_MINOR_VERSION=${OCTOMAP_MINOR_VERSION}
-                   -DOCTOMAP_PATCH_VERSION=${OCTOMAP_PATCH_VERSION})
+                   -DOCTOMAP_PATCH_VERSION=${OCTOMAP_PATCH_VERSION}  -DHPP_FCL_HAVE_OCTOMAP=1)
         message(STATUS "FCL uses Octomap" ${OCTOMAP_MINOR_VERSION})
 else()
         message(STATUS "FCL does not use Octomap")

diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc
@@ -2361,7 +2361,8 @@ namespace hpp {
                 core::segment_t interval(0, p1->initial().rows()-1);
                 core::segments_t intervals;
                 intervals.push_back(interval);
-                PathPtr_t reducedPath = core::SubchainPath::create(p1,intervals);
+                core::segments_t velIntervals (1, core::segment_t (0, fullBody()->device_->numberDof()));
+                PathPtr_t reducedPath = core::SubchainPath::create(p1,intervals, velIntervals);
                 resPath->appendPath(reducedPath);
                 pathsIds.push_back(AddPath(p1,problemSolver()));
             }
@@ -2383,7 +2384,8 @@ namespace hpp {
                 core::segment_t interval(0, p2->initial().rows()-1);
                 core::segments_t intervals;
                 intervals.push_back(interval);
-                PathPtr_t reducedPath = core::SubchainPath::create(p2,intervals);
+                core::segments_t velIntervals (1, core::segment_t (0, fullBody()->device_->numberDof()));
+                PathPtr_t reducedPath = core::SubchainPath::create(p2,intervals,velIntervals);
                 resPath->appendPath(reducedPath);
             }
             catch(std::runtime_error& e)
@@ -2401,7 +2403,8 @@ namespace hpp {
                 core::segment_t interval(0, p3->initial().rows()-1);
                 core::segments_t intervals;
                 intervals.push_back(interval);
-                PathPtr_t reducedPath = core::SubchainPath::create(p3,intervals);
+                core::segments_t velIntervals (1, core::segment_t (0, fullBody()->device_->numberDof()));
+                PathPtr_t reducedPath = core::SubchainPath::create(p3,intervals,velIntervals);
                 resPath->appendPath(reducedPath);
                 pathsIds.push_back(AddPath(p3,problemSolver()));
             }
@@ -2504,7 +2507,8 @@ namespace hpp {
             core::segment_t interval(0, p2->initial().rows()-1);
             core::segments_t intervals;
             intervals.push_back(interval);
-            PathPtr_t reducedPath = core::SubchainPath::create(p2,intervals);
+            core::segments_t velIntervals (1, core::segment_t (0, fullBody()->device_->numberDof()));
+            PathPtr_t reducedPath = core::SubchainPath::create(p2,intervals,velIntervals);
             core::PathVectorPtr_t resPath = core::PathVector::create(fullBody()->device_->configSize(), fullBody()->device_->numberDof());
             resPath->appendPath(reducedPath);
 
@@ -2558,7 +2562,8 @@ namespace hpp {
         core::segment_t interval(0, p1->initial().rows()-1);
         core::segments_t intervals;
         intervals.push_back(interval);
-        PathPtr_t reducedPath = core::SubchainPath::create(p1,intervals);
+        core::segments_t velIntervals (1, core::segment_t (0, fullBody()->device_->numberDof()));
+        PathPtr_t reducedPath = core::SubchainPath::create(p1,intervals, velIntervals);
         resPath->appendPath(reducedPath);
         pathsIds.push_back((CORBA::Short)problemSolver()->addPath(resPath));