Port to kitelab 2.07.

  * Use CkwsKCDBodyAdvanced class for bodies.
  * Comply with changes in hpp-model and use BodyDistance class.

include/hpp/core/planner.hh

@@ -341,7 +341,7 @@ namespace hpp {
       /// \return const shared pointer to the body
 
       /// Look among all joints of the robot of each problem for a given name.
-      CkwsKCDBodyConstShPtr
+      CkwsKCDBodyAdvancedConstShPtr
       findBodyByJointName(const std::string& inJointName) const;
 
 

include/hpp/core/problem.hh

@@ -251,7 +251,8 @@ namespace hpp {
       /// \brief A vector of paths corresponding to this problem.
       std::vector<CkwsPathShPtr> pathVector_;
       /// \brief A map of each body to the a vector of its outer bodies.
-      std::map<CkwsKCDBodyShPtr,std::vector<CkcdObjectShPtr> > mapOuter_;
+      std::map<CkwsKCDBodyAdvancedShPtr,
+	       std::vector<CkcdObjectShPtr> > mapOuter_;
       /// \brief Penetration
       double penetration_;
       /// \brief Whether the path optimizer should be called anyway

src/planner.cc

@@ -36,7 +36,6 @@
 #include "hpp/core/planner.hh"
 #include "hpp/core/problem.hh"
 #include "hpp/model/parser.hh"
-#include "hpp/model/body.hh"
 #include <hpp/model/humanoid-robot.hh>
 
 #include "KineoUtility/kitNotificator.h"
@@ -44,6 +43,7 @@
 #include "KineoWorks2/kwsRoadmapBuilder.h"
 #include "KineoWorks2/kwsPathOptimizer.h"
 #include "KineoWorks2/kwsJoint.h"
+#include <kwsKcd2/kwsKCDBodyAdvanced.h>
 
 #include "kprParserXML/kprParserManager.h"
 #include <kprParserXML/KineoParserXML.h>
@@ -642,10 +642,10 @@ namespace hpp {
 
     // ======================================================================
 
-    CkwsKCDBodyConstShPtr
+    CkwsKCDBodyAdvancedConstShPtr
     Planner::findBodyByJointName(const std::string& inJointName) const
     {
-      CkwsKCDBodyConstShPtr kcdBody;
+      CkwsKCDBodyAdvancedConstShPtr kcdBody;
       unsigned int nbProblems = getNbHppProblems();
 
       // Loop over hppProblem.
@@ -663,7 +663,7 @@ namespace hpp {
 	      KIT_DYNAMIC_PTR_CAST(CkppJointComponent, *jointIter)) {
 	    if (kppJointInRobot->name() == inJointName) {
 	      kcdBody =
-		KIT_DYNAMIC_PTR_CAST(const CkwsKCDBody,
+		KIT_DYNAMIC_PTR_CAST(const CkwsKCDBodyAdvanced,
 				     kppJointInRobot->kwsJoint()->attachedBody());
 	      return kcdBody;
 	    }

src/problem.cc

@@ -19,8 +19,9 @@
 #include <iostream>
 
 #include <hpp/util/debug.hh>
+#include <hpp/model/device.hh>
+#include <hpp/model/body-distance.hh>
 #include <hpp/core/problem.hh>
-#include "hpp/model/body.hh"
 
 #include "KineoWorks2/kwsConfigExtractor.h"
 #include "KineoWorks2/kwsValidatorDPCollision.h"
@@ -31,6 +32,7 @@
 #include "KineoWorks2/kwsReportCfgDof.h"
 #include "KineoWorks2/kwsSteeringMethod.h"
 #include "KineoWorks2/kwsPathPlanner.h"
+#include <kwsKcd2/kwsKCDBodyAdvanced.h>
 #include "KineoModel/kppSteeringMethodComponent.h"
 
 namespace hpp {
@@ -133,12 +135,13 @@ namespace hpp {
       // Loop over bodies of robot.
       for ( CkwsDevice::TBodyIterator bodyIter = bodyVector.begin(); bodyIter < bodyVector.end(); bodyIter++ )
 	{
-	  // Try to cast body into CkwsKCDBody
-	  CkwsKCDBodyShPtr kcdBody;
-	  if ( kcdBody = boost::dynamic_pointer_cast<CkwsKCDBody> ( *bodyIter ) )
+	  // Try to cast body into CkwsKCDBodyAdvanced
+	  CkwsKCDBodyAdvancedShPtr kcdBody;
+	  if ( kcdBody = boost::dynamic_pointer_cast<CkwsKCDBodyAdvanced>
+	       ( *bodyIter ) )
 	    {
 	      // Copy list of outer objects
-	      mapOuter_[kcdBody]= kcdBody->outerObjects();
+	      mapOuter_[kcdBody]= kcdBody->obstacleObjects();
 	    }
 	}
     }
@@ -172,32 +175,39 @@ namespace hpp {
       // Add object in local list
       obstacleVector_.push_back(inObject);
 
-      // Get robot vector of bodies.
-      CkwsDevice::TBodyVector bodyVector;
-      robot_->getBodyVector ( bodyVector );
-
-      // Loop over bodies of robot.
-      for ( CkwsDevice::TBodyIterator bodyIter = bodyVector.begin();
-	    bodyIter < bodyVector.end();
-	    bodyIter++ ) {
-
-	if (model::BodyShPtr body =
-	    KIT_DYNAMIC_PTR_CAST(model::Body, *bodyIter)) {
-	  hppDout(info, "hpp::model::Body type.");
-	  body->addOuterObject(inObject, distanceComputation);
-	}
-	else if (CkwsKCDBodyShPtr kcdBody =
-		 KIT_DYNAMIC_PTR_CAST(CkwsKCDBody, *bodyIter)) {
-	  hppDout(info,"CkwsKCDBody type.");
-	  /*
-	    Append object at the end of KineoWorks set of outer objects
-	    for collision checking
-	  */
-	  std::vector<CkcdObjectShPtr> outerList = kcdBody->outerObjects();
-	  outerList.push_back(inObject);
-	  kcdBody->outerObjects(outerList);
+      // Retrieve hpp device.
+      model::DeviceShPtr hppRobot = KIT_DYNAMIC_PTR_CAST(model::Device, robot_);
+      if (!hppRobot)
+	{
+	  hppDout (notice, "Robot is not HPP Device.");
+
+	  // Get robot vector of bodies.
+	  CkwsDevice::TBodyVector bodyVector;
+	  robot_->getBodyVector ( bodyVector );
+
+	  // Loop over bodies of robot.
+	  for ( CkwsDevice::TBodyIterator bodyIter = bodyVector.begin();
+		bodyIter < bodyVector.end();
+		bodyIter++ )
+	    if (CkwsKCDBodyAdvancedShPtr kcdBody =
+		KIT_DYNAMIC_PTR_CAST(CkwsKCDBodyAdvanced, *bodyIter))
+	      {
+		hppDout(info,"CkwsKCDBodyAdvanced type.");
+		// Append object at the end of KineoWorks set of outer
+		// objects for collision checking.
+		std::vector<CkcdObjectShPtr> outerList = kcdBody->obstacleObjects();
+		outerList.push_back(inObject);
+		kcdBody->obstacleObjects(outerList);
+	      }
 	}
-      }
+      else
+	// Loop over body distances of robot.
+	BOOST_FOREACH(model::BodyDistanceShPtr bodyDistance,
+		      hppRobot->bodyDistances ())
+	  {
+	    bodyDistance->addOuterObject (inObject, distanceComputation);
+	  }
+
       return KD_OK;
     }