GCC Code Coverage Report

Directory: ./
File: unittest/factory/impulse_constraint.cpp
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 126 0.0%
Branches: 0 192 0.0%
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 ✗ std::ostream &operator<<(std::ostream &os,
30 ImpulseConstraintModelTypes::Type type) {
31 ✗ switch (type) {
32 ✗ case ImpulseConstraintModelTypes::CostModelResidualImpulseCoMEquality:
33 ✗ os << "CostModelResidualImpulseCoMEquality";
34 ✗ break;
35 ✗ case ImpulseConstraintModelTypes::
36 ConstraintModelResidualImpulseForceEquality:
37 ✗ os << "ConstraintModelResidualImpulseForceEquality";
38 ✗ break;
39 ✗ case ImpulseConstraintModelTypes::
40 ConstraintModelResidualImpulseCoPPositionInequality:
41 ✗ os << "ConstraintModelResidualImpulseCoPPositionInequality";
42 ✗ break;
43 ✗ case ImpulseConstraintModelTypes::
44 ConstraintModelResidualImpulseFrictionConeInequality:
45 ✗ os << "ConstraintModelResidualImpulseFrictionConeInequality";
46 ✗ break;
47 ✗ case ImpulseConstraintModelTypes::
48 ConstraintModelResidualImpulseWrenchConeInequality:
49 ✗ os << "ConstraintModelResidualImpulseWrenchConeInequality";
50 ✗ break;
51 ✗ case ImpulseConstraintModelTypes::NbImpulseConstraintModelTypes:
52 ✗ os << "NbImpulseConstraintModelTypes";
53 ✗ break;
54 ✗ default:
55 ✗ break;
56 }
57 ✗ return os;
58 }
59
60 ✗ ImpulseConstraintModelFactory::ImpulseConstraintModelFactory() {}
61 ✗ ImpulseConstraintModelFactory::~ImpulseConstraintModelFactory() {}
62
63 std::shared_ptr<crocoddyl::ActionModelAbstract>
64 ✗ 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 ✗ PinocchioModelFactory model_factory(model_type);
70 ✗ switch (model_type) {
71 ✗ case PinocchioModelTypes::Talos:
72 ✗ state_type = StateModelTypes::StateMultibody_Talos;
73 ✗ break;
74 ✗ case PinocchioModelTypes::RandomHumanoid:
75 ✗ state_type = StateModelTypes::StateMultibody_RandomHumanoid;
76 ✗ break;
77 ✗ case PinocchioModelTypes::HyQ:
78 ✗ state_type = StateModelTypes::StateMultibody_HyQ;
79 ✗ 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 ✗ std::shared_ptr<crocoddyl::ActionModelImpulseFwdDynamics> action;
87 ✗ std::shared_ptr<crocoddyl::StateMultibody> state;
88 ✗ std::shared_ptr<crocoddyl::ActuationModelAbstract> actuation;
89 ✗ std::shared_ptr<crocoddyl::ImpulseModelMultiple> impulse;
90 ✗ std::shared_ptr<crocoddyl::CostModelSum> cost;
91 ✗ std::shared_ptr<crocoddyl::ConstraintModelManager> constraint;
92 ✗ state = std::static_pointer_cast<crocoddyl::StateMultibody>(
93 ✗ StateModelFactory().create(state_type));
94 ✗ impulse = std::make_shared<crocoddyl::ImpulseModelMultiple>(state);
95 ✗ cost = std::make_shared<crocoddyl::CostModelSum>(state, 0);
96 ✗ constraint = std::make_shared<crocoddyl::ConstraintModelManager>(state, 0);
97 ✗ std::vector<std::size_t> frame_ids = model_factory.get_frame_ids();
98 ✗ std::vector<std::string> frame_names = model_factory.get_frame_names();
99 // Define the impulse model
100 ✗ switch (state_type) {
101 ✗ case StateModelTypes::StateMultibody_Talos:
102 case StateModelTypes::StateMultibody_RandomHumanoid:
103 ✗ for (std::size_t i = 0; i < frame_names.size(); ++i) {
104 ✗ impulse->addImpulse(frame_names[i],
105 ✗ ImpulseModelFactory().create(
106 ImpulseModelTypes::ImpulseModel6D_LOCAL,
107 ✗ model_type, frame_names[i]));
108 }
109 ✗ break;
110 ✗ case StateModelTypes::StateMultibody_HyQ:
111 ✗ for (std::size_t i = 0; i < frame_names.size(); ++i) {
112 ✗ impulse->addImpulse(frame_names[i],
113 ✗ ImpulseModelFactory().create(
114 ImpulseModelTypes::ImpulseModel3D_LOCAL,
115 ✗ model_type, frame_names[i]));
116 }
117 ✗ break;
118 ✗ default:
119 ✗ throw_pretty(__FILE__ ": Wrong StateModelTypes::Type given");
120 break;
121 }
122 // Define standard cost functions
123 ✗ cost->addCost("state",
124 ✗ CostModelFactory().create(
125 CostModelTypes::CostModelResidualState, state_type,
126 ActivationModelTypes::ActivationModelQuad, 0),
127 0.1);
128
129 // Define the constraint function
130 ✗ Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
131 ✗ crocoddyl::FrictionCone friction_cone(R, 1.);
132 ✗ crocoddyl::WrenchCone wrench_cone(R, 1., Eigen::Vector2d(0.1, 0.1));
133 ✗ crocoddyl::CoPSupport cop_support(R, Eigen::Vector2d(0.1, 0.1));
134 ✗ Eigen::VectorXd lb, ub;
135 ✗ switch (constraint_type) {
136 ✗ case ImpulseConstraintModelTypes::CostModelResidualImpulseCoMEquality:
137 ✗ constraint->addConstraint(
138 "constraint",
139 ✗ std::make_shared<crocoddyl::ConstraintModelResidual>(
140 state,
141 ✗ std::make_shared<crocoddyl::ResidualModelImpulseCoM>(state)));
142 ✗ break;
143 ✗ case ImpulseConstraintModelTypes::
144 ConstraintModelResidualImpulseForceEquality:
145 ✗ for (std::size_t i = 0; i < frame_ids.size(); ++i) {
146 ✗ constraint->addConstraint(
147 ✗ "constraint_" + std::to_string(i),
148 ✗ std::make_shared<crocoddyl::ConstraintModelResidual>(
149 ✗ state, std::make_shared<crocoddyl::ResidualModelContactForce>(
150 ✗ state, frame_ids[i], pinocchio::Force::Random(),
151 ✗ model_factory.get_contact_nc(), 0)));
152 }
153 ✗ break;
154 ✗ case ImpulseConstraintModelTypes::
155 ConstraintModelResidualImpulseCoPPositionInequality:
156 ✗ lb = cop_support.get_lb();
157 ✗ ub = cop_support.get_ub();
158 ✗ for (std::size_t i = 0; i < frame_ids.size(); ++i) {
159 ✗ constraint->addConstraint(
160 ✗ "constraint_" + std::to_string(i),
161 ✗ std::make_shared<crocoddyl::ConstraintModelResidual>(
162 state,
163 ✗ std::make_shared<crocoddyl::ResidualModelContactCoPPosition>(
164 ✗ state, frame_ids[i], cop_support, 0),
165 lb, ub));
166 }
167 ✗ break;
168 ✗ case ImpulseConstraintModelTypes::
169 ConstraintModelResidualImpulseFrictionConeInequality:
170 ✗ lb = friction_cone.get_lb();
171 ✗ ub = friction_cone.get_ub();
172 ✗ for (std::size_t i = 0; i < frame_ids.size(); ++i) {
173 ✗ constraint->addConstraint(
174 ✗ "constraint_" + std::to_string(i),
175 ✗ std::make_shared<crocoddyl::ConstraintModelResidual>(
176 state,
177 ✗ std::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
178 ✗ state, frame_ids[i], friction_cone, 0),
179 lb, ub));
180 }
181 ✗ break;
182 ✗ case ImpulseConstraintModelTypes::
183 ConstraintModelResidualImpulseWrenchConeInequality:
184 ✗ lb = wrench_cone.get_lb();
185 ✗ ub = wrench_cone.get_ub();
186 ✗ for (std::size_t i = 0; i < frame_ids.size(); ++i) {
187 ✗ constraint->addConstraint(
188 ✗ "constraint_" + std::to_string(i),
189 ✗ std::make_shared<crocoddyl::ConstraintModelResidual>(
190 state,
191 ✗ std::make_shared<crocoddyl::ResidualModelContactWrenchCone>(
192 ✗ state, frame_ids[i], wrench_cone, 0),
193 lb, ub));
194 }
195 ✗ break;
196 ✗ default:
197 ✗ throw_pretty(__FILE__ ": Wrong ImpulseConstraintModelTypes::Type given");
198 break;
199 }
200
201 ✗ double r_coeff = 0.; // TODO(cmastall): random_real_in_range(1e-16, 1e-2);
202 ✗ double damping = 0.; // TODO(cmastall): random_real_in_range(1e-16, 1e-2);
203 ✗ action = std::make_shared<crocoddyl::ActionModelImpulseFwdDynamics>(
204 ✗ state, impulse, cost, constraint, r_coeff, damping, true);
205 ✗ return action;
206 }
207
208 } // namespace unittest
209 } // namespace crocoddyl
210