Commit 795b00f0 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

[ContinuousValidation] Split pairs into model and data

parent c0715bcc
......@@ -66,7 +66,13 @@ namespace hpp {
// Get pairs checked for collision
const CollisionPairs_t& pairs () const
{
return pairs_;
return m_->pairs;
}
// Get pairs checked for collision
CollisionPairs_t& pairs ()
{
return m_->pairs;
}
// Get maximal velocity
......@@ -87,19 +93,17 @@ namespace hpp {
virtual std::ostream& print (std::ostream& os) const = 0;
protected:
mutable vector_t Vb_;
CollisionPairs_t pairs_;
/// Constructor of body pair collision
///
/// \param tolerance allowed penetration should be positive
BodyPairCollision (value_type tolerance):
IntervalValidation(tolerance), pairs_ (), maximalVelocity_(0)
IntervalValidation(tolerance), m_ (new Model), maximalVelocity_(0)
{
}
virtual void setReport (CollisionValidationReportPtr_t& report,
fcl::CollisionResult result,
CollisionPair_t _pair)
CollisionPair_t _pair) const
{
report = CollisionValidationReportPtr_t
(new CollisionValidationReport);
......@@ -109,6 +113,12 @@ namespace hpp {
}
private:
struct Model {
CollisionPairs_t pairs;
};
boost::shared_ptr<Model> m_;
mutable vector_t Vb_;
value_type maximalVelocity_;
/// Compute maximal velocity for a given velocity bound
......@@ -133,7 +143,7 @@ namespace hpp {
/// \retval report the collision validation report
/// \return true if the bodies are not in collision, else false
bool computeDistanceLowerBound(value_type &distanceLowerBound,
CollisionValidationReportPtr_t& report, pinocchio::DeviceData& data);
CollisionValidationReportPtr_t& report, pinocchio::DeviceData& data) const;
}; // class BodyPairCollision
......
......@@ -85,29 +85,29 @@ namespace hpp {
// Get coefficients and joints
const CoefficientVelocities_t& coefficients () const
{
return coefficients_;
return m_->coefficients;
}
/// Get joint a
const JointPtr_t& joint_a () const
{
return joint_a_;
return m_->joint_a;
}
/// Get joint b
const JointPtr_t& joint_b () const
{
return joint_b_;
return m_->joint_b;
}
/// Returns joint A index or -1 if no such joint exists.
size_type indexJointA () const
{
return (joint_a_ ? joint_a_->index() : 0);
return (m_->joint_a ? m_->joint_a->index() : 0);
}
/// Returns joint B index or -1 if no such joint exists.
size_type indexJointB () const
{
return (joint_b_ ? joint_b_->index() : 0);
return (m_->joint_b ? m_->joint_b->index() : 0);
}
protected:
......@@ -133,11 +133,15 @@ namespace hpp {
private:
typedef pinocchio::JointIndex JointIndex;
typedef std::vector<JointIndex> JointIndices_t;
JointPtr_t joint_a_;
JointPtr_t joint_b_;
CoefficientVelocities_t coefficients_;
JointIndices_t computeSequenceOfJoints () const;
void computeCoefficients (const JointIndices_t& joints);
struct Model {
JointPtr_t joint_a;
JointPtr_t joint_b;
CoefficientVelocities_t coefficients;
JointIndices_t computeSequenceOfJoints () const;
void computeCoefficients (const JointIndices_t& joints);
};
boost::shared_ptr<Model> m_;
}; // class SolidSolidCollision
} // namespace continuousValidation
} // namespace core
......
......@@ -144,14 +144,15 @@ namespace hpp {
bool BodyPairCollision::computeDistanceLowerBound(value_type &distanceLowerBound,
CollisionValidationReportPtr_t& report,
pinocchio::DeviceData& data)
pinocchio::DeviceData& data) const
{
using std::numeric_limits;
distanceLowerBound = numeric_limits <value_type>::infinity ();
static const fcl::CollisionRequest request (fcl::DISTANCE_LOWER_BOUND, 1);
assert (request.enable_distance_lower_bound == true);
for (CollisionPairs_t::const_iterator _pair = pairs_.begin();
_pair != pairs_.end(); ++_pair) {
const CollisionPairs_t& prs (pairs());
for (CollisionPairs_t::const_iterator _pair = prs.begin();
_pair != prs.end(); ++_pair) {
pinocchio::FclConstCollisionObjectPtr_t object_a = _pair->first ->fcl (data);
pinocchio::FclConstCollisionObjectPtr_t object_b = _pair->second->fcl (data);
fcl::CollisionResult result;
......
......@@ -54,7 +54,7 @@ namespace hpp {
{
value_type maximalVelocity = 0;
for (CoefficientVelocities_t::const_iterator itCoef =
coefficients_.begin (); itCoef != coefficients_.end (); ++itCoef) {
m_->coefficients.begin (); itCoef != m_->coefficients.end (); ++itCoef) {
const JointPtr_t& joint = itCoef->joint_;
const value_type& value = itCoef->value_;
maximalVelocity += value * Vb.segment(joint->rankInVelocity(), joint->numberDof()).norm();
......@@ -64,30 +64,31 @@ namespace hpp {
bool SolidSolidCollision::removeObjectTo_b (const CollisionObjectConstPtr_t& object)
{
const std::size_t s = pairs_.size();
for (CollisionPairs_t::iterator _pair = pairs_.begin ();
_pair != pairs_.end ();) {
CollisionPairs_t& prs (pairs());
const std::size_t s = prs.size();
for (CollisionPairs_t::iterator _pair = prs.begin ();
_pair != prs.end ();) {
if (object == _pair->second)
_pair = pairs_.erase (_pair);
_pair = prs.erase (_pair);
else
++_pair;
}
return pairs_.size() < s;
return prs.size() < s;
}
void SolidSolidCollision::addCollisionPair (const CollisionObjectConstPtr_t& left,
const CollisionObjectConstPtr_t& right)
{
// std::cout << "size = " << pairs_.size() << std::endl;
// std::cout << "capacity = " << pairs_.capacity() << std::endl;
pairs_.push_back (CollisionPair_t (left, right));
// std::cout << "size = " << pairs().size() << std::endl;
// std::cout << "capacity = " << pairs().capacity() << std::endl;
pairs().push_back (CollisionPair_t (left, right));
}
std::string SolidSolidCollision::name () const
{
std::ostringstream oss;
oss << "(" << joint_a_->name () << ",";
if (joint_b_) oss << joint_b_->name ();
oss << "(" << m_->joint_a->name () << ",";
if (m_->joint_b) oss << m_->joint_b->name ();
else oss << "obstacles";
oss << ")";
return oss.str ();
......@@ -95,25 +96,25 @@ namespace hpp {
std::ostream& SolidSolidCollision::print (std::ostream& os) const
{
os << "SolidSolidCollision: " << joint_a_->name()
<< " - " << (joint_b_ ? joint_b_->name() : "World") << '\n';
os << "SolidSolidCollision: " << m_->joint_a->name()
<< " - " << (m_->joint_b ? m_->joint_b->name() : "World") << '\n';
const pinocchio::Model& model = joint_a_->robot ()->model();
JointIndices_t joints = computeSequenceOfJoints ();
JointIndices_t joints = m_->computeSequenceOfJoints ();
for (std::size_t i = 0; i < joints.size (); ++i) {
if (i > 0) os << model.names[i] << ',';
else os << "World" << ',';
}
os << '\n';
for (std::size_t i = 0; i < coefficients_.size(); ++i)
os << coefficients_[i].value_ << ", ";
for (std::size_t i = 0; i < m_->coefficients.size(); ++i)
os << m_->coefficients[i].value_ << ", ";
return os;
}
SolidSolidCollision::JointIndices_t SolidSolidCollision::computeSequenceOfJoints () const
SolidSolidCollision::JointIndices_t SolidSolidCollision::Model::computeSequenceOfJoints () const
{
JointIndices_t joints;
const pinocchio::Model& model = joint_a_->robot ()->model();
const pinocchio::Model& model = joint_a->robot ()->model();
const JointIndex id_a = indexJointA(),
id_b = indexJointB();
JointIndex ia = id_a, ib = id_b;
......@@ -145,16 +146,16 @@ namespace hpp {
return joints;
}
void SolidSolidCollision::computeCoefficients (const JointIndices_t& joints)
void SolidSolidCollision::Model::computeCoefficients (const JointIndices_t& joints)
{
const pinocchio::Model& model = joint_a_->robot ()->model();
const pinocchio::Model& model = joint_a->robot ()->model();
JointPtr_t child;
assert (joints.size () > 1);
coefficients_.resize (joints.size () - 1);
pinocchio::DevicePtr_t robot =joint_a_->robot ();
coefficients.resize (joints.size () - 1);
pinocchio::DevicePtr_t robot =joint_a->robot ();
// Store r0 + sum of T_{i/i+1} in a variable
value_type cumulativeLength = joint_a_->linkedBody ()->radius ();
value_type cumulativeLength = joint_a->linkedBody ()->radius ();
value_type distance;
std::size_t i = 0;
while (i + 1 < joints.size()) {
......@@ -165,11 +166,11 @@ namespace hpp {
else
abort ();
assert(child);
coefficients_ [i].joint_ = child;
coefficients [i].joint_ = child;
// Go through all known types of joints
// TODO: REPLACE THESE FUNCTIONS WITH NEW API
distance = child->maximalDistanceToParent ();
coefficients_ [i].value_ =
coefficients [i].value_ =
child->upperBoundLinearVelocity () +
cumulativeLength * child->upperBoundAngularVelocity ();
cumulativeLength += distance;
......@@ -181,11 +182,13 @@ namespace hpp {
SolidSolidCollision::SolidSolidCollision (const JointPtr_t& joint_a,
const JointPtr_t& joint_b,
value_type tolerance) :
BodyPairCollision(tolerance), joint_a_ (joint_a), joint_b_ (joint_b),
coefficients_ ()
BodyPairCollision(tolerance), m_ (new Model)
{
m_->joint_a = joint_a;
m_->joint_b = joint_b;
assert (joint_a);
if (joint_b && joint_b_->robot () != joint_a_->robot ()) {
if (joint_b && joint_b->robot () != joint_a->robot ()) {
throw std::runtime_error
("Joints do not belong to the same device.");
}
......@@ -196,34 +199,35 @@ namespace hpp {
throw std::runtime_error ("tolerance should be non-negative.");
}
if (joint_a_) { assert(joint_a_->linkedBody ()); }
if (joint_b_) { assert(joint_b_->linkedBody ()); }
if (joint_a_) { assert(joint_a->linkedBody ()); }
if (joint_b_) { assert(joint_b->linkedBody ()); }
// Find sequence of joints
JointIndices_t joints (computeSequenceOfJoints ());
computeCoefficients (joints);
JointIndices_t joints (m_->computeSequenceOfJoints ());
m_->computeCoefficients (joints);
}
SolidSolidCollision::SolidSolidCollision (const JointPtr_t& joint_a,
const ConstObjectStdVector_t& objects_b,
value_type tolerance) :
BodyPairCollision(tolerance), joint_a_ (joint_a), joint_b_ (),
coefficients_ ()
BodyPairCollision(tolerance), m_ (new Model)
{
m_->joint_a = joint_a;
assert (joint_a);
BodyPtr_t body_a = joint_a_->linkedBody ();
BodyPtr_t body_a = joint_a->linkedBody ();
assert (body_a);
for (size_type i = 0; i < body_a->nbInnerObjects(); ++i) {
CollisionObjectConstPtr_t obj = body_a->innerObjectAt(i);
for (ConstObjectStdVector_t::const_iterator it = objects_b.begin ();
it != objects_b.end (); ++it) {
assert (!(*it)->joint () ||
(*it)->joint ()->robot () != joint_a_->robot ());
pairs_.push_back (CollisionPair_t(obj, *it));
(*it)->joint ()->robot () != joint_a->robot ());
pairs().push_back (CollisionPair_t(obj, *it));
}
}
// Find sequence of joints
JointIndices_t joints (computeSequenceOfJoints ());
computeCoefficients (joints);
JointIndices_t joints (m_->computeSequenceOfJoints ());
m_->computeCoefficients (joints);
}
} // namespace continuousValidation
......
Markdown is supported
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