GCC Code Coverage Report

Directory: ./
File: unittest/constrained-dynamics-derivatives.cpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 218 218 100.0%
Branches: 524 1036 50.6%
Line Branch Exec Source
1 //
2 // Copyright (c) 2019 INRIA
3 //
4
5 #include "pinocchio/algorithm/jacobian.hpp"
6 #include "pinocchio/algorithm/rnea.hpp"
7 #include "pinocchio/algorithm/rnea-derivatives.hpp"
8 #include "pinocchio/algorithm/kinematics-derivatives.hpp"
9 #include "pinocchio/algorithm/contact-dynamics.hpp"
10 #include "pinocchio/algorithm/joint-configuration.hpp"
11 #include "pinocchio/multibody/sample-models.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 using namespace Eigen;
21 using namespace pinocchio;
22
23
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_FD_with_contact_cst_gamma)
24 {
25
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::Model model;
26
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::buildModels::humanoidRandom(model, true);
27
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 pinocchio::Data data(model), data_check(model);
28
29
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 VectorXd q = VectorXd::Ones(model.nq);
30
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 normalize(model, q);
31
32
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 computeJointJacobians(model, data, q);
33
34
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 VectorXd v = VectorXd::Ones(model.nv);
35
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 VectorXd tau = VectorXd::Random(model.nv);
36
37
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 const std::string RF = "rleg6_joint";
38
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 const Model::JointIndex RF_id = model.getJointId(RF);
39 // const std::string LF = "lleg6_joint";
40 // const Model::JointIndex LF_id = model.getJointId(LF);
41
42
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x J_RF(6, model.nv);
43
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 J_RF.setZero();
44
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointJacobian(model, data, RF_id, LOCAL, J_RF);
45
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Motion::Vector6 gamma_RF;
46
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 gamma_RF.setZero();
47
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 forwardKinematics(model, data, q, v, VectorXd::Zero(model.nv));
48
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 gamma_RF += data.a[RF_id].toVector(); // Jdot * qdot
49
50 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
51 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
52
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 forwardDynamics(model, data, q, v, tau, J_RF, gamma_RF);
53 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
54
55
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 VectorXd ddq_ref = data.ddq;
56
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Force::Vector6 contact_force_ref = data.lambda_c;
57
58
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 container::aligned_vector<Force> fext((size_t)model.njoints, Force::Zero());
59
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 fext[RF_id] = ForceRef<Force::Vector6>(contact_force_ref);
60
61 // check call to RNEA
62
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 rnea(model, data_check, q, v, ddq_ref, fext);
63
64
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 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
 2 BOOST_CHECK(data_check.tau.isApprox(tau));
65
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 forwardKinematics(model, data_check, q, VectorXd::Zero(model.nv), ddq_ref);
66
9/18
✓ 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 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 1 times.
 2 BOOST_CHECK(data_check.a[RF_id].toVector().isApprox(-gamma_RF));
67
68
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data data_fd(model);
69
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 VectorXd q_plus(model.nq);
70
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 VectorXd v_eps(model.nv);
71
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v_eps.setZero();
72
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 VectorXd v_plus(v);
73
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 VectorXd tau_plus(tau);
74 2 const double eps = 1e-8;
75
76 // check: dddq_dtau and dlambda_dtau
77
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 MatrixXd dddq_dtau(model.nv, model.nv);
78
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x dlambda_dtau(6, model.nv);
79
80
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
 66 for (int k = 0; k < model.nv; ++k)
81 {
82
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 tau_plus[k] += eps;
83
84 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
85 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
86
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 forwardDynamics(model, data_fd, q, v, tau_plus, J_RF, gamma_RF);
87 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
88
89 64 const Data::TangentVectorType & ddq_plus = data_fd.ddq;
90
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 Force::Vector6 contact_force_plus = data_fd.lambda_c;
91
92
4/8
✓ 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.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
 64 dddq_dtau.col(k) = (ddq_plus - ddq_ref) / eps;
93
4/8
✓ 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.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
 64 dlambda_dtau.col(k) = (contact_force_plus - contact_force_ref) / eps;
94
95
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 tau_plus[k] -= eps;
96 }
97
98
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 MatrixXd A(model.nv + 6, model.nv + 6);
99
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 data.M.transpose().triangularView<Eigen::Upper>() = data.M.triangularView<Eigen::Upper>();
100
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 A.topLeftCorner(model.nv, model.nv) = data.M;
101
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 A.bottomLeftCorner(6, model.nv) = J_RF;
102
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 A.topRightCorner(model.nv, 6) = J_RF.transpose();
103
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 A.bottomRightCorner(6, 6).setZero();
104
105
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 MatrixXd Ainv = A.inverse();
106
9/18
✓ 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 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 1 times.
 2 BOOST_CHECK(Ainv.topRows(model.nv).leftCols(model.nv).isApprox(dddq_dtau, std::sqrt(eps)));
107
10/20
✓ 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 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 1 times.
 2 BOOST_CHECK(Ainv.bottomRows(6).leftCols(model.nv).isApprox(-dlambda_dtau, std::sqrt(eps)));
108
109 // check: dddq_dv and dlambda_dv
110
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 MatrixXd dddq_dv(model.nv, model.nv);
111
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x dlambda_dv(6, model.nv);
112
113
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
 66 for (int k = 0; k < model.nv; ++k)
114 {
115
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 v_plus[k] += eps;
116
117 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
118 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
119
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 forwardDynamics(model, data_fd, q, v_plus, tau, J_RF, gamma_RF);
120 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
121
122 64 const Data::TangentVectorType & ddq_plus = data_fd.ddq;
123
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 Force::Vector6 contact_force_plus = data_fd.lambda_c;
124
125
4/8
✓ 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.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
 64 dddq_dv.col(k) = (ddq_plus - ddq_ref) / eps;
126
4/8
✓ 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.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
 64 dlambda_dv.col(k) = (contact_force_plus - contact_force_ref) / eps;
127
128
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 v_plus[k] -= eps;
129 }
130
131
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 computeRNEADerivatives(model, data_check, q, v, VectorXd::Zero(model.nv));
132
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 MatrixXd dddq_dv_anal = -Ainv.topRows(model.nv).leftCols(model.nv) * data_check.dtau_dv;
133
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 MatrixXd dlambda_dv_anal = -Ainv.bottomRows(6).leftCols(model.nv) * data_check.dtau_dv;
134
135
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(dddq_dv_anal.isApprox(dddq_dv, std::sqrt(eps)));
136
8/16
✓ 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 23 taken 1 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 1 times.
 2 BOOST_CHECK(dlambda_dv_anal.isApprox(-dlambda_dv, std::sqrt(eps)));
137
138
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 MatrixXd dddq_dq(model.nv, model.nv);
139
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x dlambda_dq(6, model.nv);
140
141
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
 66 for (int k = 0; k < model.nv; ++k)
142 {
143
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 v_eps[k] = eps;
144
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 q_plus = integrate(model, q, v_eps);
145
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 computeJointJacobians(model, data_fd, q_plus);
146
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 getJointJacobian(model, data_fd, RF_id, LOCAL, J_RF);
147
148 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
149 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
150
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 forwardDynamics(model, data_fd, q_plus, v, tau, J_RF, gamma_RF);
151 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
152
153 64 const Data::TangentVectorType & ddq_plus = data_fd.ddq;
154
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 Force::Vector6 contact_force_plus = data_fd.lambda_c;
155
156
4/8
✓ 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.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
 64 dddq_dq.col(k) = (ddq_plus - ddq_ref) / eps;
157
4/8
✓ 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.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
 64 dlambda_dq.col(k) = (contact_force_plus - contact_force_ref) / eps;
158
159
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 v_eps[k] = 0.;
160 }
161
162
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 computeRNEADerivatives(model, data_check, q, v, ddq_ref, fext);
163
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 Data::Matrix6x v_partial_dq(6, model.nv), a_partial_dq(6, model.nv), a_partial_dv(6, model.nv),
164
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_da(6, model.nv);
165
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v_partial_dq.setZero();
166
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_dq.setZero();
167
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_dv.setZero();
168
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_da.setZero();
169
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data data_kin(model);
170
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 computeForwardKinematicsDerivatives(model, data_kin, q, VectorXd::Zero(model.nv), ddq_ref);
171
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointAccelerationDerivatives(
172 model, data_kin, RF_id, LOCAL, v_partial_dq, a_partial_dq, a_partial_dv, a_partial_da);
173
174
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 MatrixXd dddq_dq_anal = -Ainv.topRows(model.nv).leftCols(model.nv) * data_check.dtau_dq;
175
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 dddq_dq_anal -= Ainv.topRows(model.nv).rightCols(6) * a_partial_dq;
176
177
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 MatrixXd dlambda_dq_anal = Ainv.bottomRows(6).leftCols(model.nv) * data_check.dtau_dq;
178
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 dlambda_dq_anal += Ainv.bottomRows(6).rightCols(6) * a_partial_dq;
179
180
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(dddq_dq_anal.isApprox(dddq_dq, std::sqrt(eps)));
181
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(dlambda_dq_anal.isApprox(dlambda_dq, std::sqrt(eps)));
182 2 }
183
184 template<typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
185 97 VectorXd constraintDynamics(
186 const Model & model,
187 Data & data,
188 const Eigen::MatrixBase<ConfigVectorType> & q,
189 const Eigen::MatrixBase<TangentVectorType1> & v,
190 const Eigen::MatrixBase<TangentVectorType2> & tau,
191 const Model::JointIndex id)
192 {
193
1/2
✓ Branch 1 taken 97 times.
✗ Branch 2 not taken.
 97 computeJointJacobians(model, data, q);
194
1/2
✓ Branch 1 taken 97 times.
✗ Branch 2 not taken.
 97 Data::Matrix6x J(6, model.nv);
195
1/2
✓ Branch 1 taken 97 times.
✗ Branch 2 not taken.
 97 J.setZero();
196
197
1/2
✓ Branch 1 taken 97 times.
✗ Branch 2 not taken.
 97 getJointJacobian(model, data, id, LOCAL, J);
198
1/2
✓ Branch 1 taken 97 times.
✗ Branch 2 not taken.
 97 Motion::Vector6 gamma;
199
2/4
✓ Branch 1 taken 97 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 97 times.
✗ Branch 5 not taken.
 97 forwardKinematics(model, data, q, v, VectorXd::Zero(model.nv));
200
2/4
✓ Branch 2 taken 97 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 97 times.
✗ Branch 6 not taken.
 97 gamma = data.a[id].toVector();
201
202 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
203 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
204
1/2
✓ Branch 1 taken 97 times.
✗ Branch 2 not taken.
 97 forwardDynamics(model, data, q, v, tau, J, gamma);
205 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
206
207
2/4
✓ Branch 1 taken 97 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 97 times.
✗ Branch 5 not taken.
 97 VectorXd res(VectorXd::Zero(model.nv + 6));
208
209
2/4
✓ Branch 1 taken 97 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 97 times.
✗ Branch 5 not taken.
 97 res.head(model.nv) = data.ddq;
210
2/4
✓ Branch 1 taken 97 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 97 times.
✗ Branch 5 not taken.
 97 res.tail(6) = data.lambda_c;
211
212 194 return res;
213 97 }
214
215
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_FD_with_contact_varying_gamma)
216 {
217
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::Model model;
218
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::buildModels::humanoidRandom(model, true);
219
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 pinocchio::Data data(model), data_check(model);
220
221
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 VectorXd q = VectorXd::Ones(model.nq);
222
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 normalize(model, q);
223
224
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);
225
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 VectorXd tau = VectorXd::Random(model.nv);
226
227
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 const std::string RF = "rleg6_joint";
228
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 const Model::JointIndex RF_id = model.getJointId(RF);
229
230
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x J_RF(6, model.nv);
231
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 J_RF.setZero();
232
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 computeJointJacobians(model, data, q);
233
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointJacobian(model, data, RF_id, LOCAL, J_RF);
234
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Motion::Vector6 gamma_RF;
235
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 gamma_RF.setZero();
236
237
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 VectorXd x_ref = constraintDynamics(model, data, q, v, tau, RF_id);
238
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 VectorXd ddq_ref = x_ref.head(model.nv);
239
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Force::Vector6 contact_force_ref = x_ref.tail(6);
240
241
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 container::aligned_vector<Force> fext((size_t)model.njoints, Force::Zero());
242
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 fext[RF_id] = ForceRef<Force::Vector6>(contact_force_ref);
243
244 // check call to RNEA
245
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 rnea(model, data_check, q, v, ddq_ref, fext);
246
247
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 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
 2 BOOST_CHECK(data_check.tau.isApprox(tau));
248
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 forwardKinematics(model, data_check, q, v, ddq_ref);
249
8/16
✓ 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 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 1 times.
 2 BOOST_CHECK(data_check.a[RF_id].toVector().isZero());
250
251
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data data_fd(model);
252
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 VectorXd q_plus(model.nq);
253
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 VectorXd v_eps(model.nv);
254
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v_eps.setZero();
255
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 VectorXd v_plus(v);
256
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 VectorXd tau_plus(tau);
257
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 VectorXd x_plus(model.nv + 6);
258 2 const double eps = 1e-8;
259
260 // check: dddq_dtau and dlambda_dtau
261
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 MatrixXd dddq_dtau(model.nv, model.nv);
262
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x dlambda_dtau(6, model.nv);
263
264
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
 66 for (int k = 0; k < model.nv; ++k)
265 {
266
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 tau_plus[k] += eps;
267
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 x_plus = constraintDynamics(model, data, q, v, tau_plus, RF_id);
268
269
2/4
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
 64 const Data::TangentVectorType ddq_plus = x_plus.head(model.nv);
270
2/4
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
 64 Force::Vector6 contact_force_plus = x_plus.tail(6);
271
272
4/8
✓ 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.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
 64 dddq_dtau.col(k) = (ddq_plus - ddq_ref) / eps;
273
4/8
✓ 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.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
 64 dlambda_dtau.col(k) = (contact_force_plus - contact_force_ref) / eps;
274
275
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 tau_plus[k] -= eps;
276 64 }
277
278
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 MatrixXd A(model.nv + 6, model.nv + 6);
279
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 data.M.transpose().triangularView<Eigen::Upper>() = data.M.triangularView<Eigen::Upper>();
280
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 A.topLeftCorner(model.nv, model.nv) = data.M;
281
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 A.bottomLeftCorner(6, model.nv) = J_RF;
282
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 A.topRightCorner(model.nv, 6) = J_RF.transpose();
283
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 A.bottomRightCorner(6, 6).setZero();
284
285
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 MatrixXd Ainv = A.inverse();
286
9/18
✓ 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 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 1 times.
 2 BOOST_CHECK(Ainv.topRows(model.nv).leftCols(model.nv).isApprox(dddq_dtau, std::sqrt(eps)));
287
10/20
✓ 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 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 1 times.
 2 BOOST_CHECK(Ainv.bottomRows(6).leftCols(model.nv).isApprox(-dlambda_dtau, std::sqrt(eps)));
288
289 // check: dddq_dv and dlambda_dv
290
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 MatrixXd dddq_dv(model.nv, model.nv);
291
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x dlambda_dv(6, model.nv);
292
293
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
 66 for (int k = 0; k < model.nv; ++k)
294 {
295
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 v_plus[k] += eps;
296
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 x_plus = constraintDynamics(model, data, q, v_plus, tau, RF_id);
297
298
2/4
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
 64 const Data::TangentVectorType ddq_plus = x_plus.head(model.nv);
299
2/4
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
 64 Force::Vector6 contact_force_plus = x_plus.tail(6);
300
301
4/8
✓ 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.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
 64 dddq_dv.col(k) = (ddq_plus - ddq_ref) / eps;
302
4/8
✓ 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.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
 64 dlambda_dv.col(k) = (contact_force_plus - contact_force_ref) / eps;
303
304
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 v_plus[k] -= eps;
305 64 }
306
307
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 computeRNEADerivatives(model, data_check, q, v, VectorXd::Zero(model.nv));
308
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 Data::Matrix6x v_partial_dq(6, model.nv), a_partial_dq(6, model.nv), a_partial_dv(6, model.nv),
309
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_da(6, model.nv);
310
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v_partial_dq.setZero();
311
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_dq.setZero();
312
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_dv.setZero();
313
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_da.setZero();
314
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data data_kin(model);
315
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 computeForwardKinematicsDerivatives(model, data_kin, q, v, VectorXd::Zero(model.nv));
316
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointAccelerationDerivatives(
317 model, data_kin, RF_id, LOCAL, v_partial_dq, a_partial_dq, a_partial_dv, a_partial_da);
318
319
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 MatrixXd dddq_dv_anal = -Ainv.topRows(model.nv).leftCols(model.nv) * data_check.dtau_dv;
320
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 dddq_dv_anal -= Ainv.topRows(model.nv).rightCols(6) * a_partial_dv;
321
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 MatrixXd dlambda_dv_anal = -Ainv.bottomRows(6).leftCols(model.nv) * data_check.dtau_dv;
322
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 dlambda_dv_anal -= Ainv.bottomRows(6).rightCols(6) * a_partial_dv;
323
324
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(dddq_dv_anal.isApprox(dddq_dv, std::sqrt(eps)));
325
8/16
✓ 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 23 taken 1 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 1 times.
 2 BOOST_CHECK(dlambda_dv_anal.isApprox(-dlambda_dv, std::sqrt(eps)));
326
327
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 MatrixXd dddq_dq(model.nv, model.nv);
328
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x dlambda_dq(6, model.nv);
329
330
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
 66 for (int k = 0; k < model.nv; ++k)
331 {
332
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 v_eps[k] = eps;
333
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 q_plus = integrate(model, q, v_eps);
334
335
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 x_plus = constraintDynamics(model, data, q_plus, v, tau, RF_id);
336
337
2/4
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
 64 const Data::TangentVectorType ddq_plus = x_plus.head(model.nv);
338
2/4
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
 64 Force::Vector6 contact_force_plus = x_plus.tail(6);
339
340
4/8
✓ 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.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
 64 dddq_dq.col(k) = (ddq_plus - ddq_ref) / eps;
341
4/8
✓ 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.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
 64 dlambda_dq.col(k) = (contact_force_plus - contact_force_ref) / eps;
342
343
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 v_eps[k] = 0.;
344 64 }
345
346
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 computeRNEADerivatives(model, data_check, q, v, ddq_ref, fext);
347
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v_partial_dq.setZero();
348
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_dq.setZero();
349
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_dv.setZero();
350
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_da.setZero();
351
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 computeForwardKinematicsDerivatives(model, data_kin, q, v, ddq_ref);
352
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointAccelerationDerivatives(
353 model, data_kin, RF_id, LOCAL, v_partial_dq, a_partial_dq, a_partial_dv, a_partial_da);
354
355
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 MatrixXd dddq_dq_anal = -Ainv.topRows(model.nv).leftCols(model.nv) * data_check.dtau_dq;
356
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 dddq_dq_anal -= Ainv.topRows(model.nv).rightCols(6) * a_partial_dq;
357
358
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(dddq_dq_anal.isApprox(dddq_dq, std::sqrt(eps)));
359
360
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 MatrixXd dlambda_dq_anal = Ainv.bottomRows(6).leftCols(model.nv) * data_check.dtau_dq;
361
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 dlambda_dq_anal += Ainv.bottomRows(6).rightCols(6) * a_partial_dq;
362
363
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(dlambda_dq_anal.isApprox(dlambda_dq, std::sqrt(eps)));
364 2 }
365
366 BOOST_AUTO_TEST_SUITE_END()
367