Commit c7fe0565 authored by florent's avatar florent
Browse files

Install headers in include/hpp/core.

	* include/hpp/core/collision-pair.hh: new,
	* include/hpp/core/planner.hh: new,
	* include/hpp/core/problem.hh: new,
	* include/hppCore/hppColPair.h: declare as deprecated,
	* include/hppCore/hppPlanner.h: declare as deprecated,
	* include/hppCore/hppProblem.h: declare as deprecated.
parent 7c8b12e3
#ifndef HPP_CORE_COLLISION_PAIR_H
#define HPP_CORE_COLLISION_PAIR_H
#include <iostream>
#include <map>
#include <vector>
#include <utility> // pair
#include <functional> // greater
/**
\brief Collision pair generating class
*/
class ChppColPair { // friend?
typedef std::pair<unsigned int, std::vector<unsigned int> > cPair;
typedef std::map< unsigned int, std::vector<unsigned int>, std::less<unsigned int> > cMap;
std::vector<unsigned int> tmp;
/**
\brief self collision table
*/
cMap mapCol;
public:
/**
\brief constructor
*/
ChppColPair();
/**
\brief destructor
*/
~ChppColPair();
/**
\brief add a link jounsigned int j1 as collision-checking pair of j2
*/
bool addColPair(unsigned int j1, unsigned int j2);
/**
\brief add a link for jounsigned int j1 as collision-checking pair from j2from of j2to
*/
bool addColPairRange(unsigned int j1, unsigned int j2from, unsigned int j2to);
/**
\brief check if there is already key j1
*/
bool existKey(unsigned int j1);
/**
\brief check if there is already that pair in the table
*/
bool existPair(unsigned int j1, unsigned int j2);
/**
\brief check if there is value j2 for key j1
*/
bool existPairNarrow(unsigned int j1, unsigned int j2);
/**
\brief get vector of collision pair for body of joint 1
*/
std::vector<unsigned int>& getColPairList(unsigned int j, bool& flag);
void printPair();
}; // this semicolon is very important
#endif // colPair.h
/*
Research carried out within the scope of the Associated International Laboratory: Joint Japanese-French Robotics Laboratory (JRL)
Developed by Florent Lamiraux (LAAS-CNRS)
*/
#ifndef HPP_CORE_PLANNER_H
#define HPP_CORE_PLANNER_H
/*************************************
INCLUDE
**************************************/
#include <deque>
#include "KineoWorks2/kwsPathOptimizer.h"
#include "KineoWorks2/kwsRoadmapBuilder.h"
#include "KineoUtility/kitNotificator.h"
#include "hpp/core/problem.hh"
class CkwsPlusStopRdmBuilderDelegate;
KIT_PREDEF_CLASS(CkppKCDBody);
#ifndef WITHOUT_CHPPDEVICE
KIT_PREDEF_CLASS( ChppBody );
#endif
KIT_PREDEF_CLASS(CkwsConfigExtractor);
KIT_PREDEF_CLASS(ChppPlanner);
/*************************************
CLASS
**************************************/
/**
\brief Motion planning strategies for humanoid robots among obstacles.
This abstract class defines motion planning strategies for a humanoid robot in an environment with obstacles.
Different strategies can be implemented by deriving this class.
Several strategies instantiate several robots and classical path planning problems. Therefore, this class contains a vector of ChppProblem describing basic motion planning problems.
*/
class ChppPlanner {
public:
/**
\brief Empty constructor
Allocate a KineoWorks CkitNotificator object.
*/
ChppPlanner();
/**
\brief Copy constructor
*/
ChppPlanner(const ChppPlanner& inPlanner);
/**
\brief Destructor
*/
virtual ~ChppPlanner();
/**
\name Problem definition
@{
*/
/**
\brief Add a Problem to the Problem vector with the associed robot.
\param inRobot : a shared pointer to a robot
\param inPenetration dynamic penetration allowed for validating direct paths.
\return KD_OK if success, KD_ERROR otherwise
\xrefitem <send-notif> "Notification" "Send Notification" Send ID_HPP_ADD_ROBOT.
*/
ktStatus addHppProblem(CkppDeviceComponentShPtr inRobot, double inPenetration);
/**
\brief Remove a Problem at the end of the Problem vector.
\return a int
*/
ktStatus removeHppProblem();
/**
\brief Add a Problem at beginning of the Problem vector with the associed robot.
\param inRobot : a shared pointer to a robot
\param inPenetration dynamic penetration allowed for validating direct paths.
\return KD_OK if success, KD_ERROR otherwise
\xrefitem <send-notif> "Notification" "Send Notification" Send ID_HPP_ADD_ROBOT.
*/
ktStatus addHppProblemAtBeginning(CkppDeviceComponentShPtr inRobot, double inPenetration);
/**
\brief Remove a Problem at the beginning the Problem vector.
\return a int
*/
ktStatus removeHppProblemAtBeginning();
/**
\brief Retrieve the problem in the problem vector at the given rank.
\param inRank Rank of the hppProblem to retrieve
\return The inRank-th problem
*/
ChppProblem * hppProblem(unsigned int inRank) {
if(inRank<getNbHppProblems())
return &(hppProblemVector.at(inRank));
else
return NULL;
};
/**
\brief Get the number of problems in vector.
\return the number of problems in the vector
*/
unsigned int getNbHppProblems() const { return (unsigned int)hppProblemVector.size();};
/**
\brief Get robot at given rank in the Problem vector.
\param inRank
\return the shared pointer on the robot
*/
const CkppDeviceComponentShPtr robotIthProblem(unsigned int inRank) const;
/**
\brief Get current configuration of i-th robot.
\param inRank : Id of problem in vector.
\return : return a copy of the current configuration of robot.
*/
CkwsConfigShPtr robotCurrentConfIthProblem(unsigned int inRank) const;
/**
\brief Set current configuration of i-th robot.
\param inRank Id of robot in vector.
\param config A new config is allocated and placed into current config of robot.
\return KD_OK or KD_ERROR
\xrefitem <send-notif> "Notification" "Send Notification" Send ID_HPP_SET_CURRENT_CONFIG.
*/
ktStatus robotCurrentConfIthProblem(unsigned int inRank,
const CkwsConfigShPtr& config);
ktStatus robotCurrentConfIthProblem(unsigned int inRank,
const CkwsConfig& config);
/**
\brief Get initial configuration of i-th robot.
\param inRank : Id of problem in vector.
\return initial configuration of robot.
\return KD_OK or KD_ERROR
*/
CkwsConfigShPtr initConfIthProblem(unsigned int inRank) const;
/**
\brief Set initial configuration of i-th robot.
\param inRank Id of robot in vector.
\param config A new config is allocated and placed into initial config of robot.
\return KD_OK or KD_ERROR
*/
ktStatus initConfIthProblem(unsigned int inRank,
const CkwsConfigShPtr config);
/**
\brief Get goal configuration of i-th robot.
\param inRank : Id of problem in vector.
\return the goal configuration of robot.
\return KD_OK or KD_ERROR
*/
CkwsConfigShPtr goalConfIthProblem(unsigned int inRank) const;
/**
\brief Set goal configuration of i-th robot.
\param inRank Id of robot in vector.
\param config A new config is allocated and placed into goal config of robot.
\return KD_OK or KD_ERROR
*/
ktStatus goalConfIthProblem(unsigned int inRank,
const CkwsConfigShPtr config);
/**
\brief Set roadmap builder of i-th problem.
\param inRank Rank of problem in ChppPlanner::hppProblemVector.
\param inRoadmapBuilder roadmap builder.
\param inDisplay whether the roadmap should be displayed in the interface.
\xrefitem <send-notif> "Notification" "Send Notification" Send ID_HPP_ADD_ROADMAP.
\xrefitem <send-notif> "Notification" "Send Notification" Send ID_HPP_REMOVE_ROADMAP.
*/
virtual ktStatus roadmapBuilderIthProblem(unsigned int inRank, CkwsRoadmapBuilderShPtr inRoadmapBuilder,
bool inDisplay=false);
/**
\brief Get roadmap builder of i-th problem.
\param inRank Rank of problem in ChppPlanner::hppProblemVector.
\return shared pointer to roadmap builder.
*/
CkwsRoadmapBuilderShPtr roadmapBuilderIthProblem(unsigned int inRank);
/**
\brief Set path optimizer of i-th problem.
\param inRank Rank of problem in ChppPlanner::hppProblemVector.
\param inPathOptimizer path optimizer.
*/
ktStatus pathOptimizerIthProblem(unsigned int inRank, CkwsPathOptimizerShPtr inPathOptimizer);
/**
\brief Get path optimizer of i-th problem.
\param inRank Rank of problem in ChppPlanner::hppProblemVector.
\return shared pointer to path optimizer.
*/
CkwsPathOptimizerShPtr pathOptimizerIthProblem(unsigned int inRank);
/**
\brief Set steering Method of i-th problem.
\param inRank Rank of problem in ChppPlanner::hppProblemVector.
\param inSM steering Method.
*/
ktStatus steeringMethodIthProblem(unsigned int inRank, CkwsSteeringMethodShPtr inSM);
/**
\brief Get steering Method of i-th problem.
\param inRank Rank of problem in ChppPlanner::hppProblemVector.
\return shared pointer to steering Method.
*/
CkwsSteeringMethodShPtr steeringMethodIthProblem(unsigned int inRank);
/**
\brief Set configuration extractor to given problem
\param inRank Rank of problem in ChppPlanner::hppProblemVector.
\param inConfigExtractor Configuration extractor
A configuration extractor attempts at extracting a collision-free
configuration in the neighborhood of the initial configuration of
a path planning problem when the initial configuration is in
collision.
*/
ktStatus configExtractorIthProblem(unsigned int inRank,
const CkwsConfigExtractorShPtr& inConfigExtractor);
/**
\brief Set dynamic penetration of given problem
\param inRank Rank of problem in ChppPlanner::hppProblemVector.
\param inPenetration dynamic penetration allowed for validating a direct path.
*/
ktStatus penetration(unsigned int inRank, double inPenetration);
/**
\brief Get dynamic penetration of given problem
\param inRank Rank of problem in ChppPlanner::hppProblemVector.
\return dynamic penetration allowed for validating a direct path.
*/
double penetration(unsigned int inRank) const;
/**
* \brief Initialize the list of obstacles.
* \param collisionList list of obstacles.
* \xrefitem <send-notif> "Notification" "Send Notification" Send ID_HPP_REMOVE_OBSTACLES.
* \xrefitem <send-notif> "Notification" "Send Notification" Send ID_HPP_SET_OBSTACLE_LIST.
*
* Replaces previous list of obstacle if any. Copy the list of obstacle in each problem of the object.
*/
virtual ktStatus obstacleList(std::vector< CkcdObjectShPtr > collisionList);
/**
* \brief return a shared pointer to the obstacle list
*/
const std::vector< CkcdObjectShPtr > obstacleList();
/**
\brief Add obstacle to the list.
\param object a new object.
\param inDistanceComputation whether distance to this object should be computed in ChppBody class.
\xrefitem <send-notif> "Notification" "Send Notification" Send ID_HPP_ADD_OBSTACLE.
Add the obstacle to each problem of the object.
*/
virtual ktStatus addObstacle(CkcdObjectShPtr object,
bool inDistanceComputation = true);
/**
*@}
*/
/**
\name Parser
@{
*/
/**
\brief Parsing a file
*/
ktStatus parseFile(const std::string& inFileName);
/**
*@}
*/
/**
\name Initialization
@{
*/
/**
\brief Initialization of the path planning problem
*/
virtual ktStatus initializeProblem() {return KD_OK;};
/**
*@}
*/
/**
\name Problem resolution
@{
*/
/**
\brief Compute a solution path for each problem.
\return KD_OK or KD_ERROR
*/
virtual ktStatus solve();
/**
\brief Solve a problem in the vector.
\param inRank the rank of the problem in vector.
\return KD_OK or KD_ERROR
See ChppProblem::solve() for more information.
If successful, the function stores the resulting path in the hppProblem.
*/
virtual ktStatus solveOneProblem(unsigned int inRank);
/**
\brief Interrupt current roadmap builder.
\note if no roadmap builder is running, this will interrupt the next one that starts.
*/
void interruptPathPlanning();
/**
\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.
*/
unsigned int getNbPaths(unsigned int inRank) const;
/**
\brief Get path of given rank in given problem.
*/
CkwsPathShPtr getPath(unsigned int inRank, unsigned int pathId) const;
/**
\brief Add a path to a given problem.
\param inRank rank of problem in vector.
\param kwsPath the path to add.
\return KD_ERROR if inRank too big, KD_OK otherwise.
*/
ktStatus addPath(unsigned int inRank, CkwsPathShPtr kwsPath);
/**
*@}
*/
/**
\name Search functions
@{
*/
/**
* \brief Find a body by the name of its joint
* \param inJointName name of the joint
* \return const shared pointer to the body
*
Look among all the joitns of the robot of each problem for a given name.
*/
CkwsKCDBodyConstShPtr findBodyByJointName(const std::string& inJointName) const;
/**
*@}
*/
private:
/**
\brief pointer to a KineoWorks notificator.
*/
CkitNotificatorShPtr attNotificator;
/**
\brief Vector (deque) of path planning problems.
Some iterative path planning methods for humanoid robots
solve iteratively different basic path planning problems. Each of these problems is defined by class ChppProblem.
These basic problems are put into a vector.
*/
std::deque<ChppProblem> hppProblemVector;
/**
\brief Obstacles are a list of KCD fobjects.
*/
std::vector< CkcdObjectShPtr > attObstacleList;
/**
\brief Roadmap builder delegate enabling to interrupt roadmap builder.
*/
CkwsPlusStopRdmBuilderDelegate* attStopRdmBuilderDelegate;
// for notification.
public:
static const CkitNotification::TType ID_HPP_ADD_ROBOT;
static const CkitNotification::TType ID_HPP_SET_CURRENT_CONFIG;
static const CkitNotification::TType ID_HPP_REMOVE_OBSTACLES;
static const CkitNotification::TType ID_HPP_SET_OBSTACLE_LIST;
static const CkitNotification::TType ID_HPP_ADD_OBSTACLE;
static const CkitNotification::TType ID_HPP_REMOVE_ROADMAPBUILDER;
static const CkitNotification::TType ID_HPP_ADD_ROADMAPBUILDER;
// key to retrieve for notification.
static const std::string ROBOT_KEY;
static const std::string OBSTACLE_KEY;
static const std::string CONFIG_KEY;
static const std::string ROADMAP_KEY;
};
#endif
/*
Research carried out within the scope of the Associated International Laboratory: Joint Japanese-French Robotics Laboratory (JRL)
Developed by Florent Lamiraux (LAAS-CNRS) and Eiichi Yoshida (JRL-FRANCE/LAAS-CNRS/AIST)
*/
#ifndef HPP_CORE_PROBLEM_H
#define HPP_CORE_PROBLEM_H
#if DEBUG==3
#include "kwsioConfig.h"
#define HPPPROBLEM_ODEBUG3(x) std::cout << "ChppProblem:" << x << std::endl
#define HPPPROBLEM_ODEBUG2(x) std::cout << "ChppProblem:" << x << std::endl
#define HPPPROBLEM_ODEBUG1(x) std::cerr << "ChppProblem:" << x << std::endl
#elif DEBUG==2
#include "kwsioConfig.h"
#define HPPPROBLEM_ODEBUG3(x)
#define HPPPROBLEM_ODEBUG2(x) std::cout << "ChppProblem:" << x << std::endl
#define HPPPROBLEM_ODEBUG1(x) std::cerr << "ChppProblem:" << x << std::endl
#elif DEBUG==1
#include "kwsioConfig.h"
#define HPPPROBLEM_ODEBUG3(x)
#define HPPPROBLEM_ODEBUG2(x)
#define HPPPROBLEM_ODEBUG1(x) std::cerr << "ChppProblem:" << x << std::endl
#else
#define HPPPROBLEM_ODEBUG3(x)
#define HPPPROBLEM_ODEBUG2(x)
#define HPPPROBLEM_ODEBUG1(x)
#endif
/*************************************
INCLUDE
**************************************/
#include "KineoWorks2/kwsPathOptimizer.h"
#include "KineoWorks2/kwsRoadmapBuilder.h"
#include "KineoWorks2/kwsConfig.h"
#include "KineoModel/kppDeviceComponent.h"
#include "kcd2/kcdInterface.h"
#include "kwsKcd2/kwsKCDBody.h"
#include "KineoUtility/kitNotificator.h"
KIT_PREDEF_CLASS(CkwsConfigExtractor);
/*************************************
CLASS
**************************************/
/**
\brief Defines a path planning problem for one robot.
A path planning problem is defined by
\li a robot: a KineoWorks device,
\li a set of obstacles: a list of Kcd objects,
\li a steering method: stored in the KwsDevice,
\li initial and goal configurations,
\li a roadmap : to store a roadmap
\li a roadmapBuilder : roadmap strategy
\li a pathOptimizer : to Optimise the path
*/
class ChppProblem
{
public:
/**
* \brief Create a path planning problem with no initial nor goal configuration.
*
* \param inRobot robot associated to the path planning problem.
* \param inPenetration dynamic penetration allowed for validating direct paths.
*/
ChppProblem (CkppDeviceComponentShPtr inRobot, double inPenetration);
/**
* \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.
* \param inPenetration dynamic penetration allowed for validating direct paths.
*/
ChppProblem (CkppDeviceComponentShPtr inRobot,
const std::vector<CkcdObjectShPtr>& inObstacleList,
double inPenetration);
/**
\brief Copy constructor
*/
ChppProblem(const ChppProblem& inProblem);
/**
\name Problem definition
@{
*/
/**
* \brief return shared pointer to robot.
*/
const CkppDeviceComponentShPtr& getRobot() const {
return attRobot;
};
/**
* \brief Get shared pointer to initial configuration.
*/
const CkwsConfigShPtr& initConfig() const {
return attInitConf;
};
/**
* \brief Set initial configuration.
*/
ktStatus initConfig ( const CkwsConfigShPtr& inConfig ) {
if (inConfig->device() != attRobot) {
HPPPROBLEM_ODEBUG1
(":goalConfig: configuration device does not match problem device.");
return KD_ERROR;
}
attInitConf = inConfig;
return KD_OK;
};