Verified Commit 9c706b85 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

algo/joint-config: add dDifference algo

parent c0497fd6
......@@ -315,6 +315,59 @@ namespace pinocchio
dIntegrate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,JacobianMatrixType>(model, q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,J),arg);
}
/**
*
* @brief Computes the Jacobian of a small variation of the configuration vector into the tangent space at identity.
*
* @details This jacobian has to be interpreted in terms of Lie group, not vector space: as such,
* it is expressed in the tangent space only, not the configuration space.
* Calling \f$ d(q0, q1) \f$ the difference function, these jacobians satisfy the following relationships in the
* tangent space:
* - Jacobian relative to q0: \f$ d(q_0 \oplus \delta q_0, q_1) \ominus d(q_0, q_1) = J_{q_0} \delta q_0 + o(\| \delta q_0 \|)\f$.
* - Jacobian relative to q1: \f$ d(q_0, q_1 \oplus \delta q_1) \ominus d(q_0, q_1) = J_{q_1} \delta q_1 + o(\| \delta q_1 \|)\f$.
*
* @param[in] model Model of the kinematic tree on which the difference operation is performed.
* @param[in] q0 Initial configuration (size model.nq)
* @param[in] q1 Joint velocity (size model.nv)
* @param[out] J Jacobian of the Difference operation, either with respect to q0 or q1 (size model.nv x model.nv).
* @param[in] arg Argument (either q0 or q1) with respect to which the differentiation is performed.
*
*/
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,
const Eigen::MatrixBase<ConfigVector2> & q1,
const Eigen::MatrixBase<JacobianMatrix> & J,
const ArgumentPosition arg);
/**
*
* @brief Computes the Jacobian of a small variation of the configuration vector into the tangent space at identity.
*
* @details This jacobian has to be interpreted in terms of Lie group, not vector space: as such,
* it is expressed in the tangent space only, not the configuration space.
* Calling \f$ d(q0, q1) \f$ the difference function, these jacobians satisfy the following relationships in the
* tangent space:
* - Jacobian relative to q0: \f$ d(q_0 \oplus \delta q_0, q_1) \ominus d(q_0, q_1) = J_{q_0} \delta q_0 + o(\| \delta q_0 \|)\f$.
* - Jacobian relative to q1: \f$ d(q_0, q_1 \oplus \delta q_1) \ominus d(q_0, q_1) = J_{q_1} \delta q_1 + o(\| \delta q_1 \|)\f$.
*
* @param[in] model Model of the kinematic tree on which the difference operation is performed.
* @param[in] q0 Initial configuration (size model.nq)
* @param[in] q1 Joint velocity (size model.nv)
* @param[out] J Jacobian of the Difference operation, either with respect to q0 or q1 (size model.nv x model.nv).
* @param[in] arg Argument (either q0 or q1) with respect to which the differentiation is performed.
*
*/
template<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,
const Eigen::MatrixBase<ConfigVector2> & q1,
const Eigen::MatrixBase<JacobianMatrix> & J,
const ArgumentPosition arg)
{
dDifference<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVector1,ConfigVector2,JacobianMatrix>
(model, q0.derived(), q1.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix,J),arg);
}
/**
*
* @brief Overall squared distance between two configuration vectors
......
......@@ -170,6 +170,29 @@ namespace pinocchio
}
}
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,
const Eigen::MatrixBase<ConfigVector2> & q1,
const Eigen::MatrixBase<JacobianMatrix> & J,
const ArgumentPosition arg)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(q0.size() == model.nq, "The configuration vector q0 is not of the right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(q1.size() == model.nq, "The configuration vector q1 is not of the right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(J.rows() == model.nv, "The output argument is not of the right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(J.cols() == model.nv, "The output argument is not of the right size");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
typedef dDifferenceStep<LieGroup_t,ConfigVector1,ConfigVector2,JacobianMatrix> Algo;
typename Algo::ArgsType args(q0.derived(),q1.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix,J),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 ConfigVectorIn1, typename ConfigVectorIn2>
Scalar
squaredDistanceSum(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
......
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