Commit eabf3a19 authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

multibody/liegroup: setup visitors for dintegrateTransportInplace

parent f0a2816f
......@@ -232,6 +232,43 @@ namespace pinocchio
PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_5(dIntegrateTransportStepAlgo);
template<typename Visitor, typename JointModel> struct dIntegrateTransportInPlaceStepAlgo;
template<typename LieGroup_t, typename ConfigVectorIn, typename TangentVectorIn, typename JacobianMatrixType>
struct dIntegrateTransportInPlaceStep
: public fusion::JointUnaryVisitorBase< dIntegrateTransportInPlaceStep<LieGroup_t,ConfigVectorIn,TangentVectorIn,JacobianMatrixType> >
{
typedef boost::fusion::vector<const ConfigVectorIn &,
const TangentVectorIn &,
JacobianMatrixType &,
const ArgumentPosition &
> ArgsType;
PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_4(dIntegrateTransportInPlaceStepAlgo, dIntegrateTransportInPlaceStep)
};
template<typename Visitor, typename JointModel>
struct dIntegrateTransportInPlaceStepAlgo
{
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.dIntegrateTransportInPlace(jmodel.jointConfigSelector (q.derived()),
jmodel.jointVelocitySelector(v.derived()),
jmodel.jointRows(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,mat)),
arg);
}
};
PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_4(dIntegrateTransportInPlaceStepAlgo);
template<typename Visitor, typename JointModel> struct dDifferenceStepAlgo;
......
......@@ -210,7 +210,58 @@ PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout) const;
/**
* @brief TransportInPlace an input matrix to the manifold defined by the dIntegrate computation.
*
* @details This input and output has to be interpreted in terms of Lie group, not vector space:
* Thus, dIntegrate(q, v, J, arg) creates a manifold manifold M given by a small variation of the configuration vector or the tangent vector into the tangent space at identity.
* We are moving our input matrix onto this manifold M.
* @param[in] q configuration vector.
* @param[in] v tangent vector
* @param[in,out] J the input matrix
* @param[in] arg ARG0 (resp. ARG1) to get the Jacobian with respect to q (resp. v).
*/
template<class Config_t, class Tangent_t, class Jacobian_t>
void dIntegrateTransportInPlace(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<Jacobian_t> & J,
const ArgumentPosition arg) const;
/**
* @brief Transportinplace an input matrix to the manifold defined by the dIntegrate computation.
*
* @details This input and output has to be interpreted in terms of Lie group, not vector space:
* Thus, dIntegrate(q, v, J, ARG0) creates a manifold manifold M given by a small variation of the configuration vector into the tangent space at identity.
* We are moving our input matrix onto this manifold M.
* @param[in] q configuration vector.
* @param[in] v tangent vector
* @param[in,out] J the input matrix
*
*/
template <class Config_t, class Tangent_t, class Jacobian_t>
void dIntegrateTransportInPlace_dq(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<Jacobian_t> & J) const;
/**
* @brief TransportInPlace an input matrix to the manifold defined by the dIntegrate computation.
*
* @details This input and output has to be interpreted in terms of Lie group, not vector space:
* Thus, dIntegrate(q, v, J, ARG1) creates a manifold manifold M given by a small variation of the tangent vector into the tangent space at identity.
* We are moving our input matrix onto this manifold M.
* @param[in] q configuration vector.
* @param[in] v tangent vector
* @param[in,out] J the input matrix
*
* @param[out] Jout Transported matrix
*/
template <class Config_t, class Tangent_t, class Jacobian_t>
void dIntegrateTransportInPlace_dv(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<Jacobian_t> & J) const;
/**
* @brief Interpolation between two joint's configurations
*
......
......@@ -141,7 +141,55 @@ namespace pinocchio {
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout));
}
template <class Derived>
template<class Config_t, class Tangent_t, class Jacobian_t>
void LieGroupBase<Derived>::dIntegrateTransportInPlace(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<Jacobian_t> & J,
const ArgumentPosition arg) const
{
assert((arg==ARG0||arg==ARG1) && "arg should be either ARG0 or ARG1");
switch (arg) {
case ARG0:
dIntegrateTransportInPlace_dq(q.derived(),v.derived(),PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J)); return;
case ARG1:
dIntegrateTransportInPlace_dv(q.derived(),v.derived(),PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J)); return;
default: return;
}
}
template <class Derived>
template <class Config_t, class Tangent_t, class Jacobian_t>
void LieGroupBase<Derived>::dIntegrateTransportInPlace_dq(
const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<Jacobian_t> & J) const
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t , ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t , TangentVector_t);
//EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(JacobianOut_t, JacobianMatrix_t);
derived().dIntegrateTransportInPlace_dq_impl(q.derived(),
v.derived(),
PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J));
}
template <class Derived>
template <class Config_t, class Tangent_t, class Jacobian_t>
void LieGroupBase<Derived>::dIntegrateTransportInPlace_dv(
const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<Jacobian_t> & J) const
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t , ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t , TangentVector_t);
//EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(JacobianOut_t, JacobianMatrix_t);
derived().dIntegrateTransportInPlace_dv_impl(q.derived(),
v.derived(),
PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J));
}
/**
* @brief Interpolation between two joint's configurations
*
......
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