Skip to content

GitLab

Unverified Commit f3b2d68b authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #591 from jcarpent/devel 

Add dIntegrate for model + bug fixes
parents 730ffe6b b0272c7a
Hide whitespace changes
Inline Side-by-side
src/algorithm/joint-configuration.hpp
View file @ f3b2d68b
//
// Copyright (c) 2016-2018 CNRS
// Copyright (c) 2016-2018 CNRS INRIA
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -37,6 +37,24 @@ namespace pinocchio
integrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v);
/**
* @brief Computes the Jacobian of a small variation of the configuration vector or the tangent vector into the tangent space at identity.
*
* @param[in] model Model that must be integrated
* @param[in] q Initial configuration (size model.nq)
* @param[in] v Velocity (size model.nv)
* @param[out] J Jacobian of the Integrate operation, either with respect to q or v (size model.nv x model.nv).
* @param[in] arg Argument (either q or v) with respect to which the
*
*/
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType>
void dIntegrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v,
const Eigen::MatrixBase<JacobianMatrixType> & J,
const ArgumentPosition arg);
/**
* @brief Interpolate the model between two configurations
*
......
src/algorithm/joint-configuration.hxx
View file @ f3b2d68b
//
// Copyright (c) 2016-2018 CNRS
// Copyright (c) 2016-2018 CNRS INRIA
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -58,6 +58,34 @@ namespace pinocchio
}
return res;
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType>
void dIntegrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v,
const Eigen::MatrixBase<JacobianMatrixType> & J,
const ArgumentPosition arg)
{
return dIntegrate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,JacobianMatrixType>(model, q.derived(), v.derived(), J.derived(),arg);
}
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType>
void dIntegrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v,
const Eigen::MatrixBase<JacobianMatrixType> & J,
const ArgumentPosition arg)
{
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
typedef dIntegrateStep<LieGroup_t,ConfigVectorType,TangentVectorType,JacobianMatrixType> Algo;
typename Algo::ArgsType args(q.derived(),v.derived(),EIGEN_CONST_CAST(JacobianMatrixType,J),arg);
for(JointIndex i=1; i<(JointIndex)model.njoints; ++i)
{
Algo::run(model.joints[i], args);
}
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
inline typename EIGEN_PLAIN_TYPE(ConfigVectorIn1)
......
src/macros.hpp
View file @ f3b2d68b
......@@ -49,6 +49,9 @@ namespace pinocchio
/// \brief Empty macro argument
#define PINOCCHIO_MACRO_EMPTY_ARG
/// \brief Helper to declare that a parameter is unused
#define PINOCCHIO_UNUSED_VARIABLE(var) (void)(var)
/// Ensure that a matrix (or vector) is of correct size (compile-time and run-time assertion)
#define PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(type,M,nrows,ncols) \
EIGEN_STATIC_ASSERT( (type::RowsAtCompileTime == Eigen::Dynamic || type::RowsAtCompileTime == nrows) \
......
src/multibody/joint/joint-base.hpp
View file @ f3b2d68b
//
// Copyright (c) 2015-2016,2018 CNRS
// Copyright (c) 2015-2018 CNRS INRIA
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
// This file is part of Pinocchio
......@@ -154,18 +154,113 @@ struct CastType< NewScalar, JointModelTpl<Scalar,Options> > \
typedef typename Mat::template FixedSegmentReturnType<NV>::Type Type;
typedef typename Mat::template ConstFixedSegmentReturnType<NV>::Type ConstType;
};
template<typename D>
static typename SegmentReturn<D>::ConstType
segment(const Eigen::MatrixBase<D> & mat,
typename Eigen::DenseBase<D>::Index start,
typename Eigen::DenseBase<D>::Index size = NV)
{
PINOCCHIO_UNUSED_VARIABLE(size);
return mat.template segment<NV>(start);
}
template<typename D>
static typename SegmentReturn<D>::Type
segment(Eigen::MatrixBase<D> & mat,
typename Eigen::DenseBase<D>::Index start,
typename Eigen::DenseBase<D>::Index size = NV)
{
PINOCCHIO_UNUSED_VARIABLE(size);
return mat.template segment<NV>(start);
}
template<class Mat>
struct ColsReturn
{
typedef typename Mat::template NColsBlockXpr<NV>::Type Type;
typedef typename Mat::template ConstNColsBlockXpr<NV>::Type ConstType;
};
template<typename D>
static typename ColsReturn<D>::ConstType
middleCols(const Eigen::MatrixBase<D> & mat,
typename Eigen::DenseBase<D>::Index start,
typename Eigen::DenseBase<D>::Index size = NV)
{
PINOCCHIO_UNUSED_VARIABLE(size);
return mat.template middleCols<NV>(start);
}
template<typename D>
static typename ColsReturn<D>::Type
middleCols(Eigen::MatrixBase<D> & mat,
typename Eigen::DenseBase<D>::Index start,
typename Eigen::DenseBase<D>::Index size = NV)
{
PINOCCHIO_UNUSED_VARIABLE(size);
return mat.template middleCols<NV>(start);
}
template<class Mat>
struct RowsReturn
{
typedef typename Mat::template NRowsBlockXpr<NV>::Type Type;
typedef typename Mat::template ConstNRowsBlockXpr<NV>::Type ConstType;
};
template<typename D>
static typename RowsReturn<D>::ConstType
middleRows(const Eigen::MatrixBase<D> & mat,
typename Eigen::DenseBase<D>::Index start,
typename Eigen::DenseBase<D>::Index size = NV)
{
PINOCCHIO_UNUSED_VARIABLE(size);
return mat.template middleRows<NV>(start);
}
template<typename D>
static typename RowsReturn<D>::Type
middleRows(Eigen::MatrixBase<D> & mat,
typename Eigen::DenseBase<D>::Index start,
typename Eigen::DenseBase<D>::Index size = NV)
{
PINOCCHIO_UNUSED_VARIABLE(size);
return mat.template middleRows<NV>(start);
}
template<class Mat>
struct BlockReturn
{
typedef Eigen::Block<Mat, NV, NV> Type;
typedef const Eigen::Block<const Mat, NV, NV> ConstType;
};
template<typename D>
static typename BlockReturn<D>::ConstType
block(const Eigen::MatrixBase<D> & mat,
typename Eigen::DenseBase<D>::Index row_id,
typename Eigen::DenseBase<D>::Index col_id,
typename Eigen::DenseBase<D>::Index row_size_block = NV,
typename Eigen::DenseBase<D>::Index col_size_block = NV)
{
PINOCCHIO_UNUSED_VARIABLE(row_size_block);
PINOCCHIO_UNUSED_VARIABLE(col_size_block);
return mat.template block<NV,NV>(row_id,col_id);
}
template<typename D>
static typename BlockReturn<D>::Type
block(Eigen::MatrixBase<D> & mat,
typename Eigen::DenseBase<D>::Index row_id,
typename Eigen::DenseBase<D>::Index col_id,
typename Eigen::DenseBase<D>::Index row_size_block = NV,
typename Eigen::DenseBase<D>::Index col_size_block = NV)
{
PINOCCHIO_UNUSED_VARIABLE(row_size_block);
PINOCCHIO_UNUSED_VARIABLE(col_size_block);
return mat.template block<NV,NV>(row_id,col_id);
}
};
template<>
......@@ -177,18 +272,103 @@ struct CastType< NewScalar, JointModelTpl<Scalar,Options> > \
typedef typename Mat::SegmentReturnType Type;
typedef typename Mat::ConstSegmentReturnType ConstType;
};
template<typename D>
static typename SegmentReturn<D>::ConstType
segment(const Eigen::MatrixBase<D> & mat,
typename Eigen::DenseBase<D>::Index start,
typename Eigen::DenseBase<D>::Index size)
{
return mat.segment(start,size);
}
template<typename D>
static typename SegmentReturn<D>::Type
segment(Eigen::MatrixBase<D> & mat,
typename Eigen::DenseBase<D>::Index start,
typename Eigen::DenseBase<D>::Index size)
{
return mat.segment(start,size);
}
template<class Mat>
struct ColsReturn
{
typedef typename Mat::ColsBlockXpr Type;
typedef typename Mat::ConstColsBlockXpr ConstType;
};
template<typename D>
static typename ColsReturn<D>::ConstType
middleCols(const Eigen::MatrixBase<D> & mat,
typename Eigen::DenseBase<D>::Index start,
typename Eigen::DenseBase<D>::Index size)
{
return mat.middleCols(start,size);
}
template<typename D>
static typename ColsReturn<D>::Type
middleCols(Eigen::MatrixBase<D> & mat,
typename Eigen::DenseBase<D>::Index start,
typename Eigen::DenseBase<D>::Index size)
{
return mat.middleCols(start,size);
}
template<class Mat>
struct RowsReturn
{
typedef typename Mat::RowsBlockXpr Type;
typedef typename Mat::ConstRowsBlockXpr ConstType;
};
template<typename D>
static typename RowsReturn<D>::ConstType
middleRows(const Eigen::MatrixBase<D> & mat,
typename Eigen::DenseBase<D>::Index start,
typename Eigen::DenseBase<D>::Index size)
{
return mat.middleRows(start,size);
}
template<typename D>
static typename RowsReturn<D>::Type
middleRows(Eigen::MatrixBase<D> & mat,
typename Eigen::DenseBase<D>::Index start,
typename Eigen::DenseBase<D>::Index size)
{
return mat.middleRows(start,size);
}
template<class Mat>
struct BlockReturn
{
typedef Eigen::Block<Mat> Type;
typedef const Eigen::Block<const Mat> ConstType;
};
template<typename D>
static typename BlockReturn<D>::ConstType
block(const Eigen::MatrixBase<D> & mat,
typename Eigen::DenseBase<D>::Index row_id,
typename Eigen::DenseBase<D>::Index col_id,
typename Eigen::DenseBase<D>::Index row_size_block,
typename Eigen::DenseBase<D>::Index col_size_block)
{
return mat.block(row_id,col_id,row_size_block,col_size_block);
}
template<typename D>
static typename BlockReturn<D>::Type
block(Eigen::MatrixBase<D> & mat,
typename Eigen::DenseBase<D>::Index row_id,
typename Eigen::DenseBase<D>::Index col_id,
typename Eigen::DenseBase<D>::Index row_size_block,
typename Eigen::DenseBase<D>::Index col_size_block)
{
return mat.block(row_id,col_id,row_size_block,col_size_block);
}
};
template<typename Derived>
......@@ -304,46 +484,108 @@ struct CastType< NewScalar, JointModelTpl<Scalar,Options> > \
// Const access
template<typename D>
typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
jointConfigSelector(const Eigen::MatrixBase<D>& a) const { return derived().jointConfigSelector_impl(a); }
jointConfigSelector(const Eigen::MatrixBase<D>& a) const
{ return derived().jointConfigSelector_impl(a); }
template<typename D>
typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
jointConfigSelector_impl(const Eigen::MatrixBase<D>& a) const { return a.template segment<NQ>(i_q); }
jointConfigSelector_impl(const Eigen::MatrixBase<D>& a) const
{ return SizeDepType<NQ>::segment(a,i_q,nq()); }
// Non-const access
template<typename D>
typename SizeDepType<NQ>::template SegmentReturn<D>::Type
jointConfigSelector( Eigen::MatrixBase<D>& a) const { return derived().jointConfigSelector_impl(a); }
jointConfigSelector( Eigen::MatrixBase<D>& a) const
{ return derived().jointConfigSelector_impl(a); }
template<typename D>
typename SizeDepType<NQ>::template SegmentReturn<D>::Type
jointConfigSelector_impl( Eigen::MatrixBase<D>& a) const { return a.template segment<NQ>(i_q); }
jointConfigSelector_impl( Eigen::MatrixBase<D>& a) const
{ return SizeDepType<NQ>::segment(a,i_q,nq()); }
/* Acces to dedicated segment in robot config velocity space. */
// Const access
template<typename D>
typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
jointVelocitySelector(const Eigen::MatrixBase<D>& a) const { return derived().jointVelocitySelector_impl(a); }
jointVelocitySelector(const Eigen::MatrixBase<D>& a) const
{ return derived().jointVelocitySelector_impl(a); }
template<typename D>
typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
jointVelocitySelector_impl(const Eigen::MatrixBase<D>& a) const { return a.template segment<NV>(i_v); }
jointVelocitySelector_impl(const Eigen::MatrixBase<D>& a) const
{ return SizeDepType<NV>::segment(a,i_v,nv()); }
// Non-const access
template<typename D>
typename SizeDepType<NV>::template SegmentReturn<D>::Type
jointVelocitySelector( Eigen::MatrixBase<D>& a) const { return derived().jointVelocitySelector_impl(a); }
jointVelocitySelector( Eigen::MatrixBase<D>& a) const
{ return derived().jointVelocitySelector_impl(a); }
template<typename D>
typename SizeDepType<NV>::template SegmentReturn<D>::Type
jointVelocitySelector_impl( Eigen::MatrixBase<D>& a) const { return a.template segment<NV>(i_v); }
jointVelocitySelector_impl( Eigen::MatrixBase<D>& a) const
{ return SizeDepType<NV>::segment(a,i_v,nv()); }
template<typename D>
typename SizeDepType<NV>::template ColsReturn<D>::ConstType
jointCols(const Eigen::MatrixBase<D>& A) const { return derived().jointCols_impl(A); }
jointCols(const Eigen::MatrixBase<D>& A) const
{ return derived().jointCols_impl(A); }
template<typename D>
typename SizeDepType<NV>::template ColsReturn<D>::ConstType
jointCols_impl(const Eigen::MatrixBase<D>& A) const { return A.template middleCols<NV>(i_v); }
jointCols_impl(const Eigen::MatrixBase<D>& A) const
{ return SizeDepType<NV>::middleCols(A,i_v,nv()); }
template<typename D>
typename SizeDepType<NV>::template ColsReturn<D>::Type
jointCols(Eigen::MatrixBase<D>& A) const { return derived().jointCols_impl(A); }
jointCols(Eigen::MatrixBase<D>& A) const
{ return derived().jointCols_impl(A); }
template<typename D>
typename SizeDepType<NV>::template ColsReturn<D>::Type
jointCols_impl(Eigen::MatrixBase<D>& A) const { return A.template middleCols<NV>(i_v); }
jointCols_impl(Eigen::MatrixBase<D>& A) const
{ return SizeDepType<NV>::middleCols(A,i_v,nv()); }
template<typename D>
typename SizeDepType<NV>::template RowsReturn<D>::ConstType
jointRows(const Eigen::MatrixBase<D>& A) const
{ return derived().jointRows_impl(A); }
template<typename D>
typename SizeDepType<NV>::template RowsReturn<D>::ConstType
jointRows_impl(const Eigen::MatrixBase<D>& A) const
{ return SizeDepType<NV>::middleRows(A,i_v,nv()); }
template<typename D>
typename SizeDepType<NV>::template RowsReturn<D>::Type
jointRows(Eigen::MatrixBase<D>& A) const
{ return derived().jointRows_impl(A); }
template<typename D>
typename SizeDepType<NV>::template RowsReturn<D>::Type
jointRows_impl(Eigen::MatrixBase<D>& A) const
{ return SizeDepType<NV>::middleRows(A,i_v,nv()); }
/// \brief Returns a block of dimension nv()xnv() located at position i_v,i_v in the matrix Mat
template<typename D>
typename SizeDepType<NV>::template BlockReturn<D>::ConstType
jointBlock(const Eigen::MatrixBase<D> & Mat) const
{ return derived().jointBlock_impl(Mat); }
template<typename D>
typename SizeDepType<NV>::template BlockReturn<D>::ConstType
jointBlock_impl(const Eigen::MatrixBase<D> & Mat) const
{ return SizeDepType<NV>::block(Mat,i_v,i_v,nv(),nv()); }
template<typename D>
typename SizeDepType<NV>::template BlockReturn<D>::Type
jointBlock(Eigen::MatrixBase<D> & Mat) const
{ return derived().jointBlock_impl(Mat); }
template<typename D>
typename SizeDepType<NV>::template BlockReturn<D>::Type
jointBlock_impl(Eigen::MatrixBase<D> & Mat) const
{ return SizeDepType<NV>::block(Mat,i_v,i_v,nv(),nv()); }
protected:
......@@ -357,13 +599,14 @@ struct CastType< NewScalar, JointModelTpl<Scalar,Options> > \
///
/// Copy of stand-alone JointModelBase are prevented, but can be used from inhereting
/// objects. Copy is done by calling copy operator.
inline JointModelBase( const JointModelBase& clone) { *this = clone; }
inline JointModelBase(const JointModelBase & clone)
{ *this = clone; }
/// Copy operator: protected.
///
/// Copy of stand-alone JointModelBase are prevented, but can be used from inhereting
/// objects.
inline JointModelBase& operator=(const JointModelBase& clone)
inline JointModelBase & operator=(const JointModelBase & clone)
{
i_id = clone.i_id;
i_q = clone.i_q;
......
src/multibody/liegroup/liegroup-algo.hxx
View file @ f3b2d68b
......@@ -147,6 +147,43 @@ namespace pinocchio
SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_3(IntegrateStepAlgo);
template<typename Visitor, typename JointModel> struct dIntegrateStepAlgo;
template<typename LieGroup_t, typename ConfigVectorIn, typename TangentVectorIn, typename JacobianMatrixType>
struct dIntegrateStep
: public fusion::JointVisitorBase< dIntegrateStep<LieGroup_t,ConfigVectorIn,TangentVectorIn,JacobianMatrixType> >
{
typedef boost::fusion::vector<const ConfigVectorIn &,
const TangentVectorIn &,
JacobianMatrixType &,
const ArgumentPosition &
> ArgsType;
SE3_DETAILS_VISITOR_METHOD_ALGO_4(dIntegrateStepAlgo, dIntegrateStep)
};
template<typename Visitor, typename JointModel>
struct dIntegrateStepAlgo
{
template<typename ConfigVectorIn, typename TangentVector, typename JacobianMatrixType>
static void run(const JointModelBase<JointModel> & jmodel,
const Eigen::MatrixBase<ConfigVectorIn> & q,
const Eigen::MatrixBase<TangentVector> & v,
const Eigen::MatrixBase<JacobianMatrixType> & mat,
const ArgumentPosition & arg)
{
typedef typename Visitor::LieGroupMap LieGroupMap;
typename LieGroupMap::template operation<JointModel>::type lgo;
lgo.dIntegrate(jmodel.jointConfigSelector (q.derived()),
jmodel.jointVelocitySelector(v.derived()),
jmodel.jointBlock(EIGEN_CONST_CAST(JacobianMatrixType,mat)),
arg);
}
};
SE3_DETAILS_DISPATCH_JOINT_COMPOSITE_4(dIntegrateStepAlgo);
template<typename Visitor, typename JointModel> struct InterpolateStepAlgo;
template<typename LieGroup_t, typename ConfigVectorIn1, typename ConfigVectorIn2, typename Scalar, typename ConfigVectorOut>
......
src/multibody/liegroup/liegroup-base.hpp
View file @ f3b2d68b
......@@ -94,10 +94,10 @@ SE3_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
const Eigen::MatrixBase<Jacobian_t> & J) const;
/**
* @brief Computes the Jacobian of a small variation of the configuration vector into tangent space at identity.
* @brief Computes the Jacobian of a small variation of the configuration vector or the tangent vector into tangent space at identity.
*
* @details This Jacobian corresponds to the Jacobian of \f$ (\bm{q} \oplus \delta \bm{q}) \oplus \bm{v} \f$ with
* \f$ \delta \bm{q} \rightarrow 0 \f$.
* \f$ \delta \bm{q} \rightarrow 0 \f$ if arg == ARG0 or \f$ \delta \bm{v} \rightarrow 0 \f$ if arg == ARG1.
*
* @param[in] q configuration vector.
* @param[in] v tangent vector.
......@@ -108,7 +108,29 @@ SE3_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
template <ArgumentPosition arg, class Config_t, class Tangent_t, class JacobianOut_t>
void dIntegrate(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t>& J) const;
const Eigen::MatrixBase<JacobianOut_t>& J) const
{
PINOCCHIO_STATIC_ASSERT(arg==ARG0||arg==ARG1, arg_SHOULD_BE_ARG0_OR_ARG1);
return dIntegrate(q,v,J,arg);
}
/**
* @brief Computes the Jacobian of a small variation of the configuration vector or the tangent vector into tangent space at identity.
*
* @details This Jacobian corresponds to the Jacobian of \f$ (\bm{q} \oplus \delta \bm{q}) \oplus \bm{v} \f$ with
* \f$ \delta \bm{q} \rightarrow 0 \f$ if arg == ARG0 or \f$ \delta \bm{v} \rightarrow 0 \f$ if arg == ARG1.
*
* @param[in] q configuration vector.
* @param[in] v tangent vector.
* @param[in] arg ARG0 (resp. ARG1) to get the Jacobian with respect to q (resp. v).
*
* @param[out] J the Jacobian of the Integrate operation w.r.t. the argument arg.
*/
template<class Config_t, class Tangent_t, class JacobianOut_t>
void dIntegrate(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t> & J,
const ArgumentPosition arg) const;
/**
* @brief Computes the Jacobian of a small variation of the configuration vector into tangent space at identity.
......@@ -213,7 +235,7 @@ SE3_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
PINOCCHIO_DEPRECATED;
/**
* @brief Computes the Jacobian of the difference operation.
* @brief Computes the Jacobian of the difference operation with respect to q0 or q1.
*
* @param[in] q0 the initial configuration vector.
* @param[in] q1 the terminal configuration vector.
......
src/multibody/liegroup/liegroup-base.hxx
View file @ f3b2d68b
......@@ -50,12 +50,14 @@ namespace pinocchio {
}
template <class Derived>
template <ArgumentPosition arg, class Config_t, class Tangent_t, class JacobianOut_t>
template<class Config_t, class Tangent_t, class JacobianOut_t>
void LieGroupBase<Derived>::dIntegrate(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t>& J) const
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t>& J,
const ArgumentPosition arg) const
{
PINOCCHIO_STATIC_ASSERT(arg==ARG0||arg==ARG1, arg_SHOULD_BE_ARG0_OR_ARG1);
assert((arg==ARG0||arg==ARG1) && "arg should be either ARG0 or ARG1");
switch (arg) {
case ARG0:
dIntegrate_dq(q,v,J); return;
......
unittest/joint-configurations.cpp
View file @ f3b2d68b
......@@ -86,6 +86,7 @@ BOOST_AUTO_TEST_CASE ( integration_test )
qs[0] = Eigen::VectorXd::Ones(model.nq);
qs[0].segment<4>(3) /= qs[0].segment<4>(3).norm(); // quaternion of freeflyer
qs[0].segment<4>(7) /= qs[0].segment<4>(7).norm(); // quaternion of spherical joint
qs[0].segment<2>(11+2) /= qs[0].segment<2>(11+2).norm(); // planar joint
qdots[0] = Eigen::VectorXd::Zero(model.nv);
results[0] = integrate(model,qs[0],qdots[0]);
......@@ -93,6 +94,66 @@ BOOST_AUTO_TEST_CASE ( integration_test )
BOOST_CHECK_MESSAGE(results[0].isApprox(qs[0], 1e-12), "integration of full body with zero velocity - wrong results");
}
BOOST_AUTO_TEST_CASE ( diff_integration_test )
{
Model model; buildModel(model);
std::vector<Eigen::VectorXd> qs(2);
std::vector<Eigen::VectorXd> vs(2);
std::vector<Eigen::MatrixXd> results(2,Eigen::MatrixXd::Zero(model.nv,model.nv));
std::vector<Eigen::MatrixXd> results_fd(2,Eigen::MatrixXd::Zero(model.nv,model.nv));