Commit 766900f9 authored by Nicolas Mansard's avatar Nicolas Mansard Committed by Valenza Florian
Browse files

Change the prototype of CRBA::groupAct.

parent 3aa0311a
......@@ -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;
......
......@@ -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";
......
Supports Markdown
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