GCC Code Coverage Report

Directory: ./
File: unittest/closed-loop-dynamics.cpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 78 78 100.0%
Branches: 215 428 50.2%
Line Branch Exec Source
1 //
2 // Copyright (c) 2020-2022 INRIA
3 //
4
5 #include <iostream>
6
7 #include "pinocchio/algorithm/frames.hpp"
8 #include "pinocchio/algorithm/jacobian.hpp"
9 #include "pinocchio/algorithm/contact-info.hpp"
10 #include "pinocchio/algorithm/proximal.hpp"
11 #include "pinocchio/algorithm/constrained-dynamics.hpp"
12 #include "pinocchio/algorithm/contact-dynamics.hpp"
13 #include "pinocchio/algorithm/joint-configuration.hpp"
14 #include "pinocchio/multibody/sample-models.hpp"
15 #include "pinocchio/spatial/classic-acceleration.hpp"
16 #include "pinocchio/spatial/explog.hpp"
17
18 #include <boost/test/unit_test.hpp>
19 #include <boost/utility/binary.hpp>
20
21 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
22
23 using namespace pinocchio;
24 using namespace Eigen;
25
26
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(closed_loop_constraint_6D_LOCAL)
27 {
28
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::Model model;
29
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::buildModels::humanoidRandom(model, true);
30
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 pinocchio::Data data(model), data_ref(model);
31
32
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.);
33
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.);
34
35
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 const VectorXd q = randomConfiguration(model);
36
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 const VectorXd v = VectorXd::Random(model.nv);
37
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 const VectorXd tau = VectorXd::Random(model.nv);
38
39
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 const std::string RF = "rleg6_joint";
40
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 const std::string LF = "lleg6_joint";
41
42 // Contact models and data
43
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
44
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas;
45
46 RigidConstraintModel ci_RF_LF(
47
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 CONTACT_6D, model, model.getJointId(RF), model.getJointId(LF), LOCAL);
48
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 ci_RF_LF.joint1_placement.setRandom();
49
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 ci_RF_LF.joint2_placement.setRandom();
50
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 contact_models.push_back(ci_RF_LF);
51
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 contact_datas.push_back(RigidConstraintData(ci_RF_LF));
52
53 2 Eigen::DenseIndex constraint_dim = 0;
54
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
 4 for (size_t k = 0; k < contact_models.size(); ++k)
55 2 constraint_dim += contact_models[k].size();
56
57 2 const double mu0 = 0.;
58
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 ProximalSettings prox_settings(1e-12, mu0, 1);
59
60
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Eigen::MatrixXd J_ref(constraint_dim, model.nv);
61
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 J_ref.setZero();
62
63
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 computeAllTerms(model, data_ref, q, v);
64
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 data_ref.M.triangularView<Eigen::StrictlyLower>() =
65
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 4 data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
66
67
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Data::Matrix6x J_RF_local(6, model.nv), J_LF_local(6, model.nv);
68
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 J_RF_local.setZero();
69
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 J_LF_local.setZero();
70
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 getFrameJacobian(
71 model, data_ref, model.getJointId(RF), ci_RF_LF.joint1_placement, LOCAL, J_RF_local);
72
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 getFrameJacobian(
73 model, data_ref, model.getJointId(LF), ci_RF_LF.joint2_placement, LOCAL, J_LF_local);
74
75
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 const SE3 oMc1_ref = data_ref.oMi[ci_RF_LF.joint1_id] * ci_RF_LF.joint1_placement;
76
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 const SE3 oMc2_ref = data_ref.oMi[ci_RF_LF.joint2_id] * ci_RF_LF.joint2_placement;
77
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 const SE3 c1Mc2_ref = oMc1_ref.actInv(oMc2_ref);
78
79
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 2 J_ref = J_RF_local - c1Mc2_ref.toActionMatrix() * J_LF_local;
80
81
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Eigen::VectorXd rhs_ref(constraint_dim);
82
83
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 const Motion vc1_ref = ci_RF_LF.joint1_placement.actInv(data_ref.v[ci_RF_LF.joint1_id]);
84
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 const Motion vc2_ref = ci_RF_LF.joint2_placement.actInv(data_ref.v[ci_RF_LF.joint2_id]);
85
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 const Motion constraint_velocity_error_ref = vc1_ref - c1Mc2_ref.act(vc2_ref);
86
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 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 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 1 times.
 2 BOOST_CHECK(constraint_velocity_error_ref.isApprox(Motion(J_ref * v)));
87
88
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 const Motion ac1_ref = ci_RF_LF.joint1_placement.actInv(data_ref.a[ci_RF_LF.joint1_id]);
89
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 const Motion ac2_ref = ci_RF_LF.joint2_placement.actInv(data_ref.a[ci_RF_LF.joint2_id]);
90 const Motion constraint_acceleration_error_ref =
91
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 2 ac1_ref - c1Mc2_ref.act(ac2_ref) + constraint_velocity_error_ref.cross(c1Mc2_ref.act(vc2_ref));
92
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 rhs_ref.segment<6>(0) = constraint_acceleration_error_ref.toVector();
93
94 Eigen::MatrixXd KKT_matrix_ref =
95
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Eigen::MatrixXd::Zero(model.nv + constraint_dim, model.nv + constraint_dim);
96
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 KKT_matrix_ref.bottomRightCorner(model.nv, model.nv) = data_ref.M;
97
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 KKT_matrix_ref.topRightCorner(constraint_dim, model.nv) = J_ref;
98
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 KKT_matrix_ref.bottomLeftCorner(model.nv, constraint_dim) = J_ref.transpose();
99
100 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
101 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
102
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 forwardDynamics(model, data_ref, q, v, tau, J_ref, rhs_ref, mu0);
103 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
104
105
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 forwardKinematics(model, data_ref, q, v, data_ref.ddq);
106
107
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 1 times.
 2 BOOST_CHECK((J_ref * data_ref.ddq + rhs_ref).isZero());
108
109
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 initConstraintDynamics(model, data, contact_models);
110
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 constraintDynamics(model, data, q, v, tau, contact_models, contact_datas, prox_settings);
111
112
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 1 times.
 2 BOOST_CHECK((J_ref * data.ddq + rhs_ref).isZero());
113
114
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_ref.ddq.isApprox(data.ddq));
115
116
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 const Eigen::MatrixXd KKT_matrix = data.contact_chol.matrix();
117
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(KKT_matrix.isApprox(KKT_matrix_ref));
118
119 // Check with finite differences the error computations
120
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data data_plus(model);
121 2 const double dt = 1e-8;
122
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 Eigen::VectorXd q_plus = integrate(model, q, (v * dt).eval());
123
124
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 forwardKinematics(model, data_plus, q_plus, v, Eigen::VectorXd::Zero(model.nv));
125
126
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 const SE3 oMc1_plus_ref = data_plus.oMi[ci_RF_LF.joint1_id] * ci_RF_LF.joint1_placement;
127
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 const SE3 oMc2_plus_ref = data_plus.oMi[ci_RF_LF.joint2_id] * ci_RF_LF.joint2_placement;
128
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 const SE3 c1Mc2_plus_ref = oMc1_plus_ref.actInv(oMc2_plus_ref);
129
130 // Position level
131
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 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 1 times.
 2 BOOST_CHECK(contact_datas[0].c1Mc2.isApprox(c1Mc2_ref));
132
133 // Velocity level
134
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 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 1 times.
 2 BOOST_CHECK(contact_datas[0].contact1_velocity.isApprox(vc1_ref));
135
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 1 times.
 2 BOOST_CHECK(contact_datas[0].contact2_velocity.isApprox(vc2_ref));
136
137 const Motion constraint_velocity_error_fd =
138
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 2 -c1Mc2_ref.act(log6(c1Mc2_ref.actInv(c1Mc2_plus_ref))) / dt;
139
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(constraint_velocity_error_ref.isApprox(constraint_velocity_error_fd, math::sqrt(dt)));
140
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 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 1 times.
 2 BOOST_CHECK(contact_datas[0].contact_velocity_error.isApprox(constraint_velocity_error_ref));
141
142 // Acceleration level
143
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 const Motion vc1_plus_ref = ci_RF_LF.joint1_placement.actInv(data_plus.v[ci_RF_LF.joint1_id]);
144
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 const Motion vc2_plus_ref = ci_RF_LF.joint2_placement.actInv(data_plus.v[ci_RF_LF.joint2_id]);
145
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 const Motion constraint_velocity_error_plus_ref = vc1_plus_ref - c1Mc2_plus_ref.act(vc2_plus_ref);
146 const Motion constraint_acceleration_error_fd =
147
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 (constraint_velocity_error_plus_ref - constraint_velocity_error_ref) / dt;
148
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(
149 constraint_acceleration_error_ref.isApprox(constraint_acceleration_error_fd, math::sqrt(dt)));
150 2 }
151
152 BOOST_AUTO_TEST_SUITE_END()
153