Commit 5f9bb719 authored by florent's avatar florent
Browse files

Make ChppCore class closer to hppModel implementation.

parent 663cc860
2008/01/09
Make ChppCore class closer to hppModel implementation.
2007/11/19
Remove reference to kwsPlusRoadmap.
2007/11/08
......
......@@ -36,20 +36,66 @@ CLASS
class ChppBody : public CkwsKCDBody
{
public:
std::string name();
/**
\brief Creation of a body
\param inName Name of the new body.
\return A shared pointer to a new body.
*/
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);
/**
\brief Get name of object.
*/
const std::string& name() {return attName;};
/**
\name Collision lists
@{
*/
/**
\brief Attach objects to the body.
\param inInnerObjects list of objects to attach to the body
Previous objects if any are detached.
Objects are put in the left test tree of attExactAnalyzer for exact distance computation.
*/
void setInnerObjects (const std::vector< CkcdObjectShPtr > &inInnerObjects);
/**
\brief Attach objects to the body in specified position
\param inInnerObjects list of objects to attach to the body
\param inPlacementVector Vector of homogeneous matrix specifying the position of each object in inInnerObjects.
Previous objects if any are detached.
void setOuterObjects (const std::vector< CkcdObjectShPtr > &i_outerObjects);
Objects are put in the left test tree of attExactAnalyzer for exact distance computation.
*/
void setInnerObjects (const std::vector< CkcdObjectShPtr > &inInnerObjects,
const std::vector< CkitMat4 > &inPlacementVector);
/**
\brief Defines the list of objects to be tested for collision with this body.
\param inOuterObjects list of objects to be tested for collision for this body
Previous objects if any are removed.
Objects are put in the right test tree of attExactAnalyzer for exact distance computation.
*/
void setOuterObjects (const std::vector< CkcdObjectShPtr > &inOuterObjects);
/**
@}
*/
/**
\brief Add geometry to the body
\param inSolidComponentRef Reference to the solid component to add.
\param inPosition Position of the object before attaching it to the body (default value=Identity).
\return true if success, false otherwise.
The input solid component is dynamically cast into
......@@ -60,32 +106,99 @@ public:
\note The body must be attached to a joint.
*/
bool addSolidComponent(const CkppSolidComponentRefShPtr& inSolidComponentRef);
bool addSolidComponent(const CkppSolidComponentRefShPtr& inSolidComponentRef, const CkitMat4& inPosition=CkitMat4());
/**
\name Collision and distance computation
@{
*/
/**
\brief Compute exact distance and closest points between body and set of outer objects.
\retval outDistance Distance between body and outer objects
\retval outPointBody Closest point on body
\retval outPointEnv Closest point in outer object set
\retval outObjectBody Closest object on body
\retval outObjectEnv Closest object in outer object list
*/
ktStatus getExactDistance(double& outDistance, CkitPoint3& outPointBody, CkitPoint3& outPointEnv,
CkcdObjectShPtr &outObjectBody, CkcdObjectShPtr &outObjectEnv);
/**
\brief Compute a lower bound of distance between body and set of outer objects.
ktStatus getExactDistance(double &dist, CkitPoint3& o_point1, CkitPoint3& o_point2,
CkcdObjectShPtr &object1, CkcdObjectShPtr &object2);
ktStatus getEstimatedDistance(double &dist,
CkcdObjectShPtr &object1, CkcdObjectShPtr &object2);
\retval outDistance Distance between body and outer objects
\retval outPointBody Closest point on body
\retval outPointEnv Closest point in outer object set
\retval outObjectBody Closest object on body
\retval outObjectEnv Closest object in outer object list
*/
ktStatus getEstimatedDistance(double &outDistance,
CkcdObjectShPtr &outObjectBody, CkcdObjectShPtr &outObjectEnv);
bool getCollisionVec(unsigned int &nbCollisions, std::vector<CkcdObjectShPtr> &objectVec1,
std::vector<CkcdObjectShPtr> &objectVec2);
bool getCollision(unsigned int &nbCollisions,
CkcdObjectShPtr &object1, CkcdObjectShPtr &object2);
/**
\brief Compute the set of inner and outer objects that are in collision with each other.
\retval outNbCollision Number of pairs of objects in collision.
\retval outObjectBodyVector Vector of objects in collision of the body.
\retval outObjectEnvVector Vector of objects in collision in the outer object list.
\return Whether there is a collision.
*/
bool getCollisionVec(unsigned int &outNbCollision, std::vector<CkcdObjectShPtr>& outObjectBodyVector,
std::vector<CkcdObjectShPtr>& outObjectEnvVector);
/**
\brief Compute collision and return the two first objects found in collision.
\retval outNbCollision Number of pairs of object in collision found.
\retval outObjectBody First object of the body found in collision.
\retval outObjectEnv First object in the body outer list found in collision.
\return Whether there is a collision.
*/
bool getCollision(unsigned int& outNbCollision,
CkcdObjectShPtr &outObjectBody, CkcdObjectShPtr &outObjectEnv);
bool printCollisionStatus(const bool& detailInfoFlag = false);
void printCollisionStatusFast();
/**
\brief Compute the minimum distance to the obstacle
\return the minimum distance to the obstacle
*/
double getMinDistance();
/**
@}
*/
protected:
ChppBody(std::string inName) : bodyName(inName) {};
/**
\brief Constructor by name.
*/
ChppBody(std::string inName) : attName(inName) {};
/**
\brief Initialization of body
*/
ktStatus init(const ChppBodyWkPtr bodyWkPtr);
private:
std::string bodyName;
/**
\brief Name of the body.
*/
std::string attName;
CkcdAnalysisShPtr m_exact_analyzer;
std::vector< CkcdObjectShPtr > inner, outer;
/**
\brief Collision analyser for this body
*/
CkcdAnalysisShPtr attExactAnalyzer;
};
......
/*
Research carried out within the scope of the Associated International Laboratory: Joint Japanese-French Robotics Laboratory (JRL)
Developed by Florent Lamiraux (LAAS-CNRS)
Author: Florent Lamiraux (LAAS-CNRS)
*/
#include "hppModel/hppBody.h"
#include <iostream>
#include "KineoModel/kppSolidComponentRef.h"
#include "KineoKCDModel/kppKCDPolyhedron.h"
#include "KineoKCDModel/kppKCDAssembly.h"
#include "KineoModel/kppJointComponent.h"
#include "hppModel/hppBody.h"
//=============================================================================
ChppBodyShPtr ChppBody::create(std::string inName)
......@@ -31,73 +33,56 @@ ChppBodyShPtr ChppBody::create(std::string inName)
ktStatus ChppBody::init(const ChppBodyWkPtr bodyWkPtr)
{
m_exact_analyzer = CkcdAnalysis::create();
attExactAnalyzer = CkcdAnalysis::create();
// m_exact_analyzer->setAnalysisType(CkcdAnalysis::EXHAUSTIVE_BOOLEAN_COLLISION);
m_exact_analyzer->analysisType(CkcdAnalysisType::EXACT_DISTANCE);
// attExactAnalyzer->setAnalysisType(CkcdAnalysis::EXHAUSTIVE_BOOLEAN_COLLISION);
attExactAnalyzer->analysisType(CkcdAnalysisType::EXACT_DISTANCE);
return CkwsKCDBody::init(bodyWkPtr);
}
//=============================================================================
std::string ChppBody::name()
{
return bodyName;
}
//=============================================================================
void ChppBody::setInnerObjects (const std::vector< CkcdObjectShPtr > &i_innerObjects,
const std::vector< CkitMat4 > &matList)
void ChppBody::setInnerObjects (const std::vector<CkcdObjectShPtr> &inInnerObjects)
{
innerObjects(i_innerObjects, matList);
inner = i_innerObjects;
//debug
//std::cout<<"ChppBody::setInnerObjects() called "<<std::endl;
innerObjects(inInnerObjects);
// retrieve the test trees associated with the objects and the ignored object list
CkcdTestTreeShPtr tree1 = CkcdTestTree::collectTestTrees(i_innerObjects, std::vector< CkcdObjectShPtr >());
CkcdTestTreeShPtr tree = CkcdTestTree::collectTestTrees(inInnerObjects, std::vector<CkcdObjectShPtr>());
m_exact_analyzer->leftTestTree(tree1);
attExactAnalyzer->leftTestTree(tree);
}
//=============================================================================
void ChppBody::setInnerObjects (const std::vector< CkcdObjectShPtr > &i_innerObjects)
void ChppBody::setInnerObjects (const std::vector< CkcdObjectShPtr > &inInnerObjects,
const std::vector< CkitMat4 > &inPlacementVector)
{
innerObjects(i_innerObjects);
inner = i_innerObjects;
//debug
//std::cout<<"ChppBody::setInnerObjects() called "<<std::endl;
innerObjects(inInnerObjects, inPlacementVector);
// retrieve the test trees associated with the objects and the ignored object list
CkcdTestTreeShPtr tree1 = CkcdTestTree::collectTestTrees(i_innerObjects, std::vector< CkcdObjectShPtr >());
CkcdTestTreeShPtr tree = CkcdTestTree::collectTestTrees(inInnerObjects, std::vector<CkcdObjectShPtr>());
m_exact_analyzer->leftTestTree(tree1);
attExactAnalyzer->leftTestTree(tree);
}
//=============================================================================
void ChppBody::setOuterObjects (const std::vector< CkcdObjectShPtr > &i_outerObjects)
void ChppBody::setOuterObjects (const std::vector<CkcdObjectShPtr> &inOuterObjects)
{
outerObjects(i_outerObjects);
outer = i_outerObjects;
//debug
//std::cout<<"ChppBody::setOuterObjects() called "<<std::endl;
outerObjects(inOuterObjects);
// retrieve the test trees associated with the objects and the ignored object list
CkcdTestTreeShPtr tree2 = CkcdTestTree::collectTestTrees(i_outerObjects, std::vector< CkcdObjectShPtr >());
CkcdTestTreeShPtr tree = CkcdTestTree::collectTestTrees(inOuterObjects, std::vector< CkcdObjectShPtr >());
m_exact_analyzer->rightTestTree(tree2);
attExactAnalyzer->rightTestTree(tree);
}
//=============================================================================
bool ChppBody::addSolidComponent(const CkppSolidComponentRefShPtr& inSolidComponentRef)
bool ChppBody::addSolidComponent(const CkppSolidComponentRefShPtr& inSolidComponentRef,
const CkitMat4& inPosition)
{
CkppSolidComponentShPtr solidComponent = inSolidComponentRef->referencedSolidComponent();
......@@ -120,23 +105,37 @@ bool ChppBody::addSolidComponent(const CkppSolidComponentRefShPtr& inSolidCompon
}
/*
Add object to the inner object list
Add object to the inner object list in the given position.
*/
std::vector<CkcdObjectShPtr> innerObjectList = innerObjects();
std::vector<CkitMat4> objectPosList = innerObjectRelativePositions();
innerObjectList.push_back(kcdObject);
objectPosList.push_back(inPosition);
setInnerObjects(innerObjectList);
setInnerObjects(innerObjectList, objectPosList);
/*
Attach solid component to the joint associated to the body
*/
CkwsJointShPtr bodyJoint = joint();
if (bodyJoint) {
if (CkppJointComponentShPtr kppJoint = KIT_DYNAMIC_PTR_CAST(CkppJointComponent, bodyJoint)) {
kppJoint->addSolidComponentRef(inSolidComponentRef);
return true;
}
#if 0
ChppJoint* bodyJoint = hppJoint();
if (bodyJoint != NULL) {
CkppJointComponentShPtr kppJoint = bodyJoint->kppJoint();
solidComponent->setAbsolutePosition(inPosition);
kppJoint->addSolidComponentRef(inSolidComponentRef);
return true;
}
#else
CkwsJointShPtr bodyKwsJoint = CkwsBody::joint();
CkppJointComponentShPtr bodyKppJoint = KIT_DYNAMIC_PTR_CAST(CkppJointComponent, bodyKwsJoint);
// Test that body is attached to a joint
if (bodyKppJoint) {
solidComponent->setAbsolutePosition(inPosition);
bodyKppJoint->addSolidComponentRef(inSolidComponentRef);
return true;
}
#endif
else {
std::cerr << "ChppBody::addSolidComponent: the body is not attached to any joint" << std::endl;
}
......@@ -145,20 +144,21 @@ bool ChppBody::addSolidComponent(const CkppSolidComponentRefShPtr& inSolidCompon
//=============================================================================
ktStatus ChppBody::getExactDistance(double &dist, CkitPoint3& o_point1, CkitPoint3& o_point2,
CkcdObjectShPtr &object1, CkcdObjectShPtr &object2)
ktStatus ChppBody::getExactDistance(double& outDistance, CkitPoint3& outPointBody, CkitPoint3& outPointEnv,
CkcdObjectShPtr &outObjectBody, CkcdObjectShPtr &outObjectEnv)
{
m_exact_analyzer->analysisType(CkcdAnalysisType::EXACT_DISTANCE);
attExactAnalyzer->analysisType(CkcdAnalysisType::EXACT_DISTANCE);
m_exact_analyzer->compute();
unsigned int nbDistances = m_exact_analyzer->countExactDistanceReports();
attExactAnalyzer->compute();
unsigned int nbDistances = attExactAnalyzer->countExactDistanceReports();
if(nbDistances == 0) {
//no distance information available, return 0 for instance;
dist = 0;
outDistance = 0;
object1.reset();
object2.reset();
outObjectBody.reset();
outObjectEnv.reset();
return KD_ERROR;
}
......@@ -168,10 +168,10 @@ ktStatus ChppBody::getExactDistance(double &dist, CkitPoint3& o_point1, CkitPoin
CkitPoint3 leftPoint, rightPoint;
//distances are ordered from lowest value, to highest value.
distanceReport = m_exact_analyzer->exactDistanceReport(0);
distanceReport = attExactAnalyzer->exactDistanceReport(0);
//exact distance between two lists is always stored at the first rank of Distance reports.
dist = distanceReport->distance();
outDistance = distanceReport->distance();
if(distanceReport->countPairs()>0)
{
......@@ -179,19 +179,19 @@ ktStatus ChppBody::getExactDistance(double &dist, CkitPoint3& o_point1, CkitPoin
// get points in the frame of the object
distanceReport->getPoints(0,leftPoint,rightPoint) ;
// get geometry
object1 = distanceReport->pair(0).first->geometry();
object2 = distanceReport->pair(0).second->geometry();
object1->getAbsolutePosition(firstPos);
object2->getAbsolutePosition(secondPos);
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.
o_point1= firstPos * leftPoint;
o_point2 = secondPos * rightPoint;
outPointBody= firstPos * leftPoint;
outPointEnv = secondPos * rightPoint;
}
// get object
object1 = distanceReport->leftCollisionEntity();
object2 = distanceReport->rightCollisionEntity();
outObjectBody = distanceReport->leftCollisionEntity();
outObjectEnv = distanceReport->rightCollisionEntity();
return KD_OK;
}
......@@ -200,20 +200,20 @@ ktStatus ChppBody::getExactDistance(double &dist, CkitPoint3& o_point1, CkitPoin
//=============================================================================
ktStatus ChppBody::getEstimatedDistance(double &dist,
CkcdObjectShPtr &object1, CkcdObjectShPtr &object2)
ktStatus ChppBody::getEstimatedDistance(double &outDistance,
CkcdObjectShPtr &outObjectBody, CkcdObjectShPtr &outObjectEnv)
{
m_exact_analyzer->analysisType(CkcdAnalysisType::ESTIMATED_DISTANCE);
m_exact_analyzer->compute();
attExactAnalyzer->analysisType(CkcdAnalysisType::ESTIMATED_DISTANCE);
attExactAnalyzer->compute();
unsigned int nbDistances = m_exact_analyzer->countEstimatedDistanceReports();
unsigned int nbDistances = attExactAnalyzer->countEstimatedDistanceReports();
if(nbDistances == 0) {
//no distance information available, return 0 for instance;
dist = 0;
outDistance = 0;
object1.reset();
object2.reset();
outObjectBody.reset();
outObjectEnv.reset();
return KD_ERROR;
}
......@@ -223,16 +223,16 @@ ktStatus ChppBody::getEstimatedDistance(double &dist,
//distances are ordered from lowest value, to highest value.
distanceReport = m_exact_analyzer->estimatedDistanceReport(0);
distanceReport = attExactAnalyzer->estimatedDistanceReport(0);
//exact distance between two lists is always stored at the first rank of Distance reports.
dist = distanceReport->distance();
outDistance = distanceReport->distance();
// no points for estimated distance
// get object
object1 = distanceReport->leftCollisionEntity();
object2 = distanceReport->rightCollisionEntity();
outObjectBody = distanceReport->leftCollisionEntity();
outObjectEnv = distanceReport->rightCollisionEntity();
return KD_OK;
......@@ -242,38 +242,38 @@ ktStatus ChppBody::getEstimatedDistance(double &dist,
//=============================================================================
bool ChppBody::getCollisionVec(unsigned int &nbCollisions,
std::vector<CkcdObjectShPtr> &objectVec1,
std::vector<CkcdObjectShPtr> &objectVec2)
bool ChppBody::getCollisionVec(unsigned int &outNbCollision,
std::vector<CkcdObjectShPtr> &outObjectBodyVector,
std::vector<CkcdObjectShPtr> &outObjectEnvVector)
{
//here we consider that collision lists (CkcdCollisionList) have been given to the analysis
m_exact_analyzer->analysisType(CkcdAnalysisType::EXHAUSTIVE_BOOLEAN_COLLISION);
// m_exact_analyzer->analysisType(CkcdAnalysis::FAST_BOOLEAN_COLLISION);
attExactAnalyzer->analysisType(CkcdAnalysisType::EXHAUSTIVE_BOOLEAN_COLLISION);
// attExactAnalyzer->analysisType(CkcdAnalysis::FAST_BOOLEAN_COLLISION);
m_exact_analyzer->compute();
nbCollisions = m_exact_analyzer->countCollisionReports();
attExactAnalyzer->compute();
outNbCollision = attExactAnalyzer->countCollisionReports();
objectVec1.clear();
objectVec2.clear();
outObjectBodyVector.clear();
outObjectEnvVector.clear();
if(nbCollisions > 0) {
if(outNbCollision > 0) {
//there is at least one collision, get more information on that collision
CkcdCollisionReportShPtr collisionReport;
CkcdObjectShPtr object1, object2;
CkcdObjectShPtr objectBody, objectEnv;
for(unsigned int i=0; i<nbCollisions; i++){
for(unsigned int i=0; i<outNbCollision; i++){
collisionReport = m_exact_analyzer->collisionReport(i);
collisionReport = attExactAnalyzer->collisionReport(i);
//retrieve colliding objects
object1 = collisionReport->pair(i).first->geometry() ;
object2 = collisionReport->pair(i).second->geometry() ;
objectBody = collisionReport->pair(i).first->geometry() ;
objectEnv = collisionReport->pair(i).second->geometry() ;
//here we could obtain more information, like which triangles are colliding.
//See CkcdPolyhedronTriangle for more information
objectVec1.push_back(object1);
objectVec2.push_back(object2);
outObjectBodyVector.push_back(objectBody);
outObjectEnvVector.push_back(objectEnv);
}
return true;
}
......@@ -284,25 +284,25 @@ bool ChppBody::getCollisionVec(unsigned int &nbCollisions,
//=============================================================================
bool ChppBody::getCollision(unsigned int &nbCollisions,
CkcdObjectShPtr &object1, CkcdObjectShPtr &object2)
bool ChppBody::getCollision(unsigned int &outNbCollision,
CkcdObjectShPtr &outObjectBody, CkcdObjectShPtr &outObjectEnv)
{
//here we consider that collision lists (CkcdCollisionList) have been given to the analysis
// m_exact_analyzer->analysisType(CkcdAnalysis::EXHAUSTIVE_BOOLEAN_COLLISION);
m_exact_analyzer->analysisType(CkcdAnalysisType::FAST_BOOLEAN_COLLISION);
m_exact_analyzer->compute();
nbCollisions = m_exact_analyzer->countCollisionReports();
// attExactAnalyzer->analysisType(CkcdAnalysis::EXHAUSTIVE_BOOLEAN_COLLISION);
attExactAnalyzer->analysisType(CkcdAnalysisType::FAST_BOOLEAN_COLLISION);
attExactAnalyzer->compute();
outNbCollision = attExactAnalyzer->countCollisionReports();
if(nbCollisions > 0) {
if(outNbCollision > 0) {
//there is at least one collision, get more information on that collision
CkcdCollisionReportShPtr collisionReport;
collisionReport = m_exact_analyzer->collisionReport(0);
collisionReport = attExactAnalyzer->collisionReport(0);
//retrieve colliding objects
object1 = collisionReport->pair(0).first->geometry() ;
object2 = collisionReport->pair(0).second->geometry() ;
outObjectBody = collisionReport->pair(0).first->geometry() ;
outObjectEnv = collisionReport->pair(0).second->geometry() ;
//here we could obtain more information, like which triangles are colliding.
//See CkcdPolyhedronTriangle for more information
......@@ -310,13 +310,62 @@ bool ChppBody::getCollision(unsigned int &nbCollisions,
}
else{
object1.reset();
object2.reset();
outObjectBody.reset();
outObjectEnv.reset();
return false;
}
}
//============================================================================
// ==========================================================================
// return minimum distance.
// collision: dist = 0;
// otherwise: smallest distance
double ChppBody::getMinDistance()
{
unsigned int nbCollisions;
std::vector<CkcdObjectShPtr> objectVec1, objectVec2;
if(getCollisionVec(nbCollisions, objectVec1, objectVec2) == true){
std::cout<<" ------- "<<nbCollisions<<" collision(s) detected.-------- " <<std::endl;
for(unsigned int i=0; i<nbCollisions; i++){
CkppPolyhedronShPtr kppPolyhedron1 = KIT_DYNAMIC_PTR_CAST(CkppPolyhedron, objectVec1[i]);
CkppPolyhedronShPtr kppPolyhedron2 = KIT_DYNAMIC_PTR_CAST(CkppPolyhedron, objectVec2[i]);
std::cout<<kppPolyhedron1->name()<<" and "<<kppPolyhedron2->name()<<std::endl;
}
std::cout<<" --------------------------"<<std::endl;
return 0;
}
// std::cout<<std::endl<<" no collision detected. Closest objects: ";
double dist;
CkitPoint3 o_point1, o_point2;
CkcdObjectShPtr object1, object2;
if(getExactDistance(dist, o_point1, o_point2, object1, object2) == KD_OK){
return dist;
/*
CkppPolyhedronShPtr kppPolyhedron1 = KIT_DYNAMIC_PTR_CAST(CkppPolyhedron, object1);
CkppPolyhedronShPtr kppPolyhedron2 = KIT_DYNAMIC_PTR_CAST(CkppPolyhedron, object2);
std::cout<<std::endl<<" kppPolyhedron "<<kppPolyhedron1->name()<<" and "<<kppPolyhedron2->name()
<<", distance "<<dist<<std::endl;
std::cout<<" at ["<<o_point1[0]<<"] ["<<o_point1[1]<<"] ["<<o_point1[0]<<"] and ["
<<o_point2[0]<<"] ["<<o_point2[1]<<"] ["<<o_point2[0]<<"]"<<std::endl;
*/
}