diff --git a/CMakeLists.txt b/CMakeLists.txt
index e7ac94b0471a2b46b7819b4b918077efff412563..39f7a5c03c324b510521b68303b12cef31b0f10c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -40,6 +40,7 @@ SET(${PROJECT_NAME}_HEADERS
spatial/inertia.hpp
spatial/fwd.hpp
spatial/skew.hpp
+ spatial/act-on-set.hpp
multibody/constraint.hpp
multibody/force-set.hpp
multibody/joint.hpp
diff --git a/src/spatial/act-on-set.hpp b/src/spatial/act-on-set.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..e5f57c5241b8751dd523f6212276d77bfdc865fe
--- /dev/null
+++ b/src/spatial/act-on-set.hpp
@@ -0,0 +1,188 @@
+#ifndef __se3_act_on_set_hpp__
+#define __se3_act_on_set_hpp__
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include "pinocchio/spatial/fwd.hpp"
+
+namespace se3
+{
+ namespace forceSet
+ {
+ /* SE3 action on a set of forces, represented by a 6xN matrix whose each
+ * column represent a spatial force. */
+ template<typename Mat,typename MatRet>
+ static void se3Action( const SE3 & m,
+ const Eigen::MatrixBase<Mat> & iF,
+ Eigen::MatrixBase<MatRet> & jF );
+ } // namespace forceSet
+
+ namespace motionSet
+ {
+ /* SE3 action on a set of motions, represented by a 6xN matrix whose each
+ * column represent a spatial motion. */
+ template<typename Mat,typename MatRet>
+ static void se3Action( const SE3 & m,
+ const Eigen::MatrixBase<Mat> & iV,
+ Eigen::MatrixBase<MatRet> & jV );
+ } // namespace MotionSet
+
+ /* --- DETAILS --------------------------------------------------------- */
+
+ namespace internal
+ {
+
+ template<typename Mat,typename MatRet, int NCOLS>
+ struct ForceSetSe3Action
+ {
+ /* Compute jF = jXi * iF, where jXi is the dual action matrix associated
+ * with m, and iF, jF are matrices whose columns are forces. The resolution
+ * is done by block operation. It is less efficient than the colwise
+ * operation and should not be used. */
+ static void run( const SE3 & m,
+ const Eigen::MatrixBase<Mat> & iF,
+ Eigen::MatrixBase<MatRet> & jF );
+ // {
+ // typename Mat::template ConstNRowsBlockXpr<3>::Type linear = iF.template topRows<3>();
+ // typename MatRet::template ConstNRowsBlockXpr<3>::Type angular = iF.template bottomRows<3>();
+
+ // jF.template topRows <3>().noalias() = m.rotation()*linear;
+ // jF.template bottomRows<3>().noalias()
+ // = skew(m.translation())*jF.template topRows<3>() +
+ // m.rotation()*angular;
+ // }
+
+ };
+
+ template<typename Mat,typename MatRet>
+ struct ForceSetSe3Action<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 SE3 & m,
+ const Eigen::MatrixBase<Mat> & iF,
+ Eigen::MatrixBase<MatRet> & jF )
+ {
+ EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat);
+ EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatRet);
+ Eigen::VectorBlock<const Mat,3> linear = iF.template head<3>();
+ Eigen::VectorBlock<const Mat,3> angular = iF.template tail<3>();
+
+ jF.template head <3>() = m.rotation()*linear;
+ jF.template tail <3>() = (m.translation().cross(jF.template head<3>())
+ + m.rotation()*angular);
+ }
+ };
+
+ /* 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<typename Mat,typename MatRet,int NCOLS>
+ void ForceSetSe3Action<Mat,MatRet,NCOLS>::
+ run( const SE3 & m,
+ const Eigen::MatrixBase<Mat> & iF,
+ Eigen::MatrixBase<MatRet> & jF )
+ {
+ for(int col=0;col<jF.cols();++col)
+ {
+ typename MatRet::ColXpr jFc = jF.col(col);
+ forceSet::se3Action(m,iF.col(col),jFc);
+ }
+ }
+
+ } // namespace internal
+
+ namespace forceSet
+ {
+ template<typename Mat,typename MatRet>
+ static void se3Action( const SE3 & m,
+ const Eigen::MatrixBase<Mat> & iF,
+ Eigen::MatrixBase<MatRet> & jF )
+ {
+ internal::ForceSetSe3Action<Mat,MatRet,Mat::ColsAtCompileTime>::run(m,iF,jF);
+ }
+
+ } // namespace forceSet
+
+
+ namespace internal
+ {
+
+ template<typename Mat,typename MatRet, int NCOLS>
+ struct MotionSetSe3Action
+ {
+ /* Compute jF = jXi * iF, where jXi is the action matrix associated
+ * with m, and iF, jF are matrices whose columns are motions. The resolution
+ * is done by block operation. It is less efficient than the colwise
+ * operation and should not be used. */
+ static void run( const SE3 & m,
+ const Eigen::MatrixBase<Mat> & iF,
+ Eigen::MatrixBase<MatRet> & jF );
+ // {
+ // typename Mat::template ConstNRowsBlockXpr<3>::Type linear = iF.template topRows<3>();
+ // typename MatRet::template ConstNRowsBlockXpr<3>::Type angular = iF.template bottomRows<3>();
+
+ // jF.template topRows <3>().noalias() = m.rotation()*linear;
+ // jF.template bottomRows<3>().noalias()
+ // = skew(m.translation())*jF.template topRows<3>() +
+ // m.rotation()*angular;
+ // }
+
+ };
+
+ template<typename Mat,typename MatRet>
+ struct MotionSetSe3Action<Mat,MatRet,1>
+ {
+ /* Compute jV = jXi * iV, where jXi is the action matrix associated with m,
+ * and iV, jV are 6D vectors representing spatial velocities. */
+ static void run( const SE3 & m,
+ const Eigen::MatrixBase<Mat> & iV,
+ Eigen::MatrixBase<MatRet> & jV )
+ {
+ EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat);
+ EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatRet);
+ Eigen::VectorBlock<const Mat,3> linear = iV.template head<3>();
+ Eigen::VectorBlock<const Mat,3> angular = iV.template tail<3>();
+
+ /* ( R*v + px(Rw), Rw ) */
+ jV.template tail <3>() = m.rotation()*angular;
+ jV.template head <3>() = (m.translation().cross(jV.template tail<3>())
+ + m.rotation()*linear);
+ }
+ };
+
+ /* 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<typename Mat,typename MatRet,int NCOLS>
+ void MotionSetSe3Action<Mat,MatRet,NCOLS>::
+ run( const SE3 & m,
+ const Eigen::MatrixBase<Mat> & iV,
+ Eigen::MatrixBase<MatRet> & jV )
+ {
+ for(int col=0;col<jV.cols();++col)
+ {
+ typename MatRet::ColXpr jVc = jV.col(col);
+ motionSet::se3Action(m,iV.col(col),jVc);
+ }
+ }
+
+ } // namespace internal
+
+ namespace motionSet
+ {
+ template<typename Mat,typename MatRet>
+ static void se3Action( const SE3 & m,
+ const Eigen::MatrixBase<Mat> & iV,
+ Eigen::MatrixBase<MatRet> & jV )
+ {
+ internal::MotionSetSe3Action<Mat,MatRet,Mat::ColsAtCompileTime>::run(m,iV,jV);
+ }
+
+ } // namespace motionSet
+
+
+} // namespace se3
+
+#endif // ifndef __se3_act_on_set_hpp__
+
diff --git a/unittest/tspatial.cpp b/unittest/tspatial.cpp
index 8440cb5a93bbfe0c346cd0b99a4c2a4eb612b4a6..1a76f19e69c449decdb73e43ff26de6b08833a7e 100644
--- a/unittest/tspatial.cpp
+++ b/unittest/tspatial.cpp
@@ -4,6 +4,7 @@
#include "pinocchio/spatial/motion.hpp"
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/spatial/inertia.hpp"
+#include "pinocchio/spatial/act-on-set.hpp"
#define TEST_SE3
#define TEST_MOTION
@@ -241,12 +242,33 @@ bool testInertia()
return true;
}
+bool testActOnSet()
+{
+ const int N = 20;
+ typedef Eigen::Matrix<double,6,N> Matrix6N;
+ se3::SE3 jMi = se3::SE3::Random();
+
+ Matrix6N iF = Matrix6N::Random(),jF;
+ se3::forceSet::se3Action(jMi,iF,jF);
+ for( int k=0;k<N;++k )
+ assert( jMi.act(se3::Force(iF.col(k))).toVector().isApprox( jF.col(k) ));
+
+ Matrix6N iV = Matrix6N::Random(),jV;
+ se3::motionSet::se3Action(jMi,iV,jV);
+ for( int k=0;k<N;++k )
+ assert( jMi.act(se3::Motion(iV.col(k))).toVector().isApprox( jV.col(k) ));
+
+ return true;
+}
+
+
int main()
{
testSE3();
testMotion();
testForce();
testInertia();
+ testActOnSet();
return 1;
}