| Directory: | ./ |
|---|---|
| File: | unittest/contact-models.cpp |
| Date: | 2025-02-12 21:03:38 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 225 | 225 | 100.0% |
| Branches: | 655 | 1292 | 50.7% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2019-2021 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/contact-jacobian.hpp" | ||
| 13 | #include "pinocchio/algorithm/joint-configuration.hpp" | ||
| 14 | #include "pinocchio/multibody/sample-models.hpp" | ||
| 15 | #include "pinocchio/utils/timer.hpp" | ||
| 16 | #include "pinocchio/spatial/classic-acceleration.hpp" | ||
| 17 | |||
| 18 | #include <iostream> | ||
| 19 | |||
| 20 | #include <boost/test/unit_test.hpp> | ||
| 21 | #include <boost/utility/binary.hpp> | ||
| 22 | |||
| 23 | using namespace pinocchio; | ||
| 24 | using namespace Eigen; | ||
| 25 | |||
| 26 | template<typename T> | ||
| 27 | 128 | bool within(const T & elt, const std::vector<T> & vec) | |
| 28 | { | ||
| 29 | 128 | typename std::vector<T>::const_iterator it; | |
| 30 | |||
| 31 |
1/2✓ Branch 3 taken 128 times.
✗ Branch 4 not taken.
|
128 | it = std::find(vec.begin(), vec.end(), elt); |
| 32 |
2/2✓ Branch 2 taken 60 times.
✓ Branch 3 taken 68 times.
|
128 | if (it != vec.end()) |
| 33 | 60 | return true; | |
| 34 | else | ||
| 35 | 68 | return false; | |
| 36 | } | ||
| 37 | |||
| 38 | template<typename Matrix> | ||
| 39 | bool within(const typename Matrix::Scalar & elt, const Eigen::MatrixBase<Matrix> & mat) | ||
| 40 | { | ||
| 41 | for (DenseIndex i = 0; i < mat.rows(); ++i) | ||
| 42 | for (DenseIndex j = 0; j < mat.rows(); ++j) | ||
| 43 | { | ||
| 44 | if (elt == mat(i, j)) | ||
| 45 | return true; | ||
| 46 | } | ||
| 47 | |||
| 48 | return false; | ||
| 49 | } | ||
| 50 | |||
| 51 | BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) | ||
| 52 | |||
| 53 |
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(contact_models) |
| 54 | { | ||
| 55 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | pinocchio::Model model; |
| 56 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | pinocchio::buildModels::humanoidRandom(model, true); |
| 57 | |||
| 58 | // Check complete constructor | ||
| 59 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const SE3 M(SE3::Random()); |
| 60 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintModel cmodel2(CONTACT_3D, model, 0, M); |
| 61 |
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(cmodel2.type == CONTACT_3D); |
| 62 |
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(cmodel2.joint1_id == 0); |
| 63 |
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(cmodel2.joint1_placement.isApprox(M)); |
| 64 |
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 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK(cmodel2.size() == 3); |
| 65 | |||
| 66 | // Check contructor with two arguments | ||
| 67 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintModel cmodel2prime(CONTACT_3D, model, 0); |
| 68 |
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(cmodel2prime.type == CONTACT_3D); |
| 69 |
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(cmodel2prime.joint1_id == 0); |
| 70 |
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(cmodel2prime.joint1_placement.isIdentity()); |
| 71 |
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 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK(cmodel2prime.size() == 3); |
| 72 | |||
| 73 | // Check default copy constructor | ||
| 74 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintModel cmodel3(cmodel2); |
| 75 |
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(cmodel3 == cmodel2); |
| 76 | |||
| 77 | // Check complete constructor 6D | ||
| 78 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintModel cmodel4(CONTACT_6D, model, 0); |
| 79 |
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(cmodel4.type == CONTACT_6D); |
| 80 |
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(cmodel4.joint1_id == 0); |
| 81 |
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(cmodel4.joint1_placement.isIdentity()); |
| 82 |
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 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK(cmodel4.size() == 6); |
| 83 | 2 | } | |
| 84 | |||
| 85 | 3 | void check_A1_and_A2( | |
| 86 | const Model & model, | ||
| 87 | const Data & data, | ||
| 88 | const RigidConstraintModel & cmodel, | ||
| 89 | RigidConstraintData & cdata) | ||
| 90 | { | ||
| 91 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | const RigidConstraintModel::Matrix36 A1 = cmodel.getA1(cdata); |
| 92 |
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.
|
3 | const RigidConstraintModel::Matrix36 A1_ref = cdata.oMc1.toActionMatrixInverse().topRows<3>(); |
| 93 | |||
| 94 |
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.
|
3 | BOOST_CHECK(A1.isApprox(A1_ref)); |
| 95 | |||
| 96 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | const RigidConstraintModel::Matrix36 A2 = cmodel.getA2(cdata); |
| 97 | const RigidConstraintModel::Matrix36 A2_ref = | ||
| 98 |
6/12✓ 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.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
|
3 | -cdata.c1Mc2.rotation() * cdata.oMc2.toActionMatrixInverse().topRows<3>(); |
| 99 | |||
| 100 |
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.
|
3 | BOOST_CHECK(A2.isApprox(A2_ref)); |
| 101 | |||
| 102 | // Check Jacobian | ||
| 103 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | Data::MatrixXs J_ref(3, model.nv); |
| 104 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | J_ref.setZero(); |
| 105 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | getConstraintJacobian(model, data, cmodel, cdata, J_ref); |
| 106 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | const Data::Matrix6x J1 = getJointJacobian(model, data, cmodel.joint1_id, WORLD); |
| 107 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | const Data::Matrix6x J2 = getJointJacobian(model, data, cmodel.joint2_id, WORLD); |
| 108 |
4/8✓ 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.
|
3 | const Data::Matrix3x J = A1 * J1 + A2 * J2; |
| 109 | |||
| 110 |
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.
|
3 | BOOST_CHECK(J.isApprox(J_ref)); |
| 111 | |||
| 112 | // Check Jacobian matrix product | ||
| 113 | 3 | const Eigen::DenseIndex m = 40; | |
| 114 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
3 | const Data::MatrixXs mat = Data::MatrixXs::Random(model.nv, m); |
| 115 | |||
| 116 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
3 | Data::MatrixXs res(cmodel.size(), m); |
| 117 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | res.setZero(); |
| 118 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | cmodel.jacobian_matrix_product(model, data, cdata, mat, res); |
| 119 | |||
| 120 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
3 | const Data::MatrixXs res_ref = J_ref * mat; |
| 121 | |||
| 122 |
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.
|
3 | BOOST_CHECK(res.isApprox(res_ref)); |
| 123 | 3 | } | |
| 124 | |||
| 125 |
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(contact_models_sparsity_and_jacobians) |
| 126 | { | ||
| 127 | |||
| 128 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | pinocchio::Model model; |
| 129 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | pinocchio::buildModels::humanoidRandom(model, true); |
| 130 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data data(model); |
| 131 | |||
| 132 |
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.); |
| 133 |
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.); |
| 134 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 135 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeJointJacobians(model, data, q); |
| 136 | |||
| 137 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RF = "rleg6_joint"; |
| 138 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string LF = "lleg6_joint"; |
| 139 | |||
| 140 | // 6D - LOCAL | ||
| 141 | { | ||
| 142 |
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 | RigidConstraintModel cm_RF_LOCAL(CONTACT_6D, model, model.getJointId(RF), SE3::Random(), LOCAL); |
| 143 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintData cd_RF_LOCAL(cm_RF_LOCAL); |
| 144 |
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 | RigidConstraintModel cm_LF_LOCAL(CONTACT_6D, model, model.getJointId(LF), SE3::Random(), LOCAL); |
| 145 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintData cd_LF_LOCAL(cm_LF_LOCAL); |
| 146 | RigidConstraintModel clm_RF_LF_LOCAL( | ||
| 147 | CONTACT_6D, model, cm_RF_LOCAL.joint1_id, cm_RF_LOCAL.joint1_placement, cm_LF_LOCAL.joint1_id, | ||
| 148 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | cm_LF_LOCAL.joint1_placement, LOCAL); |
| 149 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintData cld_RF_LF_LOCAL(clm_RF_LF_LOCAL); |
| 150 | |||
| 151 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data::Matrix6x J_RF_LOCAL(6, model.nv); |
| 152 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_RF_LOCAL.setZero(); |
| 153 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getFrameJacobian( |
| 154 | model, data, cm_RF_LOCAL.joint1_id, cm_RF_LOCAL.joint1_placement, cm_RF_LOCAL.reference_frame, | ||
| 155 | J_RF_LOCAL); | ||
| 156 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data::Matrix6x J_LF_LOCAL(6, model.nv); |
| 157 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_LF_LOCAL.setZero(); |
| 158 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getFrameJacobian( |
| 159 | model, data, cm_LF_LOCAL.joint1_id, cm_LF_LOCAL.joint1_placement, cm_LF_LOCAL.reference_frame, | ||
| 160 | J_LF_LOCAL); | ||
| 161 | |||
| 162 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (DenseIndex k = 0; k < model.nv; ++k) |
| 163 | { | ||
| 164 |
9/18✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 32 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 32 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 32 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 32 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 32 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 32 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 32 times.
|
64 | BOOST_CHECK(J_RF_LOCAL.col(k).isZero() != cm_RF_LOCAL.colwise_joint1_sparsity[k]); |
| 165 |
9/18✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 32 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 32 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 32 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 32 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 32 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 32 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 32 times.
|
64 | BOOST_CHECK(J_LF_LOCAL.col(k).isZero() != cm_LF_LOCAL.colwise_joint1_sparsity[k]); |
| 166 | } | ||
| 167 |
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(cm_RF_LOCAL.colwise_joint2_sparsity.isZero()); |
| 168 |
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(cm_LF_LOCAL.colwise_joint2_sparsity.isZero()); |
| 169 | |||
| 170 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const SE3 oMc1 = data.oMi[clm_RF_LF_LOCAL.joint1_id] * clm_RF_LF_LOCAL.joint1_placement; |
| 171 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const SE3 oMc2 = data.oMi[clm_RF_LF_LOCAL.joint2_id] * clm_RF_LF_LOCAL.joint2_placement; |
| 172 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const SE3 c1Mc2 = oMc1.actInv(oMc2); |
| 173 |
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 Data::Matrix6x J_clm_LOCAL = J_RF_LOCAL - c1Mc2.toActionMatrix() * J_LF_LOCAL; |
| 174 | |||
| 175 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (DenseIndex k = 0; k < model.nv; ++k) |
| 176 | { | ||
| 177 |
9/18✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 32 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 32 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 32 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 32 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 32 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 32 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 32 times.
|
64 | BOOST_CHECK(J_clm_LOCAL.col(k).isZero() != within(k, clm_RF_LF_LOCAL.colwise_span_indexes)); |
| 178 | } | ||
| 179 | |||
| 180 | // Check Jacobian | ||
| 181 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data::MatrixXs J_RF_LOCAL_sparse(6, model.nv); |
| 182 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_RF_LOCAL_sparse.setZero(); // TODO: change input type when all the API would be refactorized |
| 183 | // with CRTP on contact constraints | ||
| 184 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getConstraintJacobian(model, data, cm_RF_LOCAL, cd_RF_LOCAL, J_RF_LOCAL_sparse); |
| 185 |
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(J_RF_LOCAL.isApprox(J_RF_LOCAL_sparse)); |
| 186 | |||
| 187 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data::MatrixXs J_LF_LOCAL_sparse(6, model.nv); |
| 188 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_LF_LOCAL_sparse.setZero(); // TODO: change input type when all the API would be refactorized |
| 189 | // with CRTP on contact constraints | ||
| 190 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getConstraintJacobian(model, data, cm_LF_LOCAL, cd_LF_LOCAL, J_LF_LOCAL_sparse); |
| 191 |
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(J_LF_LOCAL.isApprox(J_LF_LOCAL_sparse)); |
| 192 | |||
| 193 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data::MatrixXs J_clm_LOCAL_sparse(6, model.nv); |
| 194 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_clm_LOCAL_sparse.setZero(); // TODO: change input type when all the API would be refactorized |
| 195 | // with CRTP on contact constraints | ||
| 196 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getConstraintJacobian(model, data, clm_RF_LF_LOCAL, cld_RF_LF_LOCAL, J_clm_LOCAL_sparse); |
| 197 |
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(J_clm_LOCAL.isApprox(J_clm_LOCAL_sparse)); |
| 198 | 2 | } | |
| 199 | |||
| 200 | // 6D - LOCAL_WORLD_ALIGNED | ||
| 201 | { | ||
| 202 | RigidConstraintModel cm_RF_LWA( | ||
| 203 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | CONTACT_6D, model, model.getJointId(RF), SE3::Random(), LOCAL_WORLD_ALIGNED); |
| 204 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintData cd_RF_LWA(cm_RF_LWA); |
| 205 | RigidConstraintModel cm_LF_LWA( | ||
| 206 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | CONTACT_6D, model, model.getJointId(LF), SE3::Random(), LOCAL_WORLD_ALIGNED); |
| 207 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintData cd_LF_LWA(cm_LF_LWA); |
| 208 | RigidConstraintModel clm_RF_LF_LWA( | ||
| 209 | CONTACT_6D, model, cm_RF_LWA.joint1_id, cm_RF_LWA.joint1_placement, cm_LF_LWA.joint1_id, | ||
| 210 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | cm_LF_LWA.joint1_placement, LOCAL_WORLD_ALIGNED); |
| 211 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintData cld_RF_LF_LWA(clm_RF_LF_LWA); |
| 212 | |||
| 213 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data::Matrix6x J_RF_LOCAL(6, model.nv); |
| 214 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_RF_LOCAL.setZero(); |
| 215 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getFrameJacobian( |
| 216 | model, data, cm_RF_LWA.joint1_id, cm_RF_LWA.joint1_placement, LOCAL, J_RF_LOCAL); | ||
| 217 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data::Matrix6x J_LF_LOCAL(6, model.nv); |
| 218 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_LF_LOCAL.setZero(); |
| 219 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getFrameJacobian( |
| 220 | model, data, cm_LF_LWA.joint1_id, cm_LF_LWA.joint1_placement, LOCAL, J_LF_LOCAL); | ||
| 221 | |||
| 222 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data::Matrix6x J_RF_LWA(6, model.nv); |
| 223 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_RF_LWA.setZero(); |
| 224 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getFrameJacobian( |
| 225 | model, data, cm_RF_LWA.joint1_id, cm_RF_LWA.joint1_placement, LOCAL_WORLD_ALIGNED, J_RF_LWA); | ||
| 226 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data::Matrix6x J_LF_LWA(6, model.nv); |
| 227 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_LF_LWA.setZero(); |
| 228 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getFrameJacobian( |
| 229 | model, data, cm_LF_LWA.joint1_id, cm_LF_LWA.joint1_placement, LOCAL_WORLD_ALIGNED, J_LF_LWA); | ||
| 230 | |||
| 231 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (DenseIndex k = 0; k < model.nv; ++k) |
| 232 | { | ||
| 233 |
9/18✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 32 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 32 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 32 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 32 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 32 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 32 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 32 times.
|
64 | BOOST_CHECK(J_RF_LWA.col(k).isZero() != cm_RF_LWA.colwise_joint1_sparsity[k]); |
| 234 |
9/18✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 32 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 32 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 32 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 32 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 32 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 32 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 32 times.
|
64 | BOOST_CHECK(J_LF_LWA.col(k).isZero() != cm_LF_LWA.colwise_joint1_sparsity[k]); |
| 235 | } | ||
| 236 |
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(cm_RF_LWA.colwise_joint2_sparsity.isZero()); |
| 237 |
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(cm_LF_LWA.colwise_joint2_sparsity.isZero()); |
| 238 | |||
| 239 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const SE3 oMc1 = data.oMi[clm_RF_LF_LWA.joint1_id] * clm_RF_LF_LWA.joint1_placement; |
| 240 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const SE3 oMc2 = data.oMi[clm_RF_LF_LWA.joint2_id] * clm_RF_LF_LWA.joint2_placement; |
| 241 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const SE3 c1Mc2 = oMc1.actInv(oMc2); |
| 242 |
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 SE3 oMc1_lwa = SE3(oMc1.rotation(), SE3::Vector3::Zero()); |
| 243 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const SE3 oMc2_lwa = oMc1_lwa * c1Mc2; |
| 244 | const Data::Matrix6x J_clm_LWA = | ||
| 245 |
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 | oMc1_lwa.toActionMatrix() * J_RF_LOCAL - oMc2_lwa.toActionMatrix() * J_LF_LOCAL; |
| 246 | |||
| 247 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (DenseIndex k = 0; k < model.nv; ++k) |
| 248 | { | ||
| 249 |
9/18✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 32 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 32 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 32 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 32 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 32 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 32 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 32 times.
|
64 | BOOST_CHECK(J_clm_LWA.col(k).isZero() != within(k, clm_RF_LF_LWA.colwise_span_indexes)); |
| 250 | } | ||
| 251 | |||
| 252 | // Check Jacobian | ||
| 253 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data::MatrixXs J_RF_LWA_sparse(6, model.nv); |
| 254 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_RF_LWA_sparse.setZero(); // TODO: change input type when all the API would be refactorized |
| 255 | // with CRTP on contact constraints | ||
| 256 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getConstraintJacobian(model, data, cm_RF_LWA, cd_RF_LWA, J_RF_LWA_sparse); |
| 257 |
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(J_RF_LWA.isApprox(J_RF_LWA_sparse)); |
| 258 | |||
| 259 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data::MatrixXs J_LF_LWA_sparse(6, model.nv); |
| 260 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_LF_LWA_sparse.setZero(); // TODO: change input type when all the API would be refactorized |
| 261 | // with CRTP on contact constraints | ||
| 262 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getConstraintJacobian(model, data, cm_LF_LWA, cd_LF_LWA, J_LF_LWA_sparse); |
| 263 |
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(J_LF_LWA.isApprox(J_LF_LWA_sparse)); |
| 264 | |||
| 265 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data::MatrixXs J_clm_LWA_sparse(6, model.nv); |
| 266 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_clm_LWA_sparse.setZero(); // TODO: change input type when all the API would be refactorized |
| 267 | // with CRTP on contact constraints | ||
| 268 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getConstraintJacobian(model, data, clm_RF_LF_LWA, cld_RF_LF_LWA, J_clm_LWA_sparse); |
| 269 |
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(J_clm_LWA.isApprox(J_clm_LWA_sparse)); |
| 270 | 2 | } | |
| 271 | |||
| 272 | // 3D - LOCAL | ||
| 273 | { | ||
| 274 |
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 | RigidConstraintModel cm_RF_LOCAL(CONTACT_3D, model, model.getJointId(RF), SE3::Random(), LOCAL); |
| 275 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintData cd_RF_LOCAL(cm_RF_LOCAL); |
| 276 |
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 | RigidConstraintModel cm_LF_LOCAL(CONTACT_3D, model, model.getJointId(LF), SE3::Random(), LOCAL); |
| 277 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintData cd_LF_LOCAL(cm_LF_LOCAL); |
| 278 | RigidConstraintModel clm_RF_LF_LOCAL( | ||
| 279 | CONTACT_3D, model, cm_RF_LOCAL.joint1_id, cm_RF_LOCAL.joint1_placement, cm_LF_LOCAL.joint1_id, | ||
| 280 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | cm_LF_LOCAL.joint1_placement, LOCAL); |
| 281 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintData cld_RF_LF_LOCAL(clm_RF_LF_LOCAL); |
| 282 | |||
| 283 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data::Matrix6x J_RF_LOCAL(6, model.nv); |
| 284 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_RF_LOCAL.setZero(); |
| 285 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getFrameJacobian( |
| 286 | model, data, cm_RF_LOCAL.joint1_id, cm_RF_LOCAL.joint1_placement, cm_RF_LOCAL.reference_frame, | ||
| 287 | J_RF_LOCAL); | ||
| 288 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data::Matrix6x J_LF_LOCAL(6, model.nv); |
| 289 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_LF_LOCAL.setZero(); |
| 290 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getFrameJacobian( |
| 291 | model, data, cm_LF_LOCAL.joint1_id, cm_LF_LOCAL.joint1_placement, cm_LF_LOCAL.reference_frame, | ||
| 292 | J_LF_LOCAL); | ||
| 293 | |||
| 294 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (DenseIndex k = 0; k < model.nv; ++k) |
| 295 | { | ||
| 296 |
10/20✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 32 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 32 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 32 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 32 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 32 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 32 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 32 times.
✗ Branch 31 not taken.
✗ Branch 35 not taken.
✓ Branch 36 taken 32 times.
|
64 | BOOST_CHECK( |
| 297 | J_RF_LOCAL.middleRows<3>(SE3::LINEAR).col(k).isZero() | ||
| 298 | != cm_RF_LOCAL.colwise_joint1_sparsity[k]); | ||
| 299 |
10/20✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 32 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 32 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 32 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 32 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 32 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 32 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 32 times.
✗ Branch 31 not taken.
✗ Branch 35 not taken.
✓ Branch 36 taken 32 times.
|
64 | BOOST_CHECK( |
| 300 | J_LF_LOCAL.middleRows<3>(SE3::LINEAR).col(k).isZero() | ||
| 301 | != cm_LF_LOCAL.colwise_joint1_sparsity[k]); | ||
| 302 | } | ||
| 303 |
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(cm_RF_LOCAL.colwise_joint2_sparsity.isZero()); |
| 304 |
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(cm_LF_LOCAL.colwise_joint2_sparsity.isZero()); |
| 305 | |||
| 306 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const SE3 oMc1 = data.oMi[clm_RF_LF_LOCAL.joint1_id] * clm_RF_LF_LOCAL.joint1_placement; |
| 307 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const SE3 oMc2 = data.oMi[clm_RF_LF_LOCAL.joint2_id] * clm_RF_LF_LOCAL.joint2_placement; |
| 308 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const SE3 c1Mc2 = oMc1.actInv(oMc2); |
| 309 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Data::Matrix3x J_clm_LOCAL = J_RF_LOCAL.middleRows<3>(SE3::LINEAR) |
| 310 |
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 | - c1Mc2.rotation() * J_LF_LOCAL.middleRows<3>(SE3::LINEAR); |
| 311 | |||
| 312 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (DenseIndex k = 0; k < model.nv; ++k) |
| 313 | { | ||
| 314 |
9/18✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 32 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 32 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 32 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 32 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 32 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 32 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 32 times.
|
64 | BOOST_CHECK(J_clm_LOCAL.col(k).isZero(0) != within(k, clm_RF_LF_LOCAL.colwise_span_indexes)); |
| 315 | } | ||
| 316 | |||
| 317 | // Check Jacobian | ||
| 318 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data::MatrixXs J_RF_LOCAL_sparse(3, model.nv); |
| 319 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_RF_LOCAL_sparse.setZero(); // TODO: change input type when all the API would be refactorized |
| 320 | // with CRTP on contact constraints | ||
| 321 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getConstraintJacobian(model, data, cm_RF_LOCAL, cd_RF_LOCAL, J_RF_LOCAL_sparse); |
| 322 |
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(J_RF_LOCAL.middleRows<3>(SE3::LINEAR).isApprox(J_RF_LOCAL_sparse)); |
| 323 | |||
| 324 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data::MatrixXs J_LF_LOCAL_sparse(3, model.nv); |
| 325 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_LF_LOCAL_sparse.setZero(); // TODO: change input type when all the API would be refactorized |
| 326 | // with CRTP on contact constraints | ||
| 327 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getConstraintJacobian(model, data, cm_LF_LOCAL, cd_LF_LOCAL, J_LF_LOCAL_sparse); |
| 328 |
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(J_LF_LOCAL.middleRows<3>(SE3::LINEAR).isApprox(J_LF_LOCAL_sparse)); |
| 329 | |||
| 330 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data::MatrixXs J_clm_LOCAL_sparse(3, model.nv); |
| 331 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_clm_LOCAL_sparse.setZero(); // TODO: change input type when all the API would be refactorized |
| 332 | // with CRTP on contact constraints | ||
| 333 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getConstraintJacobian(model, data, clm_RF_LF_LOCAL, cld_RF_LF_LOCAL, J_clm_LOCAL_sparse); |
| 334 |
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(J_clm_LOCAL.isApprox(J_clm_LOCAL_sparse)); |
| 335 | |||
| 336 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | check_A1_and_A2(model, data, cm_RF_LOCAL, cd_RF_LOCAL); |
| 337 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | check_A1_and_A2(model, data, cm_LF_LOCAL, cd_LF_LOCAL); |
| 338 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | check_A1_and_A2(model, data, clm_RF_LF_LOCAL, cld_RF_LF_LOCAL); |
| 339 | 2 | } | |
| 340 | |||
| 341 | // 3D - LOCAL_WORLD_ALIGNED | ||
| 342 | { | ||
| 343 | RigidConstraintModel cm_RF_LWA( | ||
| 344 |
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_3D, model, model.getJointId(RF), SE3::Random(), LOCAL_WORLD_ALIGNED); |
| 345 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintData cd_RF_LWA(cm_RF_LWA); |
| 346 | RigidConstraintModel cm_LF_LWA( | ||
| 347 |
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_3D, model, model.getJointId(LF), SE3::Random(), LOCAL_WORLD_ALIGNED); |
| 348 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintData cd_LF_LWA(cm_LF_LWA); |
| 349 | RigidConstraintModel clm_RF_LF_LWA( | ||
| 350 | CONTACT_3D, model, cm_RF_LWA.joint1_id, cm_RF_LWA.joint1_placement, cm_LF_LWA.joint1_id, | ||
| 351 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | cm_LF_LWA.joint1_placement, LOCAL_WORLD_ALIGNED); |
| 352 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintData cld_RF_LF_LWA(clm_RF_LF_LWA); |
| 353 | |||
| 354 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data::Matrix6x J_RF_LOCAL(6, model.nv); |
| 355 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_RF_LOCAL.setZero(); |
| 356 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getFrameJacobian( |
| 357 | model, data, cm_RF_LWA.joint1_id, cm_RF_LWA.joint1_placement, LOCAL, J_RF_LOCAL); | ||
| 358 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data::Matrix6x J_LF_LOCAL(6, model.nv); |
| 359 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_LF_LOCAL.setZero(); |
| 360 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getFrameJacobian( |
| 361 | model, data, cm_LF_LWA.joint1_id, cm_LF_LWA.joint1_placement, LOCAL, J_LF_LOCAL); | ||
| 362 | |||
| 363 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data::Matrix6x J_RF_LWA(6, model.nv); |
| 364 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_RF_LWA.setZero(); |
| 365 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getFrameJacobian( |
| 366 | model, data, cm_RF_LWA.joint1_id, cm_RF_LWA.joint1_placement, LOCAL_WORLD_ALIGNED, J_RF_LWA); | ||
| 367 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data::Matrix6x J_LF_LWA(6, model.nv); |
| 368 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_LF_LWA.setZero(); |
| 369 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getFrameJacobian( |
| 370 | model, data, cm_LF_LWA.joint1_id, cm_LF_LWA.joint1_placement, LOCAL_WORLD_ALIGNED, J_LF_LWA); | ||
| 371 | |||
| 372 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (DenseIndex k = 0; k < model.nv; ++k) |
| 373 | { | ||
| 374 |
10/20✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 32 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 32 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 32 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 32 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 32 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 32 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 32 times.
✗ Branch 31 not taken.
✗ Branch 35 not taken.
✓ Branch 36 taken 32 times.
|
64 | BOOST_CHECK( |
| 375 | J_RF_LWA.middleRows<3>(SE3::LINEAR).col(k).isZero() | ||
| 376 | != cm_RF_LWA.colwise_joint1_sparsity[k]); | ||
| 377 |
10/20✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 32 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 32 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 32 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 32 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 32 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 32 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 32 times.
✗ Branch 31 not taken.
✗ Branch 35 not taken.
✓ Branch 36 taken 32 times.
|
64 | BOOST_CHECK( |
| 378 | J_LF_LWA.middleRows<3>(SE3::LINEAR).col(k).isZero() | ||
| 379 | != cm_LF_LWA.colwise_joint1_sparsity[k]); | ||
| 380 | } | ||
| 381 |
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(cm_RF_LWA.colwise_joint2_sparsity.isZero()); |
| 382 |
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(cm_LF_LWA.colwise_joint2_sparsity.isZero()); |
| 383 | |||
| 384 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const SE3 oMc1 = data.oMi[clm_RF_LF_LWA.joint1_id] * clm_RF_LF_LWA.joint1_placement; |
| 385 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const SE3 oMc2 = data.oMi[clm_RF_LF_LWA.joint2_id] * clm_RF_LF_LWA.joint2_placement; |
| 386 |
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 SE3 oMc1_lwa = SE3(oMc1.rotation(), SE3::Vector3::Zero()); |
| 387 |
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 SE3 oMc2_lwa = SE3(oMc2.rotation(), SE3::Vector3::Zero()); |
| 388 | const Data::Matrix3x J_clm_LWA = | ||
| 389 |
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 | (oMc1_lwa.toActionMatrix() * J_RF_LOCAL - oMc2_lwa.toActionMatrix() * J_LF_LOCAL) |
| 390 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | .middleRows<3>(Motion::LINEAR); |
| 391 | |||
| 392 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (DenseIndex k = 0; k < model.nv; ++k) |
| 393 | { | ||
| 394 |
9/18✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 32 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 32 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 32 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 32 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 32 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 32 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 32 times.
|
64 | BOOST_CHECK(J_clm_LWA.col(k).isZero(0) != within(k, clm_RF_LF_LWA.colwise_span_indexes)); |
| 395 | } | ||
| 396 | |||
| 397 | // Check Jacobian | ||
| 398 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data::MatrixXs J_RF_LWA_sparse(3, model.nv); |
| 399 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_RF_LWA_sparse.setZero(); // TODO: change input type when all the API would be refactorized |
| 400 | // with CRTP on contact constraints | ||
| 401 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getConstraintJacobian(model, data, cm_RF_LWA, cd_RF_LWA, J_RF_LWA_sparse); |
| 402 |
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(J_RF_LWA.middleRows<3>(SE3::LINEAR).isApprox(J_RF_LWA_sparse)); |
| 403 | |||
| 404 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data::MatrixXs J_LF_LWA_sparse(3, model.nv); |
| 405 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_LF_LWA_sparse.setZero(); // TODO: change input type when all the API would be refactorized |
| 406 | // with CRTP on contact constraints | ||
| 407 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getConstraintJacobian(model, data, cm_LF_LWA, cd_LF_LWA, J_LF_LWA_sparse); |
| 408 |
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(J_LF_LWA.middleRows<3>(SE3::LINEAR).isApprox(J_LF_LWA_sparse)); |
| 409 | |||
| 410 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data::MatrixXs J_clm_LWA_sparse(3, model.nv); |
| 411 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | J_clm_LWA_sparse.setZero(); // TODO: change input type when all the API would be refactorized |
| 412 | // with CRTP on contact constraints | ||
| 413 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getConstraintJacobian(model, data, clm_RF_LF_LWA, cld_RF_LF_LWA, J_clm_LWA_sparse); |
| 414 |
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(J_clm_LWA.isApprox(J_clm_LWA_sparse)); |
| 415 | 2 | } | |
| 416 | 2 | } | |
| 417 | |||
| 418 | BOOST_AUTO_TEST_SUITE_END() | ||
| 419 |