Commit 1b5c9223 authored by jcarpent's avatar jcarpent
Browse files

[Spatial] Update naming of {force,motion}Set::motionAction

parent 6e7ff5ec
......@@ -301,7 +301,7 @@ namespace se3
ColsBlock dJ_cols = jmodel.jointCols(data.dJ);
const Motion ov(data.oMi[i].act(v));
motionSet::se3Action(ov,J_cols,dJ_cols);
motionSet::motionAction(ov,J_cols,dJ_cols);
data.oYo[parent] += Y;
if(parent > 0)
......
......@@ -188,7 +188,7 @@ namespace se3
ColsBlock dJcols = jmodel.jointCols(data.dJ);
ColsBlock Jcols = jmodel.jointCols(data.J);
motionSet::se3Action(ov,Jcols,dJcols);
motionSet::motionAction(ov,Jcols,dJcols);
}
};
......
......@@ -75,7 +75,7 @@ namespace se3
Jcols = oMi.act(jdata.S());
ov = oMi.act(vi); // Spatial velocity of joint i expressed in the global frame o
motionSet::se3Action(ov,Jcols,dJcols);
motionSet::motionAction(ov,Jcols,dJcols);
oa = oMi.act(ai); // Spatial acceleration of joint i expressed in the global frame o
}
......@@ -149,14 +149,14 @@ namespace se3
vtmp = data.ov[parent] - vlast;
else
vtmp = -vlast;
motionSet::se3Action(vtmp,Jcols,partial_dq_cols);
motionSet::motionAction(vtmp,Jcols,partial_dq_cols);
}
else
{
if(parent > 0)
{
vtmp = oMlast.actInv(data.ov[parent]);
motionSet::se3Action(vtmp,partial_dv_cols,partial_dq_cols);
motionSet::motionAction(vtmp,partial_dv_cols,partial_dq_cols);
}
}
......@@ -252,7 +252,7 @@ namespace se3
vtmp = -vlast;
/// also computes dvec/dq
motionSet::se3Action(vtmp,Jcols,v_partial_dq_cols);
motionSet::motionAction(vtmp,Jcols,v_partial_dq_cols);
a_partial_dv_cols = v_partial_dq_cols + dJcols;
}
......@@ -262,7 +262,7 @@ namespace se3
if(parent > 0)
{
vtmp = oMlast.actInv(data.ov[parent]);
motionSet::se3Action(vtmp,a_partial_da_cols,v_partial_dq_cols);
motionSet::motionAction(vtmp,a_partial_da_cols,v_partial_dq_cols);
}
if(parent > 0)
......@@ -270,7 +270,7 @@ namespace se3
else
vtmp = -data.v[jointId];
motionSet::se3Action(vtmp,a_partial_da_cols,a_partial_dv_cols);
motionSet::motionAction(vtmp,a_partial_da_cols,a_partial_dv_cols);
a_partial_dv_cols += oMlast.inverse().toActionMatrix() * dJcols; // TODO: optimize computations
}
......@@ -281,7 +281,7 @@ namespace se3
atmp = data.oa[parent] - alast;
else
atmp = -alast;
motionSet::se3Action(atmp,Jcols,a_partial_dq_cols);
motionSet::motionAction(atmp,Jcols,a_partial_dq_cols);
if(parent >0)
a_partial_dq_cols += vtmp.toActionMatrix() * dJcols;
......@@ -291,7 +291,7 @@ namespace se3
if(parent > 0)
{
atmp = oMlast.actInv(data.oa[parent]);
motionSet::se3Action(atmp,a_partial_da_cols,a_partial_dq_cols);
motionSet::motionAction(atmp,a_partial_da_cols,a_partial_dq_cols);
}
a_partial_dq_cols += vtmp.toActionMatrix() * v_partial_dq_cols; // TODO: optimize computations
......
//
// Copyright (c) 2015-2017 CNRS
// Copyright (c) 2015-2018 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -18,7 +18,6 @@
#ifndef __se3_act_on_set_hpp__
#define __se3_act_on_set_hpp__
#include <Eigen/Core>
#include "pinocchio/macros.hpp"
#include "pinocchio/spatial/fwd.hpp"
......@@ -48,10 +47,10 @@ namespace se3
/// \brief Action of a motion 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 Motion & v,
const Eigen::MatrixBase<Mat> & iF,
Eigen::MatrixBase<MatRet> const & jF);
template<typename MotionDerived, typename Mat,typename MatRet>
static void motionAction(const MotionBase<MotionDerived> & v,
const Eigen::MatrixBase<Mat> & iF,
Eigen::MatrixBase<MatRet> const & jF);
} // namespace forceSet
namespace motionSet
......@@ -78,10 +77,10 @@ namespace se3
/// \brief Action of a motion 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 Motion & v,
const Eigen::MatrixBase<Mat> & iF,
Eigen::MatrixBase<MatRet> const & jF);
template<typename MotionDerived, typename Mat,typename MatRet>
static void motionAction(const MotionBase<MotionDerived> & v,
const Eigen::MatrixBase<Mat> & iF,
Eigen::MatrixBase<MatRet> const & jF);
} // namespace MotionSet
/* --- DETAILS --------------------------------------------------------- */
......@@ -100,7 +99,14 @@ namespace se3
const Eigen::MatrixBase<Mat> & iF,
Eigen::MatrixBase<MatRet> const & jF);
static void run(const Motion & v,
};
template<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 MotionBase<MotionDerived> & v,
const Eigen::MatrixBase<Mat> & iF,
Eigen::MatrixBase<MatRet> const & jF);
......@@ -124,8 +130,12 @@ namespace se3
const_cast<Eigen::MatrixBase<MatRet> &>(jF).template tail <3>() = (m.translation().cross(jF.template head<3>())
+ m.rotation()*angular);
}
static void run(const Motion & v,
};
template<typename MotionDerived, typename Mat, typename MatRet>
struct ForceSetMotionAction<MotionDerived,Mat,MatRet,1>
{
static void run(const MotionBase<MotionDerived> & v,
const Eigen::MatrixBase<Mat> & iF,
Eigen::MatrixBase<MatRet> const & jF)
{
......@@ -156,9 +166,9 @@ namespace se3
}
}
template<typename Mat,typename MatRet,int NCOLS>
void ForceSetSe3Action<Mat,MatRet,NCOLS>::
run(const Motion & v,
template<typename MotionDerived, typename Mat,typename MatRet,int NCOLS>
void ForceSetMotionAction<MotionDerived,Mat,MatRet,NCOLS>::
run(const MotionBase<MotionDerived> & v,
const Eigen::MatrixBase<Mat> & iF,
Eigen::MatrixBase<MatRet> const & jF)
{
......@@ -166,7 +176,7 @@ namespace se3
{
typename MatRet::ColXpr jFc
= const_cast<Eigen::MatrixBase<MatRet> &>(jF).col(col);
forceSet::se3Action(v,iF.col(col),jFc);
forceSet::motionAction(v,iF.col(col),jFc);
}
}
......@@ -229,7 +239,7 @@ namespace se3
namespace forceSet
{
template<typename Mat,typename MatRet>
static void se3Action( const SE3 & m,
static void se3Action(const SE3 & m,
const Eigen::MatrixBase<Mat> & iF,
Eigen::MatrixBase<MatRet> const & jF)
{
......@@ -244,12 +254,12 @@ namespace se3
internal::ForceSetSe3ActionInverse<Mat,MatRet,Mat::ColsAtCompileTime>::run(m,iF,jF);
}
template<typename Mat,typename MatRet>
static void se3Action(const Motion & v,
const Eigen::MatrixBase<Mat> & iF,
Eigen::MatrixBase<MatRet> const & jF)
template<typename MotionDerived, typename Mat,typename MatRet>
static void motionAction(const MotionBase<MotionDerived> & v,
const Eigen::MatrixBase<Mat> & iF,
Eigen::MatrixBase<MatRet> const & jF)
{
internal::ForceSetSe3Action<Mat,MatRet,Mat::ColsAtCompileTime>::run(v,iF,jF);
internal::ForceSetMotionAction<MotionDerived,Mat,MatRet,Mat::ColsAtCompileTime>::run(v,iF,jF);
}
} // namespace forceSet
......@@ -262,14 +272,19 @@ namespace se3
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. */
* with m, and iF, jF are matrices whose columns are motions. */
static void run(const SE3 & m,
const Eigen::MatrixBase<Mat> & iF,
Eigen::MatrixBase<MatRet> const & jF);
static void run(const Motion & v,
};
template<typename MotionDerived, typename Mat,typename MatRet, int NCOLS>
struct MotionSetMotionAction
{
/* Compute dF = v ^ F, where is the action operation associated
* with v, and F, dF are matrices whose columns are motions. */
static void run(const MotionBase<MotionDerived> & v,
const Eigen::MatrixBase<Mat> & iF,
Eigen::MatrixBase<MatRet> const & jF);
......@@ -295,8 +310,12 @@ namespace se3
const_cast<Eigen::MatrixBase<MatRet> &>(jV).template head <3>()
= (m.translation().cross(jV.template tail<3>()) + m.rotation()*linear);
}
static void run(const Motion & v,
};
template<typename MotionDerived, typename Mat,typename MatRet>
struct MotionSetMotionAction<MotionDerived,Mat,MatRet,1>
{
static void run(const MotionBase<MotionDerived> & v,
const Eigen::MatrixBase<Mat> & iV,
Eigen::MatrixBase<MatRet> const & jV)
{
......@@ -330,9 +349,9 @@ namespace se3
}
}
template<typename Mat,typename MatRet,int NCOLS>
void MotionSetSe3Action<Mat,MatRet,NCOLS>::
run(const Motion & v,
template<typename MotionDerived, typename Mat,typename MatRet,int NCOLS>
void MotionSetMotionAction<MotionDerived,Mat,MatRet,NCOLS>::
run(const MotionBase<MotionDerived> & v,
const Eigen::MatrixBase<Mat> & iV,
Eigen::MatrixBase<MatRet> const & jV)
{
......@@ -340,7 +359,7 @@ namespace se3
{
typename MatRet::ColXpr jVc
= const_cast<Eigen::MatrixBase<MatRet> &>(jV).col(col);
motionSet::se3Action(v,iV.col(col),jVc);
motionSet::motionAction(v,iV.col(col),jVc);
}
}
......@@ -413,18 +432,18 @@ namespace se3
template<typename Mat,typename MatRet>
static void se3ActionInverse(const SE3 & m,
const Eigen::MatrixBase<Mat> & iV,
Eigen::MatrixBase<MatRet> const & jV)
const Eigen::MatrixBase<Mat> & iV,
Eigen::MatrixBase<MatRet> const & jV)
{
internal::MotionSetSe3ActionInverse<Mat,MatRet,Mat::ColsAtCompileTime>::run(m,iV,jV);
}
template<typename Mat,typename MatRet>
static void se3Action(const Motion & v,
const Eigen::MatrixBase<Mat> & iV,
Eigen::MatrixBase<MatRet> const & jV)
template<typename MotionDerived, typename Mat,typename MatRet>
static void motionAction(const MotionBase<MotionDerived> & v,
const Eigen::MatrixBase<Mat> & iV,
Eigen::MatrixBase<MatRet> const & jV)
{
internal::MotionSetSe3Action<Mat,MatRet,Mat::ColsAtCompileTime>::run(v,iV,jV);
internal::MotionSetMotionAction<MotionDerived,Mat,MatRet,Mat::ColsAtCompileTime>::run(v,iV,jV);
}
} // namespace motionSet
......
......@@ -521,7 +521,7 @@ BOOST_AUTO_TEST_CASE ( test_ActOnSet )
forceSet::se3ActionInverse(jMi.inverse(),iF,jFinv);
BOOST_CHECK(jFinv.isApprox(jF));
forceSet::se3Action(v,iF,jF);
forceSet::motionAction(v,iF,jF);
for( int k=0;k<N;++k )
BOOST_CHECK(v.cross(Force(iF.col(k))).toVector().isApprox(jF.col(k), 1e-12));
......@@ -534,7 +534,7 @@ BOOST_AUTO_TEST_CASE ( test_ActOnSet )
motionSet::se3ActionInverse(jMi.inverse(),iV,jVinv);
BOOST_CHECK(jVinv.isApprox(jV));
motionSet::se3Action(v,iV,jV);
motionSet::motionAction(v,iV,jV);
for( int k=0;k<N;++k )
BOOST_CHECK(v.cross(Motion(iV.col(k))).toVector().isApprox(jV.col(k), 1e-12));
......
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