Commit 9c9853bc authored by florent's avatar florent
Browse files

Added ChppBody::addSolidComponent to add an object to a body and insert it

in the inner object list.
Added virtual method ChppPlanner::initializeProblem() to enable customized initialization of objects
deriving from ChppPlanner.
Added functionalities to display a roadmap. Might be quickly obsolete.
parent 6f1dca37
2007/10/30
Added ChppBody::addSolidComponent to add an object to a body and insert it in the inner object list.
Added virtual method ChppPlanner::initializeProblem() to enable customized initialization of objects
deriving from ChppPlanner.
Added functionalities to display a roadmap. Might be quickly obsolete.
2007/10/23
void getInnerObjects (std::vector< CkcdObjectShPtr > & list);
void getOuterObjects (std::vector< CkcdObjectShPtr > & list);
have been removed use CkwsKCDBody::innerObjects() and CkwsKCDBody::outerObjects() instead.
2007/08/23
First version of package that compiles with hppModel package.
Robot modelling (dynamic part and geometric part) have been extracted from hppCore to hppModel.
......
......@@ -5,7 +5,7 @@ dnl
dnl Author: Florent Lamiraux LAAS-CNRS
dnl
AC_INIT([hppCore],[1.0.1],[openrobots@laas.fr])
AC_INIT([hppCore],[1.2],[openrobots@laas.fr])
AC_PREREQ(2.59)
AM_INIT_AUTOMAKE
......
......@@ -17,6 +17,7 @@ INCLUDE
#include "KineoUtility/kitInterface.h"
#include "kcd2/kcdInterface.h"
#include "kwsKcd2/kwsKCDBody.h"
#include "KineoModel/kppSolidComponentRef.h"
KIT_PREDEF_CLASS(ChppBody);
......@@ -45,9 +46,21 @@ public:
void setOuterObjects (const std::vector< CkcdObjectShPtr > &i_outerObjects);
void getInnerObjects (std::vector< CkcdObjectShPtr > & list);
void getOuterObjects (std::vector< CkcdObjectShPtr > & list);
/**
\brief Add geometry to the body
\param inSolidComponentRef Reference to the solid component to add.
\return true if success, false otherwise.
The input solid component is dynamically cast into
\li a CkppKCDPolyhedron or
\li a CkppKCDAssembly
The object is then added to the inner object list of the body.
The collision analyser attExactAnalyzer is also updated.
\note The body must be attached to a joint.
*/
bool addSolidComponent(const CkppSolidComponentRefShPtr& inSolidComponentRef);
ktStatus getExactDistance(double &dist, CkitPoint3& o_point1, CkitPoint3& o_point2,
CkcdObjectShPtr &object1, CkcdObjectShPtr &object2);
......
......@@ -202,6 +202,20 @@ class ChppPlanner {
*/
virtual ktStatus addObstacle(CkcdObjectShPtr object);
/**
*@}
*/
/**
\name Initialization
@{
*/
/**
\brief Initialization of the path planning problem
*/
virtual ktStatus initializeProblem() {return KD_OK;};
/**
*@}
*/
......@@ -225,6 +239,13 @@ class ChppPlanner {
*/
ktStatus solveOneProblem(unsigned int problemId);
/**
\brief Optimize a given path
\param inProblemId Id of the problem owning the path.
\param inPathId Id of the path in this problem.
*/
ktStatus optimizePath(unsigned int inProblemId, unsigned int inPathId);
/**
\brief Get number of paths in given problem.
*/
......@@ -240,7 +261,34 @@ class ChppPlanner {
\return KD_ERROR if problemId too big, KD_OK otherwise.
*/
ktStatus addPath(unsigned int problemId, CkwsPathShPtr kwsPath);
ktStatus addPath(unsigned int problemId, CkwsPathShPtr kwsPath);
/**
*@}*
*/
/**
\name Drawing the roadmap
@{
*/
/**
\brief Draws the roadmap of given problem in interface.
\param inProblemId the rank of the problem in vector.
Works only if the roadmap builder of corresponding problem has been initialized with
a CkwsPlusRoadmap.
*/
ktStatus drawRoadmap(unsigned int inProblemId);
/**
\brief Stop drawing the roadmap of given problem in interface.
\param inProblemId the rank of the problem in vector.
Works only if the roadmap builder of corresponding problem has been initialized with
a CkwsPlusRoadmap.
*/
ktStatus stopdrawingRoadmap(unsigned int inProblemId);
/**
*@}*
......
......@@ -24,6 +24,7 @@ INCLUDE
#include "kwsPlusRoadmap.h"
#include "KineoUtility/kitNotificator.h"
class CkwsPlusDrawRdmBuilderDelegate;
/*************************************
CLASS
......@@ -47,7 +48,15 @@ public:
*/
ChppProblem(CkppDeviceComponentShPtr inRobot);
/**
/**
* \brief Create a path planning problem with no initial nor goal configuration.
* \param inRobot robot associated to the path planning problem.
* \param inObstacleList list of obstacle of this problem.
*/
ChppProblem(CkppDeviceComponentShPtr inRobot,
const std::vector<CkcdObjectShPtr>& inObstacleList);
/**
\name Problem definition
@{
*/
......@@ -162,6 +171,25 @@ public:
*/
unsigned int getNbPaths() const;
/**
*@}
*/
/**
\name Drawing the roadmap
@{
*/
/**
\brief Draws the roadmap by inserting a delegate in the roadmapBuilder
*/
ktStatus drawRoadmap();
/**
\brief Stop drawing the roadmap
*/
ktStatus stopDrawingRoadmap();
/**
*@}
*/
......@@ -208,6 +236,10 @@ private :
*/
std::vector<CkwsPathShPtr> attPathVector;
/**
\brief Pointer to a drawing roadmap delegate.
*/
CkwsPlusDrawRdmBuilderDelegate* attDrawRoadmapDelegate;
public:
// for notification:
static const CkitNotification::TType ID_HPP_ADD_PATH;
......
......@@ -6,9 +6,11 @@
*/
#include <iostream>
#include "hppBody.h"
#include "KineoKCDModel/kppKCDPolyhedron.h"
#include "KineoKCDModel/kppKCDAssembly.h"
#include "KineoModel/kppJointComponent.h"
#include "hppBody.h"
//=============================================================================
......@@ -96,19 +98,52 @@ void ChppBody::setOuterObjects (const std::vector< CkcdObjectShPtr > &i_outerObj
//=============================================================================
void ChppBody::getInnerObjects (std::vector< CkcdObjectShPtr > & list)
bool ChppBody::addSolidComponent(const CkppSolidComponentRefShPtr& inSolidComponentRef)
{
list = inner;
}
CkppSolidComponentShPtr solidComponent = inSolidComponentRef->referencedSolidComponent();
//=============================================================================
/*
The input solid component is dynamically cast into
1. a CkppKCDPolyhedron or
2. a CkppKCDAssembly
*/
CkcdObjectShPtr kcdObject;
if (CkppKCDPolyhedronShPtr polyhedron = KIT_DYNAMIC_PTR_CAST(CkppKCDPolyhedron, solidComponent)) {
kcdObject = polyhedron;
}
else if (CkppKCDAssemblyShPtr assembly = KIT_DYNAMIC_PTR_CAST(CkppKCDAssembly, solidComponent)) {
kcdObject = assembly;
}
else {
std::cerr << "ChppBody::addSolidComponent: solid component is neither an assembly nor a polyhedron" << std::endl;
return false;
}
void ChppBody::getOuterObjects (std::vector< CkcdObjectShPtr > & list)
{
list = outer;
/*
Add object to the inner object list
*/
std::vector<CkcdObjectShPtr> innerObjectList = innerObjects();
innerObjectList.push_back(kcdObject);
setInnerObjects(innerObjectList);
/*
Attach solid component to the joint associated to the body
*/
CkwsJointShPtr bodyJoint = joint();
if (bodyJoint) {
if (CkppJointComponentShPtr kppJoint = KIT_DYNAMIC_PTR_CAST(CkppJointComponent, bodyJoint)) {
kppJoint->addSolidComponentRef(inSolidComponentRef);
return true;
}
}
else {
std::cerr << "ChppBody::addSolidComponent: the body is not attached to any joint" << std::endl;
}
return false;
}
//=============================================================================
ktStatus ChppBody::getExactDistance(double &dist, CkitPoint3& o_point1, CkitPoint3& o_point2,
......
......@@ -64,7 +64,7 @@ ChppPlanner::~ChppPlanner()
ktStatus ChppPlanner::addHppProblem(CkppDeviceComponentShPtr robot)
{
ChppProblem hppProblem(robot);
ChppProblem hppProblem(robot, mObstacleList);
cout<<"adding a problem in vector"<<endl;
// Add robot in vector .
......@@ -86,7 +86,7 @@ ktStatus ChppPlanner::addHppProblem(CkppDeviceComponentShPtr robot)
ktStatus ChppPlanner::addHppProblemAtBeginning(CkppDeviceComponentShPtr robot)
{
ChppProblem hppProblem(robot);
ChppProblem hppProblem(robot, mObstacleList);
cout<<"adding a problem at beginning of vector"<<endl;
// Add robot in vector .
......@@ -445,30 +445,62 @@ ktStatus ChppPlanner::solveOneProblem(unsigned int problemId)
return KD_OK ;
}
ktStatus ChppPlanner::optimizePath(unsigned int inProblemId, unsigned int inPathId)
{
if (inProblemId >= getNbHppProblems()) {
cerr << "ChppPlanner::optimizePath: problem Id="
<< inProblemId << " is bigger than vector size="
<< getNbHppProblems();
return KD_ERROR;
}
ChppProblem& hppProblem = hppProblemVector[inProblemId];
if (inPathId >= hppProblem.getNbPaths()) {
cerr << "ChppPlanner::optimizePath: problem Id="
<< inPathId << " is bigger than number of paths="
<< hppProblem.getNbPaths();
return KD_ERROR;
}
CkwsPathShPtr kwsPath = hppProblem.getIthPath(inPathId);
// optimizer for the path
if (hppProblem.pathOptimizer()) {
hppProblem.pathOptimizer()->optimizePath(kwsPath, hppProblem.roadmapBuilder()->penetration());
cout << "ChppPlanner::solveOneProblem: path optimized with penetration "
<< hppProblem.roadmapBuilder()->penetration()<<endl;
} else {
cerr << " no Optimizer Defined " << endl ;
}
return KD_OK;
}
// ==========================================================================
unsigned int ChppPlanner::getNbPaths(unsigned int problemId) const
unsigned int ChppPlanner::getNbPaths(unsigned int inProblemId) const
{
unsigned int nbPaths = 0;
if (problemId < getNbHppProblems()) {
nbPaths = hppProblemVector[problemId].getNbPaths();
if (inProblemId < getNbHppProblems()) {
nbPaths = hppProblemVector[inProblemId].getNbPaths();
} else {
cerr << "ChppPlanner::getNbPaths : problemId = "<< problemId << " should be smaller than nb of problems: " << getNbHppProblems() << endl;
cerr << "ChppPlanner::getNbPaths : inProblemId = "<< inProblemId << " should be smaller than nb of problems: " << getNbHppProblems() << endl;
}
return nbPaths;
}
// ==========================================================================
CkwsPathShPtr ChppPlanner::getPath(unsigned int problemId,
unsigned int pathId) const
CkwsPathShPtr ChppPlanner::getPath(unsigned int inProblemId,
unsigned int inPathId) const
{
CkwsPathShPtr resultPath;
if (problemId < getNbHppProblems()) {
if (pathId < hppProblemVector[problemId].getNbPaths()) {
resultPath = hppProblemVector[problemId].getIthPath(pathId);
if (inProblemId < getNbHppProblems()) {
if (inPathId < hppProblemVector[inProblemId].getNbPaths()) {
resultPath = hppProblemVector[inProblemId].getIthPath(inPathId);
}
}
return resultPath;
......@@ -476,13 +508,42 @@ CkwsPathShPtr ChppPlanner::getPath(unsigned int problemId,
// ==========================================================================
ktStatus ChppPlanner::addPath(unsigned int problemId, CkwsPathShPtr kwsPath)
ktStatus ChppPlanner::addPath(unsigned int inProblemId, CkwsPathShPtr kwsPath)
{
if (inProblemId >= hppProblemVector.size()) {
cerr << "ChppPlanner::addPath : inProblemId bigger than vector size." << endl;
return KD_ERROR;
}
hppProblemVector[inProblemId].addPath(kwsPath);
return KD_OK;
}
// ==========================================================================
ktStatus ChppPlanner::drawRoadmap(unsigned int inProblemId)
{
if (inProblemId >= hppProblemVector.size()) {
cerr << "ChppPlanner::drawRoadmap : inProblemId bigger than vector size." << endl;
return KD_ERROR;
}
ChppProblem& hppProblem = hppProblemVector[inProblemId];
if (hppProblem.drawRoadmap() == KD_ERROR) {
return KD_ERROR;
}
return KD_OK;
}
// ==========================================================================
ktStatus ChppPlanner::stopdrawingRoadmap(unsigned int inProblemId)
{
if (problemId > hppProblemVector.size()) {
cerr << "ChppPlanner::addPath : problemId bigger than vector size." << endl;
if (inProblemId >= hppProblemVector.size()) {
cerr << "ChppPlanner::drawRoadmap : inProblemId bigger than vector size." << endl;
return KD_ERROR;
}
hppProblemVector[problemId].addPath(kwsPath);
ChppProblem& hppProblem = hppProblemVector[inProblemId];
hppProblem.stopDrawingRoadmap();
return KD_OK;
}
......
......@@ -11,6 +11,9 @@
*******************************************/
#include <iostream>
#include "kwsPlusDrawRdmBuilderDelegate.h"
#include "hppProblem.h"
/*
......@@ -39,10 +42,20 @@ const std::string ChppProblem::DEVICE_KEY("device");
// ==========================================================================
ChppProblem::ChppProblem(CkppDeviceComponentShPtr inRobot)
ChppProblem::ChppProblem(CkppDeviceComponentShPtr inRobot) :
attNotificator(CkitNotificator::defaultNotificator()),
attRobot(inRobot), attDrawRoadmapDelegate(NULL)
{
}
// ==========================================================================
ChppProblem::ChppProblem(CkppDeviceComponentShPtr inRobot,
const std::vector<CkcdObjectShPtr>& inObstacleList) :
attNotificator(CkitNotificator::defaultNotificator()),
attRobot(inRobot), attDrawRoadmapDelegate(NULL)
{
attNotificator = CkitNotificator::defaultNotificator();
attRobot = inRobot;
obstacleList(inObstacleList);
}
// ==========================================================================
......@@ -145,9 +158,9 @@ ktStatus ChppProblem::addObstacle(const CkcdObjectShPtr& inObject)
if(hppBody = boost::dynamic_pointer_cast<ChppBody>(kcdBody)){
hppBody->setOuterObjects(collisionList);
}
else {
else
kcdBody->outerObjects(collisionList);
}
}
else {
std::cout << "ChppProblem::addObstacle: body is not KCD body. Obstacle is not inserted." << std::endl;
......@@ -253,7 +266,25 @@ CkwsPathOptimizerShPtr ChppProblem::pathOptimizer() {
}
ktStatus ChppProblem::drawRoadmap()
{
if (attDrawRoadmapDelegate != NULL) {
return KD_OK;
}
attDrawRoadmapDelegate = new CkwsPlusDrawRdmBuilderDelegate();
if (!attRoadmapBuilder) {
return KD_ERROR;
}
attRoadmapBuilder->addDelegate(attDrawRoadmapDelegate);
}
/** @}
*/
ktStatus ChppProblem::stopDrawingRoadmap()
{
if (attDrawRoadmapDelegate == NULL) {
return KD_OK;
}
if (!attRoadmapBuilder) {
return KD_OK;
}
return attRoadmapBuilder->removeDelegate(attDrawRoadmapDelegate);
}
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