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 {
/// \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;
......
......@@ -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
......
......@@ -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;
}
......
......@@ -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,6 +175,12 @@ namespace hpp {
// Add object in local list
obstacleVector_.push_back(inObject);
// 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 );
......@@ -179,25 +188,26 @@ namespace hpp {
// 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();
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->outerObjects(outerList);
kcdBody->obstacleObjects(outerList);
}
}
else
// Loop over body distances of robot.
BOOST_FOREACH(model::BodyDistanceShPtr bodyDistance,
hppRobot->bodyDistances ())
{
bodyDistance->addOuterObject (inObject, distanceComputation);
}
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