Commit 7ebafa59 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

[Minor] Improve error message + fix SolidSolidCollision::print

parent 8f2b4bb8
......@@ -126,14 +126,12 @@ namespace hpp {
std::ostream& SolidSolidCollision::print (std::ostream& os) const
{
os << "SolidSolidCollision: " << m_->joint_a->name()
<< " - " << (m_->joint_b ? m_->joint_b->name() : "World") << '\n';
const pinocchio::Model& model = joint_a()->robot ()->model();
os << "SolidSolidCollision: " << m_->joint_a->name()
<< " - " << (m_->joint_b ? m_->joint_b->name() : model.names[0]) << '\n';
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" << ',';
}
for (auto i : joints)
os << model.names[i] << ", ";
os << '\n';
for (std::size_t i = 0; i < m_->coefficients.size(); ++i)
os << m_->coefficients[i].value_ << ", ";
......@@ -144,9 +142,9 @@ namespace hpp {
{
JointIndices_t joints;
const pinocchio::Model& model = joint_a->robot ()->model();
assert(joint_a);
const JointIndex id_a = (joint_a ? joint_a->index() : 0),
const pinocchio::Model& model = joint_a->robot ()->model();
const JointIndex id_a = joint_a->index(),
id_b = (joint_b ? joint_b->index() : 0);
JointIndex ia = id_a, ib = id_b;
......@@ -169,7 +167,7 @@ namespace hpp {
else assert (model.parents[fromB.back()] == ia);
// Build sequence
joints = fromA;
joints = std::move(fromA);
joints.insert(joints.end(), fromB.rbegin(), fromB.rend());
assert(joints.front() == id_a);
assert(joints.back() == id_b);
......
......@@ -125,11 +125,13 @@ namespace hpp {
throw std::runtime_error (oss.str ().c_str ());
}
bpt::ptime timeStop(bpt::microsec_clock::universal_time());
if(static_cast<value_type>((timeStop - timeStart).total_milliseconds())
> timeOut_*1000){
value_type elapsed_ms = static_cast<value_type>(
(timeStop - timeStart).total_milliseconds());
if(elapsed_ms > timeOut_*1000) {
if (!stopWhenProblemIsSolved_
&& problem()->target()->reached (roadmap())) break;
oss << "time out reached : " << timeOut_<<" s";
oss << "time out (" << timeOut_ << "s) reached after " <<
elapsed_ms*1e-3 << "s";
throw std::runtime_error (oss.str ().c_str ());
}
......
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