Verified Commit c0497fd6 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

liegroup/algo: add dDifferenceStep

parent ad7d9d93
......@@ -171,6 +171,43 @@ namespace pinocchio
PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_4(dIntegrateStepAlgo);
template<typename Visitor, typename JointModel> struct dDifferenceStepAlgo;
template<typename LieGroup_t, typename ConfigVector1, typename ConfigVector2, typename JacobianMatrix>
struct dDifferenceStep
: public fusion::JointUnaryVisitorBase< dDifferenceStep<LieGroup_t,ConfigVector1,ConfigVector2,JacobianMatrix> >
{
typedef boost::fusion::vector<const ConfigVector1 &,
const ConfigVector2 &,
JacobianMatrix &,
const ArgumentPosition &
> ArgsType;
PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_4(dDifferenceStepAlgo, dDifferenceStep)
};
template<typename Visitor, typename JointModel>
struct dDifferenceStepAlgo
{
template<typename ConfigVector1, typename ConfigVector2, typename JacobianMatrix>
static void run(const JointModelBase<JointModel> & jmodel,
const Eigen::MatrixBase<ConfigVector1> & q0,
const Eigen::MatrixBase<ConfigVector2> & q1,
const Eigen::MatrixBase<JacobianMatrix> & mat,
const ArgumentPosition & arg)
{
typedef typename Visitor::LieGroupMap LieGroupMap;
typename LieGroupMap::template operation<JointModel>::type lgo;
lgo.dDifference(jmodel.jointConfigSelector(q0.derived()),
jmodel.jointConfigSelector(q1.derived()),
jmodel.jointBlock(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix,mat)),
arg);
}
};
PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_4(dDifferenceStepAlgo);
template<typename Visitor, typename JointModel> struct InterpolateStepAlgo;
template<typename LieGroup_t, typename ConfigVectorIn1, typename ConfigVectorIn2, typename Scalar, typename ConfigVectorOut>
......
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