GCC Code Coverage Report

Directory: ./
File: unittest/factory/contact.cpp
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 129 0.0%
Branches: 0 145 0.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2023, University of Edinburgh, Heriot-Watt University
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
7 ///////////////////////////////////////////////////////////////////////////////
8
9 #include "contact.hpp"
10
11 #include "crocoddyl/multibody/contacts/contact-1d.hpp"
12 #include "crocoddyl/multibody/contacts/contact-2d.hpp"
13 #include "crocoddyl/multibody/contacts/contact-3d.hpp"
14 #include "crocoddyl/multibody/contacts/contact-6d.hpp"
15
16 namespace crocoddyl {
17 namespace unittest {
18
19 const std::vector<ContactModelTypes::Type> ContactModelTypes::all(
20 ContactModelTypes::init_all());
21
22 ✗ std::ostream& operator<<(std::ostream& os,
23 const ContactModelTypes::Type& type) {
24 ✗ switch (type) {
25 ✗ case ContactModelTypes::ContactModel1D_LOCAL:
26 ✗ os << "ContactModel1D_LOCAL";
27 ✗ break;
28 ✗ case ContactModelTypes::ContactModel1D_WORLD:
29 ✗ os << "ContactModel1D_WORLD";
30 ✗ break;
31 ✗ case ContactModelTypes::ContactModel1D_LWA:
32 ✗ os << "ContactModel1D_LWA";
33 ✗ break;
34 ✗ case ContactModelTypes::ContactModel2D:
35 ✗ os << "ContactModel2D";
36 ✗ break;
37 ✗ case ContactModelTypes::ContactModel3D_LOCAL:
38 ✗ os << "ContactModel3D_LOCAL";
39 ✗ break;
40 ✗ case ContactModelTypes::ContactModel3D_WORLD:
41 ✗ os << "ContactModel3D_WORLD";
42 ✗ break;
43 ✗ case ContactModelTypes::ContactModel3D_LWA:
44 ✗ os << "ContactModel3D_LWA";
45 ✗ break;
46 ✗ case ContactModelTypes::ContactModel6D_LOCAL:
47 ✗ os << "ContactModel6D_LOCAL";
48 ✗ break;
49 ✗ case ContactModelTypes::ContactModel6D_WORLD:
50 ✗ os << "ContactModel6D_WORLD";
51 ✗ break;
52 ✗ case ContactModelTypes::ContactModel6D_LWA:
53 ✗ os << "ContactModel6D_LWA";
54 ✗ break;
55 ✗ case ContactModelTypes::NbContactModelTypes:
56 ✗ os << "NbContactModelTypes";
57 ✗ break;
58 ✗ default:
59 ✗ os << "Unknown type";
60 ✗ break;
61 }
62 ✗ return os;
63 }
64
65 ✗ ContactModelFactory::ContactModelFactory() {}
66 ✗ ContactModelFactory::~ContactModelFactory() {}
67
68 ✗ std::shared_ptr<crocoddyl::ContactModelAbstract> ContactModelFactory::create(
69 ContactModelTypes::Type contact_type, PinocchioModelTypes::Type model_type,
70 Eigen::Vector2d gains, const std::string frame_name, std::size_t nu) const {
71 ✗ PinocchioModelFactory model_factory(model_type);
72 std::shared_ptr<crocoddyl::StateMultibody> state =
73 ✗ std::make_shared<crocoddyl::StateMultibody>(model_factory.create());
74 ✗ std::shared_ptr<crocoddyl::ContactModelAbstract> contact;
75 ✗ std::size_t frame_id = 0;
76 ✗ if (frame_name == "") {
77 ✗ frame_id = model_factory.get_frame_ids()[0];
78 } else {
79 ✗ frame_id = state->get_pinocchio()->getFrameId(frame_name);
80 }
81 ✗ if (nu == std::numeric_limits<std::size_t>::max()) {
82 ✗ nu = state->get_nv();
83 }
84 ✗ switch (contact_type) {
85 ✗ case ContactModelTypes::ContactModel1D_LOCAL: {
86 ✗ pinocchio::SE3 M = pinocchio::SE3::Random();
87 ✗ gains[0] =
88 0; // TODO(cmastalli): remove hard-coded zero when fixed the contact
89 ✗ contact = std::make_shared<crocoddyl::ContactModel1D>(
90 ✗ state, frame_id, 0., pinocchio::ReferenceFrame::LOCAL, M.rotation(),
91 ✗ nu, gains);
92 ✗ break;
93 }
94 ✗ case ContactModelTypes::ContactModel1D_WORLD: {
95 ✗ pinocchio::SE3 M = pinocchio::SE3::Random();
96 ✗ gains[0] =
97 0; // TODO(cmastalli): remove hard-coded zero when fixed the contact
98 ✗ contact = std::make_shared<crocoddyl::ContactModel1D>(
99 ✗ state, frame_id, 0., pinocchio::ReferenceFrame::WORLD, M.rotation(),
100 ✗ nu, gains);
101 ✗ break;
102 }
103 ✗ case ContactModelTypes::ContactModel1D_LWA: {
104 ✗ pinocchio::SE3 M = pinocchio::SE3::Random();
105 ✗ gains[0] =
106 0; // TODO(cmastalli): remove hard-coded zero when fixed the contact
107 ✗ contact = std::make_shared<crocoddyl::ContactModel1D>(
108 ✗ state, frame_id, 0., pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED,
109 ✗ M.rotation(), nu, gains);
110 ✗ break;
111 }
112 ✗ case ContactModelTypes::ContactModel2D:
113 ✗ gains[0] =
114 0; // TODO(cmastalli): remove hard-coded zero when fixed the contact
115 ✗ contact = std::make_shared<crocoddyl::ContactModel2D>(
116 ✗ state, frame_id, Eigen::Vector2d::Zero(), nu, gains);
117 ✗ break;
118 ✗ case ContactModelTypes::ContactModel3D_LOCAL:
119 ✗ contact = std::make_shared<crocoddyl::ContactModel3D>(
120 ✗ state, frame_id, Eigen::Vector3d::Zero(),
121 ✗ pinocchio::ReferenceFrame::LOCAL, nu, gains);
122 ✗ break;
123 ✗ case ContactModelTypes::ContactModel3D_WORLD:
124 ✗ contact = std::make_shared<crocoddyl::ContactModel3D>(
125 ✗ state, frame_id, Eigen::Vector3d::Zero(),
126 ✗ pinocchio::ReferenceFrame::WORLD, nu, gains);
127 ✗ break;
128 ✗ case ContactModelTypes::ContactModel3D_LWA:
129 ✗ contact = std::make_shared<crocoddyl::ContactModel3D>(
130 ✗ state, frame_id, Eigen::Vector3d::Zero(),
131 ✗ pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED, nu, gains);
132 ✗ break;
133 ✗ case ContactModelTypes::ContactModel6D_LOCAL:
134 ✗ contact = std::make_shared<crocoddyl::ContactModel6D>(
135 ✗ state, frame_id, pinocchio::SE3::Identity(),
136 ✗ pinocchio::ReferenceFrame::LOCAL, nu, gains);
137 ✗ break;
138 ✗ case ContactModelTypes::ContactModel6D_WORLD:
139 ✗ contact = std::make_shared<crocoddyl::ContactModel6D>(
140 ✗ state, frame_id, pinocchio::SE3::Identity(),
141 ✗ pinocchio::ReferenceFrame::WORLD, nu, gains);
142 ✗ break;
143 ✗ case ContactModelTypes::ContactModel6D_LWA:
144 ✗ contact = std::make_shared<crocoddyl::ContactModel6D>(
145 ✗ state, frame_id, pinocchio::SE3::Identity(),
146 ✗ pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED, nu, gains);
147 ✗ break;
148 ✗ default:
149 ✗ throw_pretty(__FILE__ ": Wrong ContactModelTypes::Type given");
150 break;
151 }
152 ✗ return contact;
153 }
154
155 ✗ std::shared_ptr<crocoddyl::ContactModelAbstract> create_random_contact() {
156 static bool once = true;
157 ✗ if (once) {
158 ✗ srand((unsigned)time(NULL));
159 ✗ once = false;
160 }
161 ✗ std::shared_ptr<crocoddyl::ContactModelAbstract> contact;
162 ✗ ContactModelFactory factory;
163 ✗ if (rand() % 4 == 0) {
164 ✗ contact = factory.create(ContactModelTypes::ContactModel1D_LOCAL,
165 PinocchioModelTypes::RandomHumanoid,
166 ✗ Eigen::Vector2d::Random());
167 ✗ } else if (rand() % 4 == 1) {
168 ✗ contact = factory.create(ContactModelTypes::ContactModel2D,
169 PinocchioModelTypes::RandomHumanoid,
170 ✗ Eigen::Vector2d::Random());
171 ✗ } else if (rand() % 4 == 2) {
172 ✗ contact = factory.create(ContactModelTypes::ContactModel3D_LOCAL,
173 PinocchioModelTypes::RandomHumanoid,
174 ✗ Eigen::Vector2d::Random());
175 } else {
176 ✗ contact = factory.create(ContactModelTypes::ContactModel6D_LOCAL,
177 PinocchioModelTypes::RandomHumanoid,
178 ✗ Eigen::Vector2d::Random());
179 }
180 ✗ return contact;
181 }
182
183 } // namespace unittest
184 } // namespace crocoddyl
185