GCC Code Coverage Report

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