GCC Code Coverage Report

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