Commit 02489e60 authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

multibody/liegroup: fix visitor dintegratetransport

parent eec57deb
......@@ -194,14 +194,14 @@ namespace pinocchio
template<typename Visitor, typename JointModel> struct dIntegrateTransportStepAlgo;
template<typename LieGroup_t, typename ConfigVectorIn, typename TangentVectorIn, typename JacobianMatrixTypeIn, typename JacobianMatrixTypeOut>
template<typename LieGroup_t, typename ConfigVectorIn, typename TangentVectorIn, typename JacobianMatrixInType, typename JacobianMatrixOutType>
struct dIntegrateTransportStep
: public fusion::JointUnaryVisitorBase< dIntegrateTransportStep<LieGroup_t,ConfigVectorIn,TangentVectorIn,JacobianMatrixTypeIn,JacobianMatrixTypeOut> >
: public fusion::JointUnaryVisitorBase< dIntegrateTransportStep<LieGroup_t,ConfigVectorIn,TangentVectorIn,JacobianMatrixInType,JacobianMatrixOutType> >
{
typedef boost::fusion::vector<const ConfigVectorIn &,
const TangentVectorIn &,
JacobianMatrixTypeIn &,
JacobianMatrixTypeOut &,
const JacobianMatrixInType &,
JacobianMatrixOutType &,
const ArgumentPosition &
> ArgsType;
......@@ -216,7 +216,8 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorIn> & q,
const Eigen::MatrixBase<TangentVector> & v,
const Eigen::MatrixBase<JacobianMatrixInType> & mat_in,
const Eigen::MatrixBase<JacobianMatrixOutType> & mat_out)
const Eigen::MatrixBase<JacobianMatrixOutType> & mat_out,
const ArgumentPosition & arg)
{
typedef typename Visitor::LieGroupMap LieGroupMap;
......
......@@ -191,7 +191,7 @@ PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
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<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout) const;
/**
* @brief Transport an input matrix to the manifold defined by the dIntegrate computation.
......
......@@ -94,7 +94,8 @@ namespace pinocchio {
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
const Eigen::MatrixBase<JacobianOut_t> & Jout,
const ArgumentPosition arg) const
{
assert((arg==ARG0||arg==ARG1) && "arg should be either ARG0 or ARG1");
......
......@@ -154,14 +154,20 @@ 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}
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout) const
{
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,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,
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}
const Eigen::MatrixBase<JacobianOut_t> & Jout) const
{
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout) = Jin;
}
// template <class ConfigL_t, class ConfigR_t>
// static double squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
......
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