Commit b3f475a9 authored by florent's avatar florent
Browse files

Merge with git+ssh://softs.laas.fr/git/jrl/hppCore

Resolved silly conflicts. (Conflicts are often silly).
parents 9c9853bc 2a0c113f
<<<<<<< master
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
......@@ -15,51 +16,65 @@
hppCore can now be installed
either with a dependence to hppModel (implementing ChppDevice, ChppBody, ChppJoint)
or with a simplified implementation of ChppBody without dynamics but with enhanced distance computation facilities.
=======
hppCore 1.2 released October 10, 2007
>>>>>>> origin
2007/08/23
Replaced ChppDevice by CkppDeviceComponent in ChppPlanner and ChppProblem since
1 ChppDevice derives from CkppDeviceComponent and
2 The operations performed on objects of this type only concern CkppDeviceComponent part.
2007/08/01
15. Added a default implementation of ChppPlanner::solve so that the class is not pure virtual.
2007/09/30
21. When inserting a new problem, attach existing obstacles to the robot.
2007/09/29
20. Added API function to optimize a path.
19. Added API functions to display a roadmap in the interface.
2007/09/12
18. Add virtual method ChppPlanner::initializeProblem(). This method implemented in ChppWalkPlanner can thus be called by Corba server.
hppCore 1.1 released August 27, 2007
17. Replaced ChppDevice by CkppDeviceComponent in ChppPlanner and
ChppProblem since
1 ChppDevice derives from CkppDeviceComponent and
2 The operations performed on objects of this type only concern
CkppDeviceComponent part.
15. Added a default implementation of ChppPlanner::solve so that
the class is not pure virtual.
17. Added a default implementation of ChppPlanner::solve.
16. Improvement of collision information displaying.
15. Call path opthmizer with the penetration specified for roadmap
builder.
hppCore 1.0.1 released July 20, 2007
14. remove ChppColPair::dof since it was not used
hppCore 1.0 released July 10, 2007
13. add a function in hppDevice to ignore another device (as its bounding box for example).
2007/04/26
12. Class ChppRoadmap has been moved into package kwsPlus (class CkwsPlusRoadmap) so that it can be used outside HPP.
13. add a function in hppDevice to ignore another device (as its
bounding box for example).
12. Class ChppRoadmap has been moved into package kwsPlus (class
CkwsPlusRoadmap) so that it can be used outside HPP.
11. Modified files accordingly.
10. Added information on main page.
2007/04/25
9. Added class ChppRoadmap in hppCore : This class allows users to display a roadmap in the scene. To do so, change every occurence
of CkwsRoadmap by ChppRoadmap, and call method compute(), then display(). Calls to these methods must be done only after a
solution path has been found. Linked Device must be ChppDevice.
9. Added class ChppRoadmap in hppCore : This class allows users to
display a roadmap in the scene. To do so, change every occurence
of CkwsRoadmap by ChppRoadmap, and call method compute(), then
display(). Calls to these methods must be done only after a
solution path has been found. Linked Device must be ChppDevice.
8. Replace occurences of kwsRoadmap by hppRoadmap.
2007/04/23
7. Removed @KWSIO_XXX@ from Makefile.am
6. Moved first page of documentation to general documentation hppDoc.
2007/04/22
5. Generate doxytag file to access this documentation from depending packages.
4. Documentation is now compile at installation.
2007/04/17
3. Correction of a bug in function ChppProblem::ObstacleList(const std::vector<CkcdObjectShPtr>& inCollisionList):
the list of outer objects of each body of the robot was copied and each obstacle of the new obstacle list was added
at the end of the copy of the outer object list, but not in the outer object list.
2007/03/09
3. Correction of a bug in function ChppProblem::ObstacleList(const
std::vector<CkcdObjectShPtr>& inCollisionList):
the list of outer objects of each body of the robot was copied and
each obstacle of the new obstacle list was added
at the end of the copy of the outer object list, but not in the
outer object list.
2. Added method
ktStatus addHppProblemAtBeginning(ChppDeviceShPtr robot);
in ChppPlanner.
1. Replaced std::vector by std::deque to store vector of problem,
in order to be able to add a problem at the beginning of the vector.
in order to be able to add a problem at the beginning of the vector.
......@@ -6,12 +6,7 @@ untar the package
cd hppCore
mkdir build
cd build
../configure [OPTIONS]
Specific options are the following:
--enable-body: Provides an implementation of class ChppBody deriving from CkwsKCDBody.
If not set, the package depends from hppModel that provides an implementation of this class.
../configure
Compilation
-----------
......
No news is good news.
#!/bin/sh
aclocal
libtoolize -c
automake -ca
autoconf
......@@ -8,18 +8,20 @@ dnl
AC_INIT([hppCore],[1.2],[openrobots@laas.fr])
AC_PREREQ(2.59)
AM_INIT_AUTOMAKE
AM_INIT_AUTOMAKE([foreign no-define])
AC_PROG_INSTALL
AC_PROG_LIBTOOL
AC_PROG_CXX
PKG_CHECK_MODULES(KWSPLUS, kwsPlus)
define(kwplus_reqd, [kwsPlus >= 1.1])
AC_SUBST(KWPLUS_REQD, "kwplus_reqd")
PKG_CHECK_MODULES(KWSPLUS, kwplus_reqd)
KWSPLUS_PREFIX=`$PKG_CONFIG kwsPlus --variable=prefix`
AC_SUBST(KWSPLUS_CFLAGS)
AC_SUBST(KWSPLUS_LIBS)
AC_SUBST(KWSPLUS_PREFIX)
<<<<<<< master
AC_ARG_ENABLE(body, AC_HELP_STRING([--enable-body],
[Implement ChppBody class (see INSTALL for more information)]), body=no, body=no)
......@@ -52,6 +54,8 @@ fi
AM_CONDITIONAL(BODY, test x${body} = xyes)
=======
>>>>>>> origin
AC_OUTPUT(Makefile
src/Makefile
include/Makefile
......@@ -59,4 +63,3 @@ AC_OUTPUT(Makefile
doc/hpp.dox
hppCore.pc
)
......@@ -3,6 +3,7 @@
This package implements basic classes used as interfaces with KineoWorks.
The main classes are:
\li ChppDevice: implement a humanoid robot with a dynamical model.
\li ChppProblem: define a canonical path planning problem.
\li ChppPlanner: contains a vector of above path planning problems to implement iterative planning algorithms that use several instaciations of a robot.
......
......@@ -158,8 +158,7 @@ SKIP_FUNCTION_MACROS = YES
#---------------------------------------------------------------------------
# Configuration::additions related to external references
#---------------------------------------------------------------------------
TAGFILES = @HPPMODEL_TAGFILE@ \
@KWSPLUS_PREFIX@/share/doc/doxytag/kwsPlus.doxytag=@KWSPLUS_PREFIX@/share/doc/kwsPlus
TAGFILES = @KWSPLUS_PREFIX@/share/doc/doxytag/kwsPlus.doxytag=@KWSPLUS_PREFIX@/share/doc/kwsPlus
GENERATE_TAGFILE = hppCore.doxytag
ALLEXTERNALS = NO
EXTERNAL_GROUPS = YES
......@@ -208,8 +207,7 @@ SHOW_DIRECTORIES = NO
#---------------------------------------------------------------------------
FILE_PATTERNS = *.h *.idl
EXCLUDE_PATTERNS =
INPUT = @top_srcdir@/include/hppCore \
@top_srcdir@/include/@BODY_HEADER_PATH@ \
INPUT = @top_srcdir@/include \
@srcdir@/additionalDoc
#---------------------------------------------------------------------------
......
......@@ -6,6 +6,6 @@ includedir=@includedir@
Name: @PACKAGE@
Description: Core functions of humanoid path planner
Version: @VERSION@
Requires: kwsPlus >= 1.0, @HPPCORE_REQUIRE@
Libs: -L${libdir} -lhppCore @KWSPLUS_LIBS@ @HPPMODEL_LIBS@
Cflags: -I${includedir}/hpp @KWSPLUS_CFLAGS@ @HPPMODEL_CFLAGS@
Requires: @KWPLUS_REQD@
Libs: -Wl,-R${libdir} -L${libdir} -lhppCore
Cflags: -I${includedir}/hpp
......@@ -7,13 +7,14 @@
hppCoredir = @includedir@/hpp
hppCore_HEADERS = \
hppCore/hppColPair.h \
hppCore/hppProblem.h \
hppCore/hppPlanner.h
hppBody.h \
hppDevice.h \
hppColPair.h \
hppPolyhedron.h \
hppBox.h \
hppProblem.h \
hppPlanner.h
if BODY
hppCore_HEADERS += \
hppBody/hppBody.h
endif
# hppNotification.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 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.
*/