center-of-mass-derivatives.cpp

Go to the documentation of this file.

//
// Copyright (c) 2019-2021 CNRS INRIA
//

#include "pinocchio/algorithm/center-of-mass.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"
#include "pinocchio/algorithm/center-of-mass-derivatives.hpp"
#include "pinocchio/parsers/sample-models.hpp"

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

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_kinematics_derivatives_vcom)
{
  using namespace Eigen;
  using namespace pinocchio;
  
  Model model;
  buildModels::humanoid(model);
  
  Data data_ref(model);
  
  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);
  VectorXd vq(VectorXd::Random(model.nv));
  VectorXd aq(VectorXd::Random(model.nv));
  
  // Approximate dvcom_dq by finite diff.
  centerOfMass(model,data_ref,q,vq);
  const Eigen::Vector3d vcom0 = data_ref.vcom[0];
  const double alpha = 1e-8;
  Eigen::VectorXd dq = VectorXd::Zero(model.nv);
  Data::Matrix3x dvcom_dqn(3,model.nv);

  for(int k = 0; k < model.nv; ++k)
  {
    dq[k] = alpha;
    centerOfMass(model,data_ref,integrate(model,q,dq),vq);
    dvcom_dqn.col(k) = (data_ref.vcom[0]-vcom0)/alpha;
    dq[k] = 0;
  }

  {
    // Compute dvcom_dq using the algorithm
    Data data(model);
    Data::Matrix3x dvcom_dq = Data::Matrix3x::Zero(3,model.nv);
    centerOfMass(model,data,q,vq);
    getCenterOfMassVelocityDerivatives(model,data,dvcom_dq);
    
    // Check that algo result and finite-diff approx are similar.
    BOOST_CHECK(dvcom_dq.isApprox(dvcom_dqn,sqrt(alpha)));
  }
  
  {
    // Compute dvcom_dq using the algorithm
    Data data(model);
    Data::Matrix3x dvcom_dq = Data::Matrix3x::Zero(3,model.nv);
    computeAllTerms(model,data,q,vq);
    getCenterOfMassVelocityDerivatives(model,data,dvcom_dq);
    
    // Check that algo result and finite-diff approx are similar.
    BOOST_CHECK(dvcom_dq.isApprox(dvcom_dqn,sqrt(alpha)));
  }
}

BOOST_AUTO_TEST_SUITE_END()