GCC Code Coverage Report

Directory: ./
File: unittest/kinematics-derivatives.cpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 651 651 100.0%
Branches: 1552 3090 50.2%
Line Branch Exec Source
1 //
2 // Copyright (c) 2017-2020 CNRS INRIA
3 //
4
5 #include <iostream>
6
7 #include "pinocchio/multibody/model.hpp"
8 #include "pinocchio/multibody/data.hpp"
9 #include "pinocchio/algorithm/jacobian.hpp"
10 #include "pinocchio/algorithm/joint-configuration.hpp"
11 #include "pinocchio/algorithm/kinematics.hpp"
12 #include "pinocchio/algorithm/kinematics-derivatives.hpp"
13 #include "pinocchio/multibody/sample-models.hpp"
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_kinematics_derivatives_all)
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
28
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);
29
30
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.);
31
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.);
32
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 VectorXd q = randomConfiguration(model);
33
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));
34
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));
35
36
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 forwardKinematics(model, data_ref, q, v, a);
37
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 computeForwardKinematicsDerivatives(model, data, q, v, a);
38
39
2/2
✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
 56 for (size_t i = 1; i < (size_t)model.njoints; ++i)
40 {
41
7/14
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
✓ Branch 17 taken 27 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 27 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 27 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 27 times.
 54 BOOST_CHECK(data.oMi[i].isApprox(data_ref.oMi[i]));
42
7/14
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
✓ Branch 17 taken 27 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 27 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 27 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 27 times.
 54 BOOST_CHECK(data.v[i].isApprox(data_ref.v[i]));
43
8/16
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
✓ Branch 18 taken 27 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 27 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 27 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 27 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 27 times.
 54 BOOST_CHECK(data.ov[i].isApprox(data_ref.oMi[i].act(data_ref.v[i])));
44
7/14
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
✓ Branch 17 taken 27 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 27 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 27 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 27 times.
 54 BOOST_CHECK(data.a[i].isApprox(data_ref.a[i]));
45
8/16
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
✓ Branch 18 taken 27 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 27 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 27 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 27 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 27 times.
 54 BOOST_CHECK(data.oa[i].isApprox(data_ref.oMi[i].act(data_ref.a[i])));
46 }
47
48
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 computeJointJacobians(model, data_ref, q);
49
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.J.isApprox(data_ref.J));
50
51
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 computeJointJacobiansTimeVariation(model, data_ref, q, v);
52
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.dJ.isApprox(data_ref.dJ));
53 2 }
54
55
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_velocity)
56 {
57 using namespace Eigen;
58 using namespace pinocchio;
59
60
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Model model;
61
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 buildModels::humanoidRandom(model);
62
63
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);
64
65
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.);
66
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.);
67
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 VectorXd q = randomConfiguration(model);
68
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));
69
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));
70
71
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 computeForwardKinematicsDerivatives(model, data, q, v, a);
72
73
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 4 const Model::JointIndex jointId = model.existJointName("rarm2_joint")
74
4/12
✓ Branch 0 taken 1 times.
✗ Branch 1 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 14 not taken.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
 4 ? model.getJointId("rarm2_joint")
75
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 2 : (Model::Index)(model.njoints - 1);
76
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x partial_dq(6, model.nv);
77
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 partial_dq.setZero();
78
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x partial_dq_local_world_aligned(6, model.nv);
79
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 partial_dq_local_world_aligned.setZero();
80
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x partial_dq_local(6, model.nv);
81
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 partial_dq_local.setZero();
82
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x partial_dv(6, model.nv);
83
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 partial_dv.setZero();
84
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x partial_dv_local_world_aligned(6, model.nv);
85
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 partial_dv_local_world_aligned.setZero();
86
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x partial_dv_local(6, model.nv);
87
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 partial_dv_local.setZero();
88
89
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointVelocityDerivatives(model, data, jointId, WORLD, partial_dq, partial_dv);
90
91
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointVelocityDerivatives(
92 model, data, jointId, LOCAL_WORLD_ALIGNED, partial_dq_local_world_aligned,
93 partial_dv_local_world_aligned);
94
95
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointVelocityDerivatives(model, data, jointId, LOCAL, partial_dq_local, partial_dv_local);
96
97
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x J_ref(6, model.nv);
98
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 J_ref.setZero();
99
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x J_ref_local_world_aligned(6, model.nv);
100
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 J_ref_local_world_aligned.setZero();
101
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x J_ref_local(6, model.nv);
102
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 J_ref_local.setZero();
103
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 computeJointJacobians(model, data_ref, q);
104
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointJacobian(model, data_ref, jointId, WORLD, J_ref);
105
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointJacobian(model, data_ref, jointId, LOCAL_WORLD_ALIGNED, J_ref_local_world_aligned);
106
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointJacobian(model, data_ref, jointId, LOCAL, J_ref_local);
107
108
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(partial_dv.isApprox(J_ref));
109
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(partial_dv_local_world_aligned.isApprox(J_ref_local_world_aligned));
110
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(partial_dv_local.isApprox(J_ref_local));
111
112 // Check against finite differences
113
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x partial_dq_fd(6, model.nv);
114
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 partial_dq_fd.setZero();
115
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x partial_dq_fd_local_world_aligned(6, model.nv);
116
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 partial_dq_fd_local_world_aligned.setZero();
117
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x partial_dq_fd_local(6, model.nv);
118
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 partial_dq_fd_local.setZero();
119
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x partial_dv_fd(6, model.nv);
120
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 partial_dv_fd.setZero();
121
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x partial_dv_fd_local_world_aligned(6, model.nv);
122
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 partial_dv_fd_local_world_aligned.setZero();
123
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x partial_dv_fd_local(6, model.nv);
124
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 partial_dv_fd_local.setZero();
125 2 const double alpha = 1e-8;
126
127 // dvel/dv
128
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Eigen::VectorXd v_plus(v);
129
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data data_plus(model);
130
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 forwardKinematics(model, data_ref, q, v);
131
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 SE3 oMi_rot(SE3::Identity());
132
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 2 oMi_rot.rotation() = data_ref.oMi[jointId].rotation();
133
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 Motion v0(data_ref.oMi[jointId].act(data_ref.v[jointId]));
134
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 Motion v0_local_world_aligned(oMi_rot.act(data_ref.v[jointId]));
135
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 Motion v0_local(data_ref.v[jointId]);
136
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
 66 for (int k = 0; k < model.nv; ++k)
137 {
138
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 v_plus[k] += alpha;
139
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 forwardKinematics(model, data_plus, q, v_plus);
140
141
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 partial_dv_fd.col(k) =
142
5/10
✓ Branch 3 taken 32 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 32 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 32 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 32 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 32 times.
✗ Branch 16 not taken.
 128 (data_plus.oMi[jointId].act(data_plus.v[jointId]) - v0).toVector() / alpha;
143
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 partial_dv_fd_local_world_aligned.col(k) =
144
5/10
✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 32 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 32 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 32 times.
✗ Branch 15 not taken.
 128 (oMi_rot.act(data_plus.v[jointId]) - v0_local_world_aligned).toVector() / alpha;
145
5/10
✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 32 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 32 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 32 times.
✗ Branch 15 not taken.
 64 partial_dv_fd_local.col(k) = (data_plus.v[jointId] - v0_local).toVector() / alpha;
146
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 v_plus[k] -= alpha;
147 }
148
149
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(partial_dv.isApprox(partial_dv_fd, sqrt(alpha)));
150
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(
151 partial_dv_local_world_aligned.isApprox(partial_dv_fd_local_world_aligned, sqrt(alpha)));
152
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(partial_dv_local.isApprox(partial_dv_fd_local, sqrt(alpha)));
153
154 // dvel/dq
155
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 Eigen::VectorXd q_plus(q), v_eps(Eigen::VectorXd::Zero(model.nv));
156
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 forwardKinematics(model, data_ref, q, v);
157
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 2 v0 = data_ref.oMi[jointId].act(data_ref.v[jointId]);
158
159
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
 66 for (int k = 0; k < model.nv; ++k)
160 {
161
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 v_eps[k] += alpha;
162
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 q_plus = integrate(model, q, v_eps);
163
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 forwardKinematics(model, data_plus, q_plus, v);
164
165
1/2
✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
 64 SE3 oMi_plus_rot = data_plus.oMi[jointId];
166
2/4
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
 64 oMi_plus_rot.translation().setZero();
167
168
1/2
✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
 64 Motion v_plus_local_world_aligned = oMi_plus_rot.act(data_plus.v[jointId]);
169
4/8
✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 32 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 32 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 32 times.
✗ Branch 13 not taken.
 64 SE3::Vector3 trans = data_plus.oMi[jointId].translation() - data_ref.oMi[jointId].translation();
170
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 v_plus_local_world_aligned.linear() -= v_plus_local_world_aligned.angular().cross(trans);
171
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 partial_dq_fd.col(k) =
172
5/10
✓ Branch 3 taken 32 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 32 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 32 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 32 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 32 times.
✗ Branch 16 not taken.
 128 (data_plus.oMi[jointId].act(data_plus.v[jointId]) - v0).toVector() / alpha;
173
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 partial_dq_fd_local_world_aligned.col(k) =
174
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.
 128 (v_plus_local_world_aligned - v0_local_world_aligned).toVector() / alpha;
175
5/10
✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 32 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 32 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 32 times.
✗ Branch 15 not taken.
 64 partial_dq_fd_local.col(k) = (data_plus.v[jointId] - v0_local).toVector() / alpha;
176
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 v_eps[k] -= alpha;
177 }
178
179
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(partial_dq.isApprox(partial_dq_fd, sqrt(alpha)));
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(
181 partial_dq_local_world_aligned.isApprox(partial_dq_fd_local_world_aligned, sqrt(alpha)));
182
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(partial_dq_local.isApprox(partial_dq_fd_local, sqrt(alpha)));
183 2 }
184
185
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_acceleration)
186 {
187 using namespace Eigen;
188 using namespace pinocchio;
189
190
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Model model;
191
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 buildModels::humanoidRandom(model);
192
193
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);
194
195
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.);
196
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.);
197
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 VectorXd q = randomConfiguration(model);
198
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));
199
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));
200
201
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 computeForwardKinematicsDerivatives(model, data, q, v, a);
202
203
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 4 const Model::JointIndex jointId = model.existJointName("rarm2_joint")
204
4/12
✓ Branch 0 taken 1 times.
✗ Branch 1 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 14 not taken.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
 4 ? model.getJointId("rarm2_joint")
205
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 2 : (Model::Index)(model.njoints - 1);
206
207
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x v_partial_dq(6, model.nv);
208
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v_partial_dq.setZero();
209
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x v_partial_dq_local(6, model.nv);
210
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v_partial_dq_local.setZero();
211
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x v_partial_dq_local_world_aligned(6, model.nv);
212
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v_partial_dq_local_world_aligned.setZero();
213
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x a_partial_dq(6, model.nv);
214
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_dq.setZero();
215
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x a_partial_dq_local_world_aligned(6, model.nv);
216
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_dq_local_world_aligned.setZero();
217
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x a_partial_dq_local(6, model.nv);
218
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_dq_local.setZero();
219
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x a_partial_dv(6, model.nv);
220
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_dv.setZero();
221
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x a_partial_dv_local_world_aligned(6, model.nv);
222
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_dv_local_world_aligned.setZero();
223
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x a_partial_dv_local(6, model.nv);
224
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_dv_local.setZero();
225
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x a_partial_da(6, model.nv);
226
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_da.setZero();
227
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x a_partial_da_local_world_aligned(6, model.nv);
228
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_da_local_world_aligned.setZero();
229
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x a_partial_da_local(6, model.nv);
230
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_da_local.setZero();
231
232
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointAccelerationDerivatives(
233 model, data, jointId, WORLD, v_partial_dq, a_partial_dq, a_partial_dv, a_partial_da);
234
235
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointAccelerationDerivatives(
236 model, data, jointId, LOCAL_WORLD_ALIGNED, v_partial_dq_local_world_aligned,
237 a_partial_dq_local_world_aligned, a_partial_dv_local_world_aligned,
238 a_partial_da_local_world_aligned);
239
240
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointAccelerationDerivatives(
241 model, data, jointId, LOCAL, v_partial_dq_local, a_partial_dq_local, a_partial_dv_local,
242 a_partial_da_local);
243
244 // Check v_partial_dq against getJointVelocityDerivatives
245 {
246
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data data_v(model);
247
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 computeForwardKinematicsDerivatives(model, data_v, q, v, a);
248
249
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x v_partial_dq_ref(6, model.nv);
250
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v_partial_dq_ref.setZero();
251
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x v_partial_dq_ref_local_world_aligned(6, model.nv);
252
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v_partial_dq_ref_local_world_aligned.setZero();
253
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x v_partial_dq_ref_local(6, model.nv);
254
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v_partial_dq_ref_local.setZero();
255
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x v_partial_dv_ref(6, model.nv);
256
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v_partial_dv_ref.setZero();
257
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x v_partial_dv_ref_local_world_aligned(6, model.nv);
258
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v_partial_dv_ref_local_world_aligned.setZero();
259
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x v_partial_dv_ref_local(6, model.nv);
260
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v_partial_dv_ref_local.setZero();
261
262
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointVelocityDerivatives(model, data_v, jointId, WORLD, v_partial_dq_ref, v_partial_dv_ref);
263
264
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(v_partial_dq.isApprox(v_partial_dq_ref));
265
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(a_partial_da.isApprox(v_partial_dv_ref));
266
267
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointVelocityDerivatives(
268 model, data_v, jointId, LOCAL_WORLD_ALIGNED, v_partial_dq_ref_local_world_aligned,
269 v_partial_dv_ref_local_world_aligned);
270
271
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(v_partial_dq_local_world_aligned.isApprox(v_partial_dq_ref_local_world_aligned));
272
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(a_partial_da_local_world_aligned.isApprox(v_partial_dv_ref_local_world_aligned));
273
274
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointVelocityDerivatives(
275 model, data_v, jointId, LOCAL, v_partial_dq_ref_local, v_partial_dv_ref_local);
276
277
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(v_partial_dq_local.isApprox(v_partial_dq_ref_local));
278
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(a_partial_da_local.isApprox(v_partial_dv_ref_local));
279 2 }
280
281
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x J_ref(6, model.nv);
282
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 J_ref.setZero();
283
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x J_ref_local(6, model.nv);
284
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 J_ref_local.setZero();
285
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x J_ref_local_world_aligned(6, model.nv);
286
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 J_ref_local_world_aligned.setZero();
287
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 computeJointJacobians(model, data_ref, q);
288
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointJacobian(model, data_ref, jointId, WORLD, J_ref);
289
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointJacobian(model, data_ref, jointId, LOCAL_WORLD_ALIGNED, J_ref_local_world_aligned);
290
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointJacobian(model, data_ref, jointId, LOCAL, J_ref_local);
291
292
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(a_partial_da.isApprox(J_ref));
293
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(a_partial_da_local_world_aligned.isApprox(J_ref_local_world_aligned));
294
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(a_partial_da_local.isApprox(J_ref_local));
295
296 // Check against finite differences
297
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x a_partial_da_fd(6, model.nv);
298
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_da_fd.setZero();
299
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x a_partial_da_fd_local_world_aligned(6, model.nv);
300
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_da_fd_local_world_aligned.setZero();
301
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x a_partial_da_fd_local(6, model.nv);
302
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_da_fd_local.setZero();
303 2 const double alpha = 1e-8;
304
305
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Eigen::VectorXd v_plus(v), a_plus(a);
306
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data data_plus(model);
307
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 forwardKinematics(model, data_ref, q, v, a);
308
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 SE3 oMi_rot(SE3::Identity());
309
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 2 oMi_rot.rotation() = data_ref.oMi[jointId].rotation();
310
311 // dacc/da
312
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 Motion a0(data_ref.oMi[jointId].act(data_ref.a[jointId]));
313
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 Motion a0_local_world_aligned(oMi_rot.act(data_ref.a[jointId]));
314
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 Motion a0_local(data_ref.a[jointId]);
315
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
 66 for (int k = 0; k < model.nv; ++k)
316 {
317
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 a_plus[k] += alpha;
318
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 forwardKinematics(model, data_plus, q, v, a_plus);
319
320
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 a_partial_da_fd.col(k) =
321
5/10
✓ Branch 3 taken 32 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 32 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 32 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 32 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 32 times.
✗ Branch 16 not taken.
 128 (data_plus.oMi[jointId].act(data_plus.a[jointId]) - a0).toVector() / alpha;
322
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 a_partial_da_fd_local_world_aligned.col(k) =
323
5/10
✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 32 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 32 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 32 times.
✗ Branch 15 not taken.
 128 (oMi_rot.act(data_plus.a[jointId]) - a0_local_world_aligned).toVector() / alpha;
324
5/10
✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 32 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 32 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 32 times.
✗ Branch 15 not taken.
 64 a_partial_da_fd_local.col(k) = (data_plus.a[jointId] - a0_local).toVector() / alpha;
325
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 a_plus[k] -= alpha;
326 }
327
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(a_partial_da.isApprox(a_partial_da_fd, sqrt(alpha)));
328
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(
329 a_partial_da_local_world_aligned.isApprox(a_partial_da_fd_local_world_aligned, sqrt(alpha)));
330
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(a_partial_da_local.isApprox(a_partial_da_fd_local, sqrt(alpha)));
331
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 motionSet::se3Action(data_ref.oMi[jointId].inverse(), a_partial_da, a_partial_da_local);
332
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(a_partial_da_local.isApprox(a_partial_da_fd_local, sqrt(alpha)));
333
334 // dacc/dv
335
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x a_partial_dv_fd(6, model.nv);
336
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_dv_fd.setZero();
337
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x a_partial_dv_fd_local_world_aligned(6, model.nv);
338
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_dv_fd_local_world_aligned.setZero();
339
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x a_partial_dv_fd_local(6, model.nv);
340
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_dv_fd_local.setZero();
341
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
 66 for (int k = 0; k < model.nv; ++k)
342 {
343
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 v_plus[k] += alpha;
344
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 forwardKinematics(model, data_plus, q, v_plus, a);
345
346
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 a_partial_dv_fd.col(k) =
347
5/10
✓ Branch 3 taken 32 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 32 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 32 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 32 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 32 times.
✗ Branch 16 not taken.
 128 (data_plus.oMi[jointId].act(data_plus.a[jointId]) - a0).toVector() / alpha;
348
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 a_partial_dv_fd_local_world_aligned.col(k) =
349
5/10
✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 32 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 32 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 32 times.
✗ Branch 15 not taken.
 128 (oMi_rot.act(data_plus.a[jointId]) - a0_local_world_aligned).toVector() / alpha;
350
5/10
✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 32 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 32 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 32 times.
✗ Branch 15 not taken.
 64 a_partial_dv_fd_local.col(k) = (data_plus.a[jointId] - a0_local).toVector() / alpha;
351
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 v_plus[k] -= alpha;
352 }
353
354
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(a_partial_dv.isApprox(a_partial_dv_fd, sqrt(alpha)));
355
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(
356 a_partial_dv_local_world_aligned.isApprox(a_partial_dv_fd_local_world_aligned, sqrt(alpha)));
357
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(a_partial_dv_local.isApprox(a_partial_dv_fd_local, sqrt(alpha)));
358
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 motionSet::se3Action(data_ref.oMi[jointId].inverse(), a_partial_dv, a_partial_dv_local);
359
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(a_partial_dv_local.isApprox(a_partial_dv_fd_local, sqrt(alpha)));
360
361 // dacc/dq
362
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_dq.setZero();
363
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_dv.setZero();
364
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_da.setZero();
365
366
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_dq_local_world_aligned.setZero();
367
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_dv_local_world_aligned.setZero();
368
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_da_local_world_aligned.setZero();
369
370
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_dq_local.setZero();
371
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_dv_local.setZero();
372
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_da_local.setZero();
373
374
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x a_partial_dq_fd(6, model.nv);
375
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_dq_fd.setZero();
376
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x a_partial_dq_fd_local_world_aligned(6, model.nv);
377
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_dq_fd_local_world_aligned.setZero();
378
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x a_partial_dq_fd_local(6, model.nv);
379
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_dq_fd_local.setZero();
380
381
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 computeForwardKinematicsDerivatives(model, data, q, v, a);
382
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointAccelerationDerivatives(
383 model, data, jointId, WORLD, v_partial_dq, a_partial_dq, a_partial_dv, a_partial_da);
384
385
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointAccelerationDerivatives(
386 model, data, jointId, LOCAL_WORLD_ALIGNED, v_partial_dq_local_world_aligned,
387 a_partial_dq_local_world_aligned, a_partial_dv_local_world_aligned,
388 a_partial_da_local_world_aligned);
389
390
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointAccelerationDerivatives(
391 model, data, jointId, LOCAL, v_partial_dq_local, a_partial_dq_local, a_partial_dv_local,
392 a_partial_da_local);
393
394
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 Eigen::VectorXd q_plus(q), v_eps(Eigen::VectorXd::Zero(model.nv));
395
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 forwardKinematics(model, data_ref, q, v, a);
396
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 2 a0 = data_ref.oMi[jointId].act(data_ref.a[jointId]);
397
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 2 oMi_rot.rotation() = data_ref.oMi[jointId].rotation();
398
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 a0_local_world_aligned = oMi_rot.act(data_ref.a[jointId]);
399
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 a0_local = data_ref.a[jointId];
400
401
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
 66 for (int k = 0; k < model.nv; ++k)
402 {
403
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 v_eps[k] += alpha;
404
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 q_plus = integrate(model, q, v_eps);
405
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 forwardKinematics(model, data_plus, q_plus, v, a);
406
407
1/2
✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
 64 SE3 oMi_plus_rot = data_plus.oMi[jointId];
408
2/4
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
 64 oMi_plus_rot.translation().setZero();
409
410
1/2
✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
 64 Motion a_plus_local_world_aligned = oMi_plus_rot.act(data_plus.a[jointId]);
411 const SE3::Vector3 trans =
412
4/8
✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 32 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 32 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 32 times.
✗ Branch 13 not taken.
 64 data_plus.oMi[jointId].translation() - data_ref.oMi[jointId].translation();
413
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 a_plus_local_world_aligned.linear() -= a_plus_local_world_aligned.angular().cross(trans);
414
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 a_partial_dq_fd.col(k) =
415
5/10
✓ Branch 3 taken 32 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 32 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 32 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 32 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 32 times.
✗ Branch 16 not taken.
 128 (data_plus.oMi[jointId].act(data_plus.a[jointId]) - a0).toVector() / alpha;
416
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 a_partial_dq_fd_local_world_aligned.col(k) =
417
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.
 128 (a_plus_local_world_aligned - a0_local_world_aligned).toVector() / alpha;
418
5/10
✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 32 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 32 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 32 times.
✗ Branch 15 not taken.
 64 a_partial_dq_fd_local.col(k) = (data_plus.a[jointId] - a0_local).toVector() / alpha;
419
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 v_eps[k] -= alpha;
420 }
421
422
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(a_partial_dq.isApprox(a_partial_dq_fd, sqrt(alpha)));
423
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(
424 a_partial_dq_local_world_aligned.isApprox(a_partial_dq_fd_local_world_aligned, sqrt(alpha)));
425
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(a_partial_dq_local.isApprox(a_partial_dq_fd_local, sqrt(alpha)));
426 2 }
427
428
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_against_classic_formula)
429 {
430 using namespace Eigen;
431 using namespace pinocchio;
432
433
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Model model;
434
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 buildModels::humanoidRandom(model, true);
435
436
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);
437
438
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.);
439
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.);
440
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 VectorXd q = randomConfiguration(model);
441
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));
442
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));
443
444
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 4 const Model::JointIndex jointId = model.existJointName("rarm4_joint")
445
4/12
✓ Branch 0 taken 1 times.
✗ Branch 1 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 14 not taken.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
 4 ? model.getJointId("rarm4_joint")
446
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 2 : (Model::Index)(model.njoints - 1);
447
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x v_partial_dq(6, model.nv);
448
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v_partial_dq.setZero();
449
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x v_partial_dq_ref(6, model.nv);
450
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v_partial_dq_ref.setZero();
451
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x v_partial_dv_ref(6, model.nv);
452
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v_partial_dv_ref.setZero();
453
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x a_partial_dq(6, model.nv);
454
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_dq.setZero();
455
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x a_partial_dv(6, model.nv);
456
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_dv.setZero();
457
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x a_partial_da(6, model.nv);
458
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_da.setZero();
459
460 // LOCAL: da/dv == dJ/dt + dv/dq
461 // {
462 // Data::Matrix6x rhs(6,model.nv); rhs.setZero();
463 //
464 // v_partial_dq.setZero();
465 // a_partial_dq.setZero();
466 // a_partial_dv.setZero();
467 // a_partial_da.setZero();
468 //
469 // computeForwardKinematicsDerivatives(model,data_ref,q,v,a);
470 // computeForwardKinematicsDerivatives(model,data,q,v,a);
471 //
472 // getJointAccelerationDerivatives<LOCAL>(model,data,jointId,
473 // v_partial_dq,
474 // a_partial_dq,a_partial_dv,a_partial_da);
475 //
476 // getJointJacobianTimeVariation<LOCAL>(model,data_ref,jointId,rhs);
477 //
478 // v_partial_dq_ref.setZero(); v_partial_dv_ref.setZero();
479 // getJointVelocityDerivatives<LOCAL>(model,data_ref,jointId,
480 // v_partial_dq_ref,v_partial_dv_ref);
481 // rhs += v_partial_dq_ref;
482 // BOOST_CHECK(a_partial_dv.isApprox(rhs,1e-12));
483 //
484 // std::cout << "a_partial_dv\n" << a_partial_dv << std::endl;
485 // std::cout << "rhs\n" << rhs << std::endl;
486 // }
487
488 // WORLD: da/dv == dJ/dt + dv/dq
489 {
490
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x rhs(6, model.nv);
491
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 rhs.setZero();
492
493
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v_partial_dq.setZero();
494
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_dq.setZero();
495
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_dv.setZero();
496
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_da.setZero();
497
498
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 computeForwardKinematicsDerivatives(model, data_ref, q, v, a);
499
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 computeForwardKinematicsDerivatives(model, data, q, v, a);
500
501
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointAccelerationDerivatives(
502 model, data, jointId, WORLD, v_partial_dq, a_partial_dq, a_partial_dv, a_partial_da);
503
504
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointJacobianTimeVariation(model, data_ref, jointId, WORLD, rhs);
505
506
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v_partial_dq_ref.setZero();
507
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v_partial_dv_ref.setZero();
508
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointVelocityDerivatives(
509 model, data_ref, jointId, WORLD, v_partial_dq_ref, v_partial_dv_ref);
510
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 rhs += v_partial_dq_ref;
511
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(a_partial_dv.isApprox(rhs, 1e-12));
512
513 // std::cout << "a_partial_dv\n" << a_partial_dv << std::endl;
514 // std::cout << "rhs\n" << rhs << std::endl;
515 2 }
516
517 // // WORLD: da/dq == d/dt(dv/dq)
518 // {
519 // const double alpha = 1e-8;
520 // Eigen::VectorXd q_plus(model.nq), v_plus(model.nv);
521 //
522 // Data data_plus(model);
523 // v_plus = v + alpha * a;
524 // q_plus = integrate(model,q,alpha*v);
525 //
526 // computeForwardKinematicsDerivatives(model,data_plus,q_plus,v_plus,a);
527 // computeForwardKinematicsDerivatives(model,data_ref,q,v,a);
528 //
529 // Data::Matrix6x v_partial_dq_plus(6,model.nv); v_partial_dq_plus.setZero();
530 // Data::Matrix6x v_partial_dv_plus(6,model.nv); v_partial_dv_plus.setZero();
531 //
532 // v_partial_dq_ref.setZero(); v_partial_dv_ref.setZero();
533 //
534 // v_partial_dq.setZero();
535 // a_partial_dq.setZero();
536 // a_partial_dv.setZero();
537 // a_partial_da.setZero();
538 //
539 // getJointVelocityDerivatives<WORLD>(model,data_ref,jointId,
540 // v_partial_dq_ref,v_partial_dv_ref);
541 // getJointVelocityDerivatives<WORLD>(model,data_plus,jointId,
542 // v_partial_dq_plus,v_partial_dv_plus);
543 //
544 // getJointAccelerationDerivatives<WORLD>(model,data_ref,jointId,
545 // v_partial_dq,
546 // a_partial_dq,a_partial_dv,a_partial_da);
547 //
548 // Data::Matrix6x a_partial_dq_fd(6,model.nv); a_partial_dq_fd.setZero();
549 // {
550 // Data data_fd(model);
551 // VectorXd q_fd(model.nq), v_eps(model.nv); v_eps.setZero();
552 // for(int k = 0; k < model.nv; ++k)
553 // {
554 // v_eps[k] += alpha;
555 // q_fd = integrate(model,q,v_eps);
556 // forwardKinematics(model,data_fd,q_fd,v,a);
557 // a_partial_dq_fd.col(k) = (data_fd.oMi[jointId].act(data_fd.a[jointId]) -
558 // data_ref.oa[jointId]).toVector()/alpha; v_eps[k] = 0.;
559 // }
560 // }
561 //
562 // Data::Matrix6x rhs = (v_partial_dq_plus - v_partial_dq_ref)/alpha;
563 // BOOST_CHECK(a_partial_dq.isApprox(rhs,sqrt(alpha)));
564 //
565 // std::cout << "a_partial_dq\n" << a_partial_dq << std::endl;
566 // std::cout << "a_partial_dq_fd\n" << a_partial_dq_fd << std::endl;
567 // std::cout << "rhs\n" << rhs << std::endl;
568 // }
569
570 // LOCAL: da/dq == d/dt(dv/dq)
571 {
572 2 const double alpha = 1e-8;
573
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Eigen::VectorXd q_plus(model.nq), v_plus(model.nv);
574
575
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data data_plus(model);
576
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 v_plus = v + alpha * a;
577
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 q_plus = integrate(model, q, alpha * v);
578
579
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 computeForwardKinematicsDerivatives(model, data_plus, q_plus, v_plus, a);
580
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 computeForwardKinematicsDerivatives(model, data_ref, q, v, a);
581
582
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x v_partial_dq_plus(6, model.nv);
583
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v_partial_dq_plus.setZero();
584
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x v_partial_dv_plus(6, model.nv);
585
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v_partial_dv_plus.setZero();
586
587
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v_partial_dq_ref.setZero();
588
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v_partial_dv_ref.setZero();
589
590
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v_partial_dq.setZero();
591
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_dq.setZero();
592
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_dv.setZero();
593
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_da.setZero();
594
595
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointVelocityDerivatives(
596 model, data_ref, jointId, LOCAL, v_partial_dq_ref, v_partial_dv_ref);
597
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointVelocityDerivatives(
598 model, data_plus, jointId, LOCAL, v_partial_dq_plus, v_partial_dv_plus);
599
600
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointAccelerationDerivatives(
601 model, data_ref, jointId, LOCAL, v_partial_dq, a_partial_dq, a_partial_dv, a_partial_da);
602
603
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 rhs = (v_partial_dq_plus - v_partial_dq_ref) / alpha;
604
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(a_partial_dq.isApprox(rhs, sqrt(alpha)));
605
606 // std::cout << "a_partial_dq\n" << a_partial_dq << std::endl;
607 // std::cout << "rhs\n" << rhs << std::endl;
608 2 }
609 2 }
610
611
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_derivatives)
612 {
613 using namespace Eigen;
614 using namespace pinocchio;
615
616
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Model model;
617
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 buildModels::humanoidRandom(model, true);
618
619
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 data(model), data_ref(model), data_plus(model);
620
621
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.);
622
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.);
623
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 VectorXd q = randomConfiguration(model);
624
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);
625
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);
626
627
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 4 const Model::JointIndex joint_id = model.existJointName("rarm4_joint")
628
4/12
✓ Branch 0 taken 1 times.
✗ Branch 1 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 14 not taken.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
 4 ? model.getJointId("rarm4_joint")
629
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 2 : (Model::Index)(model.njoints - 1);
630
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x v3_partial_dq(3, model.nv);
631
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v3_partial_dq.setZero();
632
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x a3_partial_dq(3, model.nv);
633
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a3_partial_dq.setZero();
634
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x a3_partial_dv(3, model.nv);
635
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a3_partial_dv.setZero();
636
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x a3_partial_da(3, model.nv);
637
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a3_partial_da.setZero();
638
639
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x v_partial_dq_ref(6, model.nv);
640
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v_partial_dq_ref.setZero();
641
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x v_partial_dv_ref(6, model.nv);
642
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v_partial_dv_ref.setZero();
643
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x a_partial_dq_ref(6, model.nv);
644
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_dq_ref.setZero();
645
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x a_partial_dv_ref(6, model.nv);
646
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_dv_ref.setZero();
647
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x a_partial_da_ref(6, model.nv);
648
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_da_ref.setZero();
649
650
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 computeForwardKinematicsDerivatives(model, data_ref, q, 0 * v, a);
651
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 computeForwardKinematicsDerivatives(model, data, q, 0 * v, a);
652
653 // LOCAL
654
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointAccelerationDerivatives(
655 model, data_ref, joint_id, LOCAL, v_partial_dq_ref, v_partial_dv_ref, a_partial_dq_ref,
656 a_partial_dv_ref, a_partial_da_ref);
657
658
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getPointClassicAccelerationDerivatives(
659
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 4 model, data, joint_id, SE3::Identity(), LOCAL, v3_partial_dq, a3_partial_dq, a3_partial_dv,
660 a3_partial_da);
661
662
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 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
 2 BOOST_CHECK(v3_partial_dq.isApprox(v_partial_dq_ref.middleRows<3>(Motion::LINEAR)));
663
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 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
 2 BOOST_CHECK(a3_partial_dq.isApprox(a_partial_dq_ref.middleRows<3>(Motion::LINEAR)));
664
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 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
 2 BOOST_CHECK(a3_partial_dv.isApprox(a_partial_dv_ref.middleRows<3>(Motion::LINEAR)));
665
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 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
 2 BOOST_CHECK(a3_partial_da.isApprox(a_partial_da_ref.middleRows<3>(Motion::LINEAR)));
666
667 // LOCAL_WORLD_ALIGNED
668
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v_partial_dq_ref.setZero();
669
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v_partial_dv_ref.setZero();
670
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_dq_ref.setZero();
671
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_dv_ref.setZero();
672
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_da_ref.setZero();
673
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointAccelerationDerivatives(
674 model, data_ref, joint_id, LOCAL_WORLD_ALIGNED, v_partial_dq_ref, v_partial_dv_ref,
675 a_partial_dq_ref, a_partial_dv_ref, a_partial_da_ref);
676
677
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v3_partial_dq.setZero();
678
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a3_partial_dq.setZero();
679
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a3_partial_dv.setZero();
680
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a3_partial_da.setZero();
681
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getPointClassicAccelerationDerivatives(
682
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 4 model, data, joint_id, SE3::Identity(), LOCAL_WORLD_ALIGNED, v3_partial_dq, a3_partial_dq,
683 a3_partial_dv, a3_partial_da);
684
685
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 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
 2 BOOST_CHECK(v3_partial_dq.isApprox(v_partial_dq_ref.middleRows<3>(Motion::LINEAR)));
686 // BOOST_CHECK(a3_partial_dq.isApprox(a_partial_dq_ref.middleRows<3>(Motion::LINEAR)));
687
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 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
 2 BOOST_CHECK(a3_partial_dv.isApprox(a_partial_dv_ref.middleRows<3>(Motion::LINEAR)));
688
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 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
 2 BOOST_CHECK(a3_partial_da.isApprox(a_partial_da_ref.middleRows<3>(Motion::LINEAR)));
689
690 // std::cout << "a3_partial_dq:\n" << a3_partial_dq << std::endl;
691 // std::cout << "a3_partial_dq_ref:\n" << a_partial_dq_ref.middleRows<3>(Motion::LINEAR) <<
692 // std::endl;
693
694
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 const SE3 iMpoint = SE3::Random();
695
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 computeForwardKinematicsDerivatives(model, data, q, v, a);
696
697
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v3_partial_dq.setZero();
698
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a3_partial_dq.setZero();
699
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a3_partial_dv.setZero();
700
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a3_partial_da.setZero();
701
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getPointClassicAccelerationDerivatives(
702 model, data, joint_id, iMpoint, LOCAL, v3_partial_dq, a3_partial_dq, a3_partial_dv,
703 a3_partial_da);
704
705
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x v3_partial_dq_LWA(3, model.nv);
706
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v3_partial_dq_LWA.setZero();
707
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x a3_partial_dq_LWA(3, model.nv);
708
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a3_partial_dq_LWA.setZero();
709
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x a3_partial_LWA_dv(3, model.nv);
710
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a3_partial_LWA_dv.setZero();
711
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x a3_partial_LWA_da(3, model.nv);
712
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a3_partial_LWA_da.setZero();
713
714
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getPointClassicAccelerationDerivatives(
715 model, data, joint_id, iMpoint, LOCAL_WORLD_ALIGNED, v3_partial_dq_LWA, a3_partial_dq_LWA,
716 a3_partial_LWA_dv, a3_partial_LWA_da);
717
718 2 const double eps = 1e-8;
719
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Eigen::VectorXd v_plus = Eigen::VectorXd::Zero(model.nv);
720
721
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x v3_partial_dq_fd(3, model.nv);
722
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x v3_partial_dv_fd(3, model.nv);
723
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x a3_partial_dq_fd(3, model.nv);
724
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x a3_partial_dv_fd(3, model.nv);
725
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x a3_partial_da_fd(3, model.nv);
726
727
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x v3_partial_dq_LWA_fd(3, model.nv);
728
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x v3_partial_dv_LWA_fd(3, model.nv);
729
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x a3_partial_dq_LWA_fd(3, model.nv);
730
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x a3_partial_dv_LWA_fd(3, model.nv);
731
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x a3_partial_da_LWA_fd(3, model.nv);
732
733
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 const SE3 oMpoint = data.oMi[joint_id] * iMpoint;
734
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 2 const Motion::Vector3 point_vec_L = iMpoint.actInv(data.v[joint_id]).linear(); // LOCAL
735 const Motion::Vector3 point_acc_L =
736
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 classicAcceleration(data.v[joint_id], data.a[joint_id], iMpoint); // LOCAL
737
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 Motion::Vector3 point_vec_LWA = oMpoint.rotation() * point_vec_L; // LOCAL_WORLD_ALIGNED
738
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 Motion::Vector3 point_acc_LWA = oMpoint.rotation() * point_acc_L; // LOCAL_WORLD_ALIGNED
739
740 // Derivatives w.r.t q
741
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
 66 for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
742 {
743
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 v_plus[k] = eps;
744
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 const VectorXd q_plus = integrate(model, q, v_plus);
745
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 forwardKinematics(model, data_plus, q_plus, v, a);
746
747
1/2
✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
 64 const SE3 oMpoint_plus = data_plus.oMi[joint_id] * iMpoint;
748
3/6
✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 32 times.
✗ Branch 9 not taken.
 64 const Motion::Vector3 point_vec_L_plus = iMpoint.actInv(data_plus.v[joint_id]).linear();
749 const Motion::Vector3 point_acc_L_plus =
750
1/2
✓ Branch 3 taken 32 times.
✗ Branch 4 not taken.
 64 classicAcceleration(data_plus.v[joint_id], data_plus.a[joint_id], iMpoint);
751
752
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 const Motion::Vector3 point_vec_LWA_plus = oMpoint_plus.rotation() * point_vec_L_plus;
753
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 const Motion::Vector3 point_acc_LWA_plus = oMpoint_plus.rotation() * point_acc_L_plus;
754
755
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 v3_partial_dq_fd.col(k) = (point_vec_L_plus - point_vec_L) / eps;
756
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 a3_partial_dq_fd.col(k) = (point_acc_L_plus - point_acc_L) / eps;
757
758
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 v3_partial_dq_LWA_fd.col(k) = (point_vec_LWA_plus - point_vec_LWA) / eps;
759
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 a3_partial_dq_LWA_fd.col(k) = (point_acc_LWA_plus - point_acc_LWA) / eps;
760
761
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 v_plus[k] = 0.;
762 64 }
763
764
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(v3_partial_dq_fd.isApprox(v3_partial_dq, sqrt(eps)));
765
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(a3_partial_dq_fd.isApprox(a3_partial_dq, sqrt(eps)));
766
767
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(v3_partial_dq_LWA_fd.isApprox(v3_partial_dq_LWA, sqrt(eps)));
768
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(a3_partial_dq_LWA_fd.isApprox(a3_partial_dq_LWA, sqrt(eps)));
769
770 // Derivatives w.r.t v
771
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
 66 for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
772 {
773
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 v_plus = v;
774
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 v_plus[k] += eps;
775
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 forwardKinematics(model, data_plus, q, v_plus, a);
776
777
1/2
✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
 64 const SE3 oMpoint_plus = data_plus.oMi[joint_id] * iMpoint;
778
3/6
✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 32 times.
✗ Branch 9 not taken.
 64 const Motion::Vector3 point_vec_L_plus = iMpoint.actInv(data_plus.v[joint_id]).linear();
779 const Motion::Vector3 point_acc_L_plus =
780
1/2
✓ Branch 3 taken 32 times.
✗ Branch 4 not taken.
 64 classicAcceleration(data_plus.v[joint_id], data_plus.a[joint_id], iMpoint);
781
782
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 const Motion::Vector3 point_vec_LWA_plus = oMpoint_plus.rotation() * point_vec_L_plus;
783
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 const Motion::Vector3 point_acc_LWA_plus = oMpoint_plus.rotation() * point_acc_L_plus;
784
785
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 v3_partial_dv_fd.col(k) = (point_vec_L_plus - point_vec_L) / eps;
786
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 a3_partial_dv_fd.col(k) = (point_acc_L_plus - point_acc_L) / eps;
787
788
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 v3_partial_dv_LWA_fd.col(k) = (point_vec_LWA_plus - point_vec_LWA) / eps;
789
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 a3_partial_dv_LWA_fd.col(k) = (point_acc_LWA_plus - point_acc_LWA) / eps;
790 }
791
792
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(v3_partial_dv_fd.isApprox(a3_partial_da, sqrt(eps)));
793
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(a3_partial_dv_fd.isApprox(a3_partial_dv, sqrt(eps)));
794
795 // Derivatives w.r.t v
796
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Eigen::VectorXd a_plus = Eigen::VectorXd::Zero(model.nv);
797
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
 66 for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
798 {
799
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 a_plus = a;
800
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 a_plus[k] += eps;
801
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 forwardKinematics(model, data_plus, q, v, a_plus);
802
803
1/2
✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
 64 const SE3 oMpoint_plus = data_plus.oMi[joint_id] * iMpoint;
804 const Motion::Vector3 point_acc_L_plus =
805
1/2
✓ Branch 3 taken 32 times.
✗ Branch 4 not taken.
 64 classicAcceleration(data_plus.v[joint_id], data_plus.a[joint_id], iMpoint);
806
807
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 const Motion::Vector3 point_acc_LWA_plus = oMpoint_plus.rotation() * point_acc_L_plus;
808
809
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 a3_partial_da_fd.col(k) = (point_acc_L_plus - point_acc_L) / eps;
810
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 a3_partial_da_LWA_fd.col(k) = (point_acc_LWA_plus - point_acc_LWA) / eps;
811 }
812
813
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(a3_partial_da_fd.isApprox(a3_partial_da, sqrt(eps)));
814
815 // Test other signature
816
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data data_other(model);
817
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x v3_partial_dq_other(3, model.nv);
818
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v3_partial_dq_other.setZero();
819
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x v3_partial_dv_other(3, model.nv);
820
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v3_partial_dv_other.setZero();
821
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x a3_partial_dq_other(3, model.nv);
822
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a3_partial_dq_other.setZero();
823
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x a3_partial_dv_other(3, model.nv);
824
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a3_partial_dv_other.setZero();
825
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x a3_partial_da_other(3, model.nv);
826
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a3_partial_da_other.setZero();
827
828
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 computeForwardKinematicsDerivatives(model, data_other, q, v, a);
829
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getPointClassicAccelerationDerivatives(
830 model, data_other, joint_id, iMpoint, LOCAL, v3_partial_dq_other, v3_partial_dv_other,
831 a3_partial_dq_other, a3_partial_dv_other, a3_partial_da_other);
832
833
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(v3_partial_dq_other.isApprox(v3_partial_dq));
834
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(v3_partial_dv_other.isApprox(a3_partial_da));
835
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(a3_partial_dq_other.isApprox(a3_partial_dq));
836
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(a3_partial_dv_other.isApprox(a3_partial_dv));
837
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(a3_partial_da_other.isApprox(a3_partial_da));
838 2 }
839
840
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_velocity_derivatives)
841 {
842 using namespace Eigen;
843 using namespace pinocchio;
844
845
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Model model;
846
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 buildModels::humanoidRandom(model, true);
847
848
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);
849
850
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.);
851
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.);
852
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 VectorXd q = randomConfiguration(model);
853
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);
854
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);
855
856
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 const SE3 iMpoint = SE3::Random();
857
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 4 const Model::JointIndex joint_id = model.existJointName("rarm4_joint")
858
4/12
✓ Branch 0 taken 1 times.
✗ Branch 1 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 14 not taken.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
 4 ? model.getJointId("rarm4_joint")
859
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 2 : (Model::Index)(model.njoints - 1);
860
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x v3_partial_dq_L(3, model.nv);
861
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v3_partial_dq_L.setZero();
862
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x v3_partial_dv_L(3, model.nv);
863
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v3_partial_dv_L.setZero();
864
865
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 computeForwardKinematicsDerivatives(model, data, q, v, 0 * a);
866
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getPointVelocityDerivatives(
867 model, data, joint_id, iMpoint, LOCAL, v3_partial_dq_L, v3_partial_dv_L);
868
869
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x v_partial_dq_ref_L(3, model.nv);
870
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v_partial_dq_ref_L.setZero();
871
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x v_partial_dv_ref_L(3, model.nv);
872
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v_partial_dv_ref_L.setZero();
873
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x a_partial_dq_ref_L(3, model.nv);
874
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_dq_ref_L.setZero();
875
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x a_partial_dv_ref_L(3, model.nv);
876
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_dv_ref_L.setZero();
877
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x a_partial_da_ref_L(3, model.nv);
878
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_da_ref_L.setZero();
879
880
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 computeForwardKinematicsDerivatives(model, data_ref, q, v, a);
881
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getPointClassicAccelerationDerivatives(
882 model, data_ref, joint_id, iMpoint, LOCAL, v_partial_dq_ref_L, v_partial_dv_ref_L,
883 a_partial_dq_ref_L, a_partial_dv_ref_L, a_partial_da_ref_L);
884
885
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(v3_partial_dq_L.isApprox(v_partial_dq_ref_L));
886
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(v3_partial_dv_L.isApprox(v_partial_dv_ref_L));
887
888 // LOCAL_WORLD_ALIGNED
889
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x v3_partial_dq_LWA(3, model.nv);
890
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v3_partial_dq_LWA.setZero();
891
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x v3_partial_dv_LWA(3, model.nv);
892
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v3_partial_dv_LWA.setZero();
893
894
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getPointVelocityDerivatives(
895 model, data, joint_id, iMpoint, LOCAL_WORLD_ALIGNED, v3_partial_dq_LWA, v3_partial_dv_LWA);
896
897
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x v_partial_dq_ref_LWA(3, model.nv);
898
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v_partial_dq_ref_LWA.setZero();
899
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x v_partial_dv_ref_LWA(3, model.nv);
900
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 v_partial_dv_ref_LWA.setZero();
901
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x a_partial_dq_ref_LWA(3, model.nv);
902
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_dq_ref_LWA.setZero();
903
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x a_partial_dv_ref_LWA(3, model.nv);
904
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_dv_ref_LWA.setZero();
905
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x a_partial_da_ref_LWA(3, model.nv);
906
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 a_partial_da_ref_LWA.setZero();
907
908
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getPointClassicAccelerationDerivatives(
909 model, data_ref, joint_id, iMpoint, LOCAL_WORLD_ALIGNED, v_partial_dq_ref_LWA,
910 v_partial_dv_ref_LWA, a_partial_dq_ref_LWA, a_partial_dv_ref_LWA, a_partial_da_ref_LWA);
911
912
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(v3_partial_dq_LWA.isApprox(v_partial_dq_ref_LWA));
913
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(v3_partial_dv_LWA.isApprox(v_partial_dv_ref_LWA));
914 2 }
915
916
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_hessians)
917 {
918 using namespace Eigen;
919 using namespace pinocchio;
920
921
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Model model;
922
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 buildModels::humanoidRandom(model, true);
923
924
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 data(model), data_ref(model), data_plus(model);
925
926
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.);
927
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.);
928
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 VectorXd q = randomConfiguration(model);
929
930
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 4 const Model::JointIndex joint_id = model.existJointName("rarm2_joint")
931
4/12
✓ Branch 0 taken 1 times.
✗ Branch 1 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 14 not taken.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
 4 ? model.getJointId("rarm2_joint")
932
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 2 : (Model::Index)(model.njoints - 1);
933
934
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 computeJointJacobians(model, data, q);
935
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 computeJointKinematicHessians(model, data);
936
937
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data data2(model);
938
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 computeJointKinematicHessians(model, data2, q);
939
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(data2.J.isApprox(data.J));
940
941 2 const Eigen::DenseIndex matrix_offset = 6 * model.nv;
942
943
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
 66 for (int k = 0; k < model.nv; ++k)
944 {
945
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 Eigen::Map<Data::Matrix6x> dJ(data.kinematic_hessians.data() + k * matrix_offset, 6, model.nv);
946 Eigen::Map<Data::Matrix6x> dJ2(
947
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 data2.kinematic_hessians.data() + k * matrix_offset, 6, model.nv);
948
949
7/14
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 32 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 32 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 32 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 32 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 32 times.
 64 BOOST_CHECK(dJ2.isApprox(dJ));
950 }
951
952
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
 66 for (int i = 0; i < model.nv; ++i)
953 {
954
2/2
✓ Branch 0 taken 528 times.
✓ Branch 1 taken 32 times.
 1120 for (int j = i; j < model.nv; ++j)
955 {
956 1056 bool j_is_children_of_i = false;
957
2/2
✓ Branch 1 taken 4208 times.
✓ Branch 2 taken 240 times.
 8896 for (int parent = j; parent >= 0; parent = data.parents_fromRow[(size_t)parent])
958 {
959
2/2
✓ Branch 0 taken 288 times.
✓ Branch 1 taken 3920 times.
 8416 if (parent == i)
960 {
961 576 j_is_children_of_i = true;
962 576 break;
963 }
964 }
965
966
2/2
✓ Branch 0 taken 288 times.
✓ Branch 1 taken 240 times.
 1056 if (j_is_children_of_i)
967 {
968
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 256 times.
 576 if (i == j)
969 {
970 Eigen::Map<Data::Motion::Vector6> SixSi(
971
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 data.kinematic_hessians.data() + i * matrix_offset + i * 6);
972
7/14
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 32 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 32 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 32 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 32 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 32 times.
 64 BOOST_CHECK(SixSi.isZero());
973 }
974 else
975 {
976 Eigen::Map<Data::Motion::Vector6> SixSj(
977
3/6
✓ Branch 1 taken 256 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 256 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 256 times.
✗ Branch 8 not taken.
 512 data.kinematic_hessians.data() + i * matrix_offset + j * 6);
978
979 Eigen::Map<Data::Motion::Vector6> SjxSi(
980
3/6
✓ Branch 1 taken 256 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 256 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 256 times.
✗ Branch 8 not taken.
 512 data.kinematic_hessians.data() + j * matrix_offset + i * 6);
981
982
8/16
✓ Branch 1 taken 256 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 256 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 256 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 256 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 256 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 256 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 256 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 256 times.
 512 BOOST_CHECK(SixSj.isApprox(-SjxSi));
983 }
984 }
985 else
986 {
987 Eigen::Map<Data::Motion::Vector6> SixSj(
988
3/6
✓ Branch 1 taken 240 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 240 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 240 times.
✗ Branch 8 not taken.
 480 data.kinematic_hessians.data() + i * matrix_offset + j * 6);
989
990 Eigen::Map<Data::Motion::Vector6> SjxSi(
991
3/6
✓ Branch 1 taken 240 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 240 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 240 times.
✗ Branch 8 not taken.
 480 data.kinematic_hessians.data() + j * matrix_offset + i * 6);
992
993
7/14
✓ Branch 1 taken 240 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 240 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 240 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 240 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 240 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 240 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 240 times.
 480 BOOST_CHECK(SixSj.isZero());
994
7/14
✓ Branch 1 taken 240 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 240 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 240 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 240 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 240 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 240 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 240 times.
 480 BOOST_CHECK(SjxSi.isZero());
995 }
996 }
997 }
998
999 2 const double eps = 1e-8;
1000
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Data::Matrix6x J_ref(6, model.nv), J_plus(6, model.nv);
1001
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 J_ref.setZero();
1002
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 J_plus.setZero();
1003
1004
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 computeJointJacobians(model, data_ref, q);
1005
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 VectorXd v_plus(VectorXd::Zero(model.nv));
1006
1007 2 const Eigen::DenseIndex outer_offset = model.nv * 6;
1008
1009 // WORLD
1010
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointJacobian(model, data_ref, joint_id, WORLD, J_ref);
1011
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Tensor3x kinematic_hessian_world = getJointKinematicHessian(model, data, joint_id, WORLD);
1012
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
 66 for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
1013 {
1014
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 v_plus[k] = eps;
1015
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 const VectorXd q_plus = integrate(model, q, v_plus);
1016
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 computeJointJacobians(model, data_plus, q_plus);
1017
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 J_plus.setZero();
1018
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 getJointJacobian(model, data_plus, joint_id, WORLD, J_plus);
1019
1020
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 Data::Matrix6x dJ_dq_ref = (J_plus - J_ref) / eps;
1021 Eigen::Map<Data::Matrix6x> dJ_dq(
1022
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 kinematic_hessian_world.data() + k * outer_offset, 6, model.nv);
1023
1024 // std::cout << "k: " << k << std::endl;
1025 // std::cout << "dJ_dq:\n" << dJ_dq << std::endl;
1026 // std::cout << "dJ_dq_ref:\n" << dJ_dq_ref << std::endl;
1027
8/16
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 32 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 32 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 32 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 32 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 32 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 32 times.
 64 BOOST_CHECK((dJ_dq_ref - dJ_dq).isZero(sqrt(eps)));
1028
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 v_plus[k] = 0.;
1029 64 }
1030
1031 // LOCAL_WORLD_ALIGNED
1032
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 computeJointJacobians(model, data_ref, q);
1033
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointJacobian(model, data_ref, joint_id, LOCAL_WORLD_ALIGNED, J_ref);
1034 Data::Tensor3x kinematic_hessian_local_world_aligned =
1035
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointKinematicHessian(model, data, joint_id, LOCAL_WORLD_ALIGNED);
1036
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix3x dt_last_fd(3, model.nv);
1037
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
 66 for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
1038 {
1039
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 v_plus[k] = eps;
1040
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 const VectorXd q_plus = integrate(model, q, v_plus);
1041
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 computeJointJacobians(model, data_plus, q_plus);
1042
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 J_plus.setZero();
1043
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 getJointJacobian(model, data_plus, joint_id, LOCAL_WORLD_ALIGNED, J_plus);
1044
1045
2/4
✓ Branch 3 taken 32 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 32 times.
✗ Branch 7 not taken.
 64 SE3 tMt_plus = data_ref.oMi[joint_id].inverse() * data_plus.oMi[joint_id];
1046
2/4
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
 64 tMt_plus.rotation().setIdentity();
1047
1048
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 dt_last_fd.col(k) =
1049
5/10
✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 32 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 32 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 32 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 32 times.
✗ Branch 16 not taken.
 128 (data_plus.oMi[joint_id].translation() - data_ref.oMi[joint_id].translation()) / eps;
1050
1051
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 Data::Matrix6x dJ_dq_ref = (J_plus - J_ref) / eps;
1052 Eigen::Map<Data::Matrix6x> dJ_dq(
1053
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 kinematic_hessian_local_world_aligned.data() + k * outer_offset, 6, model.nv);
1054
1055 // std::cout << "k: " << k << std::endl;
1056 // std::cout << "dJ_dq:\n" << dJ_dq << std::endl;
1057 // std::cout << "dJ_dq_ref:\n" << dJ_dq_ref << std::endl;
1058
8/16
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 32 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 32 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 32 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 32 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 32 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 32 times.
 64 BOOST_CHECK((dJ_dq_ref - dJ_dq).isZero(sqrt(eps)));
1059
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 v_plus[k] = 0.;
1060 64 }
1061
1062
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x J_world(6, model.nv);
1063
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 J_world.setZero();
1064
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointJacobian(model, data_ref, joint_id, LOCAL_WORLD_ALIGNED, J_world);
1065
1066
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(dt_last_fd.isApprox(J_world.topRows<3>(), sqrt(eps)));
1067
1068 // LOCAL
1069
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 computeJointJacobians(model, data_ref, q);
1070
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointJacobian(model, data_ref, joint_id, LOCAL, J_ref);
1071
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Tensor3x kinematic_hessian_local = getJointKinematicHessian(model, data, joint_id, LOCAL);
1072
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
 66 for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
1073 {
1074
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 v_plus[k] = eps;
1075
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 const VectorXd q_plus = integrate(model, q, v_plus);
1076
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 computeJointJacobians(model, data_plus, q_plus);
1077
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 J_plus.setZero();
1078
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 getJointJacobian(model, data_plus, joint_id, LOCAL, J_plus);
1079
1080 // const SE3 tMt_plus = data_ref.oMi[joint_id].inverse() * data_plus.oMi[joint_id];
1081
1082
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 Data::Matrix6x dJ_dq_ref = (J_plus - J_ref) / eps;
1083 Eigen::Map<Data::Matrix6x> dJ_dq(
1084
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 kinematic_hessian_local.data() + k * outer_offset, 6, model.nv);
1085
1086 // std::cout << "k: " << k << std::endl;
1087 // std::cout << "dJ_dq:\n" << dJ_dq << std::endl;
1088 // std::cout << "dJ_dq_ref:\n" << dJ_dq_ref << std::endl;
1089
8/16
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 32 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 32 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 32 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 32 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 32 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 32 times.
 64 BOOST_CHECK((dJ_dq_ref - dJ_dq).isZero(sqrt(eps)));
1090
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 v_plus[k] = 0.;
1091 64 }
1092 2 }
1093
1094 BOOST_AUTO_TEST_SUITE_END()
1095