Commit 05ab620a authored by Akseppal's avatar Akseppal
Browse files

Merge remote-tracking branch 'pFernbach/topic/pinocchio' into topic/pinocchio

Conflicts:
	src/CMakeLists.txt
	src/collision-validation.cc
	src/continuous-collision-checking/dichotomy/body-pair-collision.hh
parents 0788d954 f3f81983
......@@ -121,9 +121,11 @@ ADD_REQUIRED_DEPENDENCY("hpp-util >= 3")
ADD_REQUIRED_DEPENDENCY("hpp-statistics")
ADD_REQUIRED_DEPENDENCY("hpp-constraints >= 3")
ADD_REQUIRED_DEPENDENCY("hpp-model >= 3")
ADD_REQUIRED_DEPENDENCY("hpp-model-urdf >= 3")
ADD_SUBDIRECTORY(src)
#ADD_SUBDIRECTORY(tests)
ADD_SUBDIRECTORY(tests)
# Add dependency toward hpp-core library in pkg-config file.
PKG_CONFIG_APPEND_LIBS("hpp-core")
......
......@@ -51,7 +51,7 @@ namespace hpp {
std::size_t rank = (*itJoint)->rankInConfiguration ();
(*itJoint)->jointModel().uniformlySample (rank, *config);
}*/
config = se3::randomConfiguration(*robot_->model());
config = se3::randomConfiguration(robot_->model());
// Shoot extra configuration variables
size_type extraDim = robot_->extraConfigSpace ().dimension ();
size_type offset = robot_->configSize () - extraDim;
......
......@@ -22,6 +22,7 @@
# include <hpp/core/collision-validation-report.hh>
# include <hpp/core/config-validation.hh>
# include <hpp/fcl/collision_data.h>
#include <pinocchio/multibody/geometry.hpp>
namespace hpp {
namespace core {
......
......@@ -34,7 +34,7 @@ namespace hpp {
/// Create distance computation pairs for each body of the robot
void addObstacle (const CollisionObjectConstPtr_t &object);
/// Add a list of obstacles
void obstacles (const ObjectVector_t& obstacles);
void obstacles (const ObjectStdVector_t &obstacles);
/// Compute distances between pairs of objects stored in bodies
void computeDistances ();
/// Get result of distance computations
......
......@@ -182,6 +182,8 @@ namespace hpp {
typedef std::list <NodePtr_t> Nodes_t;
typedef std::vector <NodePtr_t> NodeVector_t;
typedef pinocchio::ObjectVector_t ObjectVector_t;
typedef std::vector<CollisionObjectPtr_t> ObjectStdVector_t;
typedef boost::shared_ptr <Path> PathPtr_t;
typedef boost::shared_ptr <const Path> PathConstPtr_t;
typedef boost::shared_ptr <PathOptimizer> PathOptimizerPtr_t;
......
......@@ -66,9 +66,15 @@ namespace hpp {
/// Print report in a stream
virtual std::ostream& print (std::ostream& os) const
{
os << "Joint " << joint_->name () << ", rank: " << rank_
<< ", value out of range: " << value_ << " not in ["
<< lowerBound_ << ", " << upperBound_ << "]";
if (joint_) {
os << "Joint " << joint_->name () << ", rank: " << rank_
<< ", value out of range: " << value_ << " not in ["
<< lowerBound_ << ", " << upperBound_ << "]";
} else {
os << "Extra config space at rank: " << rank_
<< ", value out of range: " << value_ << " not in ["
<< lowerBound_ << ", " << upperBound_ << "]";
}
return os;
}
......
......@@ -539,8 +539,8 @@ namespace hpp {
/// for this object.
/// \param distance whether distance computation should be performed
/// for this object.
virtual void addObstacle (const CollisionObjectPtr_t& inObject, bool collision,
bool distance);
virtual void addObstacle (const CollisionObjectPtr_t &inObject, bool collision,
bool distance);
/// Remove collision pair between a joint and an obstacle
/// \param jointName name of the joint,
......@@ -572,9 +572,9 @@ namespace hpp {
/// \}
/// Local vector of objects considered for collision detection
const ObjectVector_t& collisionObstacles () const;
const ObjectStdVector_t& collisionObstacles () const;
/// Local vector of objects considered for distance computation
const ObjectVector_t& distanceObstacles () const;
const ObjectStdVector_t& distanceObstacles () const;
/// Set the roadmap
void roadmap (const RoadmapPtr_t& roadmap)
......@@ -660,8 +660,8 @@ namespace hpp {
/// Tolerance of path validation
value_type pathValidationTolerance_;
/// Store obstacles until call to solve.
ObjectVector_t collisionObstacles_;
ObjectVector_t distanceObstacles_;
ObjectStdVector_t collisionObstacles_;
ObjectStdVector_t distanceObstacles_;
/// Map of obstacles by names
std::map <std::string, CollisionObjectPtr_t> obstacleMap_;
// Tolerance for numerical constraint resolution
......
......@@ -209,7 +209,7 @@ namespace hpp {
/// Add obstacle to the list.
/// \param object a new object.
void addObstacle (const CollisionObjectConstPtr_t &object);
void addObstacle (const CollisionObjectPtr_t &object);
/// Remove a collision pair between a joint and an obstacle
/// \param the joint that holds the inner objects,
/// \param the obstacle to remove.
......@@ -236,9 +236,9 @@ namespace hpp {
void filterCollisionPairs ();
/// Vector of objects considered for collision detection
const ObjectVector_t& collisionObstacles () const;
const ObjectStdVector_t &collisionObstacles() const;
/// Set the vector of objects considered for collision detection
void collisionObstacles (const ObjectVector_t& collisionObstacles);
void collisionObstacles (const ObjectStdVector_t &collisionObstacles);
/// \}
/// Get a parameter named name.
......@@ -293,7 +293,7 @@ namespace hpp {
/// Path projector
PathProjectorPtr_t pathProjector_;
/// List of obstacles
ObjectVector_t collisionObstacles_;
ObjectStdVector_t collisionObstacles_;
/// Set of constraints applicable to the robot
ConstraintSetPtr_t constraints_;
/// Configuration shooter
......
......@@ -82,8 +82,11 @@ namespace hpp {
/// Get the index for a given joint
///
/// \return 0 if joint is NULL, joint->rankInVelocity()+1 otherwise.
static size_type idx(const JointConstPtr_t& joint);
/// \return 0 if joint is NULL, joint->index() otherwise.
static inline size_type idx(const JointConstPtr_t& joint)
{
return (joint ? joint->index() : 0);
}
};
} // namespace core
} // namespace hpp
......
......@@ -134,7 +134,8 @@ namespace hpp {
DeviceWkPtr_t device_;
// distance between front and rear wheel axes.
value_type rho_;
JointPtr_t xy_, rz_;
JointPtr_t turningJoint_;
size_type xy_, rz_;
std::vector<JointPtr_t> wheels_;
ReedsSheppWkPtr_t weak_;
}; // ReedsShepp
......
......@@ -33,16 +33,16 @@ continuous-collision-checking/dichotomy.cc
continuous-collision-checking/dichotomy/body-pair-collision.hh
continuous-collision-checking/progressive.cc
continuous-collision-checking/progressive/body-pair-collision.hh
diffusing-planner.cc
discretized-collision-checking.cc
discretized-path-validation.cc
distance-between-objects.cc
explicit-numerical-constraint.cc
extracted-path.hh
equation.cc
interpolated-path.cc
joint-bound-validation.cc
locked-joint.cc
#diffusing-planner.cc
#discretized-collision-checking.cc
#discretized-path-validation.cc
#distance-between-objects.cc
#explicit-numerical-constraint.cc
#extracted-path.hh
#equation.cc
#interpolated-path.cc
#joint-bound-validation.cc
#locked-joint.cc
##########################"
# nearest-neighbor/basic.hh #
# nearest-neighbor/basic.cc #
......@@ -91,4 +91,5 @@ PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} hpp-statistics)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} hpp-constraints)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} hpp-model)
INSTALL(TARGETS ${LIBRARY_NAME} DESTINATION lib)
......@@ -176,7 +176,7 @@ namespace hpp {
CollisionValidation::CollisionValidation (const DevicePtr_t& robot) :
robot_ (robot),
geomData_ (robot->geomData ()),
geomData_ (robot->geomDataPtr()),
parameterizedPairs_(), disabledPairs_(),
checkParameterized_(false)
{}
......
......@@ -46,9 +46,9 @@ namespace hpp {
}
}
void DistanceBetweenObjects::obstacles (const ObjectVector_t& obstacles)
void DistanceBetweenObjects::obstacles (const ObjectStdVector_t& obstacles)
{
for (ObjectVector_t::const_iterator itObj = obstacles.begin ();
for (ObjectStdVector_t::const_iterator itObj = obstacles.begin ();
itObj != obstacles.end (); ++itObj) {
addObstacle (*itObj);
}
......
......@@ -37,19 +37,32 @@ namespace hpp {
const JointVector_t jv = robot_->getJointVector ();
for (JointVector_t::const_iterator itJoint = jv.begin ();
itJoint != jv.end (); ++itJoint) {
size_type index = (*itJoint)->rankInConfiguration ();
for (size_type i=0; i < (*itJoint)->configSize (); ++i) {
if ((*itJoint)->isBounded (i)) {
value_type lower = (*itJoint)->lowerBound (i);
value_type upper = (*itJoint)->upperBound (i);
value_type value = config [index + i];
if (value < lower || upper < value) {
JointBoundValidationReportPtr_t report(new JointBoundValidationReport (*itJoint, i, lower, upper, value));
validationReport = report;
return false;
}
}
}
size_type index = (*itJoint)->rankInConfiguration ();
for (size_type i=0; i < (*itJoint)->configSize (); ++i) {
// To be checked but joints are now always bounded (with infinite bounds when not bounded).
value_type lower = (*itJoint)->lowerBound (i);
value_type upper = (*itJoint)->upperBound (i);
value_type value = config [index + i];
if (value < lower || upper < value) {
JointBoundValidationReportPtr_t report(new JointBoundValidationReport (*itJoint, i, lower, upper, value));
validationReport = report;
return false;
}
}
}
const pinocchio::ExtraConfigSpace& ecs = robot_->extraConfigSpace();
// Check the extra config space
// FIXME This was introduced at the same time as the integration of Pinocchio
size_type index = robot_->model().nq;
for (size_type i=0; i < ecs.dimension(); ++i) {
value_type lower = ecs.lower (i);
value_type upper = ecs.upper (i);
value_type value = config [index + i];
if (value < lower || upper < value) {
JointBoundValidationReportPtr_t report(new JointBoundValidationReport (JointPtr_t(), i, lower, upper, value));
validationReport = report;
return false;
}
}
return true;
}
......
......@@ -264,12 +264,8 @@ namespace hpp {
bool ConfigOptimizationTraits::shouldFilter (JointConstPtr_t joint,
const size_type /* iDof */)
{
// If joint if the root joint or if parent joint is the root joint and
// root joint is an anchor joint,
// filter it
//TODO
if (joint->parentJoint ())
// If joint is the root joint, filter it
if (!joint->parentJoint())
return true;
/// Filter joint name containing "base_joint..."
if (boost::algorithm::contains (joint->name (), "root_joint"))
......
......@@ -109,15 +109,13 @@ namespace hpp {
std::size_t rkCfg = joint->rankInConfiguration ();
std::size_t szCfg = joint->configSize();
Configuration_t qi = q1;
Configuration_t qTmp(path->outputSize ());
Configuration_t q_inter (path->outputSize ());
value_type t = - lt1;
for (std::size_t i = rkAtP1; i < rkAtP2; ++i) {
t += path->pathAtRank (i)->timeRange().second;
q_inter = path->pathAtRank (i)->end (),
qTmp = joint->jointModel().interpolate ( q1, q2,
q_inter.segment(rkCfg,szCfg) = joint->jointModel().interpolate ( q1, q2,
t / (t2-t1));
q_inter.segment(rkCfg,szCfg) = qTmp.segment(rkCfg,szCfg);
if (path->pathAtRank (i)->constraints ()) {
if (!path->pathAtRank (i)->constraints ()->apply (q_inter)) {
hppDout (warning, "PartialShortcut could not apply "
......
......@@ -608,11 +608,11 @@ namespace hpp {
}
if (collision){
//collisionObstacles_.push_back (object);//TODO
collisionObstacles_.push_back (object);
resetRoadmap ();
}
if (distance)
// distanceObstacles_.push_back (object);//TODO
distanceObstacles_.push_back (object);
if (problem ())
problem ()->addObstacle (object);
if (distanceBetweenObjects_) {
......@@ -648,13 +648,13 @@ namespace hpp {
{
std::list <std::string> res;
if (collision) {
for (ObjectVector_t::const_iterator it = collisionObstacles_.begin ();
for (ObjectStdVector_t::const_iterator it = collisionObstacles_.begin ();
it != collisionObstacles_.end (); ++it) {
res.push_back ((*it)->name ());
}
}
if (distance) {
for (ObjectVector_t::const_iterator it = distanceObstacles_.begin ();
for (ObjectStdVector_t::const_iterator it = distanceObstacles_.begin ();
it != distanceObstacles_.end (); ++it) {
res.push_back ((*it)->name ());
}
......@@ -662,12 +662,12 @@ namespace hpp {
return res;
}
const ObjectVector_t& ProblemSolver::collisionObstacles () const
const ObjectStdVector_t& ProblemSolver::collisionObstacles () const
{
return collisionObstacles_;
}
const ObjectVector_t& ProblemSolver::distanceObstacles () const
const ObjectStdVector_t& ProblemSolver::distanceObstacles () const
{
return distanceObstacles_;
}
......
......@@ -91,30 +91,30 @@ namespace hpp {
// ======================================================================
const ObjectVector_t& Problem::collisionObstacles () const
const ObjectStdVector_t& Problem::collisionObstacles () const
{
return collisionObstacles_;
}
// ======================================================================
void Problem::collisionObstacles (const ObjectVector_t& collisionObstacles)
void Problem::collisionObstacles (const ObjectStdVector_t& collisionObstacles)
{
// collisionObstacles_.clear (); TODO :
collisionObstacles_ = collisionObstacles;
// pass the local vector of collisions object to the problem
for (ObjectVector_t::const_iterator itObj = collisionObstacles.begin();
itObj != collisionObstacles.end(); ++itObj) {
addObstacle (*itObj);
}
collisionObstacles_.clear ();
// pass the local vector of collisions object to the problem
for (ObjectStdVector_t::const_iterator itObj = collisionObstacles.begin();
itObj != collisionObstacles.end(); ++itObj) {
addObstacle (*itObj);
}
}
// ======================================================================
void Problem::addObstacle (const CollisionObjectConstPtr_t& object)
void Problem::addObstacle (const CollisionObjectPtr_t& object)
{
// Add object in local list
// collisionObstacles_.push_back (object); TODO
collisionObstacles_.push_back (object);
robot()->geomModel().addGeometryObject(0,*object->fcl(),object->getTransform(),object->name());
// Add obstacle to path validation method
if (pathValidation_) {
pathValidation_->addObstacle (object);
......@@ -158,7 +158,7 @@ namespace hpp {
{
pathValidation_ = pathValidation;
// Insert obstacles in path validation object
for (ObjectVector_t::const_iterator it = collisionObstacles_.begin ();
for (ObjectStdVector_t::const_iterator it = collisionObstacles_.begin ();
it != collisionObstacles_.end (); ++it) {
pathValidation_->addObstacle (*it);
}
......
......@@ -48,7 +48,7 @@ namespace hpp {
RelativeMotion::matrix_type RelativeMotion::matrix (const DevicePtr_t& dev)
{
assert (dev);
const size_type N = dev->numberDof () + 1;
const size_type N = dev->model().joints.size();
matrix_type matrix (N, N);
matrix.setConstant (Unconstrained);
matrix.diagonal().setConstant(Constrained);
......@@ -64,7 +64,7 @@ namespace hpp {
assert (robot);
assert (c);
const size_type N = robot->numberDof () + 1;
const size_type N = robot->model().joints.size();
if (matrix.rows() != N || matrix.cols() != N)
throw std::invalid_argument ("Wrong RelativeMotion::matrix_type size");
......@@ -151,15 +151,5 @@ namespace hpp {
}
}
}
size_type RelativeMotion::idx(const JointConstPtr_t& joint)
{
if (joint == NULL) return 0;
if (joint->numberDof() == 0) {
const JointConstPtr_t j = getNonAnchorParent(joint);
return (j == NULL ? 0 : j->rankInVelocity() + 1);
}
return joint->rankInVelocity() + 1;
}
} // namespace core
} // namespace hpp
......@@ -30,9 +30,8 @@ namespace hpp {
{
ReedsSheppPathPtr_t path =
ReedsSheppPath::create (device_.lock (), q1, q2,
rho_ , xy_->rankInConfiguration(),
rz_->rankInConfiguration(), constraints ());
path->setWheelJoints (rz_, wheels_);
rho_ , xy_, rz_, constraints ());
path->setWheelJoints (turningJoint_, wheels_);
return path;
}
......@@ -50,21 +49,24 @@ namespace hpp {
ReedsShepp::ReedsShepp (const ProblemPtr_t& problem) :
SteeringMethod (problem), device_ (problem->robot ()),
rho_ (1.), weak_ ()
rho_ (1.), xy_ (0), rz_(2), weak_ ()
{
DevicePtr_t d (device_.lock());
xy_ = d->getJointAtConfigRank(0);
//TODO
/* if (!dynamic_cast <pinocchio::JointTranslation <2>* > (xy_)) {
std::string sn = d->rootJoint()->jointModel().shortname();
//TODO shortname() == se3::JointModelPlanar::classname();
if (sn == "planar") {
turningJoint_ = d->rootJoint();
} else if (sn == "translation" && d->rootJoint()->configSize() == 2) {
std::string sn2 = d->getJointAtConfigRank(2)->jointModel().shortname();
if (sn2 == "revoluteunbounded")
turningJoint_ = d->getJointAtConfigRank(2);
else
throw std::runtime_error ("root joint should be of type "
"se3::JointModelPlanar or JointModelTranslation + JointModelRevolute");
} else {
throw std::runtime_error ("root joint should be of type "
"pinocchio::JointTranslation <2>");
"se3::JointModelPlanar or JointModelTranslation + JointModelRevolute");
}
rz_ = d->getJointAtConfigRank(2);
if (!dynamic_cast <pinocchio::jointRotation::UnBounded*> (rz_)) {
throw std::runtime_error ("second joint should be of type "
"pinocchio::jointRotation::Unbounded");
}*/
guessWheels();
computeRadius();
......@@ -75,7 +77,10 @@ namespace hpp {
JointPtr_t xyJoint, JointPtr_t rzJoint,
std::vector <JointPtr_t> wheels) :
SteeringMethod (problem), device_ (problem->robot ()),
rho_ (turningRadius), xy_ (xyJoint), rz_ (rzJoint),
rho_ (turningRadius),
turningJoint_ (rzJoint),
xy_ (xyJoint->rankInConfiguration()),
rz_ (rzJoint->rankInConfiguration()),
wheels_ (wheels), weak_ ()
{
}
......@@ -90,7 +95,7 @@ namespace hpp {
inline value_type ReedsShepp::computeAngle(const JointPtr_t wheel) const
{
// Compute wheel position in joint RZ
const Transform3f zt (rz_->currentTransformation ());
const Transform3f zt (turningJoint_->currentTransformation ());
const Transform3f wt (zt.actInv (wheel->currentTransformation ()));
const vector3_t& wp (wt.translation ());
......@@ -106,8 +111,8 @@ namespace hpp {
inline void ReedsShepp::guessWheels()
{
wheels_.clear();
for (std::size_t i = 0; i < rz_->numberChildJoints(); ++i) {
JointPtr_t j = rz_->childJoint(i);
for (std::size_t i = 0; i < turningJoint_->numberChildJoints(); ++i) {
JointPtr_t j = turningJoint_->childJoint(i);
if (j->configSize() != 1) continue;
if (!j->isBounded(0)) continue;
if (j->name().find("wheel") == std::string::npos) continue;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment