cppad-joint-configurations.cpp

Go to the documentation of this file.

//
// Copyright (c) 2020 CNRS INRIA
//

#include "pinocchio/autodiff/cppad.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"

#include "utils/model-generator.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

using namespace pinocchio;

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_joint_configuration)
{
  typedef double Scalar;
  using CppAD::AD;
  using CppAD::NearEqual;
  
  typedef AD<Scalar> ADScalar;

  typedef pinocchio::ModelTpl<Scalar> Model;

  Model model; buildAllJointsModel(model);
  Eigen::VectorXd q2 = Eigen::VectorXd::Random(model.nq);
  Eigen::VectorXd q1 = Eigen::VectorXd::Random(model.nq);
  normalize(model,q1);
  normalize(model,q2);
  
  Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv);
  std::vector<Eigen::VectorXd> results_q(2,Eigen::VectorXd::Zero(model.nq));
  std::vector<Eigen::VectorXd> results_v(2,Eigen::VectorXd::Zero(model.nv));
  
  typedef pinocchio::ModelTpl<ADScalar> ADModel;
  typedef ADModel::ConfigVectorType ADConfigVectorType;
  ADModel ad_model = model.cast<ADScalar>();
  ADConfigVectorType ad_q1(model.nq);
  ADConfigVectorType ad_q2(model.nq);
  ADConfigVectorType ad_v(model.nv);

  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1> VectorX;
  typedef Eigen::Matrix<ADScalar,Eigen::Dynamic,1> VectorXAD;
  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic> MatrixX;
  typedef Eigen::Matrix<ADScalar,Eigen::Dynamic,Eigen::Dynamic> MatrixXAD;
  
  //Integrate
  {
    VectorXAD ad_x(model.nq+model.nv);
    ad_x << q1.cast<ADScalar>(), v.cast<ADScalar>();
    CppAD::Independent(ad_x);
    ad_q1 = ad_x.head(model.nq);
    ad_v = ad_x.tail(model.nv);
    
    VectorXAD ad_y(model.nq);
    pinocchio::integrate(ad_model,ad_q1,ad_v,ad_y);
    CppAD::ADFun<Scalar> ad_fun(ad_x,ad_y);
    
    CPPAD_TESTVECTOR(Scalar) x_eval((size_t)(ad_x.size()));
    Eigen::Map<VectorX>(x_eval.data(),ad_x.size()) << q1,v;
    CPPAD_TESTVECTOR(Scalar) y_eval((size_t)(ad_y.size()));
    y_eval = ad_fun.Forward(0,x_eval);
    results_q[0] = Eigen::Map<VectorX>(y_eval.data(),ad_y.size());
    
    pinocchio::integrate(model,q1,v,results_q[1]);
    BOOST_CHECK(results_q[0].isApprox(results_q[1]));
    
    Eigen::Map<VectorX>(x_eval.data(),ad_x.size()) << q1,VectorX::Zero(model.nv);
    y_eval = ad_fun.Forward(0,x_eval);
    results_q[0] = Eigen::Map<VectorX>(y_eval.data(),ad_y.size());
    BOOST_CHECK(results_q[0].isApprox(q1));
  }

  //Difference
  {
    VectorXAD ad_x(model.nq+model.nq);
    ad_x << q1.cast<ADScalar>(), q2.cast<ADScalar>();
    CppAD::Independent(ad_x);
    ad_q1 = ad_x.head(model.nq);
    ad_q2 = ad_x.tail(model.nq);
    
    VectorXAD ad_y(model.nv);
    pinocchio::difference(ad_model,ad_q1,ad_q2,ad_y);
    CppAD::ADFun<Scalar> ad_fun(ad_x,ad_y);
    
    CPPAD_TESTVECTOR(Scalar) x_eval((size_t)(ad_x.size()));
    Eigen::Map<VectorX>(x_eval.data(),ad_x.size()) << q1,q2;
    CPPAD_TESTVECTOR(Scalar) y_eval((size_t)(ad_y.size()));
    y_eval = ad_fun.Forward(0,x_eval);
    results_v[0] = Eigen::Map<VectorX>(y_eval.data(),ad_y.size());
    
    pinocchio::difference(model,q1,q2,results_v[1]);
    BOOST_CHECK(results_v[0].isApprox(results_v[1]));
    
    Eigen::Map<VectorX>(x_eval.data(),ad_x.size()) << q1,q1;
    y_eval = ad_fun.Forward(0,x_eval);
    results_v[0] = Eigen::Map<VectorX>(y_eval.data(),ad_y.size());
    BOOST_CHECK(results_v[0].isZero());
  }

  //dDifference
  std::vector<MatrixX> results_J0(2,MatrixX::Zero(model.nv,model.nv));
  std::vector<MatrixX> results_J1(2,MatrixX::Zero(model.nv,model.nv));
  {
    VectorXAD ad_x(model.nq+model.nq);
    ad_x << q1.cast<ADScalar>(), q2.cast<ADScalar>();
    CppAD::Independent(ad_x);
    ad_q1 = ad_x.head(model.nq);
    ad_q2 = ad_x.tail(model.nq);
    
    MatrixXAD ad_y(2*model.nv,model.nv);
    pinocchio::dDifference(ad_model,ad_q1,ad_q2,ad_y.topRows(model.nv),pinocchio::ARG0);
    pinocchio::dDifference(ad_model,ad_q1,ad_q2,ad_y.bottomRows(model.nv),pinocchio::ARG1);
    VectorXAD ad_y_flatten = Eigen::Map<VectorXAD>(ad_y.data(),ad_y.size());
    CppAD::ADFun<Scalar> ad_fun(ad_x,ad_y_flatten);
    
    CPPAD_TESTVECTOR(Scalar) x_eval((size_t)(ad_x.size()));
    Eigen::Map<VectorX>(x_eval.data(),ad_x.size()) << q1, q2;
    CPPAD_TESTVECTOR(Scalar) y_eval((size_t)(ad_y.size()));
    y_eval = ad_fun.Forward(0,x_eval);
    results_J0[0] = Eigen::Map<MatrixX>(y_eval.data(),ad_y.rows(),ad_y.cols()).topRows(model.nv);
    results_J1[0] = Eigen::Map<MatrixX>(y_eval.data(),ad_y.rows(),ad_y.cols()).bottomRows(model.nv);
    
    // w.r.t q1
    pinocchio::dDifference(model,q1,q2,results_J0[1],pinocchio::ARG0);
    BOOST_CHECK(results_J0[0].isApprox(results_J0[1]));
    
    // w.r.t q2
    pinocchio::dDifference(model,q1,q2,results_J1[1],pinocchio::ARG1);
    BOOST_CHECK(results_J1[0].isApprox(results_J1[1]));
    
    Eigen::Map<VectorX>(x_eval.data(),ad_x.size()) << q1, q1;
    y_eval = ad_fun.Forward(0,x_eval);
    results_J0[0] = Eigen::Map<MatrixX>(y_eval.data(),ad_y.rows(),ad_y.cols()).topRows(model.nv);
    results_J1[0] = Eigen::Map<MatrixX>(y_eval.data(),ad_y.rows(),ad_y.cols()).bottomRows(model.nv);
    
    BOOST_CHECK((-results_J0[0]).isIdentity());
    BOOST_CHECK(results_J1[0].isIdentity());
  }

}

BOOST_AUTO_TEST_SUITE_END()