Commit 07daea16 authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

Add assertion to check quaternion normalization.

parent 57d0563e
Pipeline #18557 failed with stage
in 3 minutes and 22 seconds
......@@ -122,9 +122,11 @@ namespace hpp {
_f != functions_.end(); ++_f) {
const DifferentiableFunction& f = **_f;
f.impl_compute(result_ [i], arg);
assert(hpp::pinocchio::checkNormalized(result_[i]));
result.vector ().segment(row, f.outputSize()) =
result_ [i].vector ();
row += f.outputSize(); ++i;
assert(hpp::pinocchio::checkNormalized(result));
}
}
void impl_jacobian (matrixOut_t jacobian, ConfigurationIn_t arg) const
......
......@@ -219,6 +219,7 @@ namespace hpp {
compute<IsRelative, (bool)ComputePosition, (bool)ComputeOrientation, (bool)OutputR3xSO3>::error (data);
result.vector() = Vindices_.rview (data.value);
assert(hpp::pinocchio::checkNormalized(result));
}
template <int _Options>
......
......@@ -319,12 +319,17 @@ namespace hpp {
template <bool compileTimeRel /* false */, bool ori /* false */> struct relativeTransform {
template <bool runtimeRel, bool pos, bool ose3> static inline void run (GTDataV<runtimeRel, pos, false, ose3>& d)
{
using hpp::pinocchio::LiegroupElement;
using hpp::pinocchio::LiegroupSpace;
// There is no joint1
const Transform3f& M2 = d.M2 ();
d.value.noalias() = M2.act (d.model.F2inJ2.translation());
assert(hpp::pinocchio::checkNormalized(LiegroupElement(d.value,LiegroupSpace::R3xSO3())));
if (!d.model.t1isZero) d.value.noalias() -= d.model.F1inJ1.translation();
assert(hpp::pinocchio::checkNormalized(LiegroupElement(d.value,LiegroupSpace::R3xSO3())));
if (!d.model.R1isID)
d.value.applyOnTheLeft(d.model.F1inJ1.rotation().transpose());
assert(hpp::pinocchio::checkNormalized(LiegroupElement(d.value,LiegroupSpace::R3xSO3())));
}
};
template <> struct relativeTransform<false, true> {
......@@ -375,8 +380,13 @@ namespace hpp {
{
static inline void error (GTDataV<rel, pos, ori, ose3>& d)
{
using hpp::pinocchio::LiegroupElement;
using hpp::pinocchio::LiegroupSpace;
relativeTransform<rel, ori>::run (d);
unary<ori>::log(d);
if (ose3)
assert(hpp::pinocchio::checkNormalized(LiegroupElement
(d.value, LiegroupSpace::R3xSO3())));
}
static inline void jacobian (GTDataJ<rel, pos, ori, ose3>& d,
......
......@@ -27,7 +27,7 @@
// DAMAGE.
#include <hpp/constraints/solver/by-substitution.hh>
#include <hpp/pinocchio/liegroup-element.hh>
#include <boost/serialization/nvp.hpp>
#include <hpp/util/serialization.hh>
......@@ -266,7 +266,9 @@ namespace hpp {
projectVectorOnKernel (from, OM_, OP_);
assert (hpp::pinocchio::checkNormalized(M));
Lge_t P (O + OP_);
assert (hpp::pinocchio::checkNormalized(P));
saturate_->saturate (P.vector (), result, saturation_);
}
......
......@@ -637,6 +637,8 @@ namespace hpp {
Data& d = datas_[i];
f.value (d.output, config);
assert (hpp::pinocchio::checkNormalized(d.output));
assert (hpp::pinocchio::checkNormalized(d.rightHandSide));
d.error = d.output - d.rightHandSide;
constraints.setInactiveRowsToZero(d.error);
if (ComputeJac) {
......
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