Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/crba.cpp	Lines:	41	41	100.0 %
Date:	2023-08-09 08:43:58	Branches:	153	302	50.7 %


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

/*
 * Test the CRBA algorithm. The code validates both the computation times and
 * the value by comparing the results of the CRBA with the reconstruction of
 * the mass matrix using the RNEA.
 * For a strong timing benchmark, see benchmark/timings.
 *
 */

#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/centroidal.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/center-of-mass.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/utils/timer.hpp"

#include <iostream>

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

template<typename JointModel>
static void addJointAndBody(pinocchio::Model & model,
                            const pinocchio::JointModelBase<JointModel> & joint,
                            const std::string & parent_name,
                            const std::string & name,
                            const pinocchio::SE3 placement = pinocchio::SE3::Random(),
                            bool setRandomLimits = true)
{
  using namespace pinocchio;
  typedef typename JointModel::ConfigVector_t CV;
  typedef typename JointModel::TangentVector_t TV;

  Model::JointIndex idx;

  if (setRandomLimits)
    idx = model.addJoint(model.getJointId(parent_name),joint,
                         SE3::Random(),
                         name + "_joint",
                         TV::Random() + TV::Constant(1),
                         TV::Random() + TV::Constant(1),
                         CV::Random() - CV::Constant(1),
                         CV::Random() + CV::Constant(1)
                         );
  else
    idx = model.addJoint(model.getJointId(parent_name),joint,
                         placement, name + "_joint");

  model.addJointFrame(idx);

  model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity());
  model.addBodyFrame(name + "_body", idx);
}

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

BOOST_AUTO_TEST_CASE ( test_crba )
{
  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model);
  pinocchio::Data data(model);

  #ifdef NDEBUG
    #ifdef _INTENSE_TESTING_
      const size_t NBT = 1000*1000;
    #else
      const size_t NBT = 10;
    #endif

    Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);

    PinocchioTicToc timer(PinocchioTicToc::US); timer.tic();
    SMOOTH(NBT)
      {
        crba(model,data,q);
      }
    timer.toc(std::cout,NBT);

  #else
    const size_t NBT = 1;
    using namespace Eigen;
    using namespace pinocchio;

    Eigen::MatrixXd M(model.nv,model.nv);
    Eigen::VectorXd q = Eigen::VectorXd::Ones(model.nq);
    q.segment <4> (3).normalize();
    Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
    Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
    data.M.fill(0);  crba(model,data,q);
    data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();

    /* Joint inertia from iterative crba. */
    const Eigen::VectorXd bias = rnea(model,data,q,v,a);
    for(int i=0;i<model.nv;++i)
    {
      M.col(i) = rnea(model,data,q,v,Eigen::VectorXd::Unit(model.nv,i)) - bias;
    }

    // std::cout << "Mcrb = [ " << data.M << "  ];" << std::endl;
    // std::cout << "Mrne = [  " << M << " ]; " << std::endl;
    BOOST_CHECK(M.isApprox(data.M,1e-12));

    std::cout << "(the time score in debug mode is not relevant)  " ;

    q = Eigen::VectorXd::Zero(model.nq);

    PinocchioTicToc timer(PinocchioTicToc::US); timer.tic();
    SMOOTH(NBT)
    {
      crba(model,data,q);
    }
    timer.toc(std::cout,NBT);

  #endif // ifndef NDEBUG

}

BOOST_AUTO_TEST_CASE(test_minimal_crba)
{
  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model);
  pinocchio::Data data(model), data_ref(model);

  model.lowerPositionLimit.head<7>().fill(-1.);
  model.upperPositionLimit.head<7>().fill(1.);

  Eigen::VectorXd q = randomConfiguration(model,model.lowerPositionLimit,model.upperPositionLimit);
  Eigen::VectorXd v(Eigen::VectorXd::Random(model.nv));

  crba(model,data_ref,q);
  data_ref.M.triangularView<Eigen::StrictlyLower>() = data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();

  crbaMinimal(model,data,q);
  data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();

  BOOST_CHECK(data.M.isApprox(data_ref.M));

  ccrba(model,data_ref,q,v);
  computeJointJacobians(model,data_ref,q);
  BOOST_CHECK(data.Ag.isApprox(data_ref.Ag));
  BOOST_CHECK(data.J.isApprox(data_ref.J));

}

BOOST_AUTO_TEST_SUITE_END ()



Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2015-2019 CNRS INRIA
3			//
4
5			/*
6			* Test the CRBA algorithm. The code validates both the computation times and
7			* the value by comparing the results of the CRBA with the reconstruction of
8			* the mass matrix using the RNEA.
9			* For a strong timing benchmark, see benchmark/timings.
10			*
11			*/
12
13			#include "pinocchio/multibody/model.hpp"
14			#include "pinocchio/multibody/data.hpp"
15			#include "pinocchio/algorithm/crba.hpp"
16			#include "pinocchio/algorithm/centroidal.hpp"
17			#include "pinocchio/algorithm/rnea.hpp"
18			#include "pinocchio/algorithm/jacobian.hpp"
19			#include "pinocchio/algorithm/center-of-mass.hpp"
20			#include "pinocchio/algorithm/joint-configuration.hpp"
21			#include "pinocchio/parsers/sample-models.hpp"
22			#include "pinocchio/utils/timer.hpp"
23
24			#include <iostream>
25
26			#include <boost/test/unit_test.hpp>
27			#include <boost/utility/binary.hpp>
28
29			template<typename JointModel>
30			static void addJointAndBody(pinocchio::Model & model,
31			const pinocchio::JointModelBase<JointModel> & joint,
32			const std::string & parent_name,
33			const std::string & name,
34			const pinocchio::SE3 placement = pinocchio::SE3::Random(),
35			bool setRandomLimits = true)
36			{
37			using namespace pinocchio;
38			typedef typename JointModel::ConfigVector_t CV;
39			typedef typename JointModel::TangentVector_t TV;
40
41			Model::JointIndex idx;
42
43			if (setRandomLimits)
44			idx = model.addJoint(model.getJointId(parent_name),joint,
45			SE3::Random(),
46			name + "_joint",
47			TV::Random() + TV::Constant(1),
48			TV::Random() + TV::Constant(1),
49			CV::Random() - CV::Constant(1),
50			CV::Random() + CV::Constant(1)
51			);
52			else
53			idx = model.addJoint(model.getJointId(parent_name),joint,
54			placement, name + "_joint");
55
56			model.addJointFrame(idx);
57
58			model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity());
59			model.addBodyFrame(name + "_body", idx);
60			}
61
62			BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
63
64	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE ( test_crba )
65			{
66	✓✗	4	pinocchio::Model model;
67	✓✗	2	pinocchio::buildModels::humanoidRandom(model);
68	✓✗	4	pinocchio::Data data(model);
69
70			#ifdef NDEBUG
71			#ifdef _INTENSE_TESTING_
72			const size_t NBT = 1000*1000;
73			#else
74			const size_t NBT = 10;
75			#endif
76
77			Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
78
79			PinocchioTicToc timer(PinocchioTicToc::US); timer.tic();
80			SMOOTH(NBT)
81			{
82			crba(model,data,q);
83			}
84			timer.toc(std::cout,NBT);
85
86			#else
87		2	const size_t NBT = 1;
88			using namespace Eigen;
89			using namespace pinocchio;
90
91	✓✗	4	Eigen::MatrixXd M(model.nv,model.nv);
92	✓✗✓✗	4	Eigen::VectorXd q = Eigen::VectorXd::Ones(model.nq);
93	✓✗✓✗	2	q.segment <4> (3).normalize();
94	✓✗✓✗	4	Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
95	✓✗✓✗	4	Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
96	✓✗✓✗	2	data.M.fill(0); crba(model,data,q);
97	✓✗✓✗ ✓✗✓✗	2	data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
98
99			/* Joint inertia from iterative crba. */
100	✓✗✓✗	4	const Eigen::VectorXd bias = rnea(model,data,q,v,a);
101	✓✓	66	for(int i=0;i<model.nv;++i)
102			{
103	✓✗✓✗ ✓✗✓✗ ✓✗	64	M.col(i) = rnea(model,data,q,v,Eigen::VectorXd::Unit(model.nv,i)) - bias;
104			}
105
106			// std::cout << "Mcrb = [ " << data.M << " ];" << std::endl;
107			// std::cout << "Mrne = [ " << M << " ]; " << std::endl;
108	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(M.isApprox(data.M,1e-12));
109
110	✓✗	2	std::cout << "(the time score in debug mode is not relevant) " ;
111
112	✓✗✓✗	2	q = Eigen::VectorXd::Zero(model.nq);
113
114	✓✗✓✗	4	PinocchioTicToc timer(PinocchioTicToc::US); timer.tic();
115	✓✓	4	SMOOTH(NBT)
116			{
117	✓✗	2	crba(model,data,q);
118			}
119	✓✗	2	timer.toc(std::cout,NBT);
120
121			#endif // ifndef NDEBUG
122
123		2	}
124
125	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_minimal_crba)
126			{
127	✓✗	4	pinocchio::Model model;
128	✓✗	2	pinocchio::buildModels::humanoidRandom(model);
129	✓✗✓✗	4	pinocchio::Data data(model), data_ref(model);
130
131	✓✗✓✗	2	model.lowerPositionLimit.head<7>().fill(-1.);
132	✓✗✓✗	2	model.upperPositionLimit.head<7>().fill(1.);
133
134	✓✗	4	Eigen::VectorXd q = randomConfiguration(model,model.lowerPositionLimit,model.upperPositionLimit);
135	✓✗✓✗	4	Eigen::VectorXd v(Eigen::VectorXd::Random(model.nv));
136
137	✓✗	2	crba(model,data_ref,q);
138	✓✗✓✗ ✓✗✓✗	2	data_ref.M.triangularView<Eigen::StrictlyLower>() = data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
139
140	✓✗	2	crbaMinimal(model,data,q);
141	✓✗✓✗ ✓✗✓✗	2	data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
142
143	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.M.isApprox(data_ref.M));
144
145	✓✗	2	ccrba(model,data_ref,q,v);
146	✓✗	2	computeJointJacobians(model,data_ref,q);
147	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.Ag.isApprox(data_ref.Ag));
148	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.J.isApprox(data_ref.J));
149
150		2	}
151
152			BOOST_AUTO_TEST_SUITE_END ()
153