From 766900f9c32406b932d767c4421dbbfa3c653c73 Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Thu, 11 Sep 2014 13:31:01 +0200
Subject: [PATCH] Change the prototype of CRBA::groupAct.
---
src/algorithm/crba.hpp | 108 +++++++++++++++++++++++------------------
unittest/crba.cpp | 3 --
2 files changed, 60 insertions(+), 51 deletions(-)
diff --git a/src/algorithm/crba.hpp b/src/algorithm/crba.hpp
index 7547004f8..3f4fdffba 100644
--- a/src/algorithm/crba.hpp
+++ b/src/algorithm/crba.hpp
@@ -35,7 +35,7 @@ namespace se3
using namespace Eigen;
using namespace se3;
- const int & i = jmodel.id();
+ const typename JointModel::Index & i = jmodel.id();
jmodel.calc(jdata.derived(),q);
data.liMi[i] = model.jointPlacements[i]*jdata.M();
@@ -46,58 +46,70 @@ namespace se3
namespace internal
{
- /* Compute jF = jXi * iF, where jXi is the dual action matrix associated with m,
- * and iF, jF are vectors. */
- template<typename D,typename Dret>
- static void actVec( const SE3 & m,
- const Eigen::MatrixBase<D> & iF,
- Eigen::MatrixBase<Dret> & jF )
- {
- EIGEN_STATIC_ASSERT_VECTOR_ONLY(D);
- EIGEN_STATIC_ASSERT_VECTOR_ONLY(Dret);
- Eigen::VectorBlock<const D,3> linear = iF.template head<3>();
- Eigen::VectorBlock<const D,3> angular = iF.template tail<3>();
+
+ 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;
+ // }
- jF.template head <3>() = m.rotation()*linear;
- jF.template tail <3>() = (m.translation().cross(jF.template head<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);
+ }
+ };
+
+ template<typename Mat,typename MatRet>
+ static void se3Action( const SE3 & m,
+ const Eigen::MatrixBase<Mat> & iF,
+ Eigen::MatrixBase<MatRet> & jF )
+ {
+ ForceSetSe3Action<Mat,MatRet,Mat::ColsAtCompileTime>::run(m,iF,jF);
}
- /* 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 column by column. */
- template<typename D,typename Dret>
- static void
- act_alt( const SE3 & m,
- const Eigen::MatrixBase<D> & iF,
- Eigen::MatrixBase<Dret> & jF )
+ 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 Dret::ColXpr jFc = jF.col(col);
- actVec(m,iF.col(col),jFc);
+ typename MatRet::ColXpr jFc = jF.col(col);
+ se3Action(m,iF.col(col),jFc);
}
}
- /* 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. */
- template<typename D,typename Dret>
- static void
- act( const SE3 & m,
- const Eigen::MatrixBase<D> & iF,
- Eigen::MatrixBase<Dret> & jF )
- {
- typename D::template ConstNRowsBlockXpr<3>::Type linear = iF.template topRows<3>();
- typename D::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;
- }
-
} // namespace internal
struct CrbaBackwardStep : public fusion::JointVisitor<CrbaBackwardStep>
@@ -120,7 +132,7 @@ namespace se3
* Yli += liXi Yi
* F[1:6,SUBTREE] = liXi F[1:6,SUBTREE]
*/
- const int & i = jmodel.id();
+ const Model::Index & i = jmodel.id();
/* F[1:6,i] = Y*S */
data.Fcrb[i].block<6,JointModel::NV>(0,jmodel.idx_v()) = data.Ycrb[i] * jdata.S();
@@ -138,9 +150,9 @@ namespace se3
/* F[1:6,SUBTREE] = liXi F[1:6,SUBTREE] */
Eigen::Block<typename Data::Matrix6x> jF
= data.Fcrb[parent].block(0,jmodel.idx_v(),6,data.nvSubtree[i]);
- internal::act_alt(data.liMi[i],
- data.Fcrb[i].block(0,jmodel.idx_v(),6,data.nvSubtree[i]),
- jF);
+ internal::se3Action(data.liMi[i],
+ data.Fcrb[i].block(0,jmodel.idx_v(),6,data.nvSubtree[i]),
+ jF);
}
// std::cout << "iYi = " << (Inertia::Matrix6)data.Ycrb[i] << std::endl;
diff --git a/unittest/crba.cpp b/unittest/crba.cpp
index c9f0e5d65..65babb0d4 100644
--- a/unittest/crba.cpp
+++ b/unittest/crba.cpp
@@ -24,12 +24,9 @@ int main(int argc, const char ** argv)
_MM_SET_DENORMALS_ZERO_MODE(_MM_DENORMALS_ZERO_ON);
#endif
-
using namespace Eigen;
using namespace se3;
- SE3::Matrix3 I3 = SE3::Matrix3::Identity();
-
se3::Model model;
std::string filename = "/home/nmansard/src/metapod/data/simple_arm.urdf";
--
GitLab