Commit 1f67d863 authored by florent's avatar florent
Browse files

Removed files that should not be there. I do not completely understand git

behavior.
parent 0365b60d
/*
Research carried out within the scope of the Associated International Laboratory: Joint Japanese-French Robotics Laboratory (JRL)
Developed by Florent Lamiraux (LAAS-CNRS)
*/
#ifndef HPPBODY_H_
#define HPPBODY_H_
/*************************************
INCLUDE
**************************************/
#include "KineoWorks2/kwsInterface.h"
#include "KineoUtility/kitDefine.h"
#include "KineoUtility/kitInterface.h"
#include "kcd2/kcdInterface.h"
#include "kwsKcd2/kwsKCDBody.h"
#include "hppPolyhedron.h"
KIT_PREDEF_CLASS(ChppBody);
/*************************************
CLASS
**************************************/
/**
\brief This class represents bodies (solid components attached to a joint). It derives from
KineoWorks CkwsKCDBody class. In order to store more information, we have derived CkwsKCDBody class.
A name is given at the creation of the body (ChppBody::create(std::string inName)).
The constructor is protected and method create returns a shared pointer to the device.
\sa Smart pointers documentation: http://www.boost.org/libs/smart_ptr/smart_ptr.htm
*/
class ChppBody : public CkwsKCDBody
{
public:
std::string name();
static ChppBodyShPtr create(std::string inName);
void setInnerObjects (const std::vector< CkcdObjectShPtr > &i_innerObjects);
void setInnerObjects (const std::vector< CkcdObjectShPtr > &i_innerObjects,
const std::vector< CkitMat4 > &matList);
void setOuterObjects (const std::vector< CkcdObjectShPtr > &i_outerObjects);
void getInnerObjects (std::vector< CkcdObjectShPtr > & list);
void getOuterObjects (std::vector< CkcdObjectShPtr > & list);
ktStatus getExactDistance(double &dist, CkitPoint3& o_point1, CkitPoint3& o_point2,
CkcdObjectShPtr &object1, CkcdObjectShPtr &object2);
ktStatus getEstimatedDistance(double &dist,
CkcdObjectShPtr &object1, CkcdObjectShPtr &object2);
bool getCollisionVec(unsigned int &nbCollisions, std::vector<CkcdObjectShPtr> &objectVec1,
std::vector<CkcdObjectShPtr> &objectVec2);
bool getCollision(unsigned int &nbCollisions,
CkcdObjectShPtr &object1, CkcdObjectShPtr &object2);
bool printCollisionStatus(const bool& detailInfoFlag = false);
void printCollisionStatusFast();
/**
\brief Functions for physical properties : Setting and getting mass.
*/
void mass(double m);
double mass();
/**
\brief Setting and getting moments of inertia.
*/
void intertia(std::vector<double> i);
std::vector<double> inertia();
/**
\brief Setting and getting relative CoM vector.
*/
void relComVec(double x, double y, double z);
void relComVec(const CkitVect3& vec);
const CkitVect3& relComVec() const;
/**
\brief Calculate absolute position of CoM from Joint information
*/
ktStatus currentComPos(CkitPoint3 &pos);
protected:
ChppBody(std::string inName) : bodyName(inName) {};
ktStatus init(const ChppBodyWkPtr bodyWkPtr);
private:
std::string bodyName;
CkcdAnalysisShPtr m_exact_analyzer;
std::vector< CkcdObjectShPtr > inner, outer;
/// physical properties
double _mass;
std::vector<double> _inertia;
CkitVect3 _relComVec;
};
#endif /*HPPBODY_H_*/
/*
Research carried out within the scope of the Associated International Laboratory: Joint Japanese-French Robotics Laboratory (JRL)
Developed by Florent Lamiraux (LAAS-CNRS) Eiichi Yoshida (JRL/LAAS-CNRS/AIST)
*/
#ifndef HPPBOX_H_
#define HPPBOX_H_
/*************************************
INCLUDE
**************************************/
#include "KineoKCDModel/kppKCDBox.h"
//#include "KineoWorks2/kwsInterface.h"
// #include "kitInterface.h"
// #include "kcd2/kcdInterface.h"
// #include "kwsKcd2/kwsKCDBody.h"
// #include "hppNotification.h"
KIT_PREDEF_CLASS(ChppBox);
/**
\brief This class represents polyhedra. It derives from KCD CkcdBox class.
In order to store more information, we have derived CkcdBox class.
A name is given at the creation of the polyhedron (ChppBox::create(std::string inName)).
The constructor is protected and method create returns a shared pointer to the device.
\sa Smart pointers documentation: http://www.boost.org/libs/smart_ptr/smart_ptr.htm
*/
class ChppBox : public CkppKCDBox {
public:
// already implemented std::string name();
static ChppBoxShPtr create(const std::string &inName,
const double i_xSize, const double i_ySize, const double i_zSize);
protected:
ChppBox() {};
void init(const ChppBoxWkPtr& inBoxWkPtr, const std::string &i_name,
const double i_xSize, const double i_ySize, const double i_zSize);
//private:
// already implemented std::string boxName;
};
#endif /*HPPBOX_H_*/
/*
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 (ISRI-AIST)
*/
#ifndef HPP_DEVICE_H
#define HPP_DEVICE_H
/*************************************
INCLUDE
**************************************/
#include "KineoWorks2/kwsInterface.h"
#include "KineoUtility/kitInterface.h"
#include "KineoModel/kppDeviceComponent.h"
#include "kcd2/kcdInterface.h"
#include "kwsKcd2/kwsKCDBody.h"
KIT_PREDEF_CLASS(ChppDevice);
/**
\brief This class represents devices (robots). It derives from KineoWorks CkwsDevice class.
In order to give a name to each robot, we have derived CkwsDevice class. The name is given at the creation of the device (ChppDevice::create()). The constructor is private and method create returns a shared pointer to the device.
\sa Smart pointers documentation: http://www.boost.org/libs/smart_ptr/smart_ptr.htm
*/
class ChppDevice : public CkppDeviceComponent {
public:
virtual ~ChppDevice();
static ChppDeviceShPtr create(const std::string inName);
static ChppDeviceShPtr createCopy(const ChppDeviceShPtr& i_deviceComponent);
virtual CkwsDeviceShPtr clone() const;
virtual CkppComponentShPtr cloneComponent() const;
virtual bool isComponentClonable() const;
/**
\brief provide the total mass of the robot.
*/
void updateTotalMass();
double totalMass() const;
/**
\brief Compute the position of the center of mass of the robot in current configuration.
\todo Eiichi: write the function.
*/
CkitPoint3 computeCenterOfMass();
/**
\brief Get a shared pointer on the joint corresponding to the robot gaze if any.
*/
const CkwsJointShPtr& gazeJoint() const;
/**
\brief Set the joint corresponding to the robot gaze.
*/
void gazeJoint(CkwsJointShPtr& inGazeJoint);
/**
\brief Get initial gaze direction (in zero-configuration)
*/
const CkitVect3& initGazeDir() const;
/**
\brief Set initial gaze direction (in zero-configuration)
*/
void initGazeDir(const CkitVect3& inInitGazeDir);
/**
\brief Compute the bounding box of the robot in current configuration.
*/
ktStatus axisAlignedBoundingBox (double& xMin, double& yMin, double& zMin,
double& xMax, double& yMax, double& zMax) const;
/**
\brief put the device give in parameter in the IgnoredOuterObject of the hppDevice (this)
\param deviceIgnored : the device to be ignored
*/
ktStatus ignoreDeviceForCollision (ChppDeviceShPtr deviceIgnored ) ;
/**
* \brief Add obstacle to the list.
* \param inObject a new object.
* \note Compute collision entities.
*/
ktStatus addObstacle(const CkcdObjectShPtr& inObject);
protected:
/**
\brief Joint corresponding to the robot gaze (head for HRP-2)
*/
CkwsJointShPtr attGazeJoint;
/**
\brief Gaze direction in zero-configuration.
*/
CkitVect3 attInitGazeDir;
protected:
/**
\brief Initialization.
*/
ktStatus init( const ChppDeviceWkPtr& i_weakPtr, const std::string& i_name);
/**
\brief Initialization with shared pointer.
*/
ktStatus init( const ChppDeviceWkPtr& i_weakPtr, const ChppDeviceShPtr& i_device);
/**
\brief Constructor.
*/
ChppDevice();
/**
\brief Copy constructor.
*/
ChppDevice(const ChppDevice& i_device);
private:
/**
\brief Name of the device.
*/
// std::string deviceName;
ChppDeviceWkPtr m_weakPtr;
/**
\brief Total Mass.
*/
double _totalMass;
void cksBodyBoundingBox(const CkwsKCDBodyShPtr& body, double& xMin, double& yMin,
double& zMin, double& xMax, double& yMax, double& zMax) const;
void ckcdObjectBoundingBox(const CkcdObjectShPtr& object, double& xMin, double& yMin,
double& zMin, double& xMax, double& yMax, double& zMax) const;
};
#endif
/*
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_PLANNER_H
#define HPP_PLANNER_H
/*************************************
INCLUDE
**************************************/
#include <deque>
#include "KineoWorks2/kwsPathOptimizer.h"
#include "KineoWorks2/kwsRoadmapBuilder.h"
#include "KineoUtility/kitNotificator.h"
#include "hppProblem.h"
#ifndef WITHOUT_CHPPDEVICE
KIT_PREDEF_CLASS( ChppBody );
#endif
using namespace std ;
/*************************************
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 Allocate a KineoWorks CkitNotificator object.
*/
ChppPlanner();
virtual ~ChppPlanner();
/**
\name Problem definition
@{
*/
/**
\brief Add a Problem to the Problem vector with the associed robot.
\param robot : a shared pointer to a robot
\return a int
\xrefitem <send-notif> "Notification" "Send Notification" Send ID_HPP_ADD_ROBOT.
*/
ktStatus addHppProblem(CkppDeviceComponentShPtr robot);
/**
\brief Add a Problem at beginning of the Problem vector with the associed robot.
\param robot : a shared pointer to a robot
\return a int
\xrefitem <send-notif> "Notification" "Send Notification" Send ID_HPP_ADD_ROBOT.
*/
ktStatus addHppProblemAtBeginning(CkppDeviceComponentShPtr robot);
/**
\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 rank
\return the shared pointer on the robot
*/
const CkppDeviceComponentShPtr robotIthProblem(unsigned int rank) const;
/**
\brief Get current configuration of i-th robot.
\param rank : Id of problem in vector.
\return : return a copy of the current configuration of robot.
*/
CkwsConfigShPtr robotCurrentConfIthProblem(unsigned int rank) const;
/**
\brief Set current configuration of i-th robot.
\param rank 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 rank,
const CkwsConfigShPtr& config);
ktStatus robotCurrentConfIthProblem(unsigned int rank,
const CkwsConfig& config);
/**
\brief Get initial configuration of i-th robot.
\param rank : Id of problem in vector.
\return initial configuration of robot.
\return KD_OK or KD_ERROR
*/
CkwsConfigShPtr initConfIthProblem(unsigned int rank) const;
/**
\brief Set initial configuration of i-th robot.
\param rank 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 rank,
const CkwsConfigShPtr config);
/**
\brief Get goal configuration of i-th robot.
\param rank : Id of problem in vector.
\return the goal configuration of robot.
\return KD_OK or KD_ERROR
*/
CkwsConfigShPtr goalConfIthProblem(unsigned int rank) const;
/**
\brief Set goal configuration of i-th robot.
\param rank 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 rank,
const CkwsConfigShPtr config);
/**
\brief Set roadmap builder of i-th problem.
\param rank Rank of problem in ChppPlanner::hppProblemVector.
\param inRoadmapBuilder roadmap builder.
*/
ktStatus roadmapBuilderIthProblem(unsigned int rank, CkwsRoadmapBuilderShPtr inRoadmapBuilder);
/**
\brief Get roadmap builder of i-th problem.
\param rank Rank of problem in ChppPlanner::hppProblemVector.
\return shared pointer to roadmap builder.
*/
CkwsRoadmapBuilderShPtr roadmapBuilderIthProblem(unsigned int rank);
/**
\brief Set path optimizer of i-th problem.
\param rank Rank of problem in ChppPlanner::hppProblemVector.
\param inPathOptimizer path optimizer.
*/
ktStatus pathOptimizerIthProblem(unsigned int rank, CkwsPathOptimizerShPtr inPathOptimizer);
/**
\brief Get path optimizer of i-th problem.
\param rank Rank of problem in ChppPlanner::hppProblemVector.
\return shared pointer to path optimizer.
*/
CkwsPathOptimizerShPtr pathOptimizerIthProblem(unsigned int rank);
/**
\brief Set steering Method of i-th problem.
\param rank Rank of problem in ChppPlanner::hppProblemVector.
\param inSM steering Method.
*/
ktStatus steeringMethodIthProblem(unsigned int rank, CkwsSteeringMethodShPtr inSM);
/**
\brief Get steering Method of i-th problem.
\param rank Rank of problem in ChppPlanner::hppProblemVector.
\return shared pointer to steering Method.
*/
CkwsSteeringMethodShPtr steeringMethodIthProblem(unsigned int rank);
/**
* \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.
* \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);
/**
*@}
*/
/**
\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 problemId the rank of the problem in vector.
\return KD_OK or KD_ERROR
If successful, the function stores the resulting path in the hppProblem (hppProblemVector[problemId].addPath()).
*/
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.
*/
unsigned int getNbPaths(unsigned int problemId) const;
/**
\brief Get path of given rank in given problem.
*/
CkwsPathShPtr getPath(unsigned int problemId, unsigned int pathId) const;
/**
\brief Add a path to a given problem.
\param problemId rank of problem in vector.
\param kwsPath the path to add.
\return KD_ERROR if problemId too big, KD_OK otherwise.
*/
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);
/**
*@}*
*/
/**
\name Search functions
@{
*/
/**
* \brief Find a body by name.
* \param inBodyName name of the body
* \return const shared pointer to the body
*
Look among all the bodies of the robot of each problem for a given name.
*/
ChppBodyConstShPtr findBodyByName(std::string inBodyName) const;
/**
*@}*
*/
/**
\name Notification management
@{
*/
/**
\brief Subscribe to an event by calling kwsNotificator->subscribe;
*/
// CkitNotificator::eventRegisterPair subscribe(int inEventType, ktStatus(*inCallback)(void*,void*),void* inPt);
/**