GCC Code Coverage Report

Directory: ./
File: unittest/factory/contact_constraint.cpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 124 135 91.9%
Branches: 124 250 49.6%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021-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_constraint.hpp"
10
11 #include "contact.hpp"
12 #include "cost.hpp"
13 #include "crocoddyl/core/constraints/residual.hpp"
14 #include "crocoddyl/multibody/actions/contact-fwddyn.hpp"
15 #include "crocoddyl/multibody/residuals/contact-control-gravity.hpp"
16 #include "crocoddyl/multibody/residuals/contact-cop-position.hpp"
17 #include "crocoddyl/multibody/residuals/contact-force.hpp"
18 #include "crocoddyl/multibody/residuals/contact-friction-cone.hpp"
19 #include "crocoddyl/multibody/residuals/contact-wrench-cone.hpp"
20
21 namespace crocoddyl {
22 namespace unittest {
23
24 const std::vector<ContactConstraintModelTypes::Type>
25 ContactConstraintModelTypes::all(ContactConstraintModelTypes::init_all());
26
27 13 std::ostream &operator<<(std::ostream &os,
28 ContactConstraintModelTypes::Type type) {
29
5/7
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 3 times.
✓ Branch 3 taken 2 times.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 13 switch (type) {
30 3 case ContactConstraintModelTypes::
31 ConstraintModelResidualContactForceEquality:
32 3 os << "ConstraintModelResidualContactForceEquality";
33 3 break;
34 2 case ContactConstraintModelTypes::
35 ConstraintModelResidualContactCoPPositionInequality:
36 2 os << "ConstraintModelResidualContactCoPPositionInequality";
37 2 break;
38 3 case ContactConstraintModelTypes::
39 ConstraintModelResidualContactFrictionConeInequality:
40 3 os << "ConstraintModelResidualContactFrictionConeInequality";
41 3 break;
42 2 case ContactConstraintModelTypes::
43 ConstraintModelResidualContactWrenchConeInequality:
44 2 os << "ConstraintModelResidualContactWrenchConeInequality";
45 2 break;
46 3 case ContactConstraintModelTypes::
47 ConstraintModelResidualContactControlGravInequality:
48 3 os << "ConstraintModelResidualContactControlGravInequality";
49 3 break;
50 ✗ case ContactConstraintModelTypes::NbContactConstraintModelTypes:
51 ✗ os << "NbContactConstraintModelTypes";
52 ✗ break;
53 ✗ default:
54 ✗ break;
55 }
56 13 return os;
57 }
58
59 13 ContactConstraintModelFactory::ContactConstraintModelFactory() {}
60 13 ContactConstraintModelFactory::~ContactConstraintModelFactory() {}
61
62 std::shared_ptr<crocoddyl::DifferentialActionModelAbstract>
63 13 ContactConstraintModelFactory::create(
64 ContactConstraintModelTypes::Type constraint_type,
65 PinocchioModelTypes::Type model_type,
66 ActuationModelTypes::Type actuation_type) const {
67 // Identify the state type given the model type
68 StateModelTypes::Type state_type;
69
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 13 PinocchioModelFactory model_factory(model_type);
70
3/4
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 13 switch (model_type) {
71 5 case PinocchioModelTypes::Talos:
72 5 state_type = StateModelTypes::StateMultibody_Talos;
73 5 break;
74 5 case PinocchioModelTypes::RandomHumanoid:
75 5 state_type = StateModelTypes::StateMultibody_RandomHumanoid;
76 5 break;
77 3 case PinocchioModelTypes::HyQ:
78 3 state_type = StateModelTypes::StateMultibody_HyQ;
79 3 break;
80 ✗ default:
81 ✗ throw_pretty(__FILE__ ": Wrong PinocchioModelTypes::Type given");
82 break;
83 }
84
85 // Create contact contact diff-action model with standard cost functions
86 13 std::shared_ptr<crocoddyl::DifferentialActionModelContactFwdDynamics> action;
87 13 std::shared_ptr<crocoddyl::StateMultibody> state;
88 13 std::shared_ptr<crocoddyl::ActuationModelAbstract> actuation;
89 13 std::shared_ptr<crocoddyl::ContactModelMultiple> contact;
90 13 std::shared_ptr<crocoddyl::CostModelSum> cost;
91 13 std::shared_ptr<crocoddyl::ConstraintModelManager> constraint;
92 26 state = std::static_pointer_cast<crocoddyl::StateMultibody>(
93
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 39 StateModelFactory().create(state_type));
94
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 13 actuation = ActuationModelFactory().create(actuation_type, state_type);
95 13 contact = std::make_shared<crocoddyl::ContactModelMultiple>(
96
2/4
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 13 times.
✗ Branch 6 not taken.
 13 state, actuation->get_nu());
97
2/4
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 13 times.
✗ Branch 6 not taken.
 13 cost = std::make_shared<crocoddyl::CostModelSum>(state, actuation->get_nu());
98 13 constraint = std::make_shared<crocoddyl::ConstraintModelManager>(
99
2/4
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 13 times.
✗ Branch 6 not taken.
 13 state, actuation->get_nu());
100
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 26 std::vector<std::size_t> frame_ids = model_factory.get_frame_ids();
101
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 26 std::vector<std::string> frame_names = model_factory.get_frame_names();
102 // Define the contact model
103
2/3
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 13 switch (state_type) {
104 10 case StateModelTypes::StateMultibody_Talos:
105 case StateModelTypes::StateMultibody_RandomHumanoid:
106
2/2
✓ Branch 1 taken 20 times.
✓ Branch 2 taken 10 times.
 30 for (std::size_t i = 0; i < frame_names.size(); ++i) {
107
1/2
✓ Branch 3 taken 20 times.
✗ Branch 4 not taken.
 40 contact->addContact(frame_names[i],
108
5/10
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 20 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 20 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 20 times.
✗ Branch 15 not taken.
 100 ContactModelFactory().create(
109 ContactModelTypes::ContactModel6D_LOCAL,
110
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 model_type, Eigen::Vector2d::Random(),
111 20 frame_names[i], actuation->get_nu()));
112 }
113 10 break;
114 3 case StateModelTypes::StateMultibody_HyQ:
115
2/2
✓ Branch 1 taken 12 times.
✓ Branch 2 taken 3 times.
 15 for (std::size_t i = 0; i < frame_names.size(); ++i) {
116
1/2
✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
 24 contact->addContact(frame_names[i],
117
5/10
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 12 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 12 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 12 times.
✗ Branch 15 not taken.
 60 ContactModelFactory().create(
118 ContactModelTypes::ContactModel3D_LOCAL,
119
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 24 model_type, Eigen::Vector2d::Random(),
120 12 frame_names[i], actuation->get_nu()));
121 }
122 3 break;
123 ✗ default:
124 ✗ throw_pretty(__FILE__ ": Wrong StateModelTypes::Type given");
125 break;
126 }
127 // Define standard cost functions
128
2/4
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 13 times.
✗ Branch 7 not taken.
 26 cost->addCost(
129 "state",
130
3/6
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 13 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 13 times.
✗ Branch 9 not taken.
 26 CostModelFactory().create(
131 CostModelTypes::CostModelResidualState, state_type,
132 ActivationModelTypes::ActivationModelQuad, actuation->get_nu()),
133 0.1);
134
2/4
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 13 times.
✗ Branch 7 not taken.
 26 cost->addCost(
135 "control",
136
3/6
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 13 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 13 times.
✗ Branch 9 not taken.
 26 CostModelFactory().create(
137 CostModelTypes::CostModelResidualControl, state_type,
138 ActivationModelTypes::ActivationModelQuad, actuation->get_nu()),
139 0.1);
140
141 // Define the constraint function
142
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 13 Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
143
1/2
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
 26 crocoddyl::FrictionCone friction_cone(R, 1.);
144
2/4
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 13 times.
✗ Branch 6 not taken.
 26 crocoddyl::WrenchCone wrench_cone(R, 1., Eigen::Vector2d(0.1, 0.1));
145
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 26 crocoddyl::CoPSupport cop_support(R, Eigen::Vector2d(0.1, 0.1));
146
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 26 Eigen::VectorXd lb, ub;
147
5/6
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 3 times.
✓ Branch 3 taken 2 times.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 13 switch (constraint_type) {
148 3 case ContactConstraintModelTypes::
149 ConstraintModelResidualContactForceEquality:
150
2/2
✓ Branch 1 taken 8 times.
✓ Branch 2 taken 3 times.
 11 for (std::size_t i = 0; i < frame_ids.size(); ++i) {
151
1/2
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
 24 constraint->addConstraint(
152
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 "constraint_" + std::to_string(i),
153
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 16 std::make_shared<crocoddyl::ConstraintModelResidual>(
154 state,
155
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 16 std::make_shared<crocoddyl::ResidualModelContactForce>(
156 8 state, frame_ids[i], pinocchio::Force::Random(),
157
3/6
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 8 times.
✗ Branch 9 not taken.
 16 model_factory.get_contact_nc(), actuation->get_nu())));
158 }
159 3 break;
160 2 case ContactConstraintModelTypes::
161 ConstraintModelResidualContactCoPPositionInequality:
162
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 lb = cop_support.get_lb();
163
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 ub = cop_support.get_ub();
164
2/2
✓ Branch 1 taken 4 times.
✓ Branch 2 taken 2 times.
 6 for (std::size_t i = 0; i < frame_ids.size(); ++i) {
165
1/2
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
 12 constraint->addConstraint(
166
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 8 "constraint_" + std::to_string(i),
167
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 std::make_shared<crocoddyl::ConstraintModelResidual>(
168 state,
169
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 std::make_shared<crocoddyl::ResidualModelContactCoPPosition>(
170
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 8 state, frame_ids[i], cop_support, actuation->get_nu()),
171 lb, ub));
172 }
173 2 break;
174 3 case ContactConstraintModelTypes::
175 ConstraintModelResidualContactFrictionConeInequality:
176
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 lb = friction_cone.get_lb();
177
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 ub = friction_cone.get_ub();
178
2/2
✓ Branch 1 taken 8 times.
✓ Branch 2 taken 3 times.
 11 for (std::size_t i = 0; i < frame_ids.size(); ++i) {
179
1/2
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
 24 constraint->addConstraint(
180
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 "constraint_" + std::to_string(i),
181
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 16 std::make_shared<crocoddyl::ConstraintModelResidual>(
182 state,
183
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 16 std::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
184
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 16 state, frame_ids[i], friction_cone, actuation->get_nu()),
185 lb, ub),
186 true);
187 }
188 3 break;
189 2 case ContactConstraintModelTypes::
190 ConstraintModelResidualContactWrenchConeInequality:
191
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 lb = wrench_cone.get_lb();
192
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 ub = wrench_cone.get_ub();
193
2/2
✓ Branch 1 taken 4 times.
✓ Branch 2 taken 2 times.
 6 for (std::size_t i = 0; i < frame_ids.size(); ++i) {
194
1/2
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
 12 constraint->addConstraint(
195
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 8 "constraint_" + std::to_string(i),
196
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 std::make_shared<crocoddyl::ConstraintModelResidual>(
197 state,
198
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 std::make_shared<crocoddyl::ResidualModelContactWrenchCone>(
199
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 8 state, frame_ids[i], wrench_cone, actuation->get_nu()),
200 lb, ub),
201 true);
202 }
203 2 break;
204 3 case ContactConstraintModelTypes::
205 ConstraintModelResidualContactControlGravInequality:
206
3/6
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
 3 lb = Eigen::VectorXd::Zero(state->get_nv());
207
4/8
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
 3 ub = Eigen::VectorXd::Random(state->get_nv()).cwiseAbs();
208
2/4
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 6 constraint->addConstraint(
209 "constraint_0",
210
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 std::make_shared<crocoddyl::ConstraintModelResidual>(
211 state,
212 3 std::make_shared<crocoddyl::ResidualModelContactControlGrav>(
213
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 state, actuation->get_nu()),
214 lb, ub));
215 3 break;
216 ✗ default:
217 ✗ throw_pretty(__FILE__ ": Wrong ContactConstraintModelTypes::Type given");
218 break;
219 }
220
221 action =
222 13 std::make_shared<crocoddyl::DifferentialActionModelContactFwdDynamics>(
223
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 13 state, actuation, contact, cost, constraint, 0., true);
224 26 return action;
225 13 }
226
227 } // namespace unittest
228 } // namespace crocoddyl
229