GCC Code Coverage Report

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