GCC Code Coverage Report

Directory:	./
File:	unittest/center-of-mass-derivatives.cpp
Date:	2025-02-12 21:03:38

	Exec	Total	Coverage
Lines:	32	32	100.0%
Branches:	84	166	50.6%

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2019-2021 CNRS INRIA
3			//
4
5			#include "pinocchio/algorithm/center-of-mass.hpp"
6			#include "pinocchio/algorithm/joint-configuration.hpp"
7			#include "pinocchio/algorithm/compute-all-terms.hpp"
8			#include "pinocchio/algorithm/center-of-mass-derivatives.hpp"
9			#include "pinocchio/multibody/sample-models.hpp"
10
11			#include <boost/test/unit_test.hpp>
12			#include <boost/utility/binary.hpp>
13
14			BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
15
16	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_kinematics_derivatives_vcom)
17			{
18			using namespace Eigen;
19			using namespace pinocchio;
20
21	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Model model;
22	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	buildModels::humanoid(model);
23
24	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data data_ref(model);
25
26	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	model.lowerPositionLimit.head<3>().fill(-1.);
27	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	model.upperPositionLimit.head<3>().fill(1.);
28	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	VectorXd q = randomConfiguration(model);
29	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd vq(VectorXd::Random(model.nv));
30	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd aq(VectorXd::Random(model.nv));
31
32			// Approximate dvcom_dq by finite diff.
33	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	centerOfMass(model, data_ref, q, vq);
34	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const Eigen::Vector3d vcom0 = data_ref.vcom[0];
35		2	const double alpha = 1e-8;
36	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Eigen::VectorXd dq = VectorXd::Zero(model.nv);
37	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data::Matrix3x dvcom_dqn(3, model.nv);
38
39	2/2 ✓ Branch 0 taken 34 times. ✓ Branch 1 taken 1 times.	70	for (int k = 0; k < model.nv; ++k)
40			{
41	1/2 ✓ Branch 1 taken 34 times. ✗ Branch 2 not taken.	68	dq[k] = alpha;
42	2/4 ✓ Branch 1 taken 34 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 34 times. ✗ Branch 5 not taken.	68	centerOfMass(model, data_ref, integrate(model, q, dq), vq);
43	4/8 ✓ Branch 2 taken 34 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 34 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 34 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 34 times. ✗ Branch 12 not taken.	68	dvcom_dqn.col(k) = (data_ref.vcom[0] - vcom0) / alpha;
44	1/2 ✓ Branch 1 taken 34 times. ✗ Branch 2 not taken.	68	dq[k] = 0;
45			}
46
47			{
48			// Compute dvcom_dq using the algorithm
49	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data data(model);
50	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Data::Matrix3x dvcom_dq = Data::Matrix3x::Zero(3, model.nv);
51	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	centerOfMass(model, data, q, vq);
52	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	getCenterOfMassVelocityDerivatives(model, data, dvcom_dq);
53
54			// Check that algo result and finite-diff approx are similar.
55	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(dvcom_dq.isApprox(dvcom_dqn, sqrt(alpha)));
56		2	}
57
58			{
59			// Compute dvcom_dq using the algorithm
60	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Data data(model);
61	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Data::Matrix3x dvcom_dq = Data::Matrix3x::Zero(3, model.nv);
62	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	computeAllTerms(model, data, q, vq);
63	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	getCenterOfMassVelocityDerivatives(model, data, dvcom_dq);
64
65			// Check that algo result and finite-diff approx are similar.
66	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(dvcom_dq.isApprox(dvcom_dqn, sqrt(alpha)));
67		2	}
68		2	}
69
70			BOOST_AUTO_TEST_SUITE_END()
71