Commit 60b66ea7 authored by Le Quang Anh's avatar Le Quang Anh Committed by Florent Lamiraux
Browse files

Add some assertions on configuration normalization.

parent 637d4f3f
Pipeline #18558 failed with stage
in 3 minutes and 6 seconds
......@@ -32,6 +32,7 @@
#include <tuple>
#include <iterator>
#include <pinocchio/math/quaternion.hpp>
#include <hpp/util/debug.hh>
#include <hpp/util/timer.hh>
#include <hpp/pinocchio/configuration.hh>
......@@ -112,8 +113,11 @@ namespace hpp {
if (constraints) {
ConfigProjectorPtr_t configProjector (constraints->configProjector ());
if (configProjector) {
assert (isNormalized(problem()->robot(), target, PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE));
assert (isNormalized(problem()->robot(), *(near->configuration()), PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE));
configProjector->projectOnKernel (*(near->configuration ()), target,
qProj_);
assert (isNormalized(problem()->robot(), qProj_, PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE));
} else {
qProj_ = target;
}
......
......@@ -111,6 +111,8 @@ namespace hpp {
}
value_type u = (param - paramRange().first) / L;
if (L == 0) u = 0;
assert(hpp::pinocchio::checkNormalized(hpp::pinocchio::LiegroupElement(initial_, space_)));
assert(hpp::pinocchio::checkNormalized(hpp::pinocchio::LiegroupElement(end_, space_)));
space_->interpolate(initial_, end_, u, result);
return true;
}
......
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