Line |
Branch |
Exec |
Source |
1 |
|
|
// |
2 |
|
|
// Copyright (c) 2023 INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#include "pinocchio/multibody/data.hpp" |
6 |
|
|
#include "pinocchio/algorithm/constraints/constraints.hpp" |
7 |
|
|
#include "pinocchio/algorithm/constraints/constraint-model-generic.hpp" |
8 |
|
|
#include "pinocchio/algorithm/constraints/constraint-data-generic.hpp" |
9 |
|
|
#include "pinocchio/algorithm/contact-info.hpp" |
10 |
|
|
#include "pinocchio/multibody/sample-models.hpp" |
11 |
|
|
|
12 |
|
|
#include <iostream> |
13 |
|
|
|
14 |
|
|
#include <boost/test/unit_test.hpp> |
15 |
|
|
#include <boost/utility/binary.hpp> |
16 |
|
|
|
17 |
|
|
using namespace pinocchio; |
18 |
|
|
using namespace Eigen; |
19 |
|
|
|
20 |
|
|
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) |
21 |
|
|
|
22 |
|
✗ |
BOOST_AUTO_TEST_CASE(contact_variants) |
23 |
|
|
{ |
24 |
|
✗ |
Model model; |
25 |
|
✗ |
buildModels::humanoidRandom(model, true); |
26 |
|
|
|
27 |
|
✗ |
Data data(model); |
28 |
|
|
|
29 |
|
✗ |
const SE3 M(SE3::Random()); |
30 |
|
✗ |
RigidConstraintModel rcm(CONTACT_3D, model, 0, M); |
31 |
|
✗ |
RigidConstraintData rcd(rcm); |
32 |
|
|
|
33 |
|
✗ |
ConstraintModel::ConstraintModelVariant constraint_model_variant = rcm; |
34 |
|
✗ |
ConstraintModel constraint_model(rcm); |
35 |
|
✗ |
ConstraintModel constraint_model_equal = rcm; |
36 |
|
|
|
37 |
|
✗ |
ConstraintData constraint_data = rcm.createData(); |
38 |
|
|
} |
39 |
|
|
|
40 |
|
✗ |
BOOST_AUTO_TEST_CASE(contact_visitors) |
41 |
|
|
{ |
42 |
|
✗ |
Model model; |
43 |
|
✗ |
buildModels::humanoidRandom(model, true); |
44 |
|
|
|
45 |
|
✗ |
Data data(model); |
46 |
|
|
|
47 |
|
✗ |
const SE3 M(SE3::Random()); |
48 |
|
✗ |
RigidConstraintModel rcm(CONTACT_3D, model, 0, M); |
49 |
|
✗ |
RigidConstraintData rcd(rcm); |
50 |
|
✗ |
BOOST_CHECK(ConstraintData(rcd) == ConstraintData(rcd)); |
51 |
|
✗ |
BOOST_CHECK(ConstraintData(rcd) == rcd); |
52 |
|
|
|
53 |
|
✗ |
ConstraintModel constraint_model(rcm); |
54 |
|
|
|
55 |
|
|
// Test create data visitor |
56 |
|
✗ |
ConstraintData constraint_data = createData(constraint_model); |
57 |
|
✗ |
constraint_data = rcd; |
58 |
|
|
|
59 |
|
|
// Test calc visitor |
60 |
|
✗ |
calc(constraint_model, constraint_data, model, data); |
61 |
|
✗ |
rcm.calc(model, data, rcd); |
62 |
|
✗ |
BOOST_CHECK(rcd == constraint_data); |
63 |
|
|
|
64 |
|
|
// Test jacobian visitor |
65 |
|
✗ |
Data::MatrixXs jacobian_matrix = Data::Matrix6x::Zero(6, model.nv), |
66 |
|
✗ |
jacobian_matrix_ref = Data::Matrix6x::Zero(6, model.nv); |
67 |
|
✗ |
jacobian(constraint_model, constraint_data, model, data, jacobian_matrix); |
68 |
|
✗ |
rcm.jacobian(model, data, rcd, jacobian_matrix_ref); |
69 |
|
✗ |
BOOST_CHECK(jacobian_matrix == jacobian_matrix_ref); |
70 |
|
|
} |
71 |
|
|
|
72 |
|
|
BOOST_AUTO_TEST_SUITE_END() |
73 |
|
|
|