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

multibody/liegroup: setup dIntegrateTransport signature

parent a8eba917
......@@ -175,11 +175,25 @@ namespace pinocchio
default:
assert(false && "Wrong Op requesed value");
break;
}
}
}
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout) const
{
}
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout) const
{
}
template <class ConfigL_t, class ConfigR_t>
Scalar squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1) const
......
......@@ -191,6 +191,46 @@ namespace pinocchio
};
PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_5(dIntegrateStepAlgo);
template<typename Visitor, typename JointModel> struct dIntegrateTransportStepAlgo;
template<typename LieGroup_t, typename ConfigVectorIn, typename TangentVectorIn, typename JacobianMatrixTypeIn, typename JacobianMatrixTypeOut>
struct dIntegrateTransportStep
: public fusion::JointUnaryVisitorBase< dIntegrateTransportStep<LieGroup_t,ConfigVectorIn,TangentVectorIn,JacobianMatrixTypeIn,JacobianMatrixTypeOut> >
{
typedef boost::fusion::vector<const ConfigVectorIn &,
const TangentVectorIn &,
JacobianMatrixTypeIn &,
JacobianMatrixTypeOut &,
const ArgumentPosition &
> ArgsType;
PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_5(dIntegrateTransportStepAlgo, dIntegrateTransportStep)
};
template<typename Visitor, typename JointModel>
struct dIntegrateTransportStepAlgo
{
template<typename ConfigVectorIn, typename TangentVector, typename JacobianMatrixInType, typename JacobianMatrixOutType>
static void run(const JointModelBase<JointModel> & jmodel,
const Eigen::MatrixBase<ConfigVectorIn> & q,
const Eigen::MatrixBase<TangentVector> & v,
const Eigen::MatrixBase<JacobianMatrixInType> & mat_in,
const Eigen::MatrixBase<JacobianMatrixOutType> & mat_out)
{
typedef typename Visitor::LieGroupMap LieGroupMap;
typename LieGroupMap::template operation<JointModel>::type lgo;
lgo.dIntegrateTransport(jmodel.jointConfigSelector (q.derived()),
jmodel.jointVelocitySelector(v.derived()),
jmodel.jointBlock(mat_in.derived()),
jmodel.jointBlock(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixOutType,mat_out)),
arg);
}
};
PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_5(dIntegrateTransportStepAlgo);
template<typename Visitor, typename JointModel> struct dDifferenceStepAlgo;
......
......@@ -154,6 +154,63 @@ PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
const Eigen::MatrixBase<JacobianOut_t> & J,
const AssignmentOperatorType op) const;
/**
* @brief Transport 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] J the input matrix
* @param[in] arg ARG0 (resp. ARG1) to get the Jacobian with respect to q (resp. v).
*
* @param[out] Jout Transported matrix
*/
template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void dIntegrateTransport(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout,
const ArgumentPosition arg) const;
/**
* @brief Transport 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] J the input matrix
*
* @param[out] Jout Transported matrix
*/
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void dIntegrateTransport_dq(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<Jacobianin_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout) const;
/**
* @brief Transport 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] J the input matrix
*
* @param[out] Jout Transported matrix
*/
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void dIntegrateTransport_dv(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout) const;
/**
* @brief Interpolation between two joint's configurations
*
......
......@@ -87,7 +87,61 @@ namespace pinocchio {
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J),
op);
}
template <class Derived>
template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void LieGroupBase<Derived>::dIntegrateTransport(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout) const
{
assert((arg==ARG0||arg==ARG1) && "arg should be either ARG0 or ARG1");
switch (arg) {
case ARG0:
dIntegrateTransport_dq(q.derived(),v.derived(),Jin.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout)); return;
case ARG1:
dIntegrateTransport_dv(q.derived(),v.derived(),Jin.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout)); return;
default: return;
}
}
template <class Derived>
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void LieGroupBase<Derived>::dIntegrateTransport_dq(
const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout) 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().dIntegrateTransport_dq_impl(q.derived(),
v.derived(),
Jin.derived(),
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout));
}
template <class Derived>
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void LieGroupBase<Derived>::dIntegrateTransport_dv(
const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout) 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().dIntegrateTransport_dq_impl(q.derived(),
v.derived(),
Jin.derived(),
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout));
}
/**
* @brief Interpolation between two joint's configurations
*
......
......@@ -345,8 +345,22 @@ namespace pinocchio
assert(false && "Wrong Op requesed value");
break;
}
}
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout) const
{
}
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout) const
{
}
// interpolate_impl use default implementation.
......@@ -621,6 +635,23 @@ namespace pinocchio
}
}
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout) const
{
}
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout) const
{
}
// interpolate_impl use default implementation.
// template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
// static void interpolate_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
......
......@@ -219,6 +219,22 @@ namespace pinocchio
}
}
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout) const
{
}
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout) const
{
}
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
static void interpolate_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
......@@ -445,6 +461,22 @@ namespace pinocchio
}
}
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout) const
{
}
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout) const
{
}
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
static void interpolate_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
......
......@@ -151,6 +151,18 @@ namespace pinocchio
}
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
const Eigen::MatrixBase<Tangent_t> & /*v*/,
const Eigen::MatrixBase<JacobianIn_t> & /*Jin*/,
const Eigen::MatrixBase<JacobianOut_t> & /*Jout*/) const {Jout = Jin}
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout) const {Jout = Jin}
// template <class ConfigL_t, class ConfigR_t>
// static double squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
// const Eigen::MatrixBase<ConfigR_t> & q1)
......
Markdown is supported
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