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

algorithm/joint-configurations: add dIntegrateTransport signature

parent afe74d5e
......@@ -287,7 +287,7 @@ namespace pinocchio
const Eigen::MatrixBase<JacobianMatrixType> & J,
const ArgumentPosition arg,
const AssignmentOperatorType op);
/**
*
* @brief Computes the Jacobian of a small variation of the configuration vector or the tangent vector into the tangent space at identity.
......@@ -345,6 +345,33 @@ namespace pinocchio
{
dIntegrate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,JacobianMatrixType>(model, q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,J),arg,op);
}
/**
*
* @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: as such,
* 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] model Model of the kinematic tree on which the integration operation is performed.
* @param[in] q Initial configuration (size model.nq)
* @param[in] v Joint velocity (size model.nv)
* @param[out] Jin Input Matrix (number of rows = model.nv).
* @param[out] Jout Output Matrix (same size as Jin).
* @param[in] arg Argument (either q or v) with respect to which the differentiation manifold M is generated.
*
*/
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType1, typename JacobianMatrixType2>
void dIntegrateTransport(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v,
const Eigen::MatrixBase<JacobianMatrixType1> & Jin,
const Eigen::MatrixBase<JacobianMatrixType2> & Jout,
const ArgumentPosition arg);
/**
*
......
......@@ -171,6 +171,31 @@ namespace pinocchio
}
}
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType1, typename JacobianMatrixType2>
void dIntegrateTransport(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v,
const Eigen::MatrixBase<JacobianMatrixType1> & J,
const Eigen::MatrixBase<JacobianMatrixType2> & Jout,
const ArgumentPosition arg)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq, "The configuration vector is not of the right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(v.size() == model.nv, "The joint velocity vector is not of the right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(Jin.rows() == model.nv, "The input matrix is not of the right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(Jout.rows() == Jin.rows(), "The output argument should be the same size as input matrix");
PINOCCHIO_CHECK_INPUT_ARGUMENT(Jout.cols() == Jin.cols(), "The output argument should be the same size as input matrix");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
typedef dIntegrateStep<LieGroup_t,ConfigVectorType,TangentVectorType,JacobianMatrixType1,JacobianMatrixType2> Algo;
typename Algo::ArgsType args(q.derived(),v.derived(),Jin.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,Jout),arg);
for(JointIndex i=1; i<(JointIndex)model.njoints; ++i)
{
Algo::run(model.joints[i], args);
}
}
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVector1, typename ConfigVector2, typename JacobianMatrix>
void dDifference(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const Eigen::MatrixBase<ConfigVector1> & 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