GCC Code Coverage Report

Directory: ./
File: unittest/factory/contact_constraint.cpp
Date: 2025-01-16 08:47:40
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 boost::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 boost::shared_ptr<crocoddyl::DifferentialActionModelContactFwdDynamics>
88 13 action;
89 13 boost::shared_ptr<crocoddyl::StateMultibody> state;
90 13 boost::shared_ptr<crocoddyl::ActuationModelAbstract> actuation;
91 13 boost::shared_ptr<crocoddyl::ContactModelMultiple> contact;
92 13 boost::shared_ptr<crocoddyl::CostModelSum> cost;
93 13 boost::shared_ptr<crocoddyl::ConstraintModelManager> constraint;
94 26 state = boost::static_pointer_cast<crocoddyl::StateMultibody>(
95
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));
96
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);
97 13 contact = boost::make_shared<crocoddyl::ContactModelMultiple>(
98
1/2
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
 13 state, actuation->get_nu());
99 cost =
100
1/2
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
 13 boost::make_shared<crocoddyl::CostModelSum>(state, actuation->get_nu());
101 13 constraint = boost::make_shared<crocoddyl::ConstraintModelManager>(
102
1/2
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
 13 state, actuation->get_nu());
103
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 26 std::vector<std::size_t> frame_ids = model_factory.get_frame_ids();
104
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 26 std::vector<std::string> frame_names = model_factory.get_frame_names();
105 // Define the contact model
106
2/3
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 13 switch (state_type) {
107 10 case StateModelTypes::StateMultibody_Talos:
108 case StateModelTypes::StateMultibody_RandomHumanoid:
109
2/2
✓ Branch 1 taken 20 times.
✓ Branch 2 taken 10 times.
 30 for (std::size_t i = 0; i < frame_names.size(); ++i) {
110
1/2
✓ Branch 3 taken 20 times.
✗ Branch 4 not taken.
 40 contact->addContact(frame_names[i],
111
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(
112 ContactModelTypes::ContactModel6D_LOCAL,
113
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 model_type, Eigen::Vector2d::Random(),
114 20 frame_names[i], actuation->get_nu()));
115 }
116 10 break;
117 3 case StateModelTypes::StateMultibody_HyQ:
118
2/2
✓ Branch 1 taken 12 times.
✓ Branch 2 taken 3 times.
 15 for (std::size_t i = 0; i < frame_names.size(); ++i) {
119
1/2
✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
 24 contact->addContact(frame_names[i],
120
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(
121 ContactModelTypes::ContactModel3D_LOCAL,
122
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 24 model_type, Eigen::Vector2d::Random(),
123 12 frame_names[i], actuation->get_nu()));
124 }
125 3 break;
126 ✗ default:
127 ✗ throw_pretty(__FILE__ ": Wrong StateModelTypes::Type given");
128 break;
129 }
130 // Define standard cost functions
131
2/4
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 13 times.
✗ Branch 7 not taken.
 26 cost->addCost(
132 "state",
133
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 13 times.
✗ Branch 7 not taken.
 26 CostModelFactory().create(
134 CostModelTypes::CostModelResidualState, state_type,
135 ActivationModelTypes::ActivationModelQuad, actuation->get_nu()),
136 0.1);
137
2/4
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 13 times.
✗ Branch 7 not taken.
 26 cost->addCost(
138 "control",
139
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 13 times.
✗ Branch 7 not taken.
 26 CostModelFactory().create(
140 CostModelTypes::CostModelResidualControl, state_type,
141 ActivationModelTypes::ActivationModelQuad, actuation->get_nu()),
142 0.1);
143
144 // Define the constraint function
145
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();
146
1/2
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
 26 crocoddyl::FrictionCone friction_cone(R, 1.);
147
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));
148
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));
149
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;
150
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) {
151 3 case ContactConstraintModelTypes::
152 ConstraintModelResidualContactForceEquality:
153
2/2
✓ Branch 1 taken 8 times.
✓ Branch 2 taken 3 times.
 11 for (std::size_t i = 0; i < frame_ids.size(); ++i) {
154
1/2
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
 24 constraint->addConstraint(
155
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),
156
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 16 boost::make_shared<crocoddyl::ConstraintModelResidual>(
157 state,
158
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 16 boost::make_shared<crocoddyl::ResidualModelContactForce>(
159 8 state, frame_ids[i], pinocchio::Force::Random(),
160
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())));
161 }
162 3 break;
163 2 case ContactConstraintModelTypes::
164 ConstraintModelResidualContactCoPPositionInequality:
165
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 lb = cop_support.get_lb();
166
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 ub = cop_support.get_ub();
167
2/2
✓ Branch 1 taken 4 times.
✓ Branch 2 taken 2 times.
 6 for (std::size_t i = 0; i < frame_ids.size(); ++i) {
168
1/2
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
 12 constraint->addConstraint(
169
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),
170
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 boost::make_shared<crocoddyl::ConstraintModelResidual>(
171 state,
172
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 boost::make_shared<crocoddyl::ResidualModelContactCoPPosition>(
173 8 state, frame_ids[i], cop_support, actuation->get_nu()),
174 lb, ub));
175 }
176 2 break;
177 3 case ContactConstraintModelTypes::
178 ConstraintModelResidualContactFrictionConeInequality:
179
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 lb = friction_cone.get_lb();
180
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 ub = friction_cone.get_ub();
181
2/2
✓ Branch 1 taken 8 times.
✓ Branch 2 taken 3 times.
 11 for (std::size_t i = 0; i < frame_ids.size(); ++i) {
182
1/2
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
 24 constraint->addConstraint(
183
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),
184
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 16 boost::make_shared<crocoddyl::ConstraintModelResidual>(
185 state,
186
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 16 boost::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
187 16 state, frame_ids[i], friction_cone, actuation->get_nu()),
188 lb, ub),
189 true);
190 }
191 3 break;
192 2 case ContactConstraintModelTypes::
193 ConstraintModelResidualContactWrenchConeInequality:
194
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 lb = wrench_cone.get_lb();
195
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 ub = wrench_cone.get_ub();
196
2/2
✓ Branch 1 taken 4 times.
✓ Branch 2 taken 2 times.
 6 for (std::size_t i = 0; i < frame_ids.size(); ++i) {
197
1/2
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
 12 constraint->addConstraint(
198
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),
199
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 boost::make_shared<crocoddyl::ConstraintModelResidual>(
200 state,
201
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 boost::make_shared<crocoddyl::ResidualModelContactWrenchCone>(
202 8 state, frame_ids[i], wrench_cone, actuation->get_nu()),
203 lb, ub),
204 true);
205 }
206 2 break;
207 3 case ContactConstraintModelTypes::
208 ConstraintModelResidualContactControlGravInequality:
209
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());
210
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();
211
2/4
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 6 constraint->addConstraint(
212 "constraint_0",
213
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 boost::make_shared<crocoddyl::ConstraintModelResidual>(
214 state,
215 3 boost::make_shared<crocoddyl::ResidualModelContactControlGrav>(
216
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 3 state, actuation->get_nu()),
217 lb, ub));
218 3 break;
219 ✗ default:
220 ✗ throw_pretty(__FILE__ ": Wrong ContactConstraintModelTypes::Type given");
221 break;
222 }
223
224 action =
225 13 boost::make_shared<crocoddyl::DifferentialActionModelContactFwdDynamics>(
226
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 13 state, actuation, contact, cost, constraint, 0., true);
227 26 return action;
228 13 }
229
230 } // namespace unittest
231 } // namespace crocoddyl
232