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 |