Commit bedb1065 authored by Antonio El Khoury's avatar Antonio El Khoury
Browse files

Port to kitelab 2.07.

  * Use CkwsKCDBodyAdvanced class for bodies.
  * Comply with changes in hpp-model and use BodyDistance class.
parent efdb0f1a
...@@ -341,7 +341,7 @@ namespace hpp { ...@@ -341,7 +341,7 @@ namespace hpp {
/// \return const shared pointer to the body /// \return const shared pointer to the body
/// Look among all joints of the robot of each problem for a given name. /// Look among all joints of the robot of each problem for a given name.
CkwsKCDBodyConstShPtr CkwsKCDBodyAdvancedConstShPtr
findBodyByJointName(const std::string& inJointName) const; findBodyByJointName(const std::string& inJointName) const;
......
...@@ -251,7 +251,8 @@ namespace hpp { ...@@ -251,7 +251,8 @@ namespace hpp {
/// \brief A vector of paths corresponding to this problem. /// \brief A vector of paths corresponding to this problem.
std::vector<CkwsPathShPtr> pathVector_; std::vector<CkwsPathShPtr> pathVector_;
/// \brief A map of each body to the a vector of its outer bodies. /// \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 /// \brief Penetration
double penetration_; double penetration_;
/// \brief Whether the path optimizer should be called anyway /// \brief Whether the path optimizer should be called anyway
......
...@@ -36,7 +36,6 @@ ...@@ -36,7 +36,6 @@
#include "hpp/core/planner.hh" #include "hpp/core/planner.hh"
#include "hpp/core/problem.hh" #include "hpp/core/problem.hh"
#include "hpp/model/parser.hh" #include "hpp/model/parser.hh"
#include "hpp/model/body.hh"
#include <hpp/model/humanoid-robot.hh> #include <hpp/model/humanoid-robot.hh>
#include "KineoUtility/kitNotificator.h" #include "KineoUtility/kitNotificator.h"
...@@ -44,6 +43,7 @@ ...@@ -44,6 +43,7 @@
#include "KineoWorks2/kwsRoadmapBuilder.h" #include "KineoWorks2/kwsRoadmapBuilder.h"
#include "KineoWorks2/kwsPathOptimizer.h" #include "KineoWorks2/kwsPathOptimizer.h"
#include "KineoWorks2/kwsJoint.h" #include "KineoWorks2/kwsJoint.h"
#include <kwsKcd2/kwsKCDBodyAdvanced.h>
#include "kprParserXML/kprParserManager.h" #include "kprParserXML/kprParserManager.h"
#include <kprParserXML/KineoParserXML.h> #include <kprParserXML/KineoParserXML.h>
...@@ -642,10 +642,10 @@ namespace hpp { ...@@ -642,10 +642,10 @@ namespace hpp {
// ====================================================================== // ======================================================================
CkwsKCDBodyConstShPtr CkwsKCDBodyAdvancedConstShPtr
Planner::findBodyByJointName(const std::string& inJointName) const Planner::findBodyByJointName(const std::string& inJointName) const
{ {
CkwsKCDBodyConstShPtr kcdBody; CkwsKCDBodyAdvancedConstShPtr kcdBody;
unsigned int nbProblems = getNbHppProblems(); unsigned int nbProblems = getNbHppProblems();
// Loop over hppProblem. // Loop over hppProblem.
...@@ -663,7 +663,7 @@ namespace hpp { ...@@ -663,7 +663,7 @@ namespace hpp {
KIT_DYNAMIC_PTR_CAST(CkppJointComponent, *jointIter)) { KIT_DYNAMIC_PTR_CAST(CkppJointComponent, *jointIter)) {
if (kppJointInRobot->name() == inJointName) { if (kppJointInRobot->name() == inJointName) {
kcdBody = kcdBody =
KIT_DYNAMIC_PTR_CAST(const CkwsKCDBody, KIT_DYNAMIC_PTR_CAST(const CkwsKCDBodyAdvanced,
kppJointInRobot->kwsJoint()->attachedBody()); kppJointInRobot->kwsJoint()->attachedBody());
return kcdBody; return kcdBody;
} }
......
...@@ -19,8 +19,9 @@ ...@@ -19,8 +19,9 @@
#include <iostream> #include <iostream>
#include <hpp/util/debug.hh> #include <hpp/util/debug.hh>
#include <hpp/model/device.hh>
#include <hpp/model/body-distance.hh>
#include <hpp/core/problem.hh> #include <hpp/core/problem.hh>
#include "hpp/model/body.hh"
#include "KineoWorks2/kwsConfigExtractor.h" #include "KineoWorks2/kwsConfigExtractor.h"
#include "KineoWorks2/kwsValidatorDPCollision.h" #include "KineoWorks2/kwsValidatorDPCollision.h"
...@@ -31,6 +32,7 @@ ...@@ -31,6 +32,7 @@
#include "KineoWorks2/kwsReportCfgDof.h" #include "KineoWorks2/kwsReportCfgDof.h"
#include "KineoWorks2/kwsSteeringMethod.h" #include "KineoWorks2/kwsSteeringMethod.h"
#include "KineoWorks2/kwsPathPlanner.h" #include "KineoWorks2/kwsPathPlanner.h"
#include <kwsKcd2/kwsKCDBodyAdvanced.h>
#include "KineoModel/kppSteeringMethodComponent.h" #include "KineoModel/kppSteeringMethodComponent.h"
namespace hpp { namespace hpp {
...@@ -133,12 +135,13 @@ namespace hpp { ...@@ -133,12 +135,13 @@ namespace hpp {
// Loop over bodies of robot. // Loop over bodies of robot.
for ( CkwsDevice::TBodyIterator bodyIter = bodyVector.begin(); bodyIter < bodyVector.end(); bodyIter++ ) for ( CkwsDevice::TBodyIterator bodyIter = bodyVector.begin(); bodyIter < bodyVector.end(); bodyIter++ )
{ {
// Try to cast body into CkwsKCDBody // Try to cast body into CkwsKCDBodyAdvanced
CkwsKCDBodyShPtr kcdBody; CkwsKCDBodyAdvancedShPtr kcdBody;
if ( kcdBody = boost::dynamic_pointer_cast<CkwsKCDBody> ( *bodyIter ) ) if ( kcdBody = boost::dynamic_pointer_cast<CkwsKCDBodyAdvanced>
( *bodyIter ) )
{ {
// Copy list of outer objects // Copy list of outer objects
mapOuter_[kcdBody]= kcdBody->outerObjects(); mapOuter_[kcdBody]= kcdBody->obstacleObjects();
} }
} }
} }
...@@ -172,32 +175,39 @@ namespace hpp { ...@@ -172,32 +175,39 @@ namespace hpp {
// Add object in local list // Add object in local list
obstacleVector_.push_back(inObject); obstacleVector_.push_back(inObject);
// Get robot vector of bodies. // Retrieve hpp device.
CkwsDevice::TBodyVector bodyVector; model::DeviceShPtr hppRobot = KIT_DYNAMIC_PTR_CAST(model::Device, robot_);
robot_->getBodyVector ( bodyVector ); if (!hppRobot)
{
// Loop over bodies of robot. hppDout (notice, "Robot is not HPP Device.");
for ( CkwsDevice::TBodyIterator bodyIter = bodyVector.begin();
bodyIter < bodyVector.end(); // Get robot vector of bodies.
bodyIter++ ) { CkwsDevice::TBodyVector bodyVector;
robot_->getBodyVector ( bodyVector );
if (model::BodyShPtr body =
KIT_DYNAMIC_PTR_CAST(model::Body, *bodyIter)) { // Loop over bodies of robot.
hppDout(info, "hpp::model::Body type."); for ( CkwsDevice::TBodyIterator bodyIter = bodyVector.begin();
body->addOuterObject(inObject, distanceComputation); bodyIter < bodyVector.end();
} bodyIter++ )
else if (CkwsKCDBodyShPtr kcdBody = if (CkwsKCDBodyAdvancedShPtr kcdBody =
KIT_DYNAMIC_PTR_CAST(CkwsKCDBody, *bodyIter)) { KIT_DYNAMIC_PTR_CAST(CkwsKCDBodyAdvanced, *bodyIter))
hppDout(info,"CkwsKCDBody type."); {
/* hppDout(info,"CkwsKCDBodyAdvanced type.");
Append object at the end of KineoWorks set of outer objects // Append object at the end of KineoWorks set of outer
for collision checking // objects for collision checking.
*/ std::vector<CkcdObjectShPtr> outerList = kcdBody->obstacleObjects();
std::vector<CkcdObjectShPtr> outerList = kcdBody->outerObjects(); outerList.push_back(inObject);
outerList.push_back(inObject); kcdBody->obstacleObjects(outerList);
kcdBody->outerObjects(outerList); }
} }
} else
// Loop over body distances of robot.
BOOST_FOREACH(model::BodyDistanceShPtr bodyDistance,
hppRobot->bodyDistances ())
{
bodyDistance->addOuterObject (inObject, distanceComputation);
}
return KD_OK; return KD_OK;
} }
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment