Commit 67cf6b10 by jcarpent

### [Spatial] Add motionSet action on Force

parent fedc2f03
 ... ... @@ -77,6 +77,7 @@ namespace se3 static void motionAction(const MotionDense & v, const Eigen::MatrixBase & iF, Eigen::MatrixBase const & jF); } // namespace forceSet namespace motionSet ... ... @@ -140,6 +141,25 @@ namespace se3 static void inertiaAction(const InertiaTpl & I, const Eigen::MatrixBase & iF, Eigen::MatrixBase 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 static void act(const Eigen::MatrixBase & iV, const ForceDense & f, Eigen::MatrixBase const & jF); /// \brief Default implementation with assignment operator= template static void act(const Eigen::MatrixBase & iV, const ForceDense & f, Eigen::MatrixBase const & jF); } // namespace MotionSet } // namespace se3 ... ...
 ... ... @@ -37,17 +37,6 @@ namespace se3 }; template 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 & v, const Eigen::MatrixBase & iF, Eigen::MatrixBase const & jF); }; template struct ForceSetSe3Action { ... ... @@ -56,7 +45,7 @@ namespace se3 static void run(const SE3 & m, const Eigen::MatrixBase & iF, Eigen::MatrixBase 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 void ForceSetSe3Action:: run(const SE3 & m, const Eigen::MatrixBase & iF, Eigen::MatrixBase const & jF) { for(int col=0;col &>(jF).col(col); forceSet::se3Action(m,iF.col(col),jFc); } } template 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 & v, const Eigen::MatrixBase & iF, Eigen::MatrixBase const & jF); }; template struct ForceSetMotionAction { template static void run(const MotionDense & v, const Eigen::MatrixBase & iF, Eigen::MatrixBase const & jF) const ForceDense & fin, ForceDense & fout) { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat); EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatRet); typedef ForceRef ForceRefOnMat; typedef ForceRef ForceRefOnMatRet; ForceRefOnMat fin(iF.derived()); ForceRefOnMatRet fout(const_cast &>(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 void ForceSetSe3Action:: run(const SE3 & m, const Eigen::MatrixBase & iF, Eigen::MatrixBase const & jF) { for(int col=0;col & v, const Eigen::MatrixBase & iF, Eigen::MatrixBase const & jF) { typename MatRet::ColXpr jFc = const_cast &>(jF).col(col); forceSet::se3Action(m,iF.col(col),jFc); EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat); EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatRet); typedef ForceRef ForceRefOnMat; typedef ForceRef ForceRefOnMatRet; ForceRefOnMat fin(iF.derived()); ForceRefOnMatRet fout(const_cast &>(jF).derived()); run(v,fin,fout); } } }; template void ForceSetMotionAction:: run(const MotionDense & v, ... ... @@ -215,7 +223,7 @@ namespace se3 forceSet::se3ActionInverse(m,iF.col(col),jFc); } } } // namespace internal namespace forceSet ... ... @@ -520,6 +528,45 @@ namespace se3 } }; template struct MotionSetActOnForce { static void run(const Eigen::MatrixBase & iV, const ForceDense & f, Eigen::MatrixBase const & jF) { for(int col=0;col &>(jF).col(col); motionSet::act(iV.col(col),f,jFc); } } }; template struct MotionSetActOnForce { /* 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 & iV, const ForceDense & f, Eigen::MatrixBase const & jF) { EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat); EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatRet); typedef MotionRef MotionRefOnMat; typedef ForceRef ForceRefOnMatRet; MotionRefOnMat vin(iV.derived()); ForceRefOnMatRet fout(const_cast &>(jF).derived()); ForceSetMotionAction::run(vin,f,fout); } }; } // namespace internal ... ... @@ -588,6 +635,22 @@ namespace se3 { internal::MotionSetInertiaAction::run(I,iV,jV); } template static void act(const Eigen::MatrixBase & iV, const ForceDense & f, Eigen::MatrixBase const & jF) { internal::MotionSetActOnForce::run(iV,f,jF); } template static void act(const Eigen::MatrixBase & iV, const ForceDense & f, Eigen::MatrixBase const & jF) { internal::MotionSetActOnForce::run(iV,f,jF); } } // namespace motionSet ... ...
 ... ... @@ -564,7 +564,7 @@ BOOST_AUTO_TEST_CASE ( test_ActOnSet ) jF_ref -= v.toDualActionMatrix() * iF3; forceSet::motionAction(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(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(iV2,f,jF); BOOST_CHECK(jF.isApprox(jF_ref,1e-12)); for( int k=0;k(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!