Skip to content

GitLab

Commit 67cf6b10 authored by jcarpent's avatar jcarpent
Browse files

[Spatial] Add motionSet action on Force

parent fedc2f03
Hide whitespace changes
Inline Side-by-side
src/spatial/act-on-set.hpp
View file @ 67cf6b10
......@@ -77,6 +77,7 @@ namespace se3
static void motionAction(const MotionDense<MotionDerived> & v,
const Eigen::MatrixBase<Mat> & iF,
Eigen::MatrixBase<MatRet> const & jF);
} // namespace forceSet
namespace motionSet
......@@ -140,6 +141,25 @@ namespace se3
static void inertiaAction(const InertiaTpl<Scalar,Options> & I,
const Eigen::MatrixBase<Mat> & iF,
Eigen::MatrixBase<MatRet> const & jF);
///
/// \brief Action of a motion set on a force object.
/// The input motion set is represented by a 6xN matrix whose each
/// column represent a spatial motion.
/// The output force set is represented by a 6xN matrix whose each
/// column represent a spatial force.
///
template<int Op, typename ForceDerived, typename Mat, typename MatRet>
static void act(const Eigen::MatrixBase<Mat> & iV,
const ForceDense<ForceDerived> & f,
Eigen::MatrixBase<MatRet> const & jF);
/// \brief Default implementation with assignment operator=
template<typename ForceDerived, typename Mat, typename MatRet>
static void act(const Eigen::MatrixBase<Mat> & iV,
const ForceDense<ForceDerived> & f,
Eigen::MatrixBase<MatRet> const & jF);
} // namespace MotionSet
} // namespace se3
......
src/spatial/act-on-set.hxx
View file @ 67cf6b10
......@@ -37,17 +37,6 @@ namespace se3
};
template<int Op, typename MotionDerived, typename Mat,typename MatRet, int NCOLS>
struct ForceSetMotionAction
{
/* Compute dF = v ^ F, where is the dual action operation associated
* with v, and F, dF are matrices whose columns are forces. */
static void run(const MotionDense<MotionDerived> & v,
const Eigen::MatrixBase<Mat> & iF,
Eigen::MatrixBase<MatRet> const & jF);
};
template<int Op, typename Mat,typename MatRet>
struct ForceSetSe3Action<Op,Mat,MatRet,1>
{
......@@ -56,7 +45,7 @@ namespace se3
static void run(const SE3 & m,
const Eigen::MatrixBase<Mat> & iF,
Eigen::MatrixBase<MatRet> const & jF)
{
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat);
EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatRet);
......@@ -84,22 +73,42 @@ namespace se3
}
};
/* Specialized implementation of block action, using colwise operation. It
* is empirically much faster than the true block operation, although I do
* not understand why. */
template<int Op,typename Mat,typename MatRet,int NCOLS>
void ForceSetSe3Action<Op,Mat,MatRet,NCOLS>::
run(const SE3 & m,
const Eigen::MatrixBase<Mat> & iF,
Eigen::MatrixBase<MatRet> const & jF)
{
for(int col=0;col<jF.cols();++col)
{
typename MatRet::ColXpr jFc
= const_cast<Eigen::MatrixBase<MatRet> &>(jF).col(col);
forceSet::se3Action<Op>(m,iF.col(col),jFc);
}
}
template<int Op, typename MotionDerived, typename Mat,typename MatRet, int NCOLS>
struct ForceSetMotionAction
{
/* Compute dF = v ^ F, where is the dual action operation associated
* with v, and F, dF are matrices whose columns are forces. */
static void run(const MotionDense<MotionDerived> & v,
const Eigen::MatrixBase<Mat> & iF,
Eigen::MatrixBase<MatRet> const & jF);
};
template<int Op, typename MotionDerived, typename Mat, typename MatRet>
struct ForceSetMotionAction<Op,MotionDerived,Mat,MatRet,1>
{
template<typename Fin, typename Fout>
static void run(const MotionDense<MotionDerived> & v,
const Eigen::MatrixBase<Mat> & iF,
Eigen::MatrixBase<MatRet> const & jF)
const ForceDense<Fin> & fin,
ForceDense<Fout> & fout)
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat);
EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatRet);
typedef ForceRef<Mat> ForceRefOnMat;
typedef ForceRef<MatRet> ForceRefOnMatRet;
ForceRefOnMat fin(iF.derived());
ForceRefOnMatRet fout(const_cast<Eigen::MatrixBase<MatRet> &>(jF).derived());
switch(Op)
{
case SETTO:
......@@ -116,25 +125,24 @@ namespace se3
break;
}
}
};
/* Specialized implementation of block action, using colwise operation. It
* is empirically much faster than the true block operation, although I do
* not understand why. */
template<int Op,typename Mat,typename MatRet,int NCOLS>
void ForceSetSe3Action<Op,Mat,MatRet,NCOLS>::
run(const SE3 & m,
const Eigen::MatrixBase<Mat> & iF,
Eigen::MatrixBase<MatRet> const & jF)
{
for(int col=0;col<jF.cols();++col)
static void run(const MotionDense<MotionDerived> & v,
const Eigen::MatrixBase<Mat> & iF,
Eigen::MatrixBase<MatRet> const & jF)
{
typename MatRet::ColXpr jFc
= const_cast<Eigen::MatrixBase<MatRet> &>(jF).col(col);
forceSet::se3Action<Op>(m,iF.col(col),jFc);
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat);
EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatRet);
typedef ForceRef<Mat> ForceRefOnMat;
typedef ForceRef<MatRet> ForceRefOnMatRet;
ForceRefOnMat fin(iF.derived());
ForceRefOnMatRet fout(const_cast<Eigen::MatrixBase<MatRet> &>(jF).derived());
run(v,fin,fout);
}
}
};
template<int Op,typename MotionDerived, typename Mat,typename MatRet,int NCOLS>
void ForceSetMotionAction<Op,MotionDerived,Mat,MatRet,NCOLS>::
run(const MotionDense<MotionDerived> & v,
......@@ -215,7 +223,7 @@ namespace se3
forceSet::se3ActionInverse<Op>(m,iF.col(col),jFc);
}
}
} // namespace internal
namespace forceSet
......@@ -520,6 +528,45 @@ namespace se3
}
};
template<int Op, typename ForceDerived, typename Mat,typename MatRet, int NCOLS>
struct MotionSetActOnForce
{
static void run(const Eigen::MatrixBase<Mat> & iV,
const ForceDense<ForceDerived> & f,
Eigen::MatrixBase<MatRet> const & jF)
{
for(int col=0;col<jF.cols();++col)
{
typename MatRet::ColXpr jFc
= const_cast<Eigen::MatrixBase<MatRet> &>(jF).col(col);
motionSet::act<Op>(iV.col(col),f,jFc);
}
}
};
template<int Op, typename ForceDerived, typename Mat, typename MatRet>
struct MotionSetActOnForce<Op,ForceDerived,Mat,MatRet,1>
{
/* Compute jF = jXi * iF, where jXi is the dual action matrix associated with m,
* and iF, jF are vectors. */
static void run(const Eigen::MatrixBase<Mat> & iV,
const ForceDense<ForceDerived> & f,
Eigen::MatrixBase<MatRet> const & jF)
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat);
EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatRet);
typedef MotionRef<Mat> MotionRefOnMat;
typedef ForceRef<MatRet> ForceRefOnMatRet;
MotionRefOnMat vin(iV.derived());
ForceRefOnMatRet fout(const_cast<Eigen::MatrixBase<MatRet> &>(jF).derived());
ForceSetMotionAction<Op,MotionRefOnMat,Mat,MatRet,1>::run(vin,f,fout);
}
};
} // namespace internal
......@@ -588,6 +635,22 @@ namespace se3
{
internal::MotionSetInertiaAction<SETTO,Scalar,Options,Mat,MatRet,MatRet::ColsAtCompileTime>::run(I,iV,jV);
}
template<int Op, typename ForceDerived, typename Mat, typename MatRet>
static void act(const Eigen::MatrixBase<Mat> & iV,
const ForceDense<ForceDerived> & f,
Eigen::MatrixBase<MatRet> const & jF)
{
internal::MotionSetActOnForce<Op,ForceDerived,Mat,MatRet,MatRet::ColsAtCompileTime>::run(iV,f,jF);
}
template<typename ForceDerived, typename Mat, typename MatRet>
static void act(const Eigen::MatrixBase<Mat> & iV,
const ForceDense<ForceDerived> & f,
Eigen::MatrixBase<MatRet> const & jF)
{
internal::MotionSetActOnForce<SETTO,ForceDerived,Mat,MatRet,MatRet::ColsAtCompileTime>::run(iV,f,jF);
}
} // namespace motionSet
......
unittest/tspatial.cpp
View file @ 67cf6b10
......@@ -564,7 +564,7 @@ BOOST_AUTO_TEST_CASE ( test_ActOnSet )
jF_ref -= v.toDualActionMatrix() * iF3;
forceSet::motionAction<RMTO>(v,iF3,jF);
BOOST_CHECK(jF.isApprox(jF_ref));
// Motion SET
Matrix6N iV = Matrix6N::Random(),jV,jV_ref,jVinv,jVinv_ref;
......@@ -634,6 +634,26 @@ BOOST_AUTO_TEST_CASE ( test_ActOnSet )
jV_ref -= I.matrix()*iV3;
motionSet::inertiaAction<RMTO>(I,iV3,jV);
BOOST_CHECK(jV.isApprox(jV_ref));
// motionSet::act
Force f = Force::Random();
motionSet::act(iV,f,jF);
for( int k=0;k<N;++k )
BOOST_CHECK(Motion(iV.col(k)).cross(f).toVector().isApprox(jF.col(k), 1e-12));
for( int k=0;k<N;++k )
jF_ref.col(k) = Force(Motion(iV.col(k)).cross(f)).toVector();
BOOST_CHECK(jF.isApprox(jF_ref));
for( int k=0;k<N;++k )
jF_ref.col(k) += Force(Motion(iV2.col(k)).cross(f)).toVector();
motionSet::act<ADDTO>(iV2,f,jF);
BOOST_CHECK(jF.isApprox(jF_ref,1e-12));
for( int k=0;k<N;++k )
jF_ref.col(k) -= Force(Motion(iV3.col(k)).cross(f)).toVector();
motionSet::act<RMTO>(iV3,f,jF);
BOOST_CHECK(jF.isApprox(jF_ref,1e-12));
}
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