Verified Commit cd231f9c authored by Justin Carpentier's avatar Justin Carpentier
Browse files

algo/joint-configuration: add dIntegrate

parent f093df59
//
// Copyright (c) 2016-2018 CNRS
// Copyright (c) 2016-2018 CNRS INRIA
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -37,6 +37,24 @@ namespace pinocchio
integrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v);
/**
* @brief Computes the Jacobian of a small variation of the configuration vector or the tangent vector into the tangent space at identity.
*
* @param[in] model Model that must be integrated
* @param[in] q Initial configuration (size model.nq)
* @param[in] v Velocity (size model.nv)
* @param[out] J Jacobian of the Integrate operation, either with respect to q or v (size model.nv x model.nv).
* @param[in] arg Argument (either q or v) with respect to which the
*
*/
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType>
void dIntegrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v,
const Eigen::MatrixBase<JacobianMatrixType> & J,
const ArgumentPosition arg);
/**
* @brief Interpolate the model between two configurations
*
......
//
// Copyright (c) 2016-2018 CNRS
// Copyright (c) 2016-2018 CNRS INRIA
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......@@ -58,6 +58,34 @@ namespace pinocchio
}
return res;
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType>
void dIntegrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v,
const Eigen::MatrixBase<JacobianMatrixType> & J,
const ArgumentPosition arg)
{
return dIntegrate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,JacobianMatrixType>(model, q.derived(), v.derived(), J.derived(),arg);
}
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType>
void dIntegrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v,
const Eigen::MatrixBase<JacobianMatrixType> & J,
const ArgumentPosition arg)
{
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
typedef dIntegrateStep<LieGroup_t,ConfigVectorType,TangentVectorType,JacobianMatrixType> Algo;
typename Algo::ArgsType args(q.derived(),v.derived(),EIGEN_CONST_CAST(JacobianMatrixType,J),arg);
for(JointIndex i=1; i<(JointIndex)model.njoints; ++i)
{
Algo::run(model.joints[i], args);
}
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorIn1, typename ConfigVectorIn2>
inline typename EIGEN_PLAIN_TYPE(ConfigVectorIn1)
......
......@@ -86,6 +86,7 @@ BOOST_AUTO_TEST_CASE ( integration_test )
qs[0] = Eigen::VectorXd::Ones(model.nq);
qs[0].segment<4>(3) /= qs[0].segment<4>(3).norm(); // quaternion of freeflyer
qs[0].segment<4>(7) /= qs[0].segment<4>(7).norm(); // quaternion of spherical joint
qs[0].segment<2>(11+2) /= qs[0].segment<2>(11+2).norm(); // planar joint
qdots[0] = Eigen::VectorXd::Zero(model.nv);
results[0] = integrate(model,qs[0],qdots[0]);
......@@ -93,6 +94,37 @@ BOOST_AUTO_TEST_CASE ( integration_test )
BOOST_CHECK_MESSAGE(results[0].isApprox(qs[0], 1e-12), "integration of full body with zero velocity - wrong results");
}
BOOST_AUTO_TEST_CASE ( diff_integration_test )
{
Model model; buildModel(model);
std::vector<Eigen::VectorXd> qs(2);
std::vector<Eigen::VectorXd> v(2);
std::vector<Eigen::MatrixXd> results(2,Eigen::MatrixXd::Zero(model.nv,model.nv));
std::vector<Eigen::MatrixXd> results_fd(2,Eigen::MatrixXd::Zero(model.nv,model.nv));
qs[0] = Eigen::VectorXd::Ones(model.nq);
qs[0].segment<4>(3) /= qs[0].segment<4>(3).norm(); // quaternion of freeflyer
qs[0].segment<4>(7) /= qs[0].segment<4>(7).norm(); // quaternion of spherical joint
qs[0].segment<2>(11+2) /= qs[0].segment<2>(11+2).norm(); // planar joint
v[0] = Eigen::VectorXd::Zero(model.nv);
v[1] = Eigen::VectorXd::Ones(model.nv);
dIntegrate(model,qs[0],v[0],results[0],ARG0);
Eigen::VectorXd q_fd(model.nq), v_fd(model.nv); v_fd.setZero();
const double eps = 1e-8;
for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
{
v_fd[k] = eps;
q_fd = integrate(model,qs[0],v_fd);
results_fd[0].col(k) = difference(model,qs[0],q_fd)/eps;
v_fd[k] = 0.;
}
BOOST_CHECK(results[0].isIdentity(sqrt(eps)));
BOOST_CHECK(results[0].isApprox(results_fd[0],sqrt(eps)));
}
BOOST_AUTO_TEST_CASE ( integrate_difference_test )
{
Model model; buildModel(model);
......
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