Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/crba.cpp	Lines:	55	55	100.0 %
Date:	2024-01-23 21:41:47	Branches:	207	410	50.5 %


//
// Copyright (c) 2015-2023 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.
 *
 */

#ifndef EIGEN_RUNTIME_NO_MALLOC
  #define EIGEN_RUNTIME_NO_MALLOC
#endif

#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));

  // Check double call
  {
    pinocchio::Data data2(model);
    crbaMinimal(model,data2,q);
    crbaMinimal(model,data2,q);

    BOOST_CHECK(data2.Ycrb[0] == data.Ycrb[0]);
  }

}

#ifndef NDEBUG

BOOST_AUTO_TEST_CASE(test_crba_malloc)
{
  using namespace pinocchio;
  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model);

  model.addJoint(size_t(model.njoints-1), pinocchio::JointModelRevoluteUnaligned(SE3::Vector3::UnitX()), SE3::Random(), "revolute_unaligned");
  pinocchio::Data data(model);

  const Eigen::VectorXd q = pinocchio::neutral(model);
  Eigen::internal::set_is_malloc_allowed(false);
  crba(model, data, q);
  Eigen::internal::set_is_malloc_allowed(true);
}

#endif

BOOST_AUTO_TEST_SUITE_END ()



Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2015-2023 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			#ifndef EIGEN_RUNTIME_NO_MALLOC
14			#define EIGEN_RUNTIME_NO_MALLOC
15			#endif
16
17			#include "pinocchio/multibody/model.hpp"
18			#include "pinocchio/multibody/data.hpp"
19			#include "pinocchio/algorithm/crba.hpp"
20			#include "pinocchio/algorithm/centroidal.hpp"
21			#include "pinocchio/algorithm/rnea.hpp"
22			#include "pinocchio/algorithm/jacobian.hpp"
23			#include "pinocchio/algorithm/center-of-mass.hpp"
24			#include "pinocchio/algorithm/joint-configuration.hpp"
25			#include "pinocchio/parsers/sample-models.hpp"
26			#include "pinocchio/utils/timer.hpp"
27
28			#include <iostream>
29
30			#include <boost/test/unit_test.hpp>
31			#include <boost/utility/binary.hpp>
32
33			template<typename JointModel>
34			static void addJointAndBody(pinocchio::Model & model,
35			const pinocchio::JointModelBase<JointModel> & joint,
36			const std::string & parent_name,
37			const std::string & name,
38			const pinocchio::SE3 placement = pinocchio::SE3::Random(),
39			bool setRandomLimits = true)
40			{
41			using namespace pinocchio;
42			typedef typename JointModel::ConfigVector_t CV;
43			typedef typename JointModel::TangentVector_t TV;
44
45			Model::JointIndex idx;
46
47			if (setRandomLimits)
48			idx = model.addJoint(model.getJointId(parent_name),joint,
49			SE3::Random(),
50			name + "_joint",
51			TV::Random() + TV::Constant(1),
52			TV::Random() + TV::Constant(1),
53			CV::Random() - CV::Constant(1),
54			CV::Random() + CV::Constant(1)
55			);
56			else
57			idx = model.addJoint(model.getJointId(parent_name),joint,
58			placement, name + "_joint");
59
60			model.addJointFrame(idx);
61
62			model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity());
63			model.addBodyFrame(name + "_body", idx);
64			}
65
66			BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
67
68	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE ( test_crba )
69			{
70	✓✗	4	pinocchio::Model model;
71	✓✗	2	pinocchio::buildModels::humanoidRandom(model);
72	✓✗	4	pinocchio::Data data(model);
73
74			#ifdef NDEBUG
75			#ifdef _INTENSE_TESTING_
76			const size_t NBT = 1000*1000;
77			#else
78			const size_t NBT = 10;
79			#endif
80
81			Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
82
83			PinocchioTicToc timer(PinocchioTicToc::US); timer.tic();
84			SMOOTH(NBT)
85			{
86			crba(model,data,q);
87			}
88			timer.toc(std::cout,NBT);
89
90			#else
91		2	const size_t NBT = 1;
92			using namespace Eigen;
93			using namespace pinocchio;
94
95	✓✗	4	Eigen::MatrixXd M(model.nv,model.nv);
96	✓✗✓✗	4	Eigen::VectorXd q = Eigen::VectorXd::Ones(model.nq);
97	✓✗✓✗	2	q.segment <4> (3).normalize();
98	✓✗✓✗	4	Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
99	✓✗✓✗	4	Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
100	✓✗✓✗	2	data.M.fill(0); crba(model,data,q);
101	✓✗✓✗ ✓✗✓✗	2	data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
102
103			/* Joint inertia from iterative crba. */
104	✓✗✓✗	4	const Eigen::VectorXd bias = rnea(model,data,q,v,a);
105	✓✓	66	for(int i=0;i<model.nv;++i)
106			{
107	✓✗✓✗ ✓✗✓✗ ✓✗	64	M.col(i) = rnea(model,data,q,v,Eigen::VectorXd::Unit(model.nv,i)) - bias;
108			}
109
110			// std::cout << "Mcrb = [ " << data.M << " ];" << std::endl;
111			// std::cout << "Mrne = [ " << M << " ]; " << std::endl;
112	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(M.isApprox(data.M,1e-12));
113
114	✓✗	2	std::cout << "(the time score in debug mode is not relevant) " ;
115
116	✓✗✓✗	2	q = Eigen::VectorXd::Zero(model.nq);
117
118	✓✗✓✗	4	PinocchioTicToc timer(PinocchioTicToc::US); timer.tic();
119	✓✓	4	SMOOTH(NBT)
120			{
121	✓✗	2	crba(model,data,q);
122			}
123	✓✗	2	timer.toc(std::cout,NBT);
124
125			#endif // ifndef NDEBUG
126
127		2	}
128
129	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_minimal_crba)
130			{
131	✓✗	4	pinocchio::Model model;
132	✓✗	2	pinocchio::buildModels::humanoidRandom(model);
133	✓✗✓✗	4	pinocchio::Data data(model), data_ref(model);
134
135	✓✗✓✗	2	model.lowerPositionLimit.head<7>().fill(-1.);
136	✓✗✓✗	2	model.upperPositionLimit.head<7>().fill(1.);
137
138	✓✗	4	Eigen::VectorXd q = randomConfiguration(model,model.lowerPositionLimit,model.upperPositionLimit);
139	✓✗✓✗	4	Eigen::VectorXd v(Eigen::VectorXd::Random(model.nv));
140
141	✓✗	2	crba(model,data_ref,q);
142	✓✗✓✗ ✓✗✓✗	2	data_ref.M.triangularView<Eigen::StrictlyLower>() = data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
143
144	✓✗	2	crbaMinimal(model,data,q);
145	✓✗✓✗ ✓✗✓✗	2	data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
146
147	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.M.isApprox(data_ref.M));
148
149	✓✗	2	ccrba(model,data_ref,q,v);
150	✓✗	2	computeJointJacobians(model,data_ref,q);
151	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.Ag.isApprox(data_ref.Ag));
152	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.J.isApprox(data_ref.J));
153
154			// Check double call
155			{
156	✓✗	4	pinocchio::Data data2(model);
157	✓✗	2	crbaMinimal(model,data2,q);
158	✓✗	2	crbaMinimal(model,data2,q);
159
160	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data2.Ycrb[0] == data.Ycrb[0]);
161			}
162
163		2	}
164
165			#ifndef NDEBUG
166
167	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_crba_malloc)
168			{
169			using namespace pinocchio;
170	✓✗	4	pinocchio::Model model;
171	✓✗	2	pinocchio::buildModels::humanoidRandom(model);
172
173	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	2	model.addJoint(size_t(model.njoints-1), pinocchio::JointModelRevoluteUnaligned(SE3::Vector3::UnitX()), SE3::Random(), "revolute_unaligned");
174	✓✗	4	pinocchio::Data data(model);
175
176	✓✗	4	const Eigen::VectorXd q = pinocchio::neutral(model);
177		2	Eigen::internal::set_is_malloc_allowed(false);
178	✓✗	2	crba(model, data, q);
179		2	Eigen::internal::set_is_malloc_allowed(true);
180		2	}
181
182			#endif
183
184			BOOST_AUTO_TEST_SUITE_END ()
185