GCC Code Coverage Report

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

	Exec	Total	Coverage
Lines:	76	76	100.0%
Branches:	207	410	50.5%

Line	Branch	Exec	Source
1			//
2			// Copyright(c) 2019 INRIA
3			//
4
5			#include "pinocchio/multibody/model.hpp"
6			#include "pinocchio/multibody/data.hpp"
7			#include "pinocchio/algorithm/kinematics.hpp"
8			#include "pinocchio/algorithm/joint-configuration.hpp"
9			#include "pinocchio/multibody/sample-models.hpp"
10
11			#include "pinocchio/spatial/classic-acceleration.hpp"
12
13			#include <iostream>
14
15			#include <boost/test/unit_test.hpp>
16			#include <boost/utility/binary.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_classic_acceleration)
21			{
22			using namespace Eigen;
23			using namespace pinocchio;
24
25			static const int num_tests = 1e2;
26
27	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Model model;
28	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	buildModels::humanoidRandom(model);
29
30	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(100);
31	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	model.upperPositionLimit.segment<4>(3).setOnes();
32	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.lowerPositionLimit.head<7>() = -model.upperPositionLimit.head<7>();
33
34	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	VectorXd q(model.nq);
35	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	q = randomConfiguration(model);
36	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));
37	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd a(VectorXd::Zero(model.nv));
38
39	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Data data_ref(model), data(model);
40
41	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const std::string RF_joint_name = "rleg6_joint";
42	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	const Model::JointIndex RF_joint_id = model.getJointId(RF_joint_name);
43
44	2/2 ✓ Branch 0 taken 100 times. ✓ Branch 1 taken 1 times.	202	for (int k = 0; k < num_tests; ++k)
45			{
46	1/2 ✓ Branch 1 taken 100 times. ✗ Branch 2 not taken.	200	q = randomConfiguration(model);
47	2/4 ✓ Branch 1 taken 100 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 100 times. ✗ Branch 5 not taken.	200	v = VectorXd::Random(model.nv);
48
49	1/2 ✓ Branch 1 taken 100 times. ✗ Branch 2 not taken.	200	forwardKinematics(model, data, q, v, a);
50	3/6 ✓ Branch 1 taken 100 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 100 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 100 times. ✗ Branch 9 not taken.	200	const SE3 RF_world_transf = SE3(data.oMi[RF_joint_id].rotation(), SE3::Vector3::Zero());
51
52	1/2 ✓ Branch 2 taken 100 times. ✗ Branch 3 not taken.	200	const Motion RF_v_global = RF_world_transf.act(data.v[RF_joint_id]);
53	1/2 ✓ Branch 2 taken 100 times. ✗ Branch 3 not taken.	200	const Motion RF_a_global = RF_world_transf.act(data.a[RF_joint_id]);
54	1/2 ✓ Branch 1 taken 100 times. ✗ Branch 2 not taken.	200	const Motion::Vector3 classic_acc = classicAcceleration(RF_v_global, RF_a_global);
55
56	1/2 ✓ Branch 1 taken 100 times. ✗ Branch 2 not taken.	200	Motion::Vector3 classic_acc_other_signature;
57	1/2 ✓ Branch 1 taken 100 times. ✗ Branch 2 not taken.	200	classicAcceleration(RF_v_global, RF_a_global, classic_acc_other_signature);
58	7/14 ✓ Branch 1 taken 100 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 100 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 100 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 100 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 100 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 100 times. ✗ Branch 22 not taken. ✗ Branch 26 not taken. ✓ Branch 27 taken 100 times.	200	BOOST_CHECK(classic_acc_other_signature.isApprox(classic_acc));
59
60			// Computes with finite differences
61		200	const double eps = 1e-5;
62		200	const double eps2 = eps * eps;
63	1/2 ✓ Branch 1 taken 100 times. ✗ Branch 2 not taken.	200	forwardKinematics(model, data_ref, q);
64	2/4 ✓ Branch 2 taken 100 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 100 times. ✗ Branch 6 not taken.	200	const SE3::Vector3 pos = data_ref.oMi[RF_joint_id].translation();
65
66	3/6 ✓ Branch 1 taken 100 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 100 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 100 times. ✗ Branch 8 not taken.	200	VectorXd v_plus = v + eps * a;
67	5/10 ✓ Branch 1 taken 100 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 100 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 100 times. ✗ Branch 8 not taken. ✓ Branch 10 taken 100 times. ✗ Branch 11 not taken. ✓ Branch 13 taken 100 times. ✗ Branch 14 not taken.	200	VectorXd q_plus = integrate(model, q, v * eps + a * eps2 / 2.);
68	1/2 ✓ Branch 1 taken 100 times. ✗ Branch 2 not taken.	200	forwardKinematics(model, data_ref, q_plus);
69	2/4 ✓ Branch 2 taken 100 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 100 times. ✗ Branch 6 not taken.	200	const SE3::Vector3 pos_plus = data_ref.oMi[RF_joint_id].translation();
70
71	3/6 ✓ Branch 1 taken 100 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 100 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 100 times. ✗ Branch 8 not taken.	200	VectorXd v_minus = v - eps * a;
72	6/12 ✓ Branch 1 taken 100 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 100 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 100 times. ✗ Branch 8 not taken. ✓ Branch 10 taken 100 times. ✗ Branch 11 not taken. ✓ Branch 13 taken 100 times. ✗ Branch 14 not taken. ✓ Branch 16 taken 100 times. ✗ Branch 17 not taken.	200	VectorXd q_minus = integrate(model, q, -v * eps - a * eps2 / 2.);
73	1/2 ✓ Branch 1 taken 100 times. ✗ Branch 2 not taken.	200	forwardKinematics(model, data_ref, q_minus);
74	2/4 ✓ Branch 2 taken 100 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 100 times. ✗ Branch 6 not taken.	200	const SE3::Vector3 pos_minus = data_ref.oMi[RF_joint_id].translation();
75
76	5/10 ✓ Branch 1 taken 100 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 100 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 100 times. ✗ Branch 8 not taken. ✓ Branch 10 taken 100 times. ✗ Branch 11 not taken. ✓ Branch 13 taken 100 times. ✗ Branch 14 not taken.	200	const SE3::Vector3 classic_acc_ref = (pos_plus + pos_minus - 2. * pos) / eps2;
77
78	7/14 ✓ Branch 1 taken 100 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 100 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 100 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 100 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 100 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 100 times. ✗ Branch 22 not taken. ✗ Branch 26 not taken. ✓ Branch 27 taken 100 times.	200	BOOST_CHECK(classic_acc.isApprox(classic_acc_ref, math::sqrt(eps) * 1e1));
79		200	}
80		2	}
81
82	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_classic_acceleration_with_placement)
83			{
84			using namespace Eigen;
85			using namespace pinocchio;
86
87			static const int num_tests = 1e2;
88
89	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	Model model;
90	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	buildModels::humanoidRandom(model);
91
92	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);
93	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	model.upperPositionLimit.segment<4>(3).setOnes();
94	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.lowerPositionLimit.head<7>() = -model.upperPositionLimit.head<7>();
95
96	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	VectorXd q(model.nq);
97	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	q = randomConfiguration(model);
98	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));
99	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	VectorXd a(VectorXd::Zero(model.nv));
100
101	2/4 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 1 times. ✗ Branch 5 not taken.	2	Data data_ref(model), data(model);
102
103	1/2 ✓ Branch 2 taken 1 times. ✗ Branch 3 not taken.	2	const std::string RF_joint_name = "rleg6_joint";
104	1/2 ✓ Branch 1 taken 1 times. ✗ Branch 2 not taken.	2	const Model::JointIndex RF_joint_id = model.getJointId(RF_joint_name);
105
106	2/2 ✓ Branch 0 taken 100 times. ✓ Branch 1 taken 1 times.	202	for (int k = 0; k < num_tests; ++k)
107			{
108	1/2 ✓ Branch 1 taken 100 times. ✗ Branch 2 not taken.	200	q = randomConfiguration(model);
109	2/4 ✓ Branch 1 taken 100 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 100 times. ✗ Branch 5 not taken.	200	v = VectorXd::Random(model.nv);
110
111	1/2 ✓ Branch 1 taken 100 times. ✗ Branch 2 not taken.	200	forwardKinematics(model, data, q, v, a);
112
113	3/6 ✓ Branch 1 taken 100 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 100 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 100 times. ✗ Branch 9 not taken.	200	const SE3 RF_world_transf = SE3(data.oMi[RF_joint_id].rotation(), SE3::Vector3::Zero());
114
115	1/2 ✓ Branch 2 taken 100 times. ✗ Branch 3 not taken.	200	const Motion RF_v_global = RF_world_transf.act(data.v[RF_joint_id]);
116	1/2 ✓ Branch 2 taken 100 times. ✗ Branch 3 not taken.	200	const Motion RF_a_global = RF_world_transf.act(data.a[RF_joint_id]);
117	1/2 ✓ Branch 1 taken 100 times. ✗ Branch 2 not taken.	200	const Motion::Vector3 RF_classic_acc_ref = classicAcceleration(RF_v_global, RF_a_global);
118
119	1/2 ✓ Branch 1 taken 100 times. ✗ Branch 2 not taken.	200	const SE3 identity_placement = SE3::Identity();
120			Motion::Vector3 RF_classic_acc =
121	1/2 ✓ Branch 1 taken 100 times. ✗ Branch 2 not taken.	200	classicAcceleration(RF_v_global, RF_a_global, identity_placement);
122
123	7/14 ✓ Branch 1 taken 100 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 100 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 100 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 100 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 100 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 100 times. ✗ Branch 22 not taken. ✗ Branch 26 not taken. ✓ Branch 27 taken 100 times.	200	BOOST_CHECK(RF_classic_acc.isApprox(RF_classic_acc_ref));
124
125	1/2 ✓ Branch 1 taken 100 times. ✗ Branch 2 not taken.	200	const SE3 random_placement = SE3::Random();
126
127		200	const Motion & v_A = data.v[RF_joint_id];
128		200	const Motion & a_A = data.a[RF_joint_id];
129
130	1/2 ✓ Branch 2 taken 100 times. ✗ Branch 3 not taken.	200	const Motion v_B = random_placement.actInv(data.v[RF_joint_id]);
131	1/2 ✓ Branch 2 taken 100 times. ✗ Branch 3 not taken.	200	const Motion a_B = random_placement.actInv(data.a[RF_joint_id]);
132
133	1/2 ✓ Branch 1 taken 100 times. ✗ Branch 2 not taken.	200	Motion::Vector3 classic_acc_B_ref = classicAcceleration(v_B, a_B);
134	1/2 ✓ Branch 1 taken 100 times. ✗ Branch 2 not taken.	200	Motion::Vector3 classic_acc_B = classicAcceleration(v_A, a_A, random_placement);
135
136	7/14 ✓ Branch 1 taken 100 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 100 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 100 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 100 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 100 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 100 times. ✗ Branch 22 not taken. ✗ Branch 26 not taken. ✓ Branch 27 taken 100 times.	200	BOOST_CHECK(classic_acc_B.isApprox(classic_acc_B_ref));
137
138			// Check other signature
139	1/2 ✓ Branch 1 taken 100 times. ✗ Branch 2 not taken.	200	Motion::Vector3 classic_acc_B_other_signature;
140	1/2 ✓ Branch 1 taken 100 times. ✗ Branch 2 not taken.	200	classicAcceleration(v_A, a_A, random_placement, classic_acc_B_other_signature);
141
142	7/14 ✓ Branch 1 taken 100 times. ✗ Branch 2 not taken. ✓ Branch 5 taken 100 times. ✗ Branch 6 not taken. ✓ Branch 11 taken 100 times. ✗ Branch 12 not taken. ✓ Branch 15 taken 100 times. ✗ Branch 16 not taken. ✓ Branch 18 taken 100 times. ✗ Branch 19 not taken. ✓ Branch 21 taken 100 times. ✗ Branch 22 not taken. ✗ Branch 26 not taken. ✓ Branch 27 taken 100 times.	200	BOOST_CHECK(classic_acc_B_other_signature.isApprox(classic_acc_B));
143			}
144		2	}
145
146			BOOST_AUTO_TEST_SUITE_END()
147