Commit 5d539a09 authored by jcarpent's avatar jcarpent
Browse files

[Spatial] Overload actOnSet function with optional operator affectation type: setTo, addTo and rmTo

parent 380452c1
This diff is collapsed.
//
// Copyright (c) 2015-2017 CNRS
// Copyright (c) 2015-2018 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -189,7 +189,6 @@ BOOST_AUTO_TEST_CASE ( test_Motion )
BOOST_AUTO_TEST_CASE (test_motion_ref)
{
using namespace se3;
typedef SE3::Matrix6 Matrix6;
typedef Motion::Vector6 Vector6;
typedef MotionRef<Vector6> MotionV6;
......@@ -310,8 +309,7 @@ BOOST_AUTO_TEST_CASE (test_force_ref)
{
using namespace se3;
typedef SE3::Matrix6 Matrix6;
typedef Force::Vector6 Vector6;
typedef ForceRef<Vector6> ForceV6;
Force f_ref(Force::Random());
......@@ -513,33 +511,129 @@ BOOST_AUTO_TEST_CASE ( test_ActOnSet )
Motion v = Motion::Random();
// Forcet SET
Matrix6N iF = Matrix6N::Random(),jF,jFinv;
Matrix6N iF = Matrix6N::Random(),jF,jFinv,jF_ref,jFinv_ref;
// forceSet::se3Action
forceSet::se3Action(jMi,iF,jF);
for( int k=0;k<N;++k )
BOOST_CHECK(jMi.act(Force(iF.col(k))).toVector().isApprox(jF.col(k), 1e-12));
jF_ref = jMi.toDualActionMatrix()*iF;
BOOST_CHECK(jF_ref.isApprox(jF));
forceSet::se3ActionInverse(jMi.inverse(),iF,jFinv);
BOOST_CHECK(jFinv.isApprox(jF));
Matrix6N iF2 = Matrix6N::Random();
jF_ref += jMi.toDualActionMatrix() * iF2;
forceSet::se3Action<ADDTO>(jMi,iF2,jF);
BOOST_CHECK(jF.isApprox(jF_ref,1e-12));
Matrix6N iF3 = Matrix6N::Random();
jF_ref -= jMi.toDualActionMatrix() * iF3;
forceSet::se3Action<RMTO>(jMi,iF3,jF);
BOOST_CHECK(jF.isApprox(jF_ref,1e-12));
// forceSet::se3ActionInverse
forceSet::se3ActionInverse(jMi,iF,jFinv);
jFinv_ref = jMi.inverse().toDualActionMatrix() * iF;
BOOST_CHECK(jFinv_ref.isApprox(jFinv));
jFinv_ref += jMi.inverse().toDualActionMatrix() * iF2;
forceSet::se3ActionInverse<ADDTO>(jMi,iF2,jFinv);
BOOST_CHECK(jFinv.isApprox(jFinv_ref,1e-12));
jFinv_ref -= jMi.inverse().toDualActionMatrix() * iF3;
forceSet::se3ActionInverse<RMTO>(jMi,iF3,jFinv);
BOOST_CHECK(jFinv.isApprox(jFinv_ref,1e-12));
// forceSet::motionAction
forceSet::motionAction(v,iF,jF);
for( int k=0;k<N;++k )
BOOST_CHECK(v.cross(Force(iF.col(k))).toVector().isApprox(jF.col(k), 1e-12));
jF_ref = v.toDualActionMatrix() * iF;
BOOST_CHECK(jF.isApprox(jF_ref));
jF_ref += v.toDualActionMatrix() * iF2;
forceSet::motionAction<ADDTO>(v,iF2,jF);
BOOST_CHECK(jF.isApprox(jF_ref));
jF_ref -= v.toDualActionMatrix() * iF3;
forceSet::motionAction<RMTO>(v,iF3,jF);
BOOST_CHECK(jF.isApprox(jF_ref));
// Motion SET
Matrix6N iV = Matrix6N::Random(),jV,jVinv;
Matrix6N iV = Matrix6N::Random(),jV,jV_ref,jVinv,jVinv_ref;
// motionSet::se3Action
motionSet::se3Action(jMi,iV,jV);
for( int k=0;k<N;++k )
BOOST_CHECK(jMi.act(Motion(iV.col(k))).toVector().isApprox(jV.col(k), 1e-12));
jV_ref = jMi.toActionMatrix()*iV;
BOOST_CHECK(jV.isApprox(jV_ref));
motionSet::se3ActionInverse(jMi.inverse(),iV,jVinv);
BOOST_CHECK(jVinv.isApprox(jV));
Matrix6N iV2 = Matrix6N::Random();
jV_ref += jMi.toActionMatrix()*iV2;
motionSet::se3Action<ADDTO>(jMi,iV2,jV);
BOOST_CHECK(jV.isApprox(jV_ref));
Matrix6N iV3 = Matrix6N::Random();
jV_ref -= jMi.toActionMatrix()*iV3;
motionSet::se3Action<RMTO>(jMi,iV3,jV);
BOOST_CHECK(jV.isApprox(jV_ref));
// motionSet::se3ActionInverse
motionSet::se3ActionInverse(jMi,iV,jVinv);
jVinv_ref = jMi.inverse().toActionMatrix() * iV;
BOOST_CHECK(jVinv.isApprox(jVinv_ref));
jVinv_ref += jMi.inverse().toActionMatrix()*iV2;
motionSet::se3ActionInverse<ADDTO>(jMi,iV2,jVinv);
BOOST_CHECK(jVinv.isApprox(jVinv_ref));
jVinv_ref -= jMi.inverse().toActionMatrix()*iV3;
motionSet::se3ActionInverse<RMTO>(jMi,iV3,jVinv);
BOOST_CHECK(jVinv.isApprox(jVinv_ref));
// motionSet::motionAction
motionSet::motionAction(v,iV,jV);
for( int k=0;k<N;++k )
BOOST_CHECK(v.cross(Motion(iV.col(k))).toVector().isApprox(jV.col(k), 1e-12));
jV_ref = v.toActionMatrix()*iV;
BOOST_CHECK(jV.isApprox(jV_ref));
jV_ref += v.toActionMatrix()*iV2;
motionSet::motionAction<ADDTO>(v,iV2,jV);
BOOST_CHECK(jV.isApprox(jV_ref));
jV_ref -= v.toActionMatrix()*iV3;
motionSet::motionAction<RMTO>(v,iV3,jV);
BOOST_CHECK(jV.isApprox(jV_ref));
// motionSet::inertiaAction
const Inertia I(Inertia::Random());
motionSet::inertiaAction(I,iV,jV);
for( int k=0;k<N;++k )
BOOST_CHECK((I*(Motion(iV.col(k)))).toVector().isApprox(jV.col(k), 1e-12));
jV_ref = I.matrix()*iV;
BOOST_CHECK(jV.isApprox(jV_ref));
jV_ref += I.matrix()*iV2;
motionSet::inertiaAction<ADDTO>(I,iV2,jV);
BOOST_CHECK(jV.isApprox(jV_ref));
jV_ref -= I.matrix()*iV3;
motionSet::inertiaAction<RMTO>(I,iV3,jV);
BOOST_CHECK(jV.isApprox(jV_ref));
}
BOOST_AUTO_TEST_CASE(test_skew)
......
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