GCC Code Coverage Report

Directory:	./
File:	unittest/energy.cpp
Date:	2025-02-12 21:03:38

	Exec	Total	Coverage
Lines:	106	107	99.1%
Branches:	346	688	50.3%

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2016-2020 CNRS INRIA
3			//
4
5			#include "pinocchio/algorithm/energy.hpp"
6			#include "pinocchio/algorithm/crba.hpp"
7			#include "pinocchio/algorithm/rnea.hpp"
8			#include "pinocchio/algorithm/joint-configuration.hpp"
9			#include "pinocchio/algorithm/center-of-mass.hpp"
10
11			#include "pinocchio/multibody/sample-models.hpp"
12
13			#include <iostream>
14			#include <boost/test/unit_test.hpp>
15			#include <boost/utility/binary.hpp>
16			#include <boost/test/tools/floating_point_comparison.hpp>
17
18			BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
19
20	33/66 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 1 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 19 taken 1 times. ✗ Branch 20 not taken. ✓ Branch 23 taken 1 times. ✗ Branch 24 not taken. ✓ Branch 27 taken 1 times. ✗ Branch 28 not taken. ✓ Branch 30 taken 1 times. ✗ Branch 31 not taken. ✓ Branch 33 taken 1 times. ✗ Branch 34 not taken. ✓ Branch 36 taken 1 times. ✗ Branch 37 not taken. ✓ Branch 41 taken 1 times. ✗ Branch 42 not taken. ✓ Branch 45 taken 1 times. ✗ Branch 46 not taken. ✓ Branch 48 taken 1 times. ✗ Branch 49 not taken. ✓ Branch 52 taken 1 times. ✗ Branch 53 not taken. ✓ Branch 55 taken 1 times. ✗ Branch 56 not taken. ✓ Branch 58 taken 1 times. ✗ Branch 59 not taken. ✓ Branch 61 taken 1 times. ✗ Branch 62 not taken. ✓ Branch 66 taken 1 times. ✗ Branch 67 not taken. ✓ Branch 70 taken 1 times. ✗ Branch 71 not taken. ✓ Branch 73 taken 1 times. ✗ Branch 74 not taken. ✓ Branch 77 taken 1 times. ✗ Branch 78 not taken. ✓ Branch 80 taken 1 times. ✗ Branch 81 not taken. ✓ Branch 83 taken 1 times. ✗ Branch 84 not taken. ✓ Branch 86 taken 1 times. ✗ Branch 87 not taken. ✓ Branch 91 taken 1 times. ✗ Branch 92 not taken. ✓ Branch 95 taken 1 times. ✗ Branch 96 not taken. ✓ Branch 98 taken 1 times. ✗ Branch 99 not taken. ✓ Branch 102 taken 1 times. ✗ Branch 103 not taken. ✓ Branch 105 taken 1 times. ✗ Branch 106 not taken. ✓ Branch 108 taken 1 times. ✗ Branch 109 not taken. ✓ Branch 111 taken 1 times. ✗ Branch 112 not taken. ✓ Branch 116 taken 1 times. ✗ Branch 117 not taken.	4	BOOST_AUTO_TEST_CASE(test_kinetic_energy)
21			{
22			using namespace Eigen;
23			using namespace pinocchio;
24
25	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Model model;
26	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	buildModels::humanoidRandom(model);
27	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data data(model);
28
29	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	const VectorXd qmax = VectorXd::Ones(model.nq);
30	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd q = randomConfiguration(model, -qmax, qmax);
31	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd v = VectorXd::Random(model.nv);
32
33	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	crba(model, data, q, Convention::WORLD);
34	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	data.M.triangularView<Eigen::StrictlyLower>() =
35	3/6 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 1 times. ✗ Branch 8 not taken.	4	data.M.transpose().triangularView<Eigen::StrictlyLower>();
36
37	5/10 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 1 times. ✗ Branch 8 not taken. ✓ Branch 10 taken 1 times. ✗ Branch 11 not taken. ✓ Branch 13 taken 1 times. ✗ Branch 14 not taken.	2	double kinetic_energy_ref = 0.5 * v.transpose() * data.M * v;
38	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	double kinetic_energy = computeKineticEnergy(model, data, q, v);
39
40	5/10 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 10 taken 1 times. ✗ Branch 11 not taken. ✓ Branch 13 taken 1 times. ✗ Branch 14 not taken. ✗ Branch 17 not taken. ✓ Branch 18 taken 1 times.	2	BOOST_CHECK_SMALL(kinetic_energy_ref - kinetic_energy, 1e-12);
41		2	}
42
43	33/66 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 1 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 19 taken 1 times. ✗ Branch 20 not taken. ✓ Branch 23 taken 1 times. ✗ Branch 24 not taken. ✓ Branch 27 taken 1 times. ✗ Branch 28 not taken. ✓ Branch 30 taken 1 times. ✗ Branch 31 not taken. ✓ Branch 33 taken 1 times. ✗ Branch 34 not taken. ✓ Branch 36 taken 1 times. ✗ Branch 37 not taken. ✓ Branch 41 taken 1 times. ✗ Branch 42 not taken. ✓ Branch 45 taken 1 times. ✗ Branch 46 not taken. ✓ Branch 48 taken 1 times. ✗ Branch 49 not taken. ✓ Branch 52 taken 1 times. ✗ Branch 53 not taken. ✓ Branch 55 taken 1 times. ✗ Branch 56 not taken. ✓ Branch 58 taken 1 times. ✗ Branch 59 not taken. ✓ Branch 61 taken 1 times. ✗ Branch 62 not taken. ✓ Branch 66 taken 1 times. ✗ Branch 67 not taken. ✓ Branch 70 taken 1 times. ✗ Branch 71 not taken. ✓ Branch 73 taken 1 times. ✗ Branch 74 not taken. ✓ Branch 77 taken 1 times. ✗ Branch 78 not taken. ✓ Branch 80 taken 1 times. ✗ Branch 81 not taken. ✓ Branch 83 taken 1 times. ✗ Branch 84 not taken. ✓ Branch 86 taken 1 times. ✗ Branch 87 not taken. ✓ Branch 91 taken 1 times. ✗ Branch 92 not taken. ✓ Branch 95 taken 1 times. ✗ Branch 96 not taken. ✓ Branch 98 taken 1 times. ✗ Branch 99 not taken. ✓ Branch 102 taken 1 times. ✗ Branch 103 not taken. ✓ Branch 105 taken 1 times. ✗ Branch 106 not taken. ✓ Branch 108 taken 1 times. ✗ Branch 109 not taken. ✓ Branch 111 taken 1 times. ✗ Branch 112 not taken. ✓ Branch 116 taken 1 times. ✗ Branch 117 not taken.	4	BOOST_AUTO_TEST_CASE(test_kinetic_energy_with_armature)
44			{
45			using namespace Eigen;
46			using namespace pinocchio;
47
48	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Model model;
49	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	buildModels::humanoidRandom(model);
50	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Data data(model), data_ref(model);
51
52	4/8 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 1 times. ✗ Branch 8 not taken. ✓ Branch 10 taken 1 times. ✗ Branch 11 not taken.	2	model.armature = VectorXd::Random(model.nv) + VectorXd::Ones(model.nv);
53
54	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	const VectorXd qmax = VectorXd::Ones(model.nq);
55	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd q = randomConfiguration(model, -qmax, qmax);
56	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd v = VectorXd::Random(model.nv);
57
58	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	crba(model, data_ref, q, Convention::WORLD);
59	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	data_ref.M.triangularView<Eigen::StrictlyLower>() =
60	3/6 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 1 times. ✗ Branch 8 not taken.	4	data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
61
62	5/10 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 1 times. ✗ Branch 8 not taken. ✓ Branch 10 taken 1 times. ✗ Branch 11 not taken. ✓ Branch 13 taken 1 times. ✗ Branch 14 not taken.	2	double kinetic_energy_ref = 0.5 * v.transpose() * data_ref.M * v;
63	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	double kinetic_energy = computeKineticEnergy(model, data, q, v);
64
65	5/10 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✗ Branch 18 not taken. ✓ Branch 19 taken 1 times.	2	BOOST_CHECK_SMALL(
66			kinetic_energy_ref - kinetic_energy, Eigen::NumTraits<double>::dummy_precision());
67		2	}
68
69	33/66 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 1 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 19 taken 1 times. ✗ Branch 20 not taken. ✓ Branch 23 taken 1 times. ✗ Branch 24 not taken. ✓ Branch 27 taken 1 times. ✗ Branch 28 not taken. ✓ Branch 30 taken 1 times. ✗ Branch 31 not taken. ✓ Branch 33 taken 1 times. ✗ Branch 34 not taken. ✓ Branch 36 taken 1 times. ✗ Branch 37 not taken. ✓ Branch 41 taken 1 times. ✗ Branch 42 not taken. ✓ Branch 45 taken 1 times. ✗ Branch 46 not taken. ✓ Branch 48 taken 1 times. ✗ Branch 49 not taken. ✓ Branch 52 taken 1 times. ✗ Branch 53 not taken. ✓ Branch 55 taken 1 times. ✗ Branch 56 not taken. ✓ Branch 58 taken 1 times. ✗ Branch 59 not taken. ✓ Branch 61 taken 1 times. ✗ Branch 62 not taken. ✓ Branch 66 taken 1 times. ✗ Branch 67 not taken. ✓ Branch 70 taken 1 times. ✗ Branch 71 not taken. ✓ Branch 73 taken 1 times. ✗ Branch 74 not taken. ✓ Branch 77 taken 1 times. ✗ Branch 78 not taken. ✓ Branch 80 taken 1 times. ✗ Branch 81 not taken. ✓ Branch 83 taken 1 times. ✗ Branch 84 not taken. ✓ Branch 86 taken 1 times. ✗ Branch 87 not taken. ✓ Branch 91 taken 1 times. ✗ Branch 92 not taken. ✓ Branch 95 taken 1 times. ✗ Branch 96 not taken. ✓ Branch 98 taken 1 times. ✗ Branch 99 not taken. ✓ Branch 102 taken 1 times. ✗ Branch 103 not taken. ✓ Branch 105 taken 1 times. ✗ Branch 106 not taken. ✓ Branch 108 taken 1 times. ✗ Branch 109 not taken. ✓ Branch 111 taken 1 times. ✗ Branch 112 not taken. ✓ Branch 116 taken 1 times. ✗ Branch 117 not taken.	4	BOOST_AUTO_TEST_CASE(test_potential_energy)
70			{
71			using namespace Eigen;
72			using namespace pinocchio;
73
74	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Model model;
75	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	buildModels::humanoidRandom(model);
76	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Data data(model), data_ref(model);
77
78	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	const VectorXd qmax = VectorXd::Ones(model.nq);
79	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd q = randomConfiguration(model, -qmax, qmax);
80
81	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	double potential_energy = computePotentialEnergy(model, data, q);
82	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	centerOfMass(model, data_ref, q);
83
84	2/4 ✓ Branch 3 taken 1 times. ✗ Branch 4 not taken. ✓ Branch 6 taken 1 times. ✗ Branch 7 not taken.	2	double potential_energy_ref = -data_ref.mass[0] * (data_ref.com[0].dot(model.gravity.linear()));
85
86	5/10 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✗ Branch 18 not taken. ✓ Branch 19 taken 1 times.	2	BOOST_CHECK_SMALL(
87			potential_energy_ref - potential_energy, Eigen::NumTraits<double>::dummy_precision());
88		2	}
89
90	33/66 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 1 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 19 taken 1 times. ✗ Branch 20 not taken. ✓ Branch 23 taken 1 times. ✗ Branch 24 not taken. ✓ Branch 27 taken 1 times. ✗ Branch 28 not taken. ✓ Branch 30 taken 1 times. ✗ Branch 31 not taken. ✓ Branch 33 taken 1 times. ✗ Branch 34 not taken. ✓ Branch 36 taken 1 times. ✗ Branch 37 not taken. ✓ Branch 41 taken 1 times. ✗ Branch 42 not taken. ✓ Branch 45 taken 1 times. ✗ Branch 46 not taken. ✓ Branch 48 taken 1 times. ✗ Branch 49 not taken. ✓ Branch 52 taken 1 times. ✗ Branch 53 not taken. ✓ Branch 55 taken 1 times. ✗ Branch 56 not taken. ✓ Branch 58 taken 1 times. ✗ Branch 59 not taken. ✓ Branch 61 taken 1 times. ✗ Branch 62 not taken. ✓ Branch 66 taken 1 times. ✗ Branch 67 not taken. ✓ Branch 70 taken 1 times. ✗ Branch 71 not taken. ✓ Branch 73 taken 1 times. ✗ Branch 74 not taken. ✓ Branch 77 taken 1 times. ✗ Branch 78 not taken. ✓ Branch 80 taken 1 times. ✗ Branch 81 not taken. ✓ Branch 83 taken 1 times. ✗ Branch 84 not taken. ✓ Branch 86 taken 1 times. ✗ Branch 87 not taken. ✓ Branch 91 taken 1 times. ✗ Branch 92 not taken. ✓ Branch 95 taken 1 times. ✗ Branch 96 not taken. ✓ Branch 98 taken 1 times. ✗ Branch 99 not taken. ✓ Branch 102 taken 1 times. ✗ Branch 103 not taken. ✓ Branch 105 taken 1 times. ✗ Branch 106 not taken. ✓ Branch 108 taken 1 times. ✗ Branch 109 not taken. ✓ Branch 111 taken 1 times. ✗ Branch 112 not taken. ✓ Branch 116 taken 1 times. ✗ Branch 117 not taken.	4	BOOST_AUTO_TEST_CASE(test_mechanical_energy)
91			{
92			using namespace Eigen;
93			using namespace pinocchio;
94
95	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Model model;
96	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	buildModels::humanoidRandom(model);
97	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Data data(model), data_ref(model);
98
99	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	const VectorXd qmax = VectorXd::Ones(model.nq);
100	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd q = randomConfiguration(model, -qmax, qmax);
101	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd v = VectorXd::Random(model.nv);
102
103	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	computeKineticEnergy(model, data_ref, q, v);
104	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	computePotentialEnergy(model, data_ref, q);
105
106		2	const double mechanical_energy_ref = data_ref.kinetic_energy + data_ref.potential_energy;
107	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	double mechanical_energy = computeMechanicalEnergy(model, data, q, v);
108
109	5/10 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✗ Branch 18 not taken. ✓ Branch 19 taken 1 times.	2	BOOST_CHECK_SMALL(
110			mechanical_energy_ref - mechanical_energy, Eigen::NumTraits<double>::dummy_precision());
111		2	}
112
113			template<typename ConfigVectorType, typename TangentVectorType>
114		2	Eigen::VectorXd evalMv(
115			const pinocchio::Model & model,
116			const Eigen::MatrixBase<ConfigVectorType> & q,
117			const Eigen::MatrixBase<TangentVectorType> & v)
118			{
119	1/2 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken.	2	pinocchio::Data data(model);
120	1/2 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken.	2	pinocchio::crba(model, data, q, pinocchio::Convention::WORLD);
121	2/4 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 2 times. ✗ Branch 5 not taken.	2	data.M.triangularView<Eigen::StrictlyLower>() =
122	2/4 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 2 times. ✗ Branch 5 not taken.	2	data.M.transpose().triangularView<Eigen::StrictlyLower>();
123	2/4 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 2 times. ✗ Branch 5 not taken.	4	return data.M * v;
124		2	}
125
126	33/66 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 1 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 19 taken 1 times. ✗ Branch 20 not taken. ✓ Branch 23 taken 1 times. ✗ Branch 24 not taken. ✓ Branch 27 taken 1 times. ✗ Branch 28 not taken. ✓ Branch 30 taken 1 times. ✗ Branch 31 not taken. ✓ Branch 33 taken 1 times. ✗ Branch 34 not taken. ✓ Branch 36 taken 1 times. ✗ Branch 37 not taken. ✓ Branch 41 taken 1 times. ✗ Branch 42 not taken. ✓ Branch 45 taken 1 times. ✗ Branch 46 not taken. ✓ Branch 48 taken 1 times. ✗ Branch 49 not taken. ✓ Branch 52 taken 1 times. ✗ Branch 53 not taken. ✓ Branch 55 taken 1 times. ✗ Branch 56 not taken. ✓ Branch 58 taken 1 times. ✗ Branch 59 not taken. ✓ Branch 61 taken 1 times. ✗ Branch 62 not taken. ✓ Branch 66 taken 1 times. ✗ Branch 67 not taken. ✓ Branch 70 taken 1 times. ✗ Branch 71 not taken. ✓ Branch 73 taken 1 times. ✗ Branch 74 not taken. ✓ Branch 77 taken 1 times. ✗ Branch 78 not taken. ✓ Branch 80 taken 1 times. ✗ Branch 81 not taken. ✓ Branch 83 taken 1 times. ✗ Branch 84 not taken. ✓ Branch 86 taken 1 times. ✗ Branch 87 not taken. ✓ Branch 91 taken 1 times. ✗ Branch 92 not taken. ✓ Branch 95 taken 1 times. ✗ Branch 96 not taken. ✓ Branch 98 taken 1 times. ✗ Branch 99 not taken. ✓ Branch 102 taken 1 times. ✗ Branch 103 not taken. ✓ Branch 105 taken 1 times. ✗ Branch 106 not taken. ✓ Branch 108 taken 1 times. ✗ Branch 109 not taken. ✓ Branch 111 taken 1 times. ✗ Branch 112 not taken. ✓ Branch 116 taken 1 times. ✗ Branch 117 not taken.	4	BOOST_AUTO_TEST_CASE(test_against_rnea)
127			{
128			using namespace Eigen;
129			using namespace pinocchio;
130
131	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Model model;
132	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	buildModels::humanoidRandom(model);
133	4/8 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 1 times. ✗ Branch 8 not taken. ✓ Branch 10 taken 1 times. ✗ Branch 11 not taken.	2	model.armature = VectorXd::Random(model.nv) + VectorXd::Ones(model.nv);
134	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	model.armature.head<6>().setZero(); // Because we do not take into account the influcent of the
135			// armature on the Coriolis terms
136			// model.gravity.setZero();
137	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Data data(model), data_ref(model);
138
139	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	const VectorXd qmax = VectorXd::Ones(model.nq);
140	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd q = randomConfiguration(model, -qmax, qmax);
141	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd v = VectorXd::Random(model.nv); // v.setZero();
142	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd a = VectorXd::Random(model.nv); // a.setZero();
143
144	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	const VectorXd tau_ref = rnea(model, data_ref, q, v, a);
145	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd tau_fd = VectorXd::Zero(model.nv);
146
147		2	const double eps = 1e-8;
148	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	computeMechanicalEnergy(model, data, q, v);
149		2	const double mechanical_energy = data.kinetic_energy - data.potential_energy;
150	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	forwardKinematics(model, data, q);
151	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const SE3 oM1 = data.oMi[1];
152	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data data_plus(model);
153	2/2 ✓ Branch 0 taken 32 times. ✓ Branch 1 taken 1 times.	66	for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
154			{
155	3/6 ✓ Branch 1 taken 32 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 32 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 32 times. ✗ Branch 8 not taken.	64	VectorXd q_plus = integrate(model, q, VectorXd::Unit(model.nv, k) * eps);
156	1/2 ✓ Branch 1 taken 32 times. ✗ Branch 2 not taken.	64	computeMechanicalEnergy(model, data_plus, q_plus, v);
157		64	const double mechanical_energy_plus = data_plus.kinetic_energy - data_plus.potential_energy;
158	1/2 ✓ Branch 2 taken 32 times. ✗ Branch 3 not taken.	64	const SE3 oM1_plus = data_plus.oMi[1];
159	1/2 ✓ Branch 1 taken 32 times. ✗ Branch 2 not taken.	64	const SE3 Mdiff = oM1.actInv(oM1_plus);
160	2/2 ✓ Branch 0 taken 6 times. ✓ Branch 1 taken 26 times.	64	if (k < 6)
161			{
162	1/2 ✓ Branch 1 taken 6 times. ✗ Branch 2 not taken.	12	Force f = Force::Zero();
163	2/4 ✓ Branch 1 taken 6 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 6 times. ✗ Branch 5 not taken.	12	f.toVector()[k] = mechanical_energy;
164	1/2 ✓ Branch 1 taken 6 times. ✗ Branch 2 not taken.	12	Force f_plus = Force::Zero();
165	2/4 ✓ Branch 1 taken 6 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 6 times. ✗ Branch 5 not taken.	12	f_plus.toVector()[k] = mechanical_energy_plus;
166	6/12 ✓ Branch 1 taken 6 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 6 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 6 times. ✗ Branch 8 not taken. ✓ Branch 10 taken 6 times. ✗ Branch 11 not taken. ✓ Branch 13 taken 6 times. ✗ Branch 14 not taken. ✓ Branch 16 taken 6 times. ✗ Branch 17 not taken.	12	tau_fd.head<6>() -= (Mdiff.act(f_plus) - f).toVector() / eps;
167			}
168			else
169			{
170	1/2 ✓ Branch 1 taken 26 times. ✗ Branch 2 not taken.	52	tau_fd[k] = -(mechanical_energy_plus - mechanical_energy) / eps;
171			}
172		64	}
173
174	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	const VectorXd Mv = evalMv(model, q, v);
175	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	const VectorXd q_plus = integrate(model, q, v * eps);
176	3/6 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 1 times. ✗ Branch 8 not taken.	2	const VectorXd v_plus = v + a * eps;
177	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	VectorXd Mv_plus = evalMv(model, q_plus, v_plus);
178
179	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	forwardKinematics(model, data, q_plus);
180	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const SE3 oM1_plus = data.oMi[1];
181	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	const SE3 Mdiff = oM1.actInv(oM1_plus);
182		✗	Mv_plus.head<6>() =
183	3/6 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 1 times. ✗ Branch 8 not taken.	2	Mdiff.act(Force(Mv_plus.head<6>()))
184	3/6 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 1 times. ✗ Branch 8 not taken.	2	.toVector(); // The torque at the free flyer level is expressed in a translated frame.
185
186	3/6 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 1 times. ✗ Branch 8 not taken.	2	tau_fd += (Mv_plus - Mv) / eps;
187	4/8 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 1 times. ✗ Branch 8 not taken. ✓ Branch 10 taken 1 times. ✗ Branch 11 not taken.	2	std::cout << "tau_fd: " << tau_fd.transpose() << std::endl;
188	4/8 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 1 times. ✗ Branch 8 not taken. ✓ Branch 10 taken 1 times. ✗ Branch 11 not taken.	2	std::cout << "tau_ref: " << tau_ref.transpose() << std::endl;
189
190	7/14 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 1 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 1 times. ✗ Branch 12 not taken. ✓ Branch 14 taken 1 times. ✗ Branch 15 not taken. ✓ Branch 17 taken 1 times. ✗ Branch 18 not taken. ✓ Branch 20 taken 1 times. ✗ Branch 21 not taken. ✗ Branch 25 not taken. ✓ Branch 26 taken 1 times.	2	BOOST_CHECK(tau_fd.isApprox(tau_ref, sqrt(eps)));
191		2	}
192
193			BOOST_AUTO_TEST_SUITE_END()
194