| Directory: | ./ |
|---|---|
| File: | unittest/constrained-dynamics.cpp |
| Date: | 2025-02-12 21:03:38 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 1107 | 1152 | 96.1% |
| Branches: | 2944 | 5941 | 49.6% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2019-2023 INRIA | ||
| 3 | // | ||
| 4 | |||
| 5 | #include "pinocchio/algorithm/aba.hpp" | ||
| 6 | #include "pinocchio/algorithm/rnea.hpp" | ||
| 7 | #include "pinocchio/algorithm/frames.hpp" | ||
| 8 | #include "pinocchio/algorithm/jacobian.hpp" | ||
| 9 | #include "pinocchio/algorithm/centroidal.hpp" | ||
| 10 | #include "pinocchio/algorithm/kinematics.hpp" | ||
| 11 | #include "pinocchio/algorithm/contact-info.hpp" | ||
| 12 | #include "pinocchio/algorithm/compute-all-terms.hpp" | ||
| 13 | #include "pinocchio/algorithm/constrained-dynamics.hpp" | ||
| 14 | #include "pinocchio/algorithm/contact-dynamics.hpp" | ||
| 15 | #include "pinocchio/algorithm/joint-configuration.hpp" | ||
| 16 | #include "pinocchio/multibody/sample-models.hpp" | ||
| 17 | #include "pinocchio/utils/timer.hpp" | ||
| 18 | #include "pinocchio/spatial/classic-acceleration.hpp" | ||
| 19 | |||
| 20 | #include <iostream> | ||
| 21 | |||
| 22 | #include <boost/test/unit_test.hpp> | ||
| 23 | #include <boost/utility/binary.hpp> | ||
| 24 | |||
| 25 | #define KP 10 | ||
| 26 | #define KD 10 | ||
| 27 | |||
| 28 | BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) | ||
| 29 | |||
| 30 | // BOOST_AUTO_TEST_CASE(contact_models) | ||
| 31 | // { | ||
| 32 | // using namespace pinocchio; | ||
| 33 | |||
| 34 | // // Check default constructor | ||
| 35 | // RigidConstraintModel cmodel1; | ||
| 36 | // BOOST_CHECK(cmodel1.type == CONTACT_UNDEFINED); | ||
| 37 | // BOOST_CHECK(cmodel1.size() == 0); | ||
| 38 | |||
| 39 | // // Check complete constructor | ||
| 40 | // const SE3 M(SE3::Random()); | ||
| 41 | // RigidConstraintModel cmodel2(CONTACT_3D,0,M); | ||
| 42 | // BOOST_CHECK(cmodel2.type == CONTACT_3D); | ||
| 43 | // BOOST_CHECK(cmodel2.joint1_id == 0); | ||
| 44 | // BOOST_CHECK(cmodel2.joint1_placement.isApprox(M)); | ||
| 45 | // BOOST_CHECK(cmodel2.size() == 3); | ||
| 46 | |||
| 47 | // // Check contructor with two arguments | ||
| 48 | // RigidConstraintModel cmodel2prime(CONTACT_3D,0); | ||
| 49 | // BOOST_CHECK(cmodel2prime.type == CONTACT_3D); | ||
| 50 | // BOOST_CHECK(cmodel2prime.joint1_id == 0); | ||
| 51 | // BOOST_CHECK(cmodel2prime.joint1_placement.isIdentity()); | ||
| 52 | // BOOST_CHECK(cmodel2prime.size() == 3); | ||
| 53 | |||
| 54 | // // Check default copy constructor | ||
| 55 | // RigidConstraintModel cmodel3(cmodel2); | ||
| 56 | // BOOST_CHECK(cmodel3 == cmodel2); | ||
| 57 | |||
| 58 | // // Check complete constructor 6D | ||
| 59 | // RigidConstraintModel cmodel4(CONTACT_6D,0); | ||
| 60 | // BOOST_CHECK(cmodel4.type == CONTACT_6D); | ||
| 61 | // BOOST_CHECK(cmodel4.joint1_id == 0); | ||
| 62 | // BOOST_CHECK(cmodel4.joint1_placement.isIdentity()); | ||
| 63 | // BOOST_CHECK(cmodel4.size() == 6); | ||
| 64 | // } | ||
| 65 | |||
| 66 | /// \brief Computes motions in the world frame | ||
| 67 | 11 | pinocchio::Motion computeAcceleration( | |
| 68 | const pinocchio::Model & model, | ||
| 69 | pinocchio::Data & data, | ||
| 70 | const pinocchio::JointIndex & joint_id, | ||
| 71 | pinocchio::ReferenceFrame reference_frame, | ||
| 72 | const pinocchio::ContactType type, | ||
| 73 | const pinocchio::SE3 & placement = pinocchio::SE3::Identity()) | ||
| 74 | { | ||
| 75 | PINOCCHIO_UNUSED_VARIABLE(model); | ||
| 76 | using namespace pinocchio; | ||
| 77 |
1/2✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
|
11 | Motion res(Motion::Zero()); |
| 78 | |||
| 79 | 11 | const Data::SE3 & oMi = data.oMi[joint_id]; | |
| 80 | 11 | const Data::SE3 & iMc = placement; | |
| 81 |
1/2✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
|
11 | const Data::SE3 oMc = oMi * iMc; |
| 82 | |||
| 83 |
1/2✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
|
11 | const Motion ov = oMi.act(data.v[joint_id]); |
| 84 |
1/2✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
|
11 | const Motion oa = oMi.act(data.a[joint_id]); |
| 85 | |||
| 86 |
2/4✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
|
11 | switch (reference_frame) |
| 87 | { | ||
| 88 | ✗ | case WORLD: | |
| 89 | ✗ | if (type == CONTACT_3D) | |
| 90 | ✗ | classicAcceleration(ov, oa, res.linear()); | |
| 91 | else | ||
| 92 | ✗ | res.linear() = oa.linear(); | |
| 93 | ✗ | res.angular() = oa.angular(); | |
| 94 | ✗ | break; | |
| 95 | 2 | case LOCAL_WORLD_ALIGNED: | |
| 96 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
|
2 | if (type == CONTACT_3D) |
| 97 |
5/10✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
|
1 | res.linear() = oMc.rotation() * classicAcceleration(data.v[joint_id], data.a[joint_id], iMc); |
| 98 | else | ||
| 99 |
6/12✓ 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.
✓ 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.
|
1 | res.linear() = oMc.rotation() * (iMc.actInv(data.a[joint_id])).linear(); |
| 100 |
5/10✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 2 times.
✗ Branch 15 not taken.
|
2 | res.angular() = oMi.rotation() * data.a[joint_id].angular(); |
| 101 | 2 | break; | |
| 102 | 9 | case LOCAL: | |
| 103 |
2/2✓ Branch 0 taken 3 times.
✓ Branch 1 taken 6 times.
|
9 | if (type == CONTACT_3D) |
| 104 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
|
3 | classicAcceleration(data.v[joint_id], data.a[joint_id], iMc, res.linear()); |
| 105 | else | ||
| 106 |
4/8✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 6 times.
✗ Branch 12 not taken.
|
6 | res.linear() = (iMc.actInv(data.a[joint_id])).linear(); |
| 107 |
6/12✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 9 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 9 times.
✗ Branch 18 not taken.
|
9 | res.angular() = iMc.rotation().transpose() * data.a[joint_id].angular(); |
| 108 | 9 | break; | |
| 109 | ✗ | default: | |
| 110 | ✗ | break; | |
| 111 | } | ||
| 112 | |||
| 113 | 22 | return res; | |
| 114 | } | ||
| 115 | |||
| 116 |
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_sparse_forward_dynamics_empty) |
| 117 | { | ||
| 118 | using namespace Eigen; | ||
| 119 | using namespace pinocchio; | ||
| 120 | |||
| 121 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | pinocchio::Model model; |
| 122 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | pinocchio::buildModels::humanoidRandom(model, true); |
| 123 |
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); |
| 124 | |||
| 125 |
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.); |
| 126 |
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.); |
| 127 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 128 | |||
| 129 |
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); |
| 130 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd tau = VectorXd::Random(model.nv); |
| 131 | |||
| 132 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RF = "rleg6_joint"; |
| 133 | // const Model::JointIndex RF_id = model.getJointId(RF); | ||
| 134 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string LF = "lleg6_joint"; |
| 135 | // const Model::JointIndex LF_id = model.getJointId(LF); | ||
| 136 | |||
| 137 | // Contact models and data | ||
| 138 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) empty_contact_models; |
| 139 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) empty_contact_datas; |
| 140 | |||
| 141 | 2 | const double mu0 = 0.; | |
| 142 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings(1e-12, mu0, 1); |
| 143 | |||
| 144 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeAllTerms(model, data_ref, q, v); |
| 145 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | data_ref.M.triangularView<Eigen::StrictlyLower>() = |
| 146 |
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>(); |
| 147 | |||
| 148 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Eigen::MatrixXd KKT_matrix_ref = Eigen::MatrixXd::Zero(model.nv, model.nv); |
| 149 |
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; |
| 150 | |||
| 151 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data, empty_contact_models); |
| 152 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraintDynamics( |
| 153 | model, data, q, v, tau, empty_contact_models, empty_contact_datas, prox_settings); | ||
| 154 | |||
| 155 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | data.M.triangularView<Eigen::StrictlyLower>() = |
| 156 |
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.M.transpose().triangularView<Eigen::StrictlyLower>(); |
| 157 | |||
| 158 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data data_ag(model); |
| 159 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ccrba(model, data_ag, q, v); |
| 160 | |||
| 161 |
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)); |
| 162 |
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.M.isApprox(data_ref.M)); |
| 163 |
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.Ag.isApprox(data_ag.Ag)); |
| 164 |
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.nle.isApprox(data_ref.nle)); |
| 165 | |||
| 166 |
2/2✓ Branch 1 taken 27 times.
✓ Branch 2 taken 1 times.
|
56 | for (Model::JointIndex k = 1; k < model.joints.size(); ++k) |
| 167 | { | ||
| 168 |
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[k].isApprox(data_ref.oMi[k])); |
| 169 |
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.liMi[k].isApprox(data_ref.liMi[k])); |
| 170 |
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[k].isApprox(data_ref.oMi[k].act(data_ref.v[k]))); |
| 171 | // Since it's gravity, we know linear can't be zero. Angular might be though. | ||
| 172 |
1/2✓ Branch 3 taken 27 times.
✗ Branch 4 not taken.
|
54 | const Motion motion_tmp = data_ref.oMi[k].act(data_ref.a_gf[k]); |
| 173 |
4/6✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 27 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
✓ Branch 9 taken 26 times.
|
54 | if (data.oa_gf[k].angular().isZero()) |
| 174 | { | ||
| 175 |
9/18✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 1 times.
|
2 | BOOST_CHECK(data.oa_gf[k].linear().isApprox(motion_tmp.linear())); |
| 176 |
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 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(motion_tmp.angular().isZero()); |
| 177 | } | ||
| 178 | else | ||
| 179 | { | ||
| 180 |
7/14✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 26 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 26 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 26 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 26 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 26 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 26 times.
|
52 | BOOST_CHECK(data.oa_gf[k].isApprox(motion_tmp)); |
| 181 | } | ||
| 182 | } | ||
| 183 | |||
| 184 | // Check that the decomposition is correct | ||
| 185 | 2 | const Data::ContactCholeskyDecomposition & contact_chol = data.contact_chol; | |
| 186 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Eigen::MatrixXd KKT_matrix = contact_chol.matrix(); |
| 187 | |||
| 188 |
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 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(KKT_matrix.bottomRightCorner(model.nv, model.nv) |
| 189 | .isApprox(KKT_matrix_ref.bottomRightCorner(model.nv, model.nv))); | ||
| 190 |
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)); |
| 191 | |||
| 192 | // Check solutions | ||
| 193 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | aba(model, data_ref, q, v, tau, Convention::WORLD); |
| 194 |
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.ddq.isApprox(data_ref.ddq)); |
| 195 | 2 | } | |
| 196 | |||
| 197 |
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_sparse_forward_dynamics_double_init) |
| 198 | { | ||
| 199 | using namespace Eigen; | ||
| 200 | using namespace pinocchio; | ||
| 201 | |||
| 202 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | pinocchio::Model model; |
| 203 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | pinocchio::buildModels::humanoidRandom(model, true); |
| 204 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | pinocchio::Data data1(model), data2(model); |
| 205 | |||
| 206 |
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.); |
| 207 |
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.); |
| 208 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 209 | |||
| 210 |
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); |
| 211 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd tau = VectorXd::Random(model.nv); |
| 212 | |||
| 213 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RF = "rleg6_joint"; |
| 214 | // const Model::JointIndex RF_id = model.getJointId(RF); | ||
| 215 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string LF = "lleg6_joint"; |
| 216 | |||
| 217 | // Contact models and data | ||
| 218 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_empty; |
| 219 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas_empty; |
| 220 | |||
| 221 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_6D; |
| 222 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas_6D; |
| 223 | |||
| 224 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_6D6D; |
| 225 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas_6D6D; |
| 226 | |||
| 227 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | RigidConstraintModel ci_RF(CONTACT_6D, model, model.getJointId(RF), LOCAL); |
| 228 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_models_6D.push_back(ci_RF); |
| 229 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | contact_datas_6D.push_back(RigidConstraintData(ci_RF)); |
| 230 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_models_6D6D.push_back(ci_RF); |
| 231 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | contact_datas_6D6D.push_back(RigidConstraintData(ci_RF)); |
| 232 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | RigidConstraintModel ci_LF(CONTACT_6D, model, model.getJointId(LF), LOCAL); |
| 233 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_models_6D6D.push_back(ci_LF); |
| 234 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | contact_datas_6D6D.push_back(RigidConstraintData(ci_LF)); |
| 235 | |||
| 236 | 2 | const double mu0 = 0.; | |
| 237 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings(1e-12, mu0, 1); |
| 238 | |||
| 239 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data1, contact_models_empty); |
| 240 |
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(data1.contact_chol.size() == (model.nv + 0)); |
| 241 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraintDynamics( |
| 242 | model, data1, q, v, tau, contact_models_empty, contact_datas_empty, prox_settings); | ||
| 243 |
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(!hasNaN(data1.ddq)); |
| 244 | |||
| 245 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data1, contact_models_6D); |
| 246 |
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(data1.contact_chol.size() == (model.nv + 1 * 6)); |
| 247 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraintDynamics(model, data1, q, v, tau, contact_models_6D, contact_datas_6D, prox_settings); |
| 248 |
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(!hasNaN(data1.ddq)); |
| 249 | |||
| 250 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data1, contact_models_6D6D); |
| 251 |
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(data1.contact_chol.size() == (model.nv + 2 * 6)); |
| 252 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraintDynamics( |
| 253 | model, data1, q, v, tau, contact_models_6D6D, contact_datas_6D6D, prox_settings); | ||
| 254 |
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(!hasNaN(data1.ddq)); |
| 255 | |||
| 256 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data2, contact_models_6D6D); |
| 257 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data2, contact_models_6D); |
| 258 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data2, contact_models_empty); |
| 259 | 2 | } | |
| 260 | |||
| 261 |
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_sparse_forward_dynamics_in_contact_6D_LOCAL) |
| 262 | { | ||
| 263 | using namespace Eigen; | ||
| 264 | using namespace pinocchio; | ||
| 265 | |||
| 266 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | pinocchio::Model model; |
| 267 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | pinocchio::buildModels::humanoidRandom(model, true); |
| 268 |
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); |
| 269 | |||
| 270 |
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.); |
| 271 |
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.); |
| 272 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 273 | |||
| 274 |
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); |
| 275 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd tau = VectorXd::Random(model.nv); |
| 276 | |||
| 277 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RF = "rleg6_joint"; |
| 278 | // const Model::JointIndex RF_id = model.getJointId(RF); | ||
| 279 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string LF = "lleg6_joint"; |
| 280 | // const Model::JointIndex LF_id = model.getJointId(LF); | ||
| 281 | |||
| 282 | // Contact models and data | ||
| 283 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; |
| 284 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas; |
| 285 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | RigidConstraintModel ci_RF(CONTACT_6D, model, model.getJointId(RF), LOCAL); |
| 286 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_RF.joint1_placement.setRandom(); |
| 287 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_models.push_back(ci_RF); |
| 288 |
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)); |
| 289 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | RigidConstraintModel ci_LF(CONTACT_6D, model, model.getJointId(LF), LOCAL); |
| 290 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_LF.joint1_placement.setRandom(); |
| 291 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_models.push_back(ci_LF); |
| 292 |
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_LF)); |
| 293 | |||
| 294 | 2 | Eigen::DenseIndex constraint_dim = 0; | |
| 295 |
2/2✓ Branch 1 taken 2 times.
✓ Branch 2 taken 1 times.
|
6 | for (size_t k = 0; k < contact_models.size(); ++k) |
| 296 | 4 | constraint_dim += contact_models[k].size(); | |
| 297 | |||
| 298 | 2 | const double mu0 = 0.; | |
| 299 | |||
| 300 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeAllTerms(model, data_ref, q, v); |
| 301 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | data_ref.M.triangularView<Eigen::StrictlyLower>() = |
| 302 |
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>(); |
| 303 | |||
| 304 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Eigen::MatrixXd J_ref(constraint_dim, model.nv); |
| 305 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_ref.setZero(); |
| 306 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Data::Matrix6x Jtmp = Data::Matrix6x::Zero(6, model.nv); |
| 307 | |||
| 308 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getJointJacobian(model, data_ref, ci_RF.joint1_id, ci_RF.reference_frame, Jtmp); |
| 309 |
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 | J_ref.middleRows<6>(0) = ci_RF.joint1_placement.inverse().toActionMatrix() * Jtmp; |
| 310 | |||
| 311 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Jtmp.setZero(); |
| 312 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getJointJacobian(model, data_ref, ci_LF.joint1_id, ci_LF.reference_frame, Jtmp); |
| 313 |
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 | J_ref.middleRows<6>(6) = ci_LF.joint1_placement.inverse().toActionMatrix() * Jtmp; |
| 314 | |||
| 315 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Eigen::VectorXd rhs_ref(constraint_dim); |
| 316 | ✗ | rhs_ref.segment<6>(0) = | |
| 317 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeAcceleration( |
| 318 | model, data_ref, ci_RF.joint1_id, ci_RF.reference_frame, ci_RF.type, ci_RF.joint1_placement) | ||
| 319 |
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 | .toVector(); |
| 320 | ✗ | rhs_ref.segment<6>(6) = | |
| 321 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeAcceleration( |
| 322 | model, data_ref, ci_LF.joint1_id, ci_LF.reference_frame, ci_LF.type, ci_LF.joint1_placement) | ||
| 323 |
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 | .toVector(); |
| 324 | |||
| 325 | Eigen::MatrixXd KKT_matrix_ref = | ||
| 326 |
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); |
| 327 |
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; |
| 328 |
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; |
| 329 |
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(); |
| 330 | |||
| 331 | PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH | ||
| 332 | PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS | ||
| 333 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | forwardDynamics(model, data_ref, q, v, tau, J_ref, rhs_ref, mu0); |
| 334 | PINOCCHIO_COMPILER_DIAGNOSTIC_POP | ||
| 335 | |||
| 336 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | forwardKinematics(model, data_ref, q, v, data_ref.ddq); |
| 337 | |||
| 338 |
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()); |
| 339 | |||
| 340 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings(1e-12, mu0, 1); |
| 341 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data, contact_models); |
| 342 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraintDynamics(model, data, q, v, tau, contact_models, contact_datas, prox_settings); |
| 343 |
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()); |
| 344 | |||
| 345 |
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()); |
| 346 | |||
| 347 | // Check that the decomposition is correct | ||
| 348 | 2 | const Data::ContactCholeskyDecomposition & contact_chol = data.contact_chol; | |
| 349 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Eigen::MatrixXd KKT_matrix = contact_chol.matrix(); |
| 350 | |||
| 351 |
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 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(KKT_matrix.bottomRightCorner(model.nv, model.nv) |
| 352 | .isApprox(KKT_matrix_ref.bottomRightCorner(model.nv, model.nv))); | ||
| 353 |
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)); |
| 354 | |||
| 355 | // Check solutions | ||
| 356 |
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.ddq.isApprox(data_ref.ddq)); |
| 357 | |||
| 358 | 2 | Eigen::DenseIndex constraint_id = 0; | |
| 359 |
2/2✓ Branch 1 taken 2 times.
✓ Branch 2 taken 1 times.
|
6 | for (size_t k = 0; k < contact_models.size(); ++k) |
| 360 | { | ||
| 361 | 4 | const RigidConstraintModel & cmodel = contact_models[k]; | |
| 362 | 4 | const RigidConstraintData & cdata = contact_datas[k]; | |
| 363 | |||
| 364 |
1/3✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
4 | switch (cmodel.type) |
| 365 | { | ||
| 366 | ✗ | case pinocchio::CONTACT_3D: { | |
| 367 | ✗ | BOOST_CHECK(cdata.contact_force.linear().isApprox( | |
| 368 | data_ref.lambda_c.segment(constraint_id, cmodel.size()))); | ||
| 369 | ✗ | break; | |
| 370 | } | ||
| 371 | |||
| 372 | 4 | case pinocchio::CONTACT_6D: { | |
| 373 | ForceRef<Data::VectorXs::FixedSegmentReturnType<6>::Type> f_ref( | ||
| 374 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
4 | data_ref.lambda_c.segment<6>(constraint_id)); |
| 375 |
7/14✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 2 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 2 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 2 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 2 times.
|
4 | BOOST_CHECK(cdata.contact_force.isApprox(f_ref)); |
| 376 | 4 | break; | |
| 377 | } | ||
| 378 | |||
| 379 | ✗ | default: | |
| 380 | ✗ | break; | |
| 381 | } | ||
| 382 | |||
| 383 | 4 | constraint_id += cmodel.size(); | |
| 384 | } | ||
| 385 | 2 | } | |
| 386 | |||
| 387 |
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_sparse_forward_dynamics_in_contact_6D_3D) |
| 388 | { | ||
| 389 | using namespace Eigen; | ||
| 390 | using namespace pinocchio; | ||
| 391 | |||
| 392 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | pinocchio::Model model; |
| 393 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | pinocchio::buildModels::humanoidRandom(model, true); |
| 394 |
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); |
| 395 | |||
| 396 |
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.); |
| 397 |
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.); |
| 398 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 399 | |||
| 400 |
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); |
| 401 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd tau = VectorXd::Random(model.nv); |
| 402 | |||
| 403 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RF = "rleg6_joint"; |
| 404 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string LF = "lleg6_joint"; |
| 405 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RA = "rarm6_joint"; |
| 406 | |||
| 407 | // Contact models and data | ||
| 408 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; |
| 409 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas; |
| 410 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | RigidConstraintModel ci_RF(CONTACT_6D, model, model.getJointId(RF), LOCAL); |
| 411 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_models.push_back(ci_RF); |
| 412 |
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)); |
| 413 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | RigidConstraintModel ci_LF(CONTACT_3D, model, model.getJointId(LF), LOCAL_WORLD_ALIGNED); |
| 414 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_models.push_back(ci_LF); |
| 415 |
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_LF)); |
| 416 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | RigidConstraintModel ci_RA(CONTACT_3D, model, model.getJointId(RA), LOCAL); |
| 417 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_models.push_back(ci_RA); |
| 418 |
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_RA)); |
| 419 | |||
| 420 | 2 | Eigen::DenseIndex constraint_dim = 0; | |
| 421 |
2/2✓ Branch 1 taken 3 times.
✓ Branch 2 taken 1 times.
|
8 | for (size_t k = 0; k < contact_models.size(); ++k) |
| 422 | 6 | constraint_dim += contact_models[k].size(); | |
| 423 | |||
| 424 | 2 | const double mu0 = 0.; | |
| 425 | |||
| 426 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Eigen::MatrixXd J_ref(constraint_dim, model.nv); |
| 427 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_ref.setZero(); |
| 428 | |||
| 429 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeAllTerms(model, data_ref, q, v); |
| 430 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | data_ref.M.triangularView<Eigen::StrictlyLower>() = |
| 431 |
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>(); |
| 432 | |||
| 433 |
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 | getJointJacobian(model, data_ref, model.getJointId(RF), LOCAL, J_ref.middleRows<6>(0)); |
| 434 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data::Matrix6x J_LF(6, model.nv); |
| 435 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_LF.setZero(); |
| 436 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | getJointJacobian(model, data_ref, model.getJointId(LF), LOCAL_WORLD_ALIGNED, J_LF); |
| 437 |
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 | J_ref.middleRows<3>(6) = J_LF.middleRows<3>(Motion::LINEAR); |
| 438 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data::Matrix6x J_RA(6, model.nv); |
| 439 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_RA.setZero(); |
| 440 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | getJointJacobian(model, data_ref, model.getJointId(RA), LOCAL, J_RA); |
| 441 |
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 | J_ref.middleRows<3>(9) = J_RA.middleRows<3>(Motion::LINEAR); |
| 442 | |||
| 443 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Eigen::VectorXd rhs_ref(constraint_dim); |
| 444 | |||
| 445 | ✗ | rhs_ref.segment<6>(0) = | |
| 446 |
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 | computeAcceleration(model, data_ref, model.getJointId(RF), ci_RF.reference_frame, ci_RF.type) |
| 447 |
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 | .toVector(); |
| 448 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | rhs_ref.segment<3>(6) = |
| 449 |
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 | computeAcceleration(model, data_ref, model.getJointId(LF), ci_LF.reference_frame, ci_LF.type) |
| 450 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
4 | .linear(); |
| 451 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | rhs_ref.segment<3>(9) = |
| 452 |
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 | computeAcceleration(model, data_ref, model.getJointId(RA), ci_RA.reference_frame, ci_RA.type) |
| 453 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
4 | .linear(); |
| 454 | |||
| 455 | Eigen::MatrixXd KKT_matrix_ref = | ||
| 456 |
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); |
| 457 |
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; |
| 458 |
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; |
| 459 |
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(); |
| 460 | |||
| 461 | PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH | ||
| 462 | PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS | ||
| 463 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | forwardDynamics(model, data_ref, q, v, tau, J_ref, rhs_ref, mu0); |
| 464 | PINOCCHIO_COMPILER_DIAGNOSTIC_POP | ||
| 465 | |||
| 466 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | forwardKinematics(model, data_ref, q, v, data_ref.ddq); |
| 467 | |||
| 468 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings(1e-12, mu0, 1); |
| 469 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data, contact_models); |
| 470 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraintDynamics(model, data, q, v, tau, contact_models, contact_datas, prox_settings); |
| 471 | |||
| 472 | // Check that the decomposition is correct | ||
| 473 | 2 | const Data::ContactCholeskyDecomposition & contact_chol = data.contact_chol; | |
| 474 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Eigen::MatrixXd KKT_matrix = contact_chol.matrix(); |
| 475 | |||
| 476 |
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 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(KKT_matrix.bottomRightCorner(model.nv, model.nv) |
| 477 | .isApprox(KKT_matrix_ref.bottomRightCorner(model.nv, model.nv))); | ||
| 478 |
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)); |
| 479 | |||
| 480 | // Check solutions | ||
| 481 |
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.ddq.isApprox(data_ref.ddq)); |
| 482 | |||
| 483 | 2 | Eigen::DenseIndex constraint_id = 0; | |
| 484 |
2/2✓ Branch 1 taken 3 times.
✓ Branch 2 taken 1 times.
|
8 | for (size_t k = 0; k < contact_models.size(); ++k) |
| 485 | { | ||
| 486 | 6 | const RigidConstraintModel & cmodel = contact_models[k]; | |
| 487 | 6 | const RigidConstraintData & cdata = contact_datas[k]; | |
| 488 | |||
| 489 |
2/3✓ Branch 0 taken 2 times.
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
6 | switch (cmodel.type) |
| 490 | { | ||
| 491 | 4 | case pinocchio::CONTACT_3D: { | |
| 492 |
9/18✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 2 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 2 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 2 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 2 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 2 times.
✗ Branch 29 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 2 times.
|
4 | BOOST_CHECK(cdata.contact_force.linear().isApprox( |
| 493 | data_ref.lambda_c.segment(constraint_id, cmodel.size()))); | ||
| 494 | 4 | break; | |
| 495 | } | ||
| 496 | |||
| 497 | 2 | case pinocchio::CONTACT_6D: { | |
| 498 | ForceRef<Data::VectorXs::FixedSegmentReturnType<6>::Type> f_ref( | ||
| 499 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | data_ref.lambda_c.segment<6>(constraint_id)); |
| 500 |
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(cdata.contact_force.isApprox(f_ref)); |
| 501 | 2 | break; | |
| 502 | } | ||
| 503 | |||
| 504 | ✗ | default: | |
| 505 | ✗ | break; | |
| 506 | } | ||
| 507 | |||
| 508 | 6 | constraint_id += cmodel.size(); | |
| 509 | } | ||
| 510 | 2 | } | |
| 511 | |||
| 512 |
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_constraint_dynamics_LOCAL_6D_loop_closure_j1j2) |
| 513 | { | ||
| 514 | using namespace Eigen; | ||
| 515 | using namespace pinocchio; | ||
| 516 | |||
| 517 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Model model; |
| 518 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | buildModels::humanoidRandom(model, true); |
| 519 |
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_fd(model); |
| 520 | |||
| 521 |
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.); |
| 522 |
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.); |
| 523 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 524 | |||
| 525 |
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); |
| 526 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd tau = VectorXd::Random(model.nv); |
| 527 | |||
| 528 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RF = "rleg6_joint"; |
| 529 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string LF = "lleg6_joint"; |
| 530 | |||
| 531 | // Contact models and data | ||
| 532 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) constraint_models; |
| 533 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) | ||
| 534 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | constraint_data, constraint_data_fd; |
| 535 | |||
| 536 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RA = "rarm5_joint"; |
| 537 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Model::JointIndex RA_id = model.getJointId(RA); |
| 538 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string LA = "larm5_joint"; |
| 539 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Model::JointIndex LA_id = model.getJointId(LA); |
| 540 | |||
| 541 | // Add loop closure constraint | ||
| 542 | RigidConstraintModel ci_closure( | ||
| 543 |
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, LA_id, SE3::Random(), RA_id, SE3::Random(), LOCAL); |
| 544 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_closure.corrector.Kp.array() = KP; |
| 545 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_closure.corrector.Kd.array() = KD; |
| 546 | |||
| 547 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraint_models.push_back(ci_closure); |
| 548 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | constraint_data.push_back(RigidConstraintData(ci_closure)); |
| 549 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | constraint_data_fd.push_back(RigidConstraintData(ci_closure)); |
| 550 | |||
| 551 | 2 | Eigen::DenseIndex constraint_dim = 0; | |
| 552 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
|
4 | for (size_t k = 0; k < constraint_models.size(); ++k) |
| 553 | 2 | constraint_dim += constraint_models[k].size(); | |
| 554 | |||
| 555 | 2 | const double mu0 = 0.; | |
| 556 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings(1e-12, mu0, 100); |
| 557 | |||
| 558 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data, constraint_models); |
| 559 | const VectorXd ddq_ref = | ||
| 560 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | constraintDynamics(model, data, q, v, tau, constraint_models, constraint_data, prox_settings); |
| 561 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const VectorXd lambda_ref = data.lambda_c; |
| 562 | |||
| 563 | // test multiple call | ||
| 564 | { | ||
| 565 | const VectorXd ddq = | ||
| 566 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | constraintDynamics(model, data, q, v, tau, constraint_models, constraint_data, prox_settings); |
| 567 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const VectorXd lambda = data.lambda_c; |
| 568 |
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(ddq_ref == ddq); |
| 569 |
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(lambda_ref == lambda_ref); |
| 570 | 2 | } | |
| 571 | 2 | } | |
| 572 | |||
| 573 |
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_sparse_forward_dynamics_in_contact_6D_LOCAL_WORLD_ALIGNED) |
| 574 | { | ||
| 575 | using namespace Eigen; | ||
| 576 | using namespace pinocchio; | ||
| 577 | |||
| 578 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | pinocchio::Model model; |
| 579 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | pinocchio::buildModels::humanoidRandom(model, true); |
| 580 |
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); |
| 581 | |||
| 582 |
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.); |
| 583 |
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.); |
| 584 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 585 | |||
| 586 |
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); |
| 587 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd tau = VectorXd::Random(model.nv); |
| 588 | |||
| 589 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RF = "rleg6_joint"; |
| 590 | // const Model::JointIndex RF_id = model.getJointId(RF); | ||
| 591 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string LF = "lleg6_joint"; |
| 592 | // const Model::JointIndex LF_id = model.getJointId(LF); | ||
| 593 | |||
| 594 | // Contact models and data | ||
| 595 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; |
| 596 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas; |
| 597 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | RigidConstraintModel ci_RF(CONTACT_6D, model, model.getJointId(RF), LOCAL_WORLD_ALIGNED); |
| 598 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_models.push_back(ci_RF); |
| 599 |
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)); |
| 600 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | RigidConstraintModel ci_LF(CONTACT_6D, model, model.getJointId(LF), LOCAL); |
| 601 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_models.push_back(ci_LF); |
| 602 |
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_LF)); |
| 603 | |||
| 604 | 2 | Eigen::DenseIndex constraint_dim = 0; | |
| 605 |
2/2✓ Branch 1 taken 2 times.
✓ Branch 2 taken 1 times.
|
6 | for (size_t k = 0; k < contact_models.size(); ++k) |
| 606 | 4 | constraint_dim += contact_models[k].size(); | |
| 607 | |||
| 608 | 2 | const double mu0 = 0.; | |
| 609 | |||
| 610 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Eigen::MatrixXd J_ref(constraint_dim, model.nv); |
| 611 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_ref.setZero(); |
| 612 | |||
| 613 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeAllTerms(model, data_ref, q, v); |
| 614 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | data_ref.M.triangularView<Eigen::StrictlyLower>() = |
| 615 |
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>(); |
| 616 | |||
| 617 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | updateFramePlacements(model, data_ref); |
| 618 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | getJointJacobian(model, data_ref, ci_RF.joint1_id, ci_RF.reference_frame, J_ref.middleRows<6>(0)); |
| 619 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | getJointJacobian(model, data_ref, ci_LF.joint1_id, ci_LF.reference_frame, J_ref.middleRows<6>(6)); |
| 620 | |||
| 621 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Eigen::VectorXd rhs_ref(constraint_dim); |
| 622 | |||
| 623 | ✗ | rhs_ref.segment<6>(0) = | |
| 624 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | computeAcceleration(model, data_ref, ci_RF.joint1_id, ci_RF.reference_frame, ci_RF.type) |
| 625 |
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 | .toVector(); |
| 626 | ✗ | rhs_ref.segment<6>(6) = | |
| 627 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | computeAcceleration(model, data_ref, ci_LF.joint1_id, ci_LF.reference_frame, ci_LF.type) |
| 628 |
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 | .toVector(); |
| 629 | |||
| 630 | Eigen::MatrixXd KKT_matrix_ref = | ||
| 631 |
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); |
| 632 |
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; |
| 633 |
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; |
| 634 |
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(); |
| 635 | |||
| 636 | PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH | ||
| 637 | PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS | ||
| 638 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | forwardDynamics(model, data_ref, q, v, tau, J_ref, rhs_ref, mu0); |
| 639 | PINOCCHIO_COMPILER_DIAGNOSTIC_POP | ||
| 640 | |||
| 641 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | forwardKinematics(model, data_ref, q, v, data_ref.ddq); |
| 642 | |||
| 643 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings(1e-12, mu0, 1); |
| 644 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data, contact_models); |
| 645 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraintDynamics(model, data, q, v, tau, contact_models, contact_datas, prox_settings); |
| 646 | |||
| 647 | // Check that the decomposition is correct | ||
| 648 | 2 | const Data::ContactCholeskyDecomposition & contact_chol = data.contact_chol; | |
| 649 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Eigen::MatrixXd KKT_matrix = contact_chol.matrix(); |
| 650 | |||
| 651 |
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 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(KKT_matrix.bottomRightCorner(model.nv, model.nv) |
| 652 | .isApprox(KKT_matrix_ref.bottomRightCorner(model.nv, model.nv))); | ||
| 653 |
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)); |
| 654 | |||
| 655 | // Check solutions | ||
| 656 |
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.ddq.isApprox(data_ref.ddq)); |
| 657 |
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()); |
| 658 | |||
| 659 | 2 | Eigen::DenseIndex constraint_id = 0; | |
| 660 |
2/2✓ Branch 1 taken 2 times.
✓ Branch 2 taken 1 times.
|
6 | for (size_t k = 0; k < contact_models.size(); ++k) |
| 661 | { | ||
| 662 | 4 | const RigidConstraintModel & cmodel = contact_models[k]; | |
| 663 | 4 | const RigidConstraintData & cdata = contact_datas[k]; | |
| 664 | |||
| 665 |
1/3✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
4 | switch (cmodel.type) |
| 666 | { | ||
| 667 | ✗ | case pinocchio::CONTACT_3D: { | |
| 668 | ✗ | BOOST_CHECK(cdata.contact_force.linear().isApprox( | |
| 669 | data_ref.lambda_c.segment(constraint_id, cmodel.size()))); | ||
| 670 | ✗ | break; | |
| 671 | } | ||
| 672 | |||
| 673 | 4 | case pinocchio::CONTACT_6D: { | |
| 674 | ForceRef<Data::VectorXs::FixedSegmentReturnType<6>::Type> f_ref( | ||
| 675 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
4 | data_ref.lambda_c.segment<6>(constraint_id)); |
| 676 |
7/14✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 2 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 2 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 2 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 2 times.
|
4 | BOOST_CHECK(cdata.contact_force.isApprox(f_ref)); |
| 677 | 4 | break; | |
| 678 | } | ||
| 679 | |||
| 680 | ✗ | default: | |
| 681 | ✗ | break; | |
| 682 | } | ||
| 683 | |||
| 684 | 4 | constraint_id += cmodel.size(); | |
| 685 | } | ||
| 686 | 2 | } | |
| 687 | |||
| 688 |
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_sparse_forward_dynamics_in_contact_specifying_joint2id) |
| 689 | { | ||
| 690 | using namespace Eigen; | ||
| 691 | using namespace pinocchio; | ||
| 692 | |||
| 693 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | pinocchio::Model model; |
| 694 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | pinocchio::buildModels::humanoidRandom(model, true); |
| 695 |
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); |
| 696 | |||
| 697 |
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.); |
| 698 |
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.); |
| 699 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 700 | |||
| 701 |
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); |
| 702 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd tau = VectorXd::Random(model.nv); |
| 703 | |||
| 704 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RF = "rleg6_joint"; |
| 705 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string LF = "lleg6_joint"; |
| 706 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RA = "rarm6_joint"; |
| 707 | |||
| 708 | // Contact models and data | ||
| 709 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; |
| 710 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas; |
| 711 | |||
| 712 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | RigidConstraintModel ci_RF(CONTACT_6D, model, 0, model.getJointId(RF), LOCAL_WORLD_ALIGNED); |
| 713 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | RigidConstraintModel ci_RF_bis(CONTACT_6D, model, model.getJointId(RF), LOCAL_WORLD_ALIGNED); |
| 714 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_RF.joint1_placement.setRandom(); |
| 715 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_RF.joint2_placement.setRandom(); |
| 716 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_RF_bis.joint1_placement = ci_RF.joint2_placement; |
| 717 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_RF_bis.joint2_placement = ci_RF.joint1_placement; |
| 718 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_models.push_back(ci_RF); |
| 719 |
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)); |
| 720 | |||
| 721 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | RigidConstraintModel ci_LF(CONTACT_6D, model, 0, model.getJointId(LF), LOCAL); |
| 722 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | RigidConstraintModel ci_LF_bis(CONTACT_6D, model, model.getJointId(LF), LOCAL); |
| 723 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_LF.joint1_placement.setRandom(); |
| 724 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_LF.joint2_placement.setRandom(); |
| 725 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_LF_bis.joint1_placement = ci_LF.joint2_placement; |
| 726 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_LF_bis.joint2_placement = ci_LF.joint1_placement; |
| 727 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_models.push_back(ci_LF); |
| 728 |
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_LF)); |
| 729 | |||
| 730 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | RigidConstraintModel ci_RA(CONTACT_6D, model, 0, model.getJointId(RA), LOCAL); |
| 731 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | RigidConstraintModel ci_RA_bis(CONTACT_6D, model, model.getJointId(RA), LOCAL); |
| 732 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_RA.joint1_placement.setRandom(); |
| 733 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_RA.joint2_placement.setRandom(); |
| 734 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_RA_bis.joint1_placement = ci_RA.joint2_placement; |
| 735 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_RA_bis.joint2_placement = ci_RA.joint1_placement; |
| 736 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_models.push_back(ci_RA); |
| 737 |
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_RA)); |
| 738 | |||
| 739 | 2 | Eigen::DenseIndex constraint_dim = 0; | |
| 740 |
2/2✓ Branch 1 taken 3 times.
✓ Branch 2 taken 1 times.
|
8 | for (size_t k = 0; k < contact_models.size(); ++k) |
| 741 | 6 | constraint_dim += contact_models[k].size(); | |
| 742 | |||
| 743 | 2 | const double mu0 = 0.; | |
| 744 | |||
| 745 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Eigen::MatrixXd J_ref(constraint_dim, model.nv); |
| 746 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_ref.setZero(); |
| 747 | |||
| 748 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeAllTerms(model, data_ref, q, v); |
| 749 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | data_ref.M.triangularView<Eigen::StrictlyLower>() = |
| 750 |
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>(); |
| 751 | |||
| 752 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | updateFramePlacements(model, data_ref); |
| 753 |
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 J_RF(6, model.nv), J_LF(6, model.nv), J_RA(6, model.nv); |
| 754 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_RF.setZero(); |
| 755 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_LF.setZero(); |
| 756 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_RA.setZero(); |
| 757 |
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 J_RF_local(6, model.nv), J_LF_local(6, model.nv), J_RA_local(6, model.nv); |
| 758 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_RF_local.setZero(); |
| 759 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_LF_local.setZero(); |
| 760 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_RA_local.setZero(); |
| 761 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getJointJacobian(model, data_ref, ci_RF.joint2_id, WORLD, J_RF); |
| 762 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getJointJacobian(model, data_ref, ci_RF.joint2_id, LOCAL, J_RF_local); |
| 763 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getJointJacobian(model, data_ref, ci_LF.joint2_id, WORLD, J_LF); |
| 764 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getJointJacobian(model, data_ref, ci_LF.joint2_id, LOCAL, J_LF_local); |
| 765 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getJointJacobian(model, data_ref, ci_RA.joint2_id, WORLD, J_RA); |
| 766 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getJointJacobian(model, data_ref, ci_RA.joint2_id, LOCAL, J_RA_local); |
| 767 | |||
| 768 | { | ||
| 769 | const SE3 oMc( | ||
| 770 | ✗ | SE3::Matrix3::Identity(), | |
| 771 |
4/8✓ 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.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
|
2 | (data_ref.oMi[ci_RF.joint1_id] * ci_RF.joint1_placement).translation()); |
| 772 |
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 | J_ref.middleRows<6>(0) = -oMc.toActionMatrixInverse() * J_RF; |
| 773 | } | ||
| 774 | |||
| 775 | { | ||
| 776 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_ref.middleRows<6>(6) = |
| 777 |
5/10✓ 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.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
|
4 | -(data_ref.oMi[ci_LF.joint1_id] * ci_LF.joint1_placement).toActionMatrixInverse() * J_LF; |
| 778 | } | ||
| 779 | |||
| 780 | { | ||
| 781 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_ref.middleRows<6>(12) = |
| 782 |
5/10✓ 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.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
|
4 | -(data_ref.oMi[ci_RA.joint1_id] * ci_RA.joint1_placement).toActionMatrixInverse() * J_RA; |
| 783 | } | ||
| 784 | |||
| 785 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Eigen::VectorXd rhs_ref(constraint_dim); |
| 786 | |||
| 787 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | forwardKinematics(model, data_ref, q, v, 0 * v); |
| 788 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const SE3 c1Mc2_1 = (data.oMi[ci_RF.joint1_id] * ci_RF.joint1_placement) |
| 789 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | .actInv(data_ref.oMi[ci_RF.joint2_id] * ci_RF.joint2_placement); |
| 790 | SE3 c1Mc2_1_W( | ||
| 791 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | (data_ref.oMi[ci_RF.joint2_id]).rotation(), |
| 792 |
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_ref.oMi[ci_RF.joint1_id] * ci_RF.joint1_placement).translation() |
| 793 |
3/6✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
|
6 | + data_ref.oMi[ci_RF.joint2_id].translation()); |
| 794 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | Motion acc_1 = c1Mc2_1_W.act(data_ref.a[ci_RF.joint2_id]); |
| 795 | |||
| 796 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const SE3 c1Mc2_2 = (data_ref.oMi[ci_LF.joint1_id] * ci_LF.joint1_placement) |
| 797 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | .actInv(data_ref.oMi[ci_LF.joint2_id] * ci_LF.joint2_placement); |
| 798 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | Motion acc_2 = c1Mc2_2.act(ci_LF.joint2_placement.actInv(data_ref.a[ci_LF.joint2_id])); |
| 799 | |||
| 800 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const SE3 c1Mc2_3 = (data_ref.oMi[ci_RA.joint1_id] * ci_RA.joint1_placement) |
| 801 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | .actInv(data_ref.oMi[ci_RA.joint2_id] * ci_RA.joint2_placement); |
| 802 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | Motion acc_3 = c1Mc2_3.act(ci_RA.joint2_placement.actInv(data_ref.a[ci_RA.joint2_id])); |
| 803 | |||
| 804 |
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 | rhs_ref.segment<6>(0) = -acc_1.toVector(); |
| 805 |
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 | rhs_ref.segment<6>(6) = -acc_2.toVector(); |
| 806 |
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 | rhs_ref.segment<6>(12) = -acc_3.toVector(); |
| 807 | |||
| 808 | Eigen::MatrixXd KKT_matrix_ref = | ||
| 809 |
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); |
| 810 |
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.topLeftCorner(constraint_dim, constraint_dim).diagonal().fill(-mu0); |
| 811 |
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; |
| 812 |
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; |
| 813 |
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(); |
| 814 | |||
| 815 | PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH | ||
| 816 | PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS | ||
| 817 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | forwardDynamics(model, data_ref, q, v, tau, J_ref, rhs_ref, mu0); |
| 818 | PINOCCHIO_COMPILER_DIAGNOSTIC_POP | ||
| 819 | |||
| 820 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | forwardKinematics(model, data_ref, q, v, 0 * data_ref.ddq); |
| 821 | |||
| 822 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings(1e-12, mu0, 1); |
| 823 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data, contact_models); |
| 824 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraintDynamics(model, data, q, v, tau, contact_models, contact_datas, prox_settings); |
| 825 | |||
| 826 |
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 | std::cout << "acc_1 ref:\n" << acc_1 << std::endl; |
| 827 |
3/6✓ 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.
|
2 | std::cout << "acc_1:\n" << contact_datas[0].contact2_acceleration_drift << std::endl; |
| 828 |
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(acc_1.isApprox(contact_datas[0].contact2_acceleration_drift)); |
| 829 | |||
| 830 |
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 | std::cout << "acc_2 ref:\n" << acc_2 << std::endl; |
| 831 |
3/6✓ 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.
|
2 | std::cout << "acc_2:\n" << contact_datas[1].contact2_acceleration_drift << std::endl; |
| 832 |
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(acc_2.isApprox(contact_datas[1].contact2_acceleration_drift)); |
| 833 | |||
| 834 |
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 | std::cout << "acc_3 ref:\n" << acc_3 << std::endl; |
| 835 |
3/6✓ 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.
|
2 | std::cout << "acc_3:\n" << contact_datas[2].contact2_acceleration_drift << std::endl; |
| 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 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(acc_3.isApprox(contact_datas[2].contact2_acceleration_drift)); |
| 837 | |||
| 838 |
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_1)); |
| 839 | |||
| 840 | const SE3 c1Mc2_1_LWA( | ||
| 841 |
5/10✓ 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 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
|
2 | contact_datas[0].oMc2.rotation(), contact_datas[0].oMc1.rotation() * c1Mc2_1.translation()); |
| 842 |
13/26✓ 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 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 39 taken 1 times.
✗ Branch 40 not taken.
✗ Branch 44 not taken.
✓ Branch 45 taken 1 times.
|
2 | BOOST_CHECK( |
| 843 | (c1Mc2_1_LWA.toActionMatrix() * (ci_RF.joint2_placement.toActionMatrixInverse() * J_RF_local)) | ||
| 844 | .isApprox(-J_ref.middleRows<6>(0))); | ||
| 845 |
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].oMc1.isApprox(ci_RF.joint1_placement)); |
| 846 | |||
| 847 |
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[1].c1Mc2.isApprox(c1Mc2_2)); |
| 848 |
11/22✓ 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 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 1 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 1 times.
✗ Branch 35 not taken.
✗ Branch 39 not taken.
✓ Branch 40 taken 1 times.
|
2 | BOOST_CHECK( |
| 849 | (contact_datas[1].oMc1.toActionMatrixInverse() * J_LF).isApprox(-J_ref.middleRows<6>(6))); | ||
| 850 |
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 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 1 times.
|
2 | BOOST_CHECK((data_ref.oMi[ci_LF.joint2_id].toActionMatrix() * J_LF_local).isApprox(J_LF)); |
| 851 |
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[1].oMc1.isApprox(ci_LF.joint1_placement)); |
| 852 |
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 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(data.oa[ci_LF.joint2_id].isApprox( |
| 853 | data_ref.oMi[ci_LF.joint2_id].act(data_ref.a[ci_LF.joint2_id]))); | ||
| 854 | |||
| 855 |
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[2].c1Mc2.isApprox(c1Mc2_3)); |
| 856 |
13/26✓ 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 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 39 taken 1 times.
✗ Branch 40 not taken.
✗ Branch 44 not taken.
✓ Branch 45 taken 1 times.
|
2 | BOOST_CHECK( |
| 857 | (c1Mc2_3.toActionMatrix() * (ci_RA.joint2_placement.toActionMatrixInverse() * J_RA_local)) | ||
| 858 | .isApprox(-J_ref.middleRows<6>(12))); | ||
| 859 |
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[2].oMc1.isApprox(ci_RA.joint1_placement)); |
| 860 |
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 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(data.oa[ci_RA.joint2_id].isApprox( |
| 861 | data_ref.oMi[ci_RA.joint2_id].act(data_ref.a[ci_RA.joint2_id]))); | ||
| 862 | |||
| 863 | // Check that the decomposition is correct | ||
| 864 | |||
| 865 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | forwardKinematics(model, data_ref, q, v, 0 * data_ref.ddq); |
| 866 | 2 | const Data::ContactCholeskyDecomposition & contact_chol = data.contact_chol; | |
| 867 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Eigen::MatrixXd KKT_matrix = contact_chol.matrix(); |
| 868 | |||
| 869 |
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 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(KKT_matrix.bottomRightCorner(model.nv, model.nv) |
| 870 | .isApprox(KKT_matrix_ref.bottomRightCorner(model.nv, model.nv))); | ||
| 871 |
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)); |
| 872 | |||
| 873 | // Check solutions | ||
| 874 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | forwardKinematics(model, data, q, v, data.ddq); |
| 875 |
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.ddq.isApprox(data_ref.ddq)); |
| 876 |
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()); |
| 877 | |||
| 878 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | Motion acc_1_final = c1Mc2_1_W.act(data.a[ci_RF.joint2_id]); |
| 879 |
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(acc_1_final.isZero()); |
| 880 | |||
| 881 |
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 | std::cout << "acc_1_final:\n" << acc_1_final << std::endl; |
| 882 | |||
| 883 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | Motion acc_2_final = c1Mc2_2.act(data.a[ci_LF.joint2_id]); |
| 884 |
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(acc_2_final.isZero()); |
| 885 | |||
| 886 |
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 | std::cout << "acc_2_final:\n" << acc_2_final << std::endl; |
| 887 | |||
| 888 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | Motion acc_3_final = c1Mc2_3.act(data.a[ci_RA.joint2_id]); |
| 889 |
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(acc_3_final.isZero()); |
| 890 | |||
| 891 |
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 | std::cout << "acc_3_final:\n" << acc_3_final << std::endl; |
| 892 | |||
| 893 | 2 | Eigen::DenseIndex constraint_id = 0; | |
| 894 |
2/2✓ Branch 1 taken 3 times.
✓ Branch 2 taken 1 times.
|
8 | for (size_t k = 0; k < contact_models.size(); ++k) |
| 895 | { | ||
| 896 | 6 | const RigidConstraintModel & cmodel = contact_models[k]; | |
| 897 | 6 | const RigidConstraintData & cdata = contact_datas[k]; | |
| 898 | |||
| 899 |
1/3✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | switch (cmodel.type) |
| 900 | { | ||
| 901 | ✗ | case pinocchio::CONTACT_3D: { | |
| 902 | ✗ | BOOST_CHECK(cdata.contact_force.linear().isApprox( | |
| 903 | data_ref.lambda_c.segment(constraint_id, cmodel.size()))); | ||
| 904 | ✗ | break; | |
| 905 | } | ||
| 906 | |||
| 907 | 6 | case pinocchio::CONTACT_6D: { | |
| 908 | ForceRef<Data::VectorXs::FixedSegmentReturnType<6>::Type> f_ref( | ||
| 909 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
6 | data_ref.lambda_c.segment<6>(constraint_id)); |
| 910 |
7/14✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 3 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 3 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 3 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 3 times.
|
6 | BOOST_CHECK(cdata.contact_force.isApprox(f_ref)); |
| 911 | 6 | break; | |
| 912 | } | ||
| 913 | |||
| 914 | ✗ | default: | |
| 915 | ✗ | break; | |
| 916 | } | ||
| 917 | |||
| 918 | 6 | constraint_id += cmodel.size(); | |
| 919 | } | ||
| 920 | |||
| 921 | // Contact models and data | ||
| 922 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
4 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_bis; |
| 923 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
4 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas_bis; |
| 924 | |||
| 925 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_models_bis.push_back(ci_RF_bis); |
| 926 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_models_bis.push_back(ci_LF_bis); |
| 927 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_models_bis.push_back(ci_RA_bis); |
| 928 | |||
| 929 | 2 | for (PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel)::const_iterator it = | |
| 930 | 2 | contact_models_bis.begin(); | |
| 931 |
2/2✓ Branch 2 taken 3 times.
✓ Branch 3 taken 1 times.
|
8 | it != contact_models_bis.end(); ++it) |
| 932 |
2/4✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
|
6 | contact_datas_bis.push_back(RigidConstraintData(*it)); |
| 933 | |||
| 934 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
4 | Data data_bis(model); |
| 935 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data_bis, contact_models_bis); |
| 936 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraintDynamics( |
| 937 | model, data_bis, q, v, tau, contact_models_bis, contact_datas_bis, prox_settings); | ||
| 938 | |||
| 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(data_bis.ddq.isApprox(data.ddq)); |
| 940 |
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 | std::cout << "ddq: " << data_bis.ddq.transpose() << std::endl; |
| 941 |
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 | std::cout << "ddq: " << data.ddq.transpose() << std::endl; |
| 942 | |||
| 943 | // Eigen::DenseIndex constraint_id = 0; | ||
| 944 |
2/2✓ Branch 1 taken 3 times.
✓ Branch 2 taken 1 times.
|
8 | for (size_t k = 0; k < contact_models.size(); ++k) |
| 945 | { | ||
| 946 | 6 | const RigidConstraintModel & cmodel = contact_models[k]; | |
| 947 | 6 | const RigidConstraintData & cdata = contact_datas[k]; | |
| 948 | 6 | const RigidConstraintModel & cmodel_bis = contact_models_bis[k]; | |
| 949 | 6 | const RigidConstraintData & cdata_bis = contact_datas_bis[k]; | |
| 950 | |||
| 951 |
6/12✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 3 times.
|
6 | BOOST_CHECK(cmodel_bis.reference_frame == cmodel.reference_frame); |
| 952 |
6/12✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 3 times.
|
6 | BOOST_CHECK(cmodel_bis.joint1_id == cmodel.joint2_id); |
| 953 |
6/12✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 3 times.
|
6 | BOOST_CHECK(cmodel_bis.joint2_id == cmodel.joint1_id); |
| 954 |
7/14✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 3 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 3 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 3 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 3 times.
|
6 | BOOST_CHECK(cdata.oMc1.isApprox(cdata_bis.oMc2)); |
| 955 |
7/14✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 3 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 3 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 3 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 3 times.
|
6 | BOOST_CHECK(cdata.oMc2.isApprox(cdata_bis.oMc1)); |
| 956 |
8/16✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 3 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 3 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 3 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 3 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 3 times.
|
6 | BOOST_CHECK(cdata.c1Mc2.isApprox(cdata_bis.c1Mc2.inverse())); |
| 957 | |||
| 958 |
3/6✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
|
6 | std::cout << "cdata.c1Mc2:\n" << cdata.c1Mc2 << std::endl; |
| 959 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
6 | Force contact_force, contact_force_bis; |
| 960 |
2/4✓ Branch 0 taken 1 times.
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
|
6 | switch (cmodel.reference_frame) |
| 961 | { | ||
| 962 | 2 | case LOCAL_WORLD_ALIGNED: { | |
| 963 |
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 | SE3 c1Mc2_LWA(SE3::Matrix3::Identity(), cdata.oMc1.rotation() * cdata.c1Mc2.translation()); |
| 964 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_force_bis = cdata_bis.contact_force; |
| 965 |
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(cdata_bis.contact1_acceleration_drift.isApprox( |
| 966 | c1Mc2_LWA.actInv(cdata.contact2_acceleration_drift))); | ||
| 967 | |||
| 968 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | contact_force = c1Mc2_LWA.actInv(cdata.contact_force); |
| 969 |
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(contact_force.isApprox(-contact_force_bis)); |
| 970 | 2 | break; | |
| 971 | } | ||
| 972 | 4 | case LOCAL: { | |
| 973 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
4 | contact_force_bis = cdata_bis.contact_force; |
| 974 |
8/16✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 2 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 2 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 2 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 2 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 2 times.
|
4 | BOOST_CHECK(cdata_bis.contact1_acceleration_drift.isApprox( |
| 975 | cdata.c1Mc2.actInv(cdata.contact2_acceleration_drift))); | ||
| 976 | |||
| 977 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
4 | contact_force = cdata.c1Mc2.actInv(cdata.contact_force); |
| 978 |
8/16✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 2 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 2 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 2 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 2 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 2 times.
|
4 | BOOST_CHECK(contact_force.isApprox(-contact_force_bis)); |
| 979 | 4 | break; | |
| 980 | } | ||
| 981 | ✗ | case WORLD: | |
| 982 | ✗ | BOOST_CHECK(false); | |
| 983 | ✗ | break; | |
| 984 | } | ||
| 985 | |||
| 986 |
5/10✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3 times.
✗ Branch 14 not taken.
|
6 | std::cout << "contact_force: " << contact_force.toVector().transpose() << std::endl; |
| 987 |
5/10✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3 times.
✗ Branch 14 not taken.
|
6 | std::cout << "contact_force_bis: " << contact_force_bis.toVector().transpose() << std::endl; |
| 988 | } | ||
| 989 | 2 | } | |
| 990 | |||
| 991 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintData) | ||
| 992 | 5 | createData( | |
| 993 | const PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintModel) & contact_models) | ||
| 994 | { | ||
| 995 | 5 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintData) contact_datas; | |
| 996 |
2/2✓ Branch 1 taken 11 times.
✓ Branch 2 taken 5 times.
|
16 | for (size_t k = 0; k < contact_models.size(); ++k) |
| 997 |
2/4✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 11 times.
✗ Branch 6 not taken.
|
11 | contact_datas.push_back(pinocchio::RigidConstraintData(contact_models[k])); |
| 998 | |||
| 999 | 5 | return contact_datas; | |
| 1000 | } | ||
| 1001 | |||
| 1002 |
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_correction_CONTACT_6D) |
| 1003 | { | ||
| 1004 | using namespace Eigen; | ||
| 1005 | using namespace pinocchio; | ||
| 1006 | |||
| 1007 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | pinocchio::Model model; |
| 1008 | // pinocchio::buildModels::humanoidRandom(model,true); | ||
| 1009 |
5/10✓ 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.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
|
2 | const JointIndex joint_id = model.addJoint(0, JointModelFreeFlyer(), SE3::Identity(), "root"); |
| 1010 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Inertia box_inertia = Inertia::FromBox(100., 1., 1., 1.); |
| 1011 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.appendBodyToJoint(joint_id, box_inertia); |
| 1012 |
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); |
| 1013 | |||
| 1014 |
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.); |
| 1015 |
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.); |
| 1016 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 1017 | |||
| 1018 |
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); |
| 1019 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd tau = VectorXd::Random(model.nv); |
| 1020 | |||
| 1021 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RF = "root"; |
| 1022 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const JointIndex RF_id = model.getJointId(RF); |
| 1023 | |||
| 1024 | // Contact models and data | ||
| 1025 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; |
| 1026 | |||
| 1027 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintModel ci_RF(CONTACT_6D, model, RF_id, LOCAL); |
| 1028 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_RF.joint1_placement.setIdentity(); |
| 1029 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_RF.joint2_placement.setIdentity(); |
| 1030 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_RF.corrector.Kp.setConstant(10.); |
| 1031 |
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 | ci_RF.corrector.Kd = 2. * ci_RF.corrector.Kp.cwiseSqrt(); |
| 1032 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_models.push_back(ci_RF); |
| 1033 | |||
| 1034 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) | ||
| 1035 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_datas = createData(contact_models); |
| 1036 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data, contact_models); |
| 1037 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraintDynamics(model, data, q, v, tau, contact_models, contact_datas); |
| 1038 | |||
| 1039 |
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 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 1 times.
|
2 | BOOST_CHECK(contact_datas[0].oMc1.isApprox(data.oMi[ci_RF.joint1_id] * ci_RF.joint1_placement)); |
| 1040 |
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 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 1 times.
|
2 | BOOST_CHECK(contact_datas[0].oMc2.isApprox(data.oMi[ci_RF.joint2_id] * ci_RF.joint2_placement)); |
| 1041 |
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 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(contact_datas[0].contact1_velocity.isApprox( |
| 1042 | contact_datas[0].oMc1.actInv(data.ov[ci_RF.joint1_id]))); | ||
| 1043 |
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.isZero()); |
| 1044 | |||
| 1045 | 2 | const double dt = 1e-8; | |
| 1046 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | const VectorXd q_plus = integrate(model, q, v * dt); |
| 1047 | |||
| 1048 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data data_plus(model); |
| 1049 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) | ||
| 1050 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_datas_plus = createData(contact_models); |
| 1051 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data_plus, contact_models); |
| 1052 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraintDynamics(model, data_plus, q_plus, v, tau, contact_models, contact_datas_plus); |
| 1053 | |||
| 1054 | const Motion contact_RF_velocity_error_fd = | ||
| 1055 |
4/8✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
|
2 | log6(contact_datas[0].c1Mc2.act(contact_datas_plus[0].c1Mc2.inverse())) / dt; |
| 1056 |
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( |
| 1057 | contact_RF_velocity_error_fd.isApprox(contact_datas[0].contact_velocity_error, sqrt(dt))); | ||
| 1058 |
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 | std::cout << "contact_RF_velocity_error_fd:\n" << contact_RF_velocity_error_fd << std::endl; |
| 1059 |
3/6✓ 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.
|
2 | std::cout << "contact_velocity_error:\n" << contact_datas[0].contact_velocity_error << std::endl; |
| 1060 | |||
| 1061 | // Simulation loop | ||
| 1062 | { | ||
| 1063 | 2 | const int N = 200; | |
| 1064 | 2 | const double dt = 1e-3; | |
| 1065 | 2 | const double mu = 1e-12; | |
| 1066 | |||
| 1067 | // model.gravity.setZero(); | ||
| 1068 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data data_sim(model); |
| 1069 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) | ||
| 1070 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_data_sim = createData(contact_models); |
| 1071 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data_sim, contact_models); |
| 1072 | |||
| 1073 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Eigen::VectorXd q0(model.nq); |
| 1074 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const SE3 M0 = SE3::Random(); |
| 1075 |
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 14 taken 1 times.
✗ Branch 15 not taken.
|
2 | q0 << M0.translation(), SE3::Quaternion(M0.rotation()).coeffs(); |
| 1076 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | const Eigen::VectorXd v0 = Eigen::VectorXd::Zero(model.nv); |
| 1077 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Eigen::VectorXd a = Eigen::VectorXd(model.nv); |
| 1078 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Eigen::VectorXd tau = Eigen::VectorXd::Zero(model.nv); |
| 1079 | |||
| 1080 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Eigen::VectorXd q(q0), v(v0); |
| 1081 | |||
| 1082 |
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 | tau = rnea(model, data_sim, q, v, Eigen::VectorXd::Zero(model.nv)); |
| 1083 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings(1e-12, mu, 1); |
| 1084 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraintDynamics( |
| 1085 | model, data_sim, q0, v0, tau, contact_models, contact_data_sim, prox_settings); | ||
| 1086 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) | ||
| 1087 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_data_sim_prev(contact_data_sim); |
| 1088 | |||
| 1089 |
2/2✓ Branch 0 taken 201 times.
✓ Branch 1 taken 1 times.
|
404 | for (int it = 0; it <= N; it++) |
| 1090 | { | ||
| 1091 |
1/2✓ Branch 1 taken 201 times.
✗ Branch 2 not taken.
|
402 | a = constraintDynamics( |
| 1092 |
1/2✓ Branch 1 taken 201 times.
✗ Branch 2 not taken.
|
402 | model, data_sim, q, v, tau, contact_models, contact_data_sim, prox_settings); |
| 1093 |
2/4✓ Branch 1 taken 201 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 201 times.
✗ Branch 5 not taken.
|
402 | v += a * dt; |
| 1094 |
2/4✓ Branch 1 taken 201 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 201 times.
✗ Branch 5 not taken.
|
402 | q = integrate(model, q, v * dt); |
| 1095 | |||
| 1096 |
2/2✓ Branch 0 taken 199 times.
✓ Branch 1 taken 2 times.
|
402 | if (it > 1) |
| 1097 | { | ||
| 1098 |
2/2✓ Branch 1 taken 199 times.
✓ Branch 2 taken 199 times.
|
796 | for (size_t k = 0; k < contact_models.size(); ++k) |
| 1099 | { | ||
| 1100 | 398 | const RigidConstraintData & cdata = contact_data_sim[k]; | |
| 1101 | 398 | const RigidConstraintData & cdata_prev = contact_data_sim_prev[k]; | |
| 1102 | |||
| 1103 |
10/20✓ Branch 1 taken 199 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 199 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 199 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 199 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 199 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 199 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 199 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 199 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 199 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 199 times.
|
398 | BOOST_CHECK( |
| 1104 | cdata.contact_placement_error.toVector().norm() | ||
| 1105 | <= cdata_prev.contact_placement_error.toVector().norm()); | ||
| 1106 | } | ||
| 1107 | } | ||
| 1108 | |||
| 1109 |
1/2✓ Branch 1 taken 201 times.
✗ Branch 2 not taken.
|
402 | contact_data_sim_prev = contact_data_sim; |
| 1110 | } | ||
| 1111 | 2 | } | |
| 1112 | 2 | } | |
| 1113 | |||
| 1114 |
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_correction_CONTACT_3D) |
| 1115 | { | ||
| 1116 | using namespace Eigen; | ||
| 1117 | using namespace pinocchio; | ||
| 1118 | |||
| 1119 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | pinocchio::Model model; |
| 1120 | // pinocchio::buildModels::humanoidRandom(model,true); | ||
| 1121 |
5/10✓ 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.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
|
2 | const JointIndex joint_id = model.addJoint(0, JointModelFreeFlyer(), SE3::Identity(), "root"); |
| 1122 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Inertia box_inertia = Inertia::FromBox(100., 1., 1., 1.); |
| 1123 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.appendBodyToJoint(joint_id, box_inertia); |
| 1124 |
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); |
| 1125 | |||
| 1126 |
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.); |
| 1127 |
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.); |
| 1128 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 1129 | |||
| 1130 |
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); |
| 1131 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd tau = VectorXd::Random(model.nv); |
| 1132 | |||
| 1133 | 2 | const double mu = 1e-8; | |
| 1134 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RF = "root"; |
| 1135 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const JointIndex RF_id = model.getJointId(RF); |
| 1136 | |||
| 1137 | // Contact models and data | ||
| 1138 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; |
| 1139 | |||
| 1140 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintModel ci_RF1(CONTACT_3D, model, RF_id, LOCAL); |
| 1141 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_RF1.joint1_placement.translation() = SE3::Vector3(0.5, 0.5, -0.5); |
| 1142 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_RF1.joint2_placement.setRandom(); |
| 1143 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_RF1.corrector.Kp.setConstant(10.); |
| 1144 |
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 | ci_RF1.corrector.Kd = 2. * ci_RF1.corrector.Kp.cwiseSqrt(); |
| 1145 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_models.push_back(ci_RF1); |
| 1146 | |||
| 1147 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintModel ci_RF2(CONTACT_3D, model, RF_id, LOCAL); |
| 1148 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_RF2.joint1_placement.translation() = SE3::Vector3(-0.5, 0.5, -0.5); |
| 1149 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_RF2.joint2_placement.setRandom(); |
| 1150 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_RF2.corrector.Kp.setConstant(10.); |
| 1151 |
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 | ci_RF2.corrector.Kd = 2. * ci_RF2.corrector.Kp.cwiseSqrt(); |
| 1152 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_models.push_back(ci_RF2); |
| 1153 | |||
| 1154 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintModel ci_RF3(CONTACT_3D, model, RF_id, LOCAL); |
| 1155 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_RF3.joint1_placement.translation() = SE3::Vector3(-0.5, -0.5, -0.5); |
| 1156 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_RF3.joint2_placement.setRandom(); |
| 1157 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_RF3.corrector.Kp.setConstant(10.); |
| 1158 |
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 | ci_RF3.corrector.Kd = 2. * ci_RF3.corrector.Kp.cwiseSqrt(); |
| 1159 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_models.push_back(ci_RF3); |
| 1160 | |||
| 1161 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintModel ci_RF4(CONTACT_3D, model, RF_id, LOCAL); |
| 1162 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_RF4.joint1_placement.translation() = SE3::Vector3(0.5, -0.5, -0.5); |
| 1163 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_RF4.joint2_placement.setRandom(); |
| 1164 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_RF4.corrector.Kp.setConstant(10.); |
| 1165 |
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 | ci_RF4.corrector.Kd = 2. * ci_RF4.corrector.Kp.cwiseSqrt(); |
| 1166 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_models.push_back(ci_RF4); |
| 1167 | |||
| 1168 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) | ||
| 1169 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_datas = createData(contact_models); |
| 1170 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings(1e-12, mu, 1); |
| 1171 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data, contact_models); |
| 1172 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraintDynamics(model, data, q, v, tau, contact_models, contact_datas, prox_settings); |
| 1173 | |||
| 1174 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | Eigen::VectorXd contact_placement_error_prev(contact_models.size() * 6); |
| 1175 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | Eigen::VectorXd contact_placement_error(contact_models.size() * 6); |
| 1176 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_placement_error_prev.setZero(); |
| 1177 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_placement_error.setZero(); |
| 1178 | |||
| 1179 | // Simulation loop | ||
| 1180 | { | ||
| 1181 | 2 | const int N = 200; | |
| 1182 | 2 | const double dt = 1e-3; | |
| 1183 | |||
| 1184 | // model.gravity.setZero(); | ||
| 1185 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data data_sim(model); |
| 1186 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) | ||
| 1187 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_data_sim = createData(contact_models); |
| 1188 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data_sim, contact_models); |
| 1189 | |||
| 1190 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Eigen::VectorXd q0(model.nq); |
| 1191 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const SE3 M0 = SE3::Random(); |
| 1192 |
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 14 taken 1 times.
✗ Branch 15 not taken.
|
2 | q0 << M0.translation(), SE3::Quaternion(M0.rotation()).coeffs(); |
| 1193 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | const Eigen::VectorXd v0 = Eigen::VectorXd::Zero(model.nv); |
| 1194 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Eigen::VectorXd a = Eigen::VectorXd(model.nv); |
| 1195 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Eigen::VectorXd tau = Eigen::VectorXd::Zero(model.nv); |
| 1196 | |||
| 1197 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Eigen::VectorXd q(q0), v(v0); |
| 1198 | |||
| 1199 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraintDynamics( |
| 1200 | model, data_sim, q0, v0, tau, contact_models, contact_data_sim, prox_settings); | ||
| 1201 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) | ||
| 1202 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_data_sim_prev(contact_data_sim); |
| 1203 | |||
| 1204 |
2/2✓ Branch 0 taken 201 times.
✓ Branch 1 taken 1 times.
|
404 | for (int it = 0; it <= N; it++) |
| 1205 | { | ||
| 1206 |
1/2✓ Branch 1 taken 201 times.
✗ Branch 2 not taken.
|
402 | a = constraintDynamics( |
| 1207 |
1/2✓ Branch 1 taken 201 times.
✗ Branch 2 not taken.
|
402 | model, data_sim, q, v, tau, contact_models, contact_data_sim, prox_settings); |
| 1208 |
2/4✓ Branch 1 taken 201 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 201 times.
✗ Branch 5 not taken.
|
402 | v += a * dt; |
| 1209 |
2/4✓ Branch 1 taken 201 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 201 times.
✗ Branch 5 not taken.
|
402 | q = integrate(model, q, v * dt); |
| 1210 | |||
| 1211 |
2/2✓ Branch 0 taken 199 times.
✓ Branch 1 taken 2 times.
|
402 | if (it > 1) |
| 1212 | { | ||
| 1213 |
2/2✓ Branch 1 taken 796 times.
✓ Branch 2 taken 199 times.
|
1990 | for (size_t k = 0; k < contact_models.size(); ++k) |
| 1214 | { | ||
| 1215 | 1592 | const RigidConstraintData & cdata = contact_data_sim[k]; | |
| 1216 | 1592 | const RigidConstraintData & cdata_prev = contact_data_sim_prev[k]; | |
| 1217 | ✗ | contact_placement_error.segment<6>(6 * (Eigen::Index)k) = | |
| 1218 |
3/6✓ Branch 1 taken 796 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 796 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 796 times.
✗ Branch 8 not taken.
|
1592 | cdata.contact_placement_error.toVector(); |
| 1219 | 1592 | contact_placement_error_prev.segment<6>(6 * (Eigen::Index)k) = | |
| 1220 |
3/6✓ Branch 1 taken 796 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 796 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 796 times.
✗ Branch 8 not taken.
|
1592 | cdata_prev.contact_placement_error.toVector(); |
| 1221 | } | ||
| 1222 |
8/16✓ Branch 1 taken 199 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 199 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 199 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 199 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 199 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 199 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 199 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 199 times.
|
398 | BOOST_CHECK(contact_placement_error.norm() <= contact_placement_error_prev.norm()); |
| 1223 | } | ||
| 1224 |
1/2✓ Branch 1 taken 201 times.
✗ Branch 2 not taken.
|
402 | contact_data_sim_prev = contact_data_sim; |
| 1225 | } | ||
| 1226 | 2 | } | |
| 1227 | 2 | } | |
| 1228 | |||
| 1229 |
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_sparse_forward_dynamics_in_contact_specifying_joint2id_case3D) |
| 1230 | { | ||
| 1231 | using namespace Eigen; | ||
| 1232 | using namespace pinocchio; | ||
| 1233 | |||
| 1234 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | pinocchio::Model model; |
| 1235 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | pinocchio::buildModels::humanoidRandom(model, true); |
| 1236 |
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); |
| 1237 | |||
| 1238 |
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.); |
| 1239 |
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.); |
| 1240 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 1241 | |||
| 1242 |
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); |
| 1243 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd tau = VectorXd::Random(model.nv); |
| 1244 | |||
| 1245 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RF = "rleg6_joint"; |
| 1246 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string LF = "lleg6_joint"; |
| 1247 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RA = "rarm6_joint"; |
| 1248 | |||
| 1249 | // Contact models and data | ||
| 1250 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; |
| 1251 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas; |
| 1252 | |||
| 1253 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | RigidConstraintModel ci_RF(CONTACT_3D, model, 0, model.getJointId(RF), LOCAL_WORLD_ALIGNED); |
| 1254 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | RigidConstraintModel ci_RF_bis(CONTACT_3D, model, model.getJointId(RF), LOCAL_WORLD_ALIGNED); |
| 1255 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_RF.joint1_placement.setRandom(); |
| 1256 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_RF.joint2_placement.setRandom(); |
| 1257 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_RF_bis.joint1_placement = ci_RF.joint2_placement; |
| 1258 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_RF_bis.joint2_placement = ci_RF.joint1_placement; |
| 1259 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_models.push_back(ci_RF); |
| 1260 |
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)); |
| 1261 | |||
| 1262 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | RigidConstraintModel ci_LF(CONTACT_3D, model, 0, model.getJointId(LF), LOCAL); |
| 1263 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | RigidConstraintModel ci_LF_bis(CONTACT_3D, model, model.getJointId(LF), LOCAL); |
| 1264 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_LF.joint1_placement.setRandom(); |
| 1265 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_LF.joint2_placement.setRandom(); |
| 1266 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_LF_bis.joint1_placement = ci_LF.joint2_placement; |
| 1267 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_LF_bis.joint2_placement = ci_LF.joint1_placement; |
| 1268 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_models.push_back(ci_LF); |
| 1269 |
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_LF)); |
| 1270 | |||
| 1271 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | RigidConstraintModel ci_RA(CONTACT_6D, model, 0, model.getJointId(RA), LOCAL); |
| 1272 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | RigidConstraintModel ci_RA_bis(CONTACT_6D, model, model.getJointId(RA), LOCAL); |
| 1273 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_RA.joint1_placement.setRandom(); |
| 1274 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_RA.joint2_placement.setRandom(); |
| 1275 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_RA_bis.joint1_placement = ci_RA.joint2_placement; |
| 1276 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_RA_bis.joint2_placement = ci_RA.joint1_placement; |
| 1277 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_models.push_back(ci_RA); |
| 1278 |
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_RA)); |
| 1279 | |||
| 1280 | 2 | Eigen::DenseIndex constraint_dim = 0; | |
| 1281 |
2/2✓ Branch 1 taken 3 times.
✓ Branch 2 taken 1 times.
|
8 | for (size_t k = 0; k < contact_models.size(); ++k) |
| 1282 | 6 | constraint_dim += contact_models[k].size(); | |
| 1283 | |||
| 1284 | 2 | const double mu0 = 0.; | |
| 1285 | |||
| 1286 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Eigen::MatrixXd J_ref(constraint_dim, model.nv); |
| 1287 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_ref.setZero(); |
| 1288 | |||
| 1289 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeAllTerms(model, data_ref, q, v); |
| 1290 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | data_ref.M.triangularView<Eigen::StrictlyLower>() = |
| 1291 |
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>(); |
| 1292 | |||
| 1293 |
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 J_RF(6, model.nv), J_LF(6, model.nv), J_RA(6, model.nv); |
| 1294 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_RF.setZero(); |
| 1295 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_LF.setZero(); |
| 1296 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_RA.setZero(); |
| 1297 |
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 J_RF_local(6, model.nv), J_LF_local(6, model.nv), J_RA_local(6, model.nv); |
| 1298 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_RF_local.setZero(); |
| 1299 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_LF_local.setZero(); |
| 1300 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_RA_local.setZero(); |
| 1301 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getJointJacobian(model, data_ref, ci_RF.joint2_id, WORLD, J_RF); |
| 1302 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getJointJacobian(model, data_ref, ci_RF.joint2_id, LOCAL, J_RF_local); |
| 1303 |
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 | J_RF_local = ci_RF.joint2_placement.toActionMatrixInverse() * J_RF_local; |
| 1304 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getJointJacobian(model, data_ref, ci_LF.joint2_id, WORLD, J_LF); |
| 1305 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getJointJacobian(model, data_ref, ci_LF.joint2_id, LOCAL, J_LF_local); |
| 1306 |
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 | J_LF_local = ci_LF.joint2_placement.toActionMatrixInverse() * J_LF_local; |
| 1307 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getJointJacobian(model, data_ref, ci_RA.joint2_id, WORLD, J_RA); |
| 1308 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getJointJacobian(model, data_ref, ci_RA.joint2_id, LOCAL, J_RA_local); |
| 1309 | |||
| 1310 | { | ||
| 1311 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const SE3 oMc2 = data_ref.oMi[ci_RF.joint2_id] * ci_RF.joint2_placement; |
| 1312 |
6/12✓ 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.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
|
2 | J_ref.middleRows<3>(0) = -oMc2.rotation() * J_RF_local.middleRows<3>(Motion::LINEAR); |
| 1313 | } | ||
| 1314 | |||
| 1315 | { | ||
| 1316 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const SE3 oMc1 = data_ref.oMi[ci_LF.joint1_id] * ci_LF.joint1_placement; |
| 1317 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const SE3 oMc2 = data_ref.oMi[ci_LF.joint2_id] * ci_LF.joint2_placement; |
| 1318 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const SE3 c1Mc2 = oMc1.actInv(oMc2); |
| 1319 |
6/12✓ 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.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
|
2 | J_ref.middleRows<3>(3) = -c1Mc2.rotation() * J_LF_local.middleRows<3>(SE3::LINEAR); |
| 1320 | } | ||
| 1321 | |||
| 1322 | { | ||
| 1323 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_ref.middleRows<6>(6) = |
| 1324 |
5/10✓ 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.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
|
4 | -(data_ref.oMi[ci_RA.joint1_id] * ci_RA.joint1_placement).toActionMatrixInverse() * J_RA; |
| 1325 | } | ||
| 1326 | |||
| 1327 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Eigen::VectorXd rhs_ref(constraint_dim); |
| 1328 | |||
| 1329 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | forwardKinematics(model, data_ref, q, v, 0 * v); |
| 1330 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Motion::Vector3 acc_1; |
| 1331 | { | ||
| 1332 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const SE3 oMc2 = data_ref.oMi[ci_RF.joint2_id] * ci_RF.joint2_placement; |
| 1333 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const Motion v2 = ci_RF.joint2_placement.actInv(data_ref.v[ci_RF.joint2_id]); |
| 1334 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const Motion a2 = ci_RF.joint2_placement.actInv(data_ref.a[ci_RF.joint2_id]); |
| 1335 |
8/16✓ 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.
✓ 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.
|
2 | acc_1 = oMc2.rotation() * (a2.linear() + v2.angular().cross(v2.linear())); |
| 1336 | } | ||
| 1337 | |||
| 1338 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Motion::Vector3 acc_2; |
| 1339 | { | ||
| 1340 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const SE3 oMc1 = data_ref.oMi[ci_LF.joint1_id] * ci_LF.joint1_placement; |
| 1341 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const SE3 oMc2 = data_ref.oMi[ci_LF.joint2_id] * ci_LF.joint2_placement; |
| 1342 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const SE3 c1Mc2 = oMc1.actInv(oMc2); |
| 1343 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const Motion v2 = ci_LF.joint2_placement.actInv(data_ref.v[ci_LF.joint2_id]); |
| 1344 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const Motion a2 = ci_LF.joint2_placement.actInv(data_ref.a[ci_LF.joint2_id]); |
| 1345 |
8/16✓ 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.
✓ 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.
|
2 | acc_2 = c1Mc2.rotation() * (a2.linear() + v2.angular().cross(v2.linear())); |
| 1346 | } | ||
| 1347 | |||
| 1348 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const SE3 c1Mc2_3 = (data_ref.oMi[ci_RA.joint1_id] * ci_RA.joint1_placement) |
| 1349 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | .actInv(data_ref.oMi[ci_RA.joint2_id] * ci_RA.joint2_placement); |
| 1350 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | Motion acc_3 = c1Mc2_3.act(ci_RA.joint2_placement.actInv(data_ref.a[ci_RA.joint2_id])); |
| 1351 | |||
| 1352 |
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<3>(0) = -acc_1; |
| 1353 |
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<3>(3) = -acc_2; |
| 1354 |
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 | rhs_ref.segment<6>(6) = -acc_3.toVector(); |
| 1355 | |||
| 1356 | Eigen::MatrixXd KKT_matrix_ref = | ||
| 1357 |
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); |
| 1358 |
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.topLeftCorner(constraint_dim, constraint_dim).diagonal().fill(-mu0); |
| 1359 |
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; |
| 1360 |
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; |
| 1361 |
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(); |
| 1362 | |||
| 1363 | PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH | ||
| 1364 | PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS | ||
| 1365 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | forwardDynamics(model, data_ref, q, v, tau, J_ref, rhs_ref, mu0); |
| 1366 | PINOCCHIO_COMPILER_DIAGNOSTIC_POP | ||
| 1367 | |||
| 1368 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | forwardKinematics(model, data_ref, q, v, 0 * data_ref.ddq); |
| 1369 | |||
| 1370 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings(1e-12, 0, 1); |
| 1371 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data, contact_models); |
| 1372 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraintDynamics(model, data, q, v, tau, contact_models, contact_datas, prox_settings); |
| 1373 | |||
| 1374 | 2 | const Data::ContactCholeskyDecomposition & contact_chol = data.contact_chol; | |
| 1375 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Eigen::MatrixXd KKT_matrix = contact_chol.matrix(); |
| 1376 | |||
| 1377 |
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 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(KKT_matrix.bottomRightCorner(model.nv, model.nv) |
| 1378 | .isApprox(KKT_matrix_ref.bottomRightCorner(model.nv, model.nv))); | ||
| 1379 |
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 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(KKT_matrix.topRightCorner(constraint_dim, model.nv).isApprox(J_ref)); |
| 1380 |
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)); |
| 1381 | |||
| 1382 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | std::cout << "KKT_matrix.topRightCorner(constraint_dim,model.nv):\n" |
| 1383 |
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.topRightCorner(constraint_dim, model.nv) << std::endl; |
| 1384 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | std::cout << "KKT_matrix_ref.topRightCorner(constraint_dim,model.nv):\n" |
| 1385 |
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.topRightCorner(constraint_dim, model.nv) << std::endl; |
| 1386 | |||
| 1387 | // Check solutions | ||
| 1388 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | forwardKinematics(model, data, q, v, data.ddq); |
| 1389 |
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.ddq.isApprox(data_ref.ddq)); |
| 1390 |
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()); |
| 1391 | |||
| 1392 |
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 | std::cout << "data_ref.ddq: " << data_ref.ddq.transpose() << std::endl; |
| 1393 |
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 | std::cout << "data.ddq: " << data.ddq.transpose() << std::endl; |
| 1394 |
6/12✓ 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.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
|
2 | std::cout << "res: " << (J_ref * data.ddq + rhs_ref).transpose() << std::endl; |
| 1395 |
6/12✓ 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.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
|
2 | std::cout << "res_ref: " << (J_ref * data_ref.ddq + rhs_ref).transpose() << std::endl; |
| 1396 | |||
| 1397 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const Motion vel_1_final = ci_RF.joint2_placement.actInv(data.v[ci_RF.joint2_id]); |
| 1398 | const Motion::Vector3 acc_1_final = | ||
| 1399 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | ci_RF.joint2_placement.actInv(data.a[ci_RF.joint2_id]).linear() |
| 1400 |
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.
|
4 | + vel_1_final.angular().cross(vel_1_final.linear()); |
| 1401 |
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(acc_1_final.isZero()); |
| 1402 | |||
| 1403 |
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 | std::cout << "acc_1_final:" << acc_1_final.transpose() << std::endl; |
| 1404 | |||
| 1405 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const Motion vel_2_final = ci_LF.joint2_placement.actInv(data.v[ci_LF.joint2_id]); |
| 1406 | const Motion::Vector3 acc_2_final = | ||
| 1407 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | ci_LF.joint2_placement.actInv(data.a[ci_LF.joint2_id]).linear() |
| 1408 |
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.
|
4 | + vel_2_final.angular().cross(vel_2_final.linear()); |
| 1409 |
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(acc_2_final.isZero()); |
| 1410 | |||
| 1411 |
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 | std::cout << "acc_2_final:" << acc_2_final.transpose() << std::endl; |
| 1412 | |||
| 1413 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | Motion acc_3_final = c1Mc2_3.act(data.a[ci_RA.joint2_id]); |
| 1414 |
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(acc_3_final.isZero()); |
| 1415 | |||
| 1416 |
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 | std::cout << "acc_3_final:\n" << acc_3_final << std::endl; |
| 1417 | |||
| 1418 | 2 | Eigen::DenseIndex constraint_id = 0; | |
| 1419 |
2/2✓ Branch 1 taken 3 times.
✓ Branch 2 taken 1 times.
|
8 | for (size_t k = 0; k < contact_models.size(); ++k) |
| 1420 | { | ||
| 1421 | 6 | const RigidConstraintModel & cmodel = contact_models[k]; | |
| 1422 | 6 | const RigidConstraintData & cdata = contact_datas[k]; | |
| 1423 | |||
| 1424 |
2/3✓ Branch 0 taken 2 times.
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
6 | switch (cmodel.type) |
| 1425 | { | ||
| 1426 | 4 | case pinocchio::CONTACT_3D: { | |
| 1427 |
9/18✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 2 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 2 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 2 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 2 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 2 times.
✗ Branch 29 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 2 times.
|
4 | BOOST_CHECK(cdata.contact_force.linear().isApprox( |
| 1428 | data_ref.lambda_c.segment(constraint_id, cmodel.size()))); | ||
| 1429 | 4 | break; | |
| 1430 | } | ||
| 1431 | |||
| 1432 | 2 | case pinocchio::CONTACT_6D: { | |
| 1433 | ForceRef<Data::VectorXs::FixedSegmentReturnType<6>::Type> f_ref( | ||
| 1434 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | data_ref.lambda_c.segment<6>(constraint_id)); |
| 1435 |
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(cdata.contact_force.isApprox(f_ref)); |
| 1436 | 2 | break; | |
| 1437 | } | ||
| 1438 | |||
| 1439 | ✗ | default: | |
| 1440 | ✗ | break; | |
| 1441 | } | ||
| 1442 | |||
| 1443 | 6 | constraint_id += cmodel.size(); | |
| 1444 | } | ||
| 1445 | |||
| 1446 | // Contact models and data | ||
| 1447 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
4 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_bis; |
| 1448 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
4 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas_bis; |
| 1449 | |||
| 1450 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_models_bis.push_back(ci_RF_bis); |
| 1451 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_models_bis.push_back(ci_LF_bis); |
| 1452 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_models_bis.push_back(ci_RA_bis); |
| 1453 | |||
| 1454 | 2 | for (PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel)::const_iterator it = | |
| 1455 | 2 | contact_models_bis.begin(); | |
| 1456 |
2/2✓ Branch 2 taken 3 times.
✓ Branch 3 taken 1 times.
|
8 | it != contact_models_bis.end(); ++it) |
| 1457 |
2/4✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
|
6 | contact_datas_bis.push_back(RigidConstraintData(*it)); |
| 1458 | |||
| 1459 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
4 | Data data_bis(model); |
| 1460 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data_bis, contact_models_bis); |
| 1461 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraintDynamics( |
| 1462 | model, data_bis, q, v, tau, contact_models_bis, contact_datas_bis, prox_settings); | ||
| 1463 | |||
| 1464 |
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_bis.ddq.isApprox(data.ddq)); |
| 1465 |
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 | std::cout << "ddq: " << data_bis.ddq.transpose() << std::endl; |
| 1466 |
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 | std::cout << "ddq: " << data.ddq.transpose() << std::endl; |
| 1467 | |||
| 1468 |
2/2✓ Branch 1 taken 3 times.
✓ Branch 2 taken 1 times.
|
8 | for (size_t k = 0; k < contact_models.size(); ++k) |
| 1469 | { | ||
| 1470 | 6 | const RigidConstraintModel & cmodel = contact_models[k]; | |
| 1471 | 6 | const RigidConstraintData & cdata = contact_datas[k]; | |
| 1472 | 6 | const RigidConstraintModel & cmodel_bis = contact_models_bis[k]; | |
| 1473 | 6 | const RigidConstraintData & cdata_bis = contact_datas_bis[k]; | |
| 1474 | |||
| 1475 |
6/12✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 3 times.
|
6 | BOOST_CHECK(cmodel_bis.reference_frame == cmodel.reference_frame); |
| 1476 |
6/12✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 3 times.
|
6 | BOOST_CHECK(cmodel_bis.joint1_id == cmodel.joint2_id); |
| 1477 |
6/12✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 3 times.
|
6 | BOOST_CHECK(cmodel_bis.joint2_id == cmodel.joint1_id); |
| 1478 |
7/14✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 3 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 3 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 3 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 3 times.
|
6 | BOOST_CHECK(cdata.oMc1.isApprox(cdata_bis.oMc2)); |
| 1479 |
7/14✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 3 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 3 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 3 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 3 times.
|
6 | BOOST_CHECK(cdata.oMc2.isApprox(cdata_bis.oMc1)); |
| 1480 |
8/16✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 3 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 3 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 3 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 3 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 3 times.
|
6 | BOOST_CHECK(cdata.c1Mc2.isApprox(cdata_bis.c1Mc2.inverse())); |
| 1481 | |||
| 1482 |
3/6✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
|
6 | std::cout << "cdata.c1Mc2:\n" << cdata.c1Mc2 << std::endl; |
| 1483 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
6 | Force contact_force, contact_force_bis; |
| 1484 |
2/4✓ Branch 0 taken 1 times.
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
|
6 | switch (cmodel.reference_frame) |
| 1485 | { | ||
| 1486 | 2 | case LOCAL_WORLD_ALIGNED: { | |
| 1487 |
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 | SE3 c1Mc2_LWA(SE3::Matrix3::Identity(), cdata.oMc1.rotation() * cdata.c1Mc2.translation()); |
| 1488 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_force_bis = cdata_bis.contact_force; |
| 1489 | |||
| 1490 |
1/2✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
|
2 | if (cmodel.type == CONTACT_3D) |
| 1491 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_force = cdata.contact_force; |
| 1492 | else | ||
| 1493 | { | ||
| 1494 | ✗ | contact_force = c1Mc2_LWA.actInv(cdata.contact_force); | |
| 1495 | ✗ | BOOST_CHECK(cdata_bis.contact1_acceleration_drift.isApprox( | |
| 1496 | c1Mc2_LWA.actInv(cdata.contact2_acceleration_drift))); | ||
| 1497 | } | ||
| 1498 |
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(contact_force.isApprox(-contact_force_bis)); |
| 1499 | 2 | break; | |
| 1500 | } | ||
| 1501 | 4 | case LOCAL: { | |
| 1502 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
4 | contact_force_bis = cdata_bis.contact_force; |
| 1503 | |||
| 1504 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
|
4 | if (cmodel.type == CONTACT_3D) |
| 1505 |
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 | contact_force.linear() = cdata.c1Mc2.actInv(cdata.contact_force).linear(); |
| 1506 | else | ||
| 1507 | { | ||
| 1508 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | contact_force = cdata.c1Mc2.actInv(cdata.contact_force); |
| 1509 |
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(cdata_bis.contact1_acceleration_drift.isApprox( |
| 1510 | cdata.c1Mc2.actInv(cdata.contact2_acceleration_drift))); | ||
| 1511 | } | ||
| 1512 |
8/16✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 2 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 2 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 2 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 2 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 2 times.
|
4 | BOOST_CHECK(contact_force.isApprox(-contact_force_bis)); |
| 1513 | 4 | break; | |
| 1514 | } | ||
| 1515 | ✗ | case WORLD: | |
| 1516 | ✗ | BOOST_CHECK(false); | |
| 1517 | ✗ | break; | |
| 1518 | } | ||
| 1519 | |||
| 1520 |
5/10✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3 times.
✗ Branch 14 not taken.
|
6 | std::cout << "contact_force: " << contact_force.toVector().transpose() << std::endl; |
| 1521 |
5/10✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3 times.
✗ Branch 14 not taken.
|
6 | std::cout << "contact_force_bis: " << contact_force_bis.toVector().transpose() << std::endl; |
| 1522 | } | ||
| 1523 | 2 | } | |
| 1524 | |||
| 1525 |
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_contact_ABA_with_armature) |
| 1526 | { | ||
| 1527 | using namespace pinocchio; | ||
| 1528 | using namespace Eigen; | ||
| 1529 | |||
| 1530 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | pinocchio::Model model; |
| 1531 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | pinocchio::buildModels::humanoidRandom(model); |
| 1532 | model.rotorInertia = | ||
| 1533 |
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 | 100. * (Model::VectorXs::Random(model.nv) + Model::VectorXs::Constant(model.nv, 1.)); |
| 1534 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | model.rotorGearRatio.fill(100); |
| 1535 | |||
| 1536 |
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); |
| 1537 | |||
| 1538 |
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.); |
| 1539 |
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.); |
| 1540 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 1541 | |||
| 1542 |
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); |
| 1543 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd tau = VectorXd::Random(model.nv); |
| 1544 | |||
| 1545 | typedef PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) | ||
| 1546 | RigidConstraintModelVector; | ||
| 1547 | typedef PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) RigidConstraintDataVector; | ||
| 1548 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const RigidConstraintModelVector empty_rigid_contact_models; |
| 1549 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintDataVector empty_rigid_contact_data; |
| 1550 | |||
| 1551 | const Data::VectorXs a = | ||
| 1552 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | contactABA(model, data, q, v, tau, empty_rigid_contact_models, empty_rigid_contact_data); |
| 1553 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | const Data::VectorXs tau_ref = rnea(model, data_ref, q, v, a); |
| 1554 | |||
| 1555 |
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(tau.isApprox(tau_ref)); |
| 1556 | 2 | } | |
| 1557 | |||
| 1558 |
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_diagonal_inertia) |
| 1559 | { | ||
| 1560 | using namespace pinocchio; | ||
| 1561 | |||
| 1562 | 2 | const double mu = 1e2; | |
| 1563 |
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 | const Inertia diagonal6_inertia(mu, Inertia::Vector3::Zero(), Symmetric3(mu, 0, mu, 0, 0, mu)); |
| 1564 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Inertia::Matrix6 diagonal6_inertia_mat = diagonal6_inertia.matrix(); |
| 1565 |
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 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(diagonal6_inertia_mat.block(Inertia::LINEAR, Inertia::ANGULAR, 3, 3).isZero()); |
| 1566 |
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 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(diagonal6_inertia_mat.block(Inertia::ANGULAR, Inertia::LINEAR, 3, 3).isZero()); |
| 1567 | |||
| 1568 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const SE3 M = SE3::Random(); |
| 1569 | // const Inertia::Matrix3 RtRmu = mu * M.rotation().transpose()*M.rotation(); | ||
| 1570 |
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 Inertia::Matrix3 RtRmu = mu * Inertia::Matrix3::Identity(); |
| 1571 |
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 | Inertia I6_translate(mu, M.translation(), Symmetric3(RtRmu)); |
| 1572 | |||
| 1573 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Inertia I6_ref = M.act(diagonal6_inertia); |
| 1574 |
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(I6_translate.isApprox(I6_ref)); |
| 1575 | |||
| 1576 |
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 | const Inertia diagonal3_inertia(mu, Inertia::Vector3::Zero(), Symmetric3(0, 0, 0, 0, 0, 0)); |
| 1577 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Inertia::Matrix6 diagonal3_inertia_mat = diagonal3_inertia.matrix(); |
| 1578 |
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 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(diagonal3_inertia_mat.block(Inertia::LINEAR, Inertia::ANGULAR, 3, 3).isZero()); |
| 1579 |
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 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(diagonal3_inertia_mat.block(Inertia::ANGULAR, Inertia::LINEAR, 3, 3).isZero()); |
| 1580 |
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 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(diagonal3_inertia_mat.block(Inertia::ANGULAR, Inertia::ANGULAR, 3, 3).isZero()); |
| 1581 | |||
| 1582 |
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 | Inertia I3_translate(mu, M.translation(), Symmetric3(0, 0, 0, 0, 0, 0)); |
| 1583 | |||
| 1584 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Inertia I3_ref = M.act(diagonal3_inertia); |
| 1585 |
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(I3_translate.isApprox(I3_ref)); |
| 1586 | 2 | } | |
| 1587 | |||
| 1588 |
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_contact_ABA_6D) |
| 1589 | { | ||
| 1590 | using namespace Eigen; | ||
| 1591 | using namespace pinocchio; | ||
| 1592 | |||
| 1593 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | pinocchio::Model model; |
| 1594 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | pinocchio::buildModels::humanoidRandom(model, true); |
| 1595 |
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); |
| 1596 | |||
| 1597 |
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.); |
| 1598 |
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.); |
| 1599 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 1600 | |||
| 1601 |
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); |
| 1602 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd tau = VectorXd::Random(model.nv); |
| 1603 | |||
| 1604 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RF = "rleg6_joint"; |
| 1605 | // const Frame & RF_frame = model.frames[model.getJointId(RF)]; | ||
| 1606 | // Frame RF_contact_frame("RF_contact_frame", | ||
| 1607 | // RF_frame.parent,model.getJointId(RF), | ||
| 1608 | // SE3::Random(),OP_FRAME); | ||
| 1609 | // model.addFrame(RF_contact_frame); | ||
| 1610 | |||
| 1611 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string LF = "lleg6_joint"; |
| 1612 | // const Frame & LF_frame = model.frames[model.getJointId(LF)]; | ||
| 1613 | // Frame LF_contact_frame("LF_contact_frame", | ||
| 1614 | // LF_frame.parent,model.getJointId(RF), | ||
| 1615 | // SE3::Random(),OP_FRAME); | ||
| 1616 | // model.addFrame(LF_contact_frame); | ||
| 1617 | // const Model::JointIndex LF_id = model.getJointId(LF); | ||
| 1618 | |||
| 1619 | // Contact models and data | ||
| 1620 | typedef PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) | ||
| 1621 | RigidConstraintModelVector; | ||
| 1622 | typedef PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) RigidConstraintDataVector; | ||
| 1623 | |||
| 1624 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const RigidConstraintModelVector empty_contact_models; |
| 1625 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintDataVector empty_contact_data; |
| 1626 | |||
| 1627 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contactABA(model, data, q, v, tau, empty_contact_models, empty_contact_data); |
| 1628 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | forwardKinematics(model, data_ref, q, v, 0 * v); |
| 1629 |
2/2✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
|
56 | for (JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id) |
| 1630 | { | ||
| 1631 |
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.liMi[joint_id].isApprox(data_ref.liMi[joint_id])); |
| 1632 |
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[joint_id].isApprox(data_ref.oMi[joint_id])); |
| 1633 |
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[joint_id].isApprox(data_ref.oMi[joint_id].act(data_ref.v[joint_id]))); |
| 1634 |
3/4✓ Branch 3 taken 27 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
✓ Branch 6 taken 26 times.
|
54 | if (data.oa_drift[joint_id].isZero()) |
| 1635 | { | ||
| 1636 |
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 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 1 times.
|
2 | BOOST_CHECK((data_ref.oMi[joint_id].act(data_ref.a[joint_id])).isZero()); |
| 1637 | } | ||
| 1638 | else | ||
| 1639 | { | ||
| 1640 |
8/16✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 26 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 26 times.
✗ Branch 12 not taken.
✓ Branch 18 taken 26 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 26 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 26 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 26 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 26 times.
|
52 | BOOST_CHECK( |
| 1641 | data.oa_drift[joint_id].isApprox(data_ref.oMi[joint_id].act(data_ref.a[joint_id]))); | ||
| 1642 | } | ||
| 1643 | } | ||
| 1644 | |||
| 1645 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeJointJacobians(model, data_ref, q); |
| 1646 |
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)); |
| 1647 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | aba(model, data_ref, q, v, tau, Convention::LOCAL); |
| 1648 | |||
| 1649 |
2/2✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
|
56 | for (JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id) |
| 1650 | { | ||
| 1651 | 54 | const Data::SE3 & oMi = data.oMi[joint_id]; | |
| 1652 |
4/8✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 27 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
|
54 | Eigen::MatrixXd U_ref = oMi.toDualActionMatrix() * data_ref.joints[joint_id].U(); |
| 1653 |
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 15 taken 27 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 27 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 27 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 27 times.
✗ Branch 26 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 27 times.
|
54 | BOOST_CHECK(data.joints[joint_id].U().isApprox(U_ref)); |
| 1654 | Eigen::MatrixXd StYS_ref = | ||
| 1655 |
6/12✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 27 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 27 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 27 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 27 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 27 times.
✗ Branch 19 not taken.
|
54 | data_ref.joints[joint_id].S().matrix().transpose() * data_ref.joints[joint_id].U(); |
| 1656 |
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 15 taken 27 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 27 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 27 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 27 times.
✗ Branch 26 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 27 times.
|
54 | BOOST_CHECK(data.joints[joint_id].StU().isApprox(StYS_ref)); |
| 1657 | const Data::Matrix6 oYaba_ref = | ||
| 1658 |
6/12✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 27 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 27 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 27 times.
✗ Branch 18 not taken.
|
54 | oMi.toDualActionMatrix() * data_ref.Yaba[joint_id] * oMi.inverse().toActionMatrix(); |
| 1659 |
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 16 taken 27 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 27 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 27 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 27 times.
|
54 | BOOST_CHECK(data.oYaba[joint_id].isApprox(oYaba_ref)); |
| 1660 |
9/18✓ 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 30 taken 27 times.
✗ Branch 31 not taken.
✗ Branch 35 not taken.
✓ Branch 36 taken 27 times.
|
54 | BOOST_CHECK(data.oa_augmented[joint_id].isApprox( |
| 1661 | model.gravity + data_ref.oMi[joint_id].act(data_ref.a_gf[joint_id]))); | ||
| 1662 | 54 | } | |
| 1663 | |||
| 1664 |
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.ddq.isApprox(data_ref.ddq)); |
| 1665 | |||
| 1666 | // Test second call | ||
| 1667 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contactABA(model, data, q, v, tau, empty_contact_models, empty_contact_data); |
| 1668 |
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.ddq.isApprox(data_ref.ddq)); |
| 1669 | |||
| 1670 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintModelVector contact_models; |
| 1671 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintDataVector contact_datas; |
| 1672 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | RigidConstraintModel ci_RF(CONTACT_6D, model, model.getJointId(RF), LOCAL); |
| 1673 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_RF.joint1_placement.setRandom(); |
| 1674 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_models.push_back(ci_RF); |
| 1675 |
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)); |
| 1676 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | RigidConstraintModel ci_LF(CONTACT_6D, model, model.getJointId(LF), LOCAL); |
| 1677 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_LF.joint1_placement.setRandom(); |
| 1678 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_models.push_back(ci_LF); |
| 1679 |
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_LF)); |
| 1680 | |||
| 1681 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintDataVector contact_datas_ref(contact_datas); |
| 1682 | |||
| 1683 | 2 | Eigen::DenseIndex constraint_dim = 0; | |
| 1684 |
2/2✓ Branch 1 taken 2 times.
✓ Branch 2 taken 1 times.
|
6 | for (size_t k = 0; k < contact_models.size(); ++k) |
| 1685 | 4 | constraint_dim += contact_models[k].size(); | |
| 1686 | |||
| 1687 | 2 | const double mu0 = 0.; | |
| 1688 | |||
| 1689 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Eigen::MatrixXd J_ref(constraint_dim, model.nv); |
| 1690 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_ref.setZero(); |
| 1691 | |||
| 1692 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings_cd(1e-12, mu0, 1); |
| 1693 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data_ref, contact_models); |
| 1694 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraintDynamics( |
| 1695 | model, data_ref, q, v, tau, contact_models, contact_datas_ref, prox_settings_cd); | ||
| 1696 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | forwardKinematics(model, data_ref, q, v, v * 0); |
| 1697 | |||
| 1698 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | updateFramePlacements(model, data_ref); |
| 1699 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data::Matrix6x Jtmp(6, model.nv); |
| 1700 | |||
| 1701 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Jtmp.setZero(); |
| 1702 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getJointJacobian(model, data_ref, ci_RF.joint1_id, ci_RF.reference_frame, Jtmp); |
| 1703 |
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 | J_ref.middleRows<6>(0) = ci_RF.joint1_placement.inverse().toActionMatrix() * Jtmp; |
| 1704 | |||
| 1705 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Jtmp.setZero(); |
| 1706 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getJointJacobian(model, data_ref, ci_LF.joint1_id, ci_LF.reference_frame, Jtmp); |
| 1707 |
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 | J_ref.middleRows<6>(6) = ci_LF.joint1_placement.inverse().toActionMatrix() * Jtmp; |
| 1708 | |||
| 1709 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Eigen::VectorXd gamma(constraint_dim); |
| 1710 | |||
| 1711 | ✗ | gamma.segment<6>(0) = | |
| 1712 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeAcceleration( |
| 1713 | model, data_ref, ci_RF.joint1_id, ci_RF.reference_frame, ci_RF.type, ci_RF.joint1_placement) | ||
| 1714 |
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 | .toVector(); |
| 1715 | ✗ | gamma.segment<6>(6) = | |
| 1716 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
4 | computeAcceleration( |
| 1717 | model, data_ref, ci_LF.joint1_id, ci_LF.reference_frame, ci_LF.type, ci_LF.joint1_placement) | ||
| 1718 |
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 | .toVector(); |
| 1719 | |||
| 1720 |
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 + gamma).isZero()); |
| 1721 | |||
| 1722 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data data_constrained_dyn(model); |
| 1723 | |||
| 1724 | PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH | ||
| 1725 | PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS | ||
| 1726 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | forwardDynamics(model, data_constrained_dyn, q, v, tau, J_ref, gamma, mu0); |
| 1727 | PINOCCHIO_COMPILER_DIAGNOSTIC_POP | ||
| 1728 | |||
| 1729 |
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_constrained_dyn.ddq + gamma).isZero()); |
| 1730 | |||
| 1731 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings; |
| 1732 | 2 | prox_settings.max_iter = 10; | |
| 1733 | 2 | prox_settings.mu = 1e8; | |
| 1734 | 2 | const double mu = prox_settings.mu; | |
| 1735 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contactABA(model, data, q, v, tau, contact_models, contact_datas, prox_settings); |
| 1736 | |||
| 1737 |
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 | std::cout << "data.ddq: " << data.ddq.transpose() << std::endl; |
| 1738 |
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 | std::cout << "data_ref.ddq: " << data_ref.ddq.transpose() << std::endl; |
| 1739 |
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 + gamma).isZero()); |
| 1740 | |||
| 1741 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | forwardKinematics(model, data_ref, q, v, 0 * v); |
| 1742 |
2/2✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
|
56 | for (JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id) |
| 1743 | { | ||
| 1744 |
3/4✓ Branch 3 taken 27 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
✓ Branch 6 taken 26 times.
|
54 | if (data.oa_drift[joint_id].isZero()) |
| 1745 | { | ||
| 1746 |
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 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 1 times.
|
2 | BOOST_CHECK((data_ref.oMi[joint_id].act(data_ref.a[joint_id])).isZero()); |
| 1747 | } | ||
| 1748 | else | ||
| 1749 | { | ||
| 1750 |
8/16✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 26 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 26 times.
✗ Branch 12 not taken.
✓ Branch 18 taken 26 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 26 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 26 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 26 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 26 times.
|
52 | BOOST_CHECK( |
| 1751 | data.oa_drift[joint_id].isApprox(data_ref.oMi[joint_id].act(data_ref.a[joint_id]))); | ||
| 1752 | } | ||
| 1753 | } | ||
| 1754 | |||
| 1755 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | aba(model, data_ref, q, v, 0 * v, Convention::WORLD); |
| 1756 |
2/2✓ Branch 1 taken 2 times.
✓ Branch 2 taken 1 times.
|
6 | for (size_t contact_id = 0; contact_id < contact_models.size(); ++contact_id) |
| 1757 | { | ||
| 1758 | 4 | const RigidConstraintModel & cmodel = contact_models[contact_id]; | |
| 1759 | 4 | const RigidConstraintData & cdata = contact_datas[contact_id]; | |
| 1760 | |||
| 1761 | 4 | const JointIndex & joint1_id = cmodel.joint1_id; | |
| 1762 | |||
| 1763 | // Check contact placement | ||
| 1764 | 4 | const SE3 & iMc = cmodel.joint1_placement; | |
| 1765 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
4 | const SE3 oMc = data_ref.oMi[joint1_id] * iMc; |
| 1766 |
7/14✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 2 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 2 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 2 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 2 times.
|
4 | BOOST_CHECK(cdata.oMc1.isApprox(oMc)); |
| 1767 | |||
| 1768 | // Check contact velocity | ||
| 1769 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
4 | const Motion contact1_velocity_ref = iMc.actInv(data_ref.v[joint1_id]); |
| 1770 |
7/14✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 2 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 2 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 2 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 2 times.
|
4 | BOOST_CHECK(cdata.contact1_velocity.isApprox(contact1_velocity_ref)); |
| 1771 | |||
| 1772 | // Check contact inertia | ||
| 1773 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
4 | Symmetric3 S(Symmetric3::Zero()); |
| 1774 |
1/2✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
|
4 | if (cmodel.type == CONTACT_6D) |
| 1775 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
4 | S.setDiagonal(Symmetric3::Vector3::Constant(mu)); |
| 1776 | |||
| 1777 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
4 | const Inertia contact_inertia(mu, oMc.translation(), S); |
| 1778 | |||
| 1779 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
4 | Inertia::Matrix6 contact_inertia_ref = Inertia::Matrix6::Zero(); |
| 1780 | |||
| 1781 |
1/2✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
|
4 | if (cmodel.type == CONTACT_6D) |
| 1782 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
4 | contact_inertia_ref.diagonal().fill(mu); |
| 1783 | else | ||
| 1784 | ✗ | contact_inertia_ref.diagonal().head<3>().fill(mu); | |
| 1785 | contact_inertia_ref = | ||
| 1786 |
5/10✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
|
4 | oMc.toDualActionMatrix() * contact_inertia_ref * oMc.toActionMatrixInverse(); |
| 1787 |
8/16✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 2 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 2 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 2 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 2 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 2 times.
|
4 | BOOST_CHECK(contact_inertia_ref.isApprox(contact_inertia.matrix())); |
| 1788 | |||
| 1789 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
4 | Inertia::Matrix6 Yaba_ref = data_ref.oMi[joint1_id].toDualActionMatrix() |
| 1790 |
2/4✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
|
8 | * model.inertias[joint1_id].matrix() |
| 1791 |
2/4✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
|
8 | * data_ref.oMi[joint1_id].toActionMatrixInverse() |
| 1792 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
4 | + contact_inertia_ref; |
| 1793 | |||
| 1794 | 4 | const JointModel & jmodel = model.joints[joint1_id]; | |
| 1795 | 4 | const JointData & jdata = data.joints[joint1_id]; | |
| 1796 | // const JointData & jdata_ref = data_ref.joints[joint_id]; | ||
| 1797 | |||
| 1798 |
5/10✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
|
4 | const MatrixXd U_ref = Yaba_ref * data_ref.J.middleCols(jmodel.idx_v(), jmodel.nv()); |
| 1799 |
6/12✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
|
4 | const MatrixXd D_ref = data_ref.J.middleCols(jmodel.idx_v(), jmodel.nv()).transpose() * U_ref; |
| 1800 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
4 | const MatrixXd Dinv_ref = D_ref.inverse(); |
| 1801 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
4 | const MatrixXd UDinv_ref = U_ref * Dinv_ref; |
| 1802 |
8/16✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 2 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 2 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 2 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 2 times.
✗ Branch 25 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 2 times.
|
4 | BOOST_CHECK(jdata.U().isApprox(U_ref)); |
| 1803 |
8/16✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 2 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 2 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 2 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 2 times.
✗ Branch 25 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 2 times.
|
4 | BOOST_CHECK(jdata.StU().isApprox(D_ref)); |
| 1804 |
8/16✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 2 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 2 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 2 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 2 times.
✗ Branch 25 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 2 times.
|
4 | BOOST_CHECK(jdata.Dinv().isApprox(Dinv_ref)); |
| 1805 |
8/16✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 2 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 2 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 2 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 2 times.
✗ Branch 25 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 2 times.
|
4 | BOOST_CHECK(jdata.UDinv().isApprox(UDinv_ref)); |
| 1806 | |||
| 1807 |
3/6✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
|
4 | Yaba_ref -= UDinv_ref * U_ref.transpose(); |
| 1808 | |||
| 1809 |
7/14✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 2 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 2 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 2 times.
|
4 | BOOST_CHECK(data.oYaba[joint1_id].isApprox(Yaba_ref)); |
| 1810 | 4 | } | |
| 1811 | |||
| 1812 | // Call the algorithm a second time | ||
| 1813 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data data2(model); |
| 1814 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings2; |
| 1815 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contactABA(model, data2, q, v, tau, contact_models, contact_datas, prox_settings2); |
| 1816 | |||
| 1817 |
6/12✓ 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 22 not taken.
✓ Branch 23 taken 1 times.
|
2 | BOOST_CHECK(prox_settings2.iter == 0); |
| 1818 | 2 | } | |
| 1819 | |||
| 1820 |
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_contact_ABA_3D) |
| 1821 | { | ||
| 1822 | using namespace Eigen; | ||
| 1823 | using namespace pinocchio; | ||
| 1824 | |||
| 1825 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | pinocchio::Model model; |
| 1826 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | pinocchio::buildModels::humanoidRandom(model, true); |
| 1827 |
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); |
| 1828 | |||
| 1829 |
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.); |
| 1830 |
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.); |
| 1831 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 1832 | |||
| 1833 |
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); |
| 1834 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd tau = VectorXd::Random(model.nv); |
| 1835 | |||
| 1836 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RF = "rleg6_joint"; |
| 1837 | // const Model::JointIndex RF_id = model.getJointId(RF); | ||
| 1838 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string LF = "lleg6_joint"; |
| 1839 | // const Model::JointIndex LF_id = model.getJointId(LF); | ||
| 1840 | |||
| 1841 | // Contact models and data | ||
| 1842 | typedef PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) | ||
| 1843 | RigidConstraintModelVector; | ||
| 1844 | typedef PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) RigidConstraintDataVector; | ||
| 1845 | |||
| 1846 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintModelVector contact_models; |
| 1847 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintDataVector contact_datas; |
| 1848 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | RigidConstraintModel ci_RF(CONTACT_3D, model, model.getJointId(RF), LOCAL); |
| 1849 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_models.push_back(ci_RF); |
| 1850 |
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)); |
| 1851 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | RigidConstraintModel ci_LF(CONTACT_3D, model, model.getJointId(LF), LOCAL); |
| 1852 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_models.push_back(ci_LF); |
| 1853 |
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_LF)); |
| 1854 | |||
| 1855 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintDataVector contact_datas_ref(contact_datas); |
| 1856 | |||
| 1857 | 2 | Eigen::DenseIndex constraint_dim = 0; | |
| 1858 |
2/2✓ Branch 1 taken 2 times.
✓ Branch 2 taken 1 times.
|
6 | for (size_t k = 0; k < contact_models.size(); ++k) |
| 1859 | 4 | constraint_dim += contact_models[k].size(); | |
| 1860 | |||
| 1861 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Eigen::MatrixXd J_ref(constraint_dim, model.nv); |
| 1862 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_ref.setZero(); |
| 1863 | |||
| 1864 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings_cd(1e-12, 0, 1); |
| 1865 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data_ref, contact_models); |
| 1866 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraintDynamics( |
| 1867 | model, data_ref, q, v, tau, contact_models, contact_datas_ref, prox_settings_cd); | ||
| 1868 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | forwardKinematics(model, data_ref, q, v, v * 0); |
| 1869 | |||
| 1870 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Data::Matrix6x Jtmp = Data::Matrix6x::Zero(6, model.nv); |
| 1871 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getJointJacobian(model, data_ref, ci_RF.joint1_id, ci_RF.reference_frame, Jtmp); |
| 1872 |
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 | J_ref.middleRows<3>(0) = Jtmp.middleRows<3>(Motion::LINEAR); |
| 1873 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Jtmp.setZero(); |
| 1874 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getJointJacobian(model, data_ref, ci_LF.joint1_id, ci_LF.reference_frame, Jtmp); |
| 1875 |
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 | J_ref.middleRows<3>(3) = Jtmp.middleRows<3>(Motion::LINEAR); |
| 1876 | |||
| 1877 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Eigen::VectorXd gamma(constraint_dim); |
| 1878 | |||
| 1879 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | gamma.segment<3>(0) = |
| 1880 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | computeAcceleration(model, data_ref, ci_RF.joint1_id, ci_RF.reference_frame, ci_RF.type) |
| 1881 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
4 | .linear(); |
| 1882 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | gamma.segment<3>(3) = |
| 1883 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | computeAcceleration(model, data_ref, ci_LF.joint1_id, ci_LF.reference_frame, ci_LF.type) |
| 1884 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
4 | .linear(); |
| 1885 | |||
| 1886 |
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 + gamma).isZero()); |
| 1887 | |||
| 1888 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data data_constrained_dyn(model); |
| 1889 | |||
| 1890 | PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH | ||
| 1891 | PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS | ||
| 1892 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | forwardDynamics(model, data_constrained_dyn, q, v, tau, J_ref, gamma, 0.); |
| 1893 | PINOCCHIO_COMPILER_DIAGNOSTIC_POP | ||
| 1894 | |||
| 1895 |
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_constrained_dyn.ddq + gamma).isZero()); |
| 1896 | |||
| 1897 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings; |
| 1898 | 2 | prox_settings.max_iter = 10; | |
| 1899 | 2 | prox_settings.mu = 1e8; | |
| 1900 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contactABA(model, data, q, v, tau, contact_models, contact_datas, prox_settings); |
| 1901 | |||
| 1902 |
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 + gamma).isZero()); |
| 1903 | |||
| 1904 | // Call the algorithm a second time | ||
| 1905 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data data2(model); |
| 1906 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings2; |
| 1907 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contactABA(model, data2, q, v, tau, contact_models, contact_datas, prox_settings2); |
| 1908 | |||
| 1909 |
6/12✓ 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 22 not taken.
✓ Branch 23 taken 1 times.
|
2 | BOOST_CHECK(prox_settings2.iter == 0); |
| 1910 | 2 | } | |
| 1911 | |||
| 1912 | BOOST_AUTO_TEST_SUITE_END() | ||
| 1913 |