Commit ba4e82a9 authored by Florent Lamiraux's avatar Florent Lamiraux Committed by Florent Lamiraux florent@laas.fr
Browse files

Remove option ENABLE_BODY and obsolete files.

parent a3757007
......@@ -40,22 +40,6 @@ IF (HPP_DEBUG)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DHPP_DEBUG")
ENDIF()
#
# Optional Dependency to package hppModel
#
IF(ENABLE_BODY)
MESSAGE(STATUS "using internal implementation of ChppBody")
SET(BODY_CFLAGS "IMPLEMENT_BODY=1")
SET(BODY_HEADER_PATH="hpp-model")
# install hpp/model/body.hh
INSTALL(FILES include/hpp-model/hpp/model/body.hh
DESTINATION include/hpp/model
)
ELSE(ENABLE_BODY)
MESSAGE(STATUS "using external implementation of hpp::model::Body")
ADD_REQUIRED_DEPENDENCY("hpp-model >= 1.8")
ENDIF(ENABLE_BODY)
# Declare Headers
SET(${PROJECT_NAME}_HEADERS
include/hpp/core/fwd.hh
......@@ -65,6 +49,7 @@ SET(${PROJECT_NAME}_HEADERS
)
ADD_REQUIRED_DEPENDENCY("kwsPlus >= 1.8")
ADD_REQUIRED_DEPENDENCY("hpp-model >= 1.8")
# Add dependency toward hpp-model library in pkg-config file.
PKG_CONFIG_APPEND_LIBS("hpp-core")
......
//
// Copyright (c) 2007, 2008, 2009, 2010, 2011 CNRS
// Authors: Florent Lamiraux
//
// This file is part of hpp-core
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core If not, see
// <http://www.gnu.org/licenses/>.
#ifndef HPP_MODEL_BODY_HH
#define HPP_MODEL_BODY_HH
/*************************************
INCLUDE
**************************************/
#include "KineoUtility/kitDefine.h"
#include "kcd2/kcdAnalysisType.h"
#include "kwsKcd2/kwsKCDBody.h"
KIT_PREDEF_CLASS(CkcdObject);
KIT_PREDEF_CLASS(CkppSolidComponentRef);
class CkitMat4;
namespace hpp {
namespace model {
KIT_PREDEF_CLASS(Body);
/// \brief Bodies (geometric objects attached to a joint).
/// It derives from KineoWorks CkwsKCDBody class.
/// Objects attached to a body (called inner objects) are used for
/// collision checking with selected objects of the environment
/// (called outer objects).
/// To attach an object to the body, call addInnerObject(). To
/// select an object for collision checking with the body, call
/// addOuterObject().
/// Distances between pairs of inner objects and outer objects can also
/// be computed. Setting <code>distanceComputation</code> to true in
/// addInnerObject() or addOuterObject() specifies that distances should
/// be computed for these objects. Each pair of such specified (inner,
/// outer) objects gives rise to one distance computation when calling
/// distAndPairsOfPoints(). The number of such pairs can be retrieved by
/// calling nbDistPairs(). distAndPairsOfPoints() also returns distances
/// and pairs of closest points for each computed pair.
/// 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 Body : public CkwsKCDBody
{
public:
/// \brief Creation of a body
/// \param name Name of the new body.
/// \return A shared pointer to a new body.
static BodyShPtr create(const std::string& name);
/// \brief Get name of object.
const std::string& name() {return name_;};
/// \name Define inner and outer objects
/// @{
/// \brief Add a geometric object to the body
/// \param solidComponentRef Reference to the solid component to add.
/// \param position Position of the object before attaching it to the body
/// (default value=Identity).
/// \param distanceComputation whether this object should be put in the
/// distance computation analysis.
/// \return true if success, false otherwise.
/// The object is added to the inner object list of the body.
/// \note The body must be attached to a joint.
bool addInnerObject(const CkppSolidComponentRefShPtr& solidComponentRef,
const CkitMat4& position=CkitMat4(),
bool distanceComputation=false);
/// \brief Add an object for collision testing with the body
/// \param outerObject new object
/// \param distanceComputation whether distance analyses should
/// be added for this object.
void addOuterObject(const CkcdObjectShPtr& outerObject,
bool distanceComputation=true);
/// \brief Reset the list of outer objects
void resetOuterObjects();
/// @}
/// \name Distance computation
/// @{
/// \brief Get number of pairs of object for which distance is computed
inline unsigned int nbDistPairs() { return distCompPairs_.size(); };
/// \brief Compute exact distance and closest points between body and set of outer objects.
/// \param pairId id of the pair of objects
/// \param type Type of distance computation (either
/// CkcdAnalysisType::EXACT_DISTANCE or
/// CkcdAnalysisType::ESTIMATED_DISTANCE)
/// \retval outDistance Distance between body and outer objects
/// \retval outPointBody Closest point on body (in global reference frame)
/// \retval outPointEnv Closest point in outer object set
/// (in global reference frame)
/// \retval outObjectBody Closest object on body
/// \retval outObjectEnv Closest object in outer object list
ktStatus distAndPairsOfPoints(unsigned int pairId,
double& outDistance,
CkitPoint3& outPointBody,
CkitPoint3& outPointEnv,
CkcdObjectShPtr &outObjectBody,
CkcdObjectShPtr &outObjectEnv,
CkcdAnalysisType::Type type=
CkcdAnalysisType::EXACT_DISTANCE);
/// @}
protected:
/// \brief Constructor by name.
Body(const std::string& name): name_(name) {};
/// \brief Initialization of body
/// \param weakPtr weak pointer to itself
ktStatus init(const BodyWkPtr weakPtr);
private:
/// \brief Name of the body.
std::string name_;
/// \brief Set of inner objects for which distance is computed
std::vector<CkcdObjectShPtr> innerObjForDist_;
/// \brief Set of outer objects for which distance is computed
std::vector<CkcdObjectShPtr> outerObjForDist_;
/// \brief Collision analyses for this body
/// Each pair (inner object, outer object) potentially defines an exact
/// distance analysis. Only inner objects specified in attDistanceObjects
/// define analyses.
std::vector<CkcdAnalysisShPtr> distCompPairs_;
/// \brief Weak pointer to itself
BodyWkPtr weakPtr_;
};
} // namespace model
} namespace hpp
#endif // HPP_MODEL_BODY_HH
......@@ -25,26 +25,12 @@ SET(${LIBRARY_NAME}_SOURCES
problem.cc
)
IF (ENABLE_BODY)
SET(${LIBRARY_NAME}_SOURCES
${${LIBRARY_NAME}_SOURCES}
body.cc
)
INCLUDE_DIRECTORIES(BEFORE ${CMAKE_SOURCE_DIR}/include/hpp-model)
SET(${LIBRARY_NAME}_SOURCE_FILES
${${LIBRARY_NAME}_SOURCE_FILES}
body.cc)
ENDIF()
ADD_LIBRARY(${LIBRARY_NAME}
SHARED
${${LIBRARY_NAME}_SOURCES}
)
IF (NOT ENABLE_BODY)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} hpp-model)
ENDIF()
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} hpp-model)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} kwsPlus)
INSTALL(TARGETS ${LIBRARY_NAME} DESTINATION lib)
\ No newline at end of file
INSTALL(TARGETS ${LIBRARY_NAME} DESTINATION lib)
//
// Copyright (c) 2007, 2008, 2009, 2010, 2011 CNRS
// Authors: Florent Lamiraux
//
// This file is part of hpp-core
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core If not, see
// <http://www.gnu.org/licenses/>.
#include <iostream>
#include "KineoModel/kppSolidComponentRef.h"
#include "KineoModel/kppJointComponent.h"
#include "KineoKCDModel/kppKCDPolyhedron.h"
#include "KineoKCDModel/kppKCDAssembly.h"
#include "kcd2/kcdExactDistanceReport.h"
#include "kcd2/kcdAnalysis.h"
#include "kcd2/kcdGeometrySubElement.h"
#include "KineoWorks2/kwsJoint.h"
#include <hpp/util/debug.hh>
#include "hpp/model/body.hh"
namespace hpp {
namespace model {
BodyShPtr Body::create(const std::string& name)
{
Body* hppBody = new Body(name);
BodyShPtr hppBodyShPtr(hppBody);
BodyWkPtr hppBodyWkPtr = hppBodyShPtr;
if (hppBody->init(hppBodyWkPtr) != KD_OK) {
hppDout(error, " failed to create an object.");
hppBodyShPtr.reset();
}
return hppBodyShPtr;
}
//=========================================================================
ktStatus Body::init(const BodyWkPtr weakPtr)
{
weakPtr_ = weakPtr;
return CkwsKCDBody::init(weakPtr);
}
//=========================================================================
bool
Body::addInnerObject(const CkppSolidComponentRefShPtr& solidComponentRef,
const CkitMat4& position,
bool distanceComputation)
{
CkppSolidComponentShPtr solidComponent =
solidComponentRef->referencedSolidComponent();
#ifdef HPP_DEBUG
std::string innerName = solidComponent->name();
std::string outerName;
#endif
/*
Attach solid component to the joint associated to the body
*/
CkwsJointShPtr bodyKwsJoint = CkwsBody::joint();
CkppJointComponentShPtr bodyKppJoint =
KIT_DYNAMIC_PTR_CAST(CkppJointComponent, bodyKwsJoint);
// Test that body is attached to a joint
if (bodyKppJoint) {
solidComponent->setAbsolutePosition(position);
bodyKppJoint->addSolidComponentRef(solidComponentRef);
}
else {
hppDout(error, "The body is not attached to any joint");
return false;
}
/*
If requested, add the object in the list of objects the distance to which
needs to be computed and build the corresponding analyses.
*/
if (distanceComputation) {
CkcdObjectShPtr innerObject =
KIT_DYNAMIC_PTR_CAST(CkcdObject, solidComponent);
if (innerObject) {
hppDout(info, ":addInnerObject: adding " << solidComponent->name()
<< " to list of objects for distance computation.");
innerObjForDist_.push_back(innerObject);
/*
Build Exact distance computation analyses for this object
*/
const std::vector<CkcdObjectShPtr>& outerList = outerObjForDist_;
for (std::vector<CkcdObjectShPtr>::const_iterator it = outerList.begin();
it != outerList.end(); it++) {
const CkcdObjectShPtr& outerObject = *it;
#ifdef HPP_DEBUG
CkppSolidComponentShPtr solidComp =
KIT_DYNAMIC_PTR_CAST(CkppSolidComponent, outerObject);
if (solidComp) {
outerName = solidComp->name();
} else {
outerName = std::string("");
}
#endif
/* Instantiate the analysis object */
CkcdAnalysisShPtr analysis = CkcdAnalysis::create();
analysis->analysisType(CkcdAnalysisType::EXACT_DISTANCE);
/*
Ignore tolerance for distance computations
*/
analysis->ignoreTolerance(true);
/* retrieve the test trees associated with the objects */
CkcdTestTreeShPtr leftTree = innerObject->collectTestTrees();
CkcdTestTreeShPtr rightTree = outerObject->collectTestTrees();
// associate the lists with the analysis object
analysis->leftTestTree(leftTree);
analysis->rightTestTree(rightTree);
hppDout(info, ":addInnerObject: creating analysis between "
<< innerName << " and "
<< outerName);
distCompPairs_.push_back(analysis);
}
}
else {
hppDout(error, "Cannot cast solid component into CkcdObject.");
}
}
return true;
}
//=========================================================================
void Body::addOuterObject(const CkcdObjectShPtr& outerObject,
bool distanceComputation)
{
#if HPP_DEBUG
std::string innerName;
std::string outerName;
CkppSolidComponentShPtr solidComp =
KIT_DYNAMIC_PTR_CAST(CkppSolidComponent, outerObject);
if (solidComp) {
outerName = solidComp->name();
} else {
outerName = std::string("");
}
#endif
/*
Append object at the end of KineoWorks set of outer objects
for collision checking
*/
std::vector<CkcdObjectShPtr> outerList = outerObjects();
outerList.push_back(outerObject);
outerObjects(outerList);
/**
If distance computation is requested, build necessary CkcdAnalysis
objects
*/
if (distanceComputation) {
/*
Store object in case inner objects are added a posteriori
*/
outerObjForDist_.push_back(outerObject);
/*
Build distance computation objects (CkcdAnalysis)
*/
const CkcdObjectShPtr& outerObject=outerObject;
const std::vector<CkcdObjectShPtr> innerList = innerObjForDist_;
for (std::vector<CkcdObjectShPtr>::const_iterator it = innerList.begin();
it != innerList.end();
it++) {
const CkcdObjectShPtr& innerObject=*it;
/* Instantiate the analysis object */
CkcdAnalysisShPtr analysis = CkcdAnalysis::create();
analysis->analysisType(CkcdAnalysisType::EXACT_DISTANCE);
/*
Ignore tolerance for distance computations
*/
analysis->ignoreTolerance(true);
/* retrieve the test trees associated with the objects */
CkcdTestTreeShPtr leftTree = innerObject->collectTestTrees();
CkcdTestTreeShPtr rightTree = outerObject->collectTestTrees();
// associate the lists with the analysis object
analysis->leftTestTree(leftTree);
analysis->rightTestTree(rightTree);
#ifdef HPP_DEBUG
solidComp = KIT_DYNAMIC_PTR_CAST(CkppSolidComponent, innerObject);
if (solidComp) {
innerName = solidComp->name();
} else {
innerName = std::string("");
}
#endif
hppDout(info, ":addInnerObject: creating analysis between "
<< innerName << " and "
<< outerName);
distCompPairs_.push_back(analysis);
}
}
}
//=========================================================================
void Body::resetOuterObjects()
{
outerObjForDist_.clear();
distCompPairs_.clear();
}
//=========================================================================
ktStatus
Body::distAndPairsOfPoints(unsigned int pairId,
double& outDistance,
CkitPoint3& outPointBody,
CkitPoint3& outPointEnv,
CkcdObjectShPtr &outObjectBody,
CkcdObjectShPtr &outObjectEnv,
CkcdAnalysisType::Type type)
{
KWS_PRECONDITION(pairId < nbDistPairs());
CkcdAnalysisShPtr analysis = distCompPairs_[pairId];
analysis->analysisType(type);
ktStatus status = analysis->compute();
if (KD_SUCCEEDED(status)) {
hppDout(info, ":distAndPairsOfPoints: compute succeeded.");
unsigned int nbDistances = analysis->countExactDistanceReports();
if(nbDistances == 0) {
//no distance information available, return 0 for instance;
hppDout(info, ":distAndPairsOfPoints: no distance report.");
outDistance = 0;
outObjectBody.reset();
outObjectEnv.reset();
return KD_OK;
}
else{
CkcdExactDistanceReportShPtr distanceReport;
CkitPoint3 leftPoint, rightPoint;
//distances are ordered from lowest value, to highest value.
distanceReport = analysis->exactDistanceReport(0);
//exact distance between two lists is always stored at the first
//rank of Distance reports.
outDistance = distanceReport->distance();
if(distanceReport->countPairs()>0) {
CkitMat4 firstPos, secondPos;
// get points in the frame of the object
distanceReport->getPoints(0,leftPoint,rightPoint) ;
// get geometry
outObjectBody = distanceReport->pair(0).first->geometry();
outObjectEnv = distanceReport->pair(0).second->geometry();
outObjectBody->getAbsolutePosition(firstPos);
outObjectEnv->getAbsolutePosition(secondPos);
//from these geometry points we can get points in absolute
//frame(world) or relative frame(geometry).
//here we want points in the absolute frame.
outPointBody= firstPos * leftPoint;
outPointEnv = secondPos * rightPoint;
}
// get object
outObjectBody = distanceReport->leftCollisionEntity();
outObjectEnv = distanceReport->rightCollisionEntity();
return KD_OK;
}
} else {
hppDout(error, "Compute failed.");
return KD_ERROR;
}
return KD_OK;
}
} // namespace model
} // namespace hpp
/*
* Copyright (c) 2010 LAAS-CNRS
*
* Author: Florent Lamiraux
*/
#ifndef HPP_CORE_FREEFLYER_JOINT_HH
#define HPP_CORE_FREEFLYER_JOINT_HH
#include <KineoModel/kppFreeFlyerJointComponent.h>
#include "joint.hh"
namespace hpp {
namespace core {
namespace io {
KIT_PREDEF_CLASS(FreeflyerJoint);
///
/// \brief Intermediate freeflyer joint.
///
/// This class implements a freeflyer joint deriving from
/// CkppFreeFlyerJointComponent only for kxml input output purposes.
///
/// Joints of this class contain inertia data as CkppDoubleProperty
/// attributes.
///
/// Once read from a file the device containing this joint will be
/// transformed into a ChppHumanoidRobot.
class FreeflyerJoint : public CkppFreeFlyerJointComponent,
public Joint
{
public:
virtual bool isComponentClonable () const
{
return false;
}
static FreeflyerJointShPtr create(const std::string& name)
{
FreeflyerJoint *ptr = new FreeflyerJoint();
FreeflyerJointShPtr shPtr = FreeflyerJointShPtr(ptr);
FreeflyerJointWkPtr wkPtr = FreeflyerJointWkPtr(shPtr);
if (ptr->init(wkPtr, name) != KD_OK) {
shPtr.reset();
return shPtr;
}
return shPtr;
}
void
fillPropertyVector(std::vector<CkppPropertyShPtr>& inOutPropertyVector)
const
{
CkppFreeFlyerJointComponent::fillPropertyVector(inOutPropertyVector);
inOutPropertyVector.push_back(mass);
inOutPropertyVector.push_back(comX);
inOutPropertyVector.push_back(comY);
inOutPropertyVector.push_back(comZ);
inOutPropertyVector.push_back(inertiaMatrixXX);
inOutPropertyVector.push_back(inertiaMatrixYY);
inOutPropertyVector.push_back(inertiaMatrixZZ);
inOutPropertyVector.push_back(inertiaMatrixXY);
inOutPropertyVector.push_back(inertiaMatrixXZ);
inOutPropertyVector.push_back(inertiaMatrixYZ);
}
virtual CkwsJointShPtr kwsJoint() const
{
return CkppJointComponent::kwsJoint();
}
protected:
FreeflyerJoint() : CkppFreeFlyerJointComponent() {}
ktStatus init (const FreeflyerJointWkPtr &inWeakPtr,
const std::string &name)
{
ktStatus status = KD_OK;
status = CkppFreeFlyerJointComponent::init(inWeakPtr, name);
if (status == KD_ERROR) return KD_ERROR;
return Joint::init(inWeakPtr);
}
};
} // namespace io
} // namespace core
} // namespace hpp
#endif //HPP_CORE_FREEFLYER_JOINT_HH
/*
* Copyright (c) 2010 LAAS-CNRS
*
* Author: Florent Lamiraux
*/
#include <KineoModel/kppSolidComponentRef.h>