GCC Code Coverage Report

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