Commit f2208652 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

[Minor] Make debug and error messages more readable.

parent 726ca86d
......@@ -19,6 +19,8 @@
#ifndef HPP_CORE_COLLISION_VALIDATION_REPORT_HH
# define HPP_CORE_COLLISION_VALIDATION_REPORT_HH
# include <hpp/util/indent.hh>
# include <hpp/pinocchio/collision-object.hh>
# include <hpp/core/validation-report.hh>
# include <hpp/fcl/collision_data.h>
......@@ -56,11 +58,11 @@ namespace hpp {
std::vector<CollisionValidationReportPtr_t> collisionReports;
virtual std::ostream& print (std::ostream& os) const
{
os <<" Number of collisions : "<<collisionReports.size()<<".";
os <<" Number of collisions : "<<collisionReports.size()<<"."<<incendl;
for(std::vector<CollisionValidationReportPtr_t>::const_iterator it = collisionReports.begin() ; it != collisionReports.end() ; ++it){
(*it)->print(os);
os << **it << iendl;
}
return os;
return os << decindent;
}
}; // class AllCollisionValidationReport
/// \}
......
......@@ -31,12 +31,12 @@ namespace hpp {
enum RelativeMotionType {
/// The relative motion is fully constrained and the constraint cannot be
/// parameterized (has constant right-hand side)
Constrained,
Constrained = 0,
/// The relative motion is fully constrained but the constraint can be
/// parameterized (has non-constant right-hand side)
Parameterized,
Parameterized = 1,
/// The relative motion is not constrained
Unconstrained
Unconstrained = 2
};
typedef Eigen::Matrix<RelativeMotionType, Eigen::Dynamic, Eigen::Dynamic> matrix_type;
......
......@@ -208,7 +208,7 @@ namespace hpp {
if (fcl::collide (_colPair->first ->fcl (), _colPair->second->fcl (),
collisionRequest_, unused) != 0) {
hppDout(warning, "Disabling collision detection between two "
"body in collision.");
"bodies in collision.");
}
disabledPairs_.push_back (*_colPair);
_colPair = collisionPairs_.erase (_colPair);
......
......@@ -42,6 +42,16 @@ namespace hpp {
m(i0,i1) = m(i1,i0) = t;
}
/// Considering the order Constrained(0) < Parameterized(1) < Unconstrained(2)
/// Change m(i0,i1) only if it decreases.
inline void symSetNoDowngrade (RelativeMotion::matrix_type& m, size_type i0, size_type i1, RelativeMotion::RelativeMotionType t)
{
assert (RelativeMotion::Constrained < RelativeMotion::Parameterized
&& RelativeMotion::Parameterized < RelativeMotion::Unconstrained);
assert (m(i0,i1) == m(i1,i0));
if (t < m(i0,i1)) m(i0,i1) = m(i1,i0) = t;
}
/// Check that a differentiable function defines a constraint of
/// relative pose between two joints
template <typename T > struct check {
......@@ -169,9 +179,12 @@ namespace hpp {
const size_type& i1, const size_type& i2,
const RelativeMotion::RelativeMotionType& type)
{
assert (Constrained < Parameterized && Parameterized < Unconstrained);
bool param = (type == Parameterized);
RelativeMotionType t = Unconstrained;
if (type == Unconstrained) return;
// Constrained(0) < Parameterized(1) < Unconstrained(2)
// If the current value is more constraining, then do not change it.
if (matrix(i1,i2) <= type) return;
symSet(matrix, i1, i2, type);
// i1 to i3
......@@ -180,7 +193,7 @@ namespace hpp {
if (i3 == i2) continue;
if (matrix(i2,i3) != Unconstrained) {
t = (!param && matrix(i2,i3) == Constrained) ? Constrained : Parameterized;
symSet(matrix, i1, i3, t);
symSetNoDowngrade(matrix, i1, i3, t);
}
}
for (size_type i0 = 0; i0 < matrix.rows(); ++i0) {
......@@ -189,7 +202,7 @@ namespace hpp {
// i0 to i2
if (matrix(i0,i1) != Unconstrained) {
t = (!param && matrix(i0,i1) == Constrained) ? Constrained : Parameterized;
symSet (matrix, i0, i2 ,t);
symSetNoDowngrade (matrix, i0, i2 ,t);
}
// from i0 to i3
......@@ -199,9 +212,10 @@ namespace hpp {
if (i3 == i1) continue;
if (i3 == i0) continue;
if (matrix(i2,i3) == Unconstrained) continue;
// If motion is already constrained, continue.
t = (!param && matrix(i0,i1) == Constrained && matrix(i2,i3) == Constrained)
? Constrained : Parameterized;
symSet (matrix, i0, i3, t);
symSetNoDowngrade (matrix, i0, i3, t);
}
}
}
......
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