GCC Code Coverage Report

Directory: ./
File: unittest/factory/impulse_constraint.cpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 116 127 91.3%
Branches: 95 192 49.5%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021-2025, University of Edinburgh, Heriot-Watt University,
5 // Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 #include "impulse_constraint.hpp"
11
12 #include "cost.hpp"
13 #include "crocoddyl/core/constraints/residual.hpp"
14 #include "crocoddyl/multibody/actions/impulse-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 #include "crocoddyl/multibody/residuals/impulse-com.hpp"
21 #include "impulse.hpp"
22
23 namespace crocoddyl {
24 namespace unittest {
25
26 const std::vector<ImpulseConstraintModelTypes::Type>
27 ImpulseConstraintModelTypes::all(ImpulseConstraintModelTypes::init_all());
28
29 12 std::ostream &operator<<(std::ostream &os,
30 ImpulseConstraintModelTypes::Type type) {
31
5/7
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 3 times.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 12 switch (type) {
32 2 case ImpulseConstraintModelTypes::CostModelResidualImpulseCoMEquality:
33 2 os << "CostModelResidualImpulseCoMEquality";
34 2 break;
35 3 case ImpulseConstraintModelTypes::
36 ConstraintModelResidualImpulseForceEquality:
37 3 os << "ConstraintModelResidualImpulseForceEquality";
38 3 break;
39 2 case ImpulseConstraintModelTypes::
40 ConstraintModelResidualImpulseCoPPositionInequality:
41 2 os << "ConstraintModelResidualImpulseCoPPositionInequality";
42 2 break;
43 3 case ImpulseConstraintModelTypes::
44 ConstraintModelResidualImpulseFrictionConeInequality:
45 3 os << "ConstraintModelResidualImpulseFrictionConeInequality";
46 3 break;
47 2 case ImpulseConstraintModelTypes::
48 ConstraintModelResidualImpulseWrenchConeInequality:
49 2 os << "ConstraintModelResidualImpulseWrenchConeInequality";
50 2 break;
51 ✗ case ImpulseConstraintModelTypes::NbImpulseConstraintModelTypes:
52 ✗ os << "NbImpulseConstraintModelTypes";
53 ✗ break;
54 ✗ default:
55 ✗ break;
56 }
57 12 return os;
58 }
59
60 12 ImpulseConstraintModelFactory::ImpulseConstraintModelFactory() {}
61 12 ImpulseConstraintModelFactory::~ImpulseConstraintModelFactory() {}
62
63 std::shared_ptr<crocoddyl::ActionModelAbstract>
64 12 ImpulseConstraintModelFactory::create(
65 ImpulseConstraintModelTypes::Type constraint_type,
66 PinocchioModelTypes::Type model_type) const {
67 // Identify the state type given the model type
68 StateModelTypes::Type state_type;
69
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 PinocchioModelFactory model_factory(model_type);
70
3/4
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 12 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 2 case PinocchioModelTypes::HyQ:
78 2 state_type = StateModelTypes::StateMultibody_HyQ;
79 2 break;
80 ✗ default:
81 ✗ throw_pretty(__FILE__ ": Wrong PinocchioModelTypes::Type given");
82 break;
83 }
84
85 // Create impulse impulse diff-action model with standard cost functions
86 12 std::shared_ptr<crocoddyl::ActionModelImpulseFwdDynamics> action;
87 12 std::shared_ptr<crocoddyl::StateMultibody> state;
88 12 std::shared_ptr<crocoddyl::ActuationModelAbstract> actuation;
89 12 std::shared_ptr<crocoddyl::ImpulseModelMultiple> impulse;
90 12 std::shared_ptr<crocoddyl::CostModelSum> cost;
91 12 std::shared_ptr<crocoddyl::ConstraintModelManager> constraint;
92 24 state = std::static_pointer_cast<crocoddyl::StateMultibody>(
93
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 36 StateModelFactory().create(state_type));
94
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 impulse = std::make_shared<crocoddyl::ImpulseModelMultiple>(state);
95
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 cost = std::make_shared<crocoddyl::CostModelSum>(state, 0);
96
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 constraint = std::make_shared<crocoddyl::ConstraintModelManager>(state, 0);
97
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 24 std::vector<std::size_t> frame_ids = model_factory.get_frame_ids();
98
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 24 std::vector<std::string> frame_names = model_factory.get_frame_names();
99 // Define the impulse model
100
2/3
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 12 switch (state_type) {
101 10 case StateModelTypes::StateMultibody_Talos:
102 case StateModelTypes::StateMultibody_RandomHumanoid:
103
2/2
✓ Branch 1 taken 20 times.
✓ Branch 2 taken 10 times.
 30 for (std::size_t i = 0; i < frame_names.size(); ++i) {
104
1/2
✓ Branch 3 taken 20 times.
✗ Branch 4 not taken.
 40 impulse->addImpulse(frame_names[i],
105
3/6
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
 60 ImpulseModelFactory().create(
106 ImpulseModelTypes::ImpulseModel6D_LOCAL,
107 20 model_type, frame_names[i]));
108 }
109 10 break;
110 2 case StateModelTypes::StateMultibody_HyQ:
111
2/2
✓ Branch 1 taken 8 times.
✓ Branch 2 taken 2 times.
 10 for (std::size_t i = 0; i < frame_names.size(); ++i) {
112
1/2
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
 16 impulse->addImpulse(frame_names[i],
113
3/6
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
 24 ImpulseModelFactory().create(
114 ImpulseModelTypes::ImpulseModel3D_LOCAL,
115 8 model_type, frame_names[i]));
116 }
117 2 break;
118 ✗ default:
119 ✗ throw_pretty(__FILE__ ": Wrong StateModelTypes::Type given");
120 break;
121 }
122 // Define standard cost functions
123
2/4
✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 12 times.
✗ Branch 7 not taken.
 24 cost->addCost("state",
124
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 24 CostModelFactory().create(
125 CostModelTypes::CostModelResidualState, state_type,
126 ActivationModelTypes::ActivationModelQuad, 0),
127 0.1);
128
129 // Define the constraint function
130
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 12 Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
131
1/2
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
 24 crocoddyl::FrictionCone friction_cone(R, 1.);
132
2/4
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
 24 crocoddyl::WrenchCone wrench_cone(R, 1., Eigen::Vector2d(0.1, 0.1));
133
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 24 crocoddyl::CoPSupport cop_support(R, Eigen::Vector2d(0.1, 0.1));
134
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 24 Eigen::VectorXd lb, ub;
135
5/6
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 3 times.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 12 switch (constraint_type) {
136 2 case ImpulseConstraintModelTypes::CostModelResidualImpulseCoMEquality:
137
2/4
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 4 constraint->addConstraint(
138 "constraint",
139
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 std::make_shared<crocoddyl::ConstraintModelResidual>(
140 state,
141
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 std::make_shared<crocoddyl::ResidualModelImpulseCoM>(state)));
142 2 break;
143 3 case ImpulseConstraintModelTypes::
144 ConstraintModelResidualImpulseForceEquality:
145
2/2
✓ Branch 1 taken 8 times.
✓ Branch 2 taken 3 times.
 11 for (std::size_t i = 0; i < frame_ids.size(); ++i) {
146
1/2
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
 24 constraint->addConstraint(
147
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),
148
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 16 std::make_shared<crocoddyl::ConstraintModelResidual>(
149
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 16 state, std::make_shared<crocoddyl::ResidualModelContactForce>(
150 8 state, frame_ids[i], pinocchio::Force::Random(),
151
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 model_factory.get_contact_nc(), 0)));
152 }
153 3 break;
154 2 case ImpulseConstraintModelTypes::
155 ConstraintModelResidualImpulseCoPPositionInequality:
156
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();
157
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();
158
2/2
✓ Branch 1 taken 4 times.
✓ Branch 2 taken 2 times.
 6 for (std::size_t i = 0; i < frame_ids.size(); ++i) {
159
1/2
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
 12 constraint->addConstraint(
160
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),
161
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 std::make_shared<crocoddyl::ConstraintModelResidual>(
162 state,
163
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 std::make_shared<crocoddyl::ResidualModelContactCoPPosition>(
164 8 state, frame_ids[i], cop_support, 0),
165 lb, ub));
166 }
167 2 break;
168 3 case ImpulseConstraintModelTypes::
169 ConstraintModelResidualImpulseFrictionConeInequality:
170
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();
171
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();
172
2/2
✓ Branch 1 taken 8 times.
✓ Branch 2 taken 3 times.
 11 for (std::size_t i = 0; i < frame_ids.size(); ++i) {
173
1/2
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
 24 constraint->addConstraint(
174
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),
175
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 16 std::make_shared<crocoddyl::ConstraintModelResidual>(
176 state,
177
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 16 std::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
178 16 state, frame_ids[i], friction_cone, 0),
179 lb, ub));
180 }
181 3 break;
182 2 case ImpulseConstraintModelTypes::
183 ConstraintModelResidualImpulseWrenchConeInequality:
184
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();
185
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();
186
2/2
✓ Branch 1 taken 4 times.
✓ Branch 2 taken 2 times.
 6 for (std::size_t i = 0; i < frame_ids.size(); ++i) {
187
1/2
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
 12 constraint->addConstraint(
188
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),
189
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 std::make_shared<crocoddyl::ConstraintModelResidual>(
190 state,
191
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 std::make_shared<crocoddyl::ResidualModelContactWrenchCone>(
192 8 state, frame_ids[i], wrench_cone, 0),
193 lb, ub));
194 }
195 2 break;
196 ✗ default:
197 ✗ throw_pretty(__FILE__ ": Wrong ImpulseConstraintModelTypes::Type given");
198 break;
199 }
200
201 12 double r_coeff = 0.; // TODO(cmastall): random_real_in_range(1e-16, 1e-2);
202 12 double damping = 0.; // TODO(cmastall): random_real_in_range(1e-16, 1e-2);
203 12 action = std::make_shared<crocoddyl::ActionModelImpulseFwdDynamics>(
204
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 state, impulse, cost, constraint, r_coeff, damping, true);
205 24 return action;
206 12 }
207
208 } // namespace unittest
209 } // namespace crocoddyl
210