Commit 7ec4e8a6 authored by florent's avatar florent
Browse files

Revert "First version of package that compiles with hppModel package."

This version was supposed to be a local branch but I pushed it by mistake.

This reverts commit 35f163de.
parent 35f163de
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.
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.
2007/08/23
Replaced ChppDevice by CkppDeviceComponent in ChppPlanner and ChppProblem since
1 ChppDevice derives from CkppDeviceComponent and
......
......@@ -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
-----------
......
......@@ -20,36 +20,6 @@ AC_SUBST(KWSPLUS_CFLAGS)
AC_SUBST(KWSPLUS_LIBS)
AC_SUBST(KWSPLUS_PREFIX)
AC_ARG_ENABLE(body, AC_HELP_STRING([--enable-body],
[Implement ChppBody class (see INSTALL for more information)]), body=no, body=no)
AC_SUBST(BODY_CFLAGS)
AC_SUBST(BODY_HEADER_PATH)
AC_SUBST(HPPCORE_REQUIRE)
AC_SUBST(HPPMODEL_CFLAGS)
AC_SUBST(HPPMODEL_LIBS)
AC_SUBST(HPPMODEL_TAGFILE, [])
AC_SUBST(HPPMODEL_PREFIX)
if test x${enable_body} = xyes; then
body=yes
fi
BODY_CFLAGS="IMPLEMENT_BODY=0"
BODY_HEADER_PATH="hppCore"
if test x${body} = xyes; then
BODY_CFLAGS="IMPLEMENT_BODY=1"
BODY_HEADER_PATH="hppBody"
else
PKG_CHECK_MODULES(HPPMODEL, hppModel)
HPPMODEL_PREFIX=`$PKG_CONFIG --variable=prefix hppModel`
HPPMODEL_TAGFILE="$HPPMODEL_PREFIX/share/doc/doxytag/hppModel.doxytag=$HPPMODEL_PREFIX/share/doc/hppModel"
HPPCORE_REQUIRE="hppModel"
fi
AM_CONDITIONAL(BODY, test x${body} = xyes)
AC_OUTPUT(Makefile
src/Makefile
include/Makefile
......
......@@ -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: kwsPlus >= 1.0
Libs: -L${libdir} -lhppCore @KWSPLUS_LIBS@
Cflags: -I${includedir}/hpp @KWSPLUS_CFLAGS@
......@@ -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
......@@ -18,6 +18,9 @@ INCLUDE
#include "kcd2/kcdInterface.h"
#include "kwsKcd2/kwsKCDBody.h"
#include "hppPolyhedron.h"
KIT_PREDEF_CLASS(ChppBody);
......@@ -62,7 +65,6 @@ public:
bool printCollisionStatus(const bool& detailInfoFlag = false);
void printCollisionStatusFast();
#if 0
/**
\brief Functions for physical properties : Setting and getting mass.
*/
......@@ -86,7 +88,7 @@ public:
\brief Calculate absolute position of CoM from Joint information
*/
ktStatus currentComPos(CkitPoint3 &pos);
#endif
protected:
ChppBody(std::string inName) : bodyName(inName) {};
......@@ -98,6 +100,11 @@ private:
CkcdAnalysisShPtr m_exact_analyzer;
std::vector< CkcdObjectShPtr > inner, outer;
/// physical properties
double _mass;
std::vector<double> _inertia;
CkitVect3 _relComVec;
};
......
/*
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) Eiichi Yoshida (JRL/LAAS-CNRS/AIST)
*/
#ifndef HPPPOLYHEDRON_H_
#define HPPPOLYHEDRON_H_
/*************************************
INCLUDE
**************************************/
#include "KineoKCDModel/kppKCDPolyhedron.h"
//#include "KineoWorks2/kwsInterface.h"
// #include "kitInterface.h"
// #include "kcd2/kcdInterface.h"
// #include "kwsKcd2/kwsKCDBody.h"
// #include "hppNotification.h"
KIT_PREDEF_CLASS(ChppPolyhedron);
/**
\brief This class represents polyhedra. It derives from KCD CkcdPolyhedron class.
In order to store more information, we have derived CkcdPolyhedron class.
A name is given at the creation of the polyhedron (ChppPolyhedron::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 ChppPolyhedron : public CkppKCDPolyhedron {
public:
//already implemented std::string name();
static ChppPolyhedronShPtr create(const std::string &inName);
void addPoint (const kcdReal i_x, const kcdReal i_y, const kcdReal i_z, unsigned int &o_rank);
protected:
ChppPolyhedron() {};
void init(const ChppPolyhedronWkPtr& inPolyhedronShPtr, const std::string &i_name);
// private:
// already implemented std::string polyhedronName;
};
#endif /*HPPPOLYHEDRON_H_*/
......@@ -7,20 +7,16 @@
lib_LTLIBRARIES = libhppCore.la
libhppCore_la_SOURCES = \
hppPolyhedron.cpp \
hppBox.cpp \
hppBody.cpp \
hppDevice.cpp \
hppColPair.cpp \
hppProblem.cpp \
hppPlanner.cpp
libhppCore_la_CPPFLAGS = -I$(srcdir)/../include/hppCore @KWSPLUS_CFLAGS@
libhppCore_la_CPPFLAGS = -I$(srcdir)/../include @KWSPLUS_CFLAGS@
libhppCore_la_LDFLAGS =\
@KWSPLUS_LIBS@ \
-release ${PACKAGE_VERSION}
if BODY
libhppCore_la_SOURCES += \
hppBody.cpp
libhppCore_la_CPPFLAGS += -I$(srcdir)/../include/hppBody -DIMPLEMENT_HPPBODY
else
libhppCore_la_CPPFLAGS += $(HPPMODEL_CFLAGS)
endif
......@@ -5,10 +5,8 @@
*/
#include <iostream>
#include "hppBody.h"
#include "KineoKCDModel/kppKCDPolyhedron.h"
#include <iostream>
//=============================================================================
......@@ -369,7 +367,6 @@ void ChppBody::printCollisionStatusFast()
}
#if 0
//=============================================================================
void ChppBody::mass(double m)
......@@ -438,5 +435,3 @@ ktStatus ChppBody::currentComPos(CkitPoint3 &pos)
return KD_OK;
}
#endif
/*
Research carried out within the scope of the Associated International Laboratory: Joint Japanese-French Robotics Laboratory (JRL)
Developed by Florent Lamiraux (LAAS-CNRS)
*/
#include "hppBox.h"
ChppBoxShPtr ChppBox::create(const std::string &inName,
const double i_xSize, const double i_ySize, const double i_zSize)
{
ChppBox *hppBox = new ChppBox();
ChppBoxShPtr hppBoxShPtr(hppBox);
hppBox->init(hppBoxShPtr, inName, i_xSize, i_ySize, i_zSize);
return hppBoxShPtr;
}
void ChppBox::init(const ChppBoxWkPtr& inBoxWkPtr, const std::string &i_name,
const double i_xSize, const double i_ySize, const double i_zSize)
{
CkppKCDBox::init(inBoxWkPtr, i_name, i_xSize, i_ySize, i_zSize);
}
// already implemented
/*
std::string ChppBox::name()
{
return boxName;
}
*/
/*
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)
*/
#include <iostream>
#include "hppDevice.h"
#include "hppBody.h"
// ==========================================================================
ChppDevice::ChppDevice() : _totalMass(0.0)
{
/**
\brief Constructor of an empty robot with a given name.
*/
attGazeJoint.reset();
}
// ==========================================================================
ChppDevice::ChppDevice(const ChppDevice& i_device) :
CkppDeviceComponent( i_device )
{
// no op
}
ChppDevice::~ChppDevice()
{
// no op
}
// ==========================================================================
ChppDeviceShPtr ChppDevice::create(std::string inName)
{
ChppDevice *hppDevice = new ChppDevice();
ChppDeviceShPtr hppDeviceShPtr(hppDevice);
if (hppDevice->init(hppDeviceShPtr, inName) != KD_OK) {
hppDeviceShPtr.reset();
}
return hppDeviceShPtr;
}
// ==========================================================================
ChppDeviceShPtr ChppDevice::createCopy(const ChppDeviceShPtr& i_device)
{
ChppDevice* ptr = new ChppDevice(*i_device);
ChppDeviceShPtr shPtr(ptr);
if(KD_OK != ptr->init(shPtr, i_device))
{
shPtr.reset();
}
return shPtr;
}
// ==========================================================================
CkwsDeviceShPtr ChppDevice::clone() const
{
return ChppDevice::createCopy(m_weakPtr.lock());
}
// ==========================================================================
CkppComponentShPtr ChppDevice::cloneComponent() const
{
return ChppDevice::createCopy(m_weakPtr.lock());
}
// ==========================================================================
bool ChppDevice::isComponentClonable() const
{
return true;
}
// ==========================================================================
ktStatus ChppDevice::init(const ChppDeviceWkPtr& inDevWkPtr, const std::string &i_name)
{
ktStatus success = CkppDeviceComponent::init(inDevWkPtr, i_name);
if(KD_OK == success)
{
m_weakPtr = inDevWkPtr;
}
return success;
}
// ==========================================================================
ktStatus ChppDevice::init(const ChppDeviceWkPtr& i_weakPtr,
const ChppDeviceShPtr& i_device)
{
ktStatus success = CkppDeviceComponent::init(i_weakPtr, i_device);
if(KD_OK == success)
{
m_weakPtr = i_weakPtr;
}
return success;
}
// ==========================================================================
/*
std::string ChppDevice::name()
{
return deviceName;
}
*/
// ==========================================================================
void ChppDevice::updateTotalMass()