GCC Code Coverage Report

Directory: ./
File: unittest/factory/impulse_cost.cpp
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 92 0.0%
Branches: 0 168 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 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
7 ///////////////////////////////////////////////////////////////////////////////
8
9 #include "impulse_cost.hpp"
10
11 #include "action.hpp"
12 #include "crocoddyl/core/costs/residual.hpp"
13 #include "crocoddyl/multibody/residuals/contact-cop-position.hpp"
14 #include "crocoddyl/multibody/residuals/contact-force.hpp"
15 #include "crocoddyl/multibody/residuals/contact-friction-cone.hpp"
16 #include "crocoddyl/multibody/residuals/contact-wrench-cone.hpp"
17 #include "crocoddyl/multibody/residuals/impulse-com.hpp"
18
19 namespace crocoddyl {
20 namespace unittest {
21
22 const std::vector<ImpulseCostModelTypes::Type> ImpulseCostModelTypes::all(
23 ImpulseCostModelTypes::init_all());
24
25 ✗ std::ostream& operator<<(std::ostream& os, ImpulseCostModelTypes::Type type) {
26 ✗ switch (type) {
27 ✗ case ImpulseCostModelTypes::CostModelResidualImpulseCoM:
28 ✗ os << "CostModelResidualImpulseCoM";
29 ✗ break;
30 ✗ case ImpulseCostModelTypes::CostModelResidualContactForce:
31 ✗ os << "CostModelResidualContactForce";
32 ✗ break;
33 ✗ case ImpulseCostModelTypes::CostModelResidualContactCoPPosition:
34 ✗ os << "CostModelResidualContactCoPPosition";
35 ✗ break;
36 ✗ case ImpulseCostModelTypes::CostModelResidualContactFrictionCone:
37 ✗ os << "CostModelResidualContactFrictionCone";
38 ✗ break;
39 ✗ case ImpulseCostModelTypes::CostModelResidualContactWrenchCone:
40 ✗ os << "CostModelResidualContactWrenchCone";
41 ✗ break;
42 ✗ case ImpulseCostModelTypes::NbImpulseCostModelTypes:
43 ✗ os << "NbImpulseCostModelTypes";
44 ✗ break;
45 ✗ default:
46 ✗ break;
47 }
48 ✗ return os;
49 }
50
51 ✗ ImpulseCostModelFactory::ImpulseCostModelFactory() {}
52 ✗ ImpulseCostModelFactory::~ImpulseCostModelFactory() {}
53
54 ✗ std::shared_ptr<crocoddyl::ActionModelAbstract> ImpulseCostModelFactory::create(
55 ImpulseCostModelTypes::Type cost_type, PinocchioModelTypes::Type model_type,
56 ActivationModelTypes::Type activation_type) const {
57 // Create impulse action model with no cost
58 ✗ std::shared_ptr<crocoddyl::ActionModelImpulseFwdDynamics> action;
59 ✗ switch (model_type) {
60 ✗ case PinocchioModelTypes::Talos:
61 ✗ action = ActionModelFactory().create_impulseFwdDynamics(
62 ✗ StateModelTypes::StateMultibody_Talos);
63 ✗ break;
64 ✗ case PinocchioModelTypes::HyQ:
65 ✗ action = ActionModelFactory().create_impulseFwdDynamics(
66 ✗ StateModelTypes::StateMultibody_HyQ);
67 ✗ break;
68 ✗ default:
69 ✗ throw_pretty(__FILE__ ": Wrong PinocchioModelTypes::Type given");
70 break;
71 }
72 ✗ action->get_costs()->removeCost("state");
73
74 // Create cost
75 ✗ std::shared_ptr<crocoddyl::CostModelAbstract> cost;
76 ✗ PinocchioModelFactory model_factory(model_type);
77 std::shared_ptr<crocoddyl::StateMultibody> state =
78 ✗ std::static_pointer_cast<crocoddyl::StateMultibody>(action->get_state());
79 ✗ Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
80 ✗ std::vector<std::size_t> frame_ids = model_factory.get_frame_ids();
81 ✗ switch (cost_type) {
82 ✗ case ImpulseCostModelTypes::CostModelResidualImpulseCoM:
83 ✗ cost = std::make_shared<crocoddyl::CostModelResidual>(
84 ✗ state, ActivationModelFactory().create(activation_type, 3),
85 ✗ std::make_shared<crocoddyl::ResidualModelImpulseCoM>(state));
86 ✗ action->get_costs()->addCost("cost_0", cost, 0.01);
87 ✗ break;
88 ✗ case ImpulseCostModelTypes::CostModelResidualContactForce:
89 ✗ for (std::size_t i = 0; i < frame_ids.size(); ++i) {
90 ✗ cost = std::make_shared<crocoddyl::CostModelResidual>(
91 state,
92 ✗ ActivationModelFactory().create(activation_type,
93 model_factory.get_contact_nc()),
94 ✗ std::make_shared<crocoddyl::ResidualModelContactForce>(
95 ✗ state, frame_ids[i], pinocchio::Force::Random(),
96 ✗ model_factory.get_contact_nc(), 0));
97 ✗ action->get_costs()->addCost("cost_" + std::to_string(i), cost, 0.01);
98 }
99 ✗ break;
100 ✗ case ImpulseCostModelTypes::CostModelResidualContactCoPPosition:
101 ✗ for (std::size_t i = 0; i < frame_ids.size(); ++i) {
102 ✗ cost = std::make_shared<crocoddyl::CostModelResidual>(
103 ✗ state, ActivationModelFactory().create(activation_type, 4),
104 ✗ std::make_shared<crocoddyl::ResidualModelContactCoPPosition>(
105 ✗ state, frame_ids[i],
106 ✗ CoPSupport(Eigen::Matrix3d::Identity(),
107 ✗ Eigen::Vector2d(0.1, 0.1)),
108 ✗ 0));
109 ✗ action->get_costs()->addCost("cost_" + std::to_string(i), cost, 0.01);
110 }
111 ✗ break;
112 ✗ case ImpulseCostModelTypes::CostModelResidualContactFrictionCone:
113 ✗ for (std::size_t i = 0; i < frame_ids.size(); ++i) {
114 ✗ cost = std::make_shared<crocoddyl::CostModelResidual>(
115 ✗ state, ActivationModelFactory().create(activation_type, 5),
116 ✗ std::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
117 ✗ state, frame_ids[i], crocoddyl::FrictionCone(R, 1.), 0));
118 ✗ action->get_costs()->addCost("cost_" + std::to_string(i), cost, 0.01);
119 }
120 ✗ break;
121 ✗ case ImpulseCostModelTypes::CostModelResidualContactWrenchCone:
122 ✗ for (std::size_t i = 0; i < frame_ids.size(); ++i) {
123 ✗ cost = std::make_shared<crocoddyl::CostModelResidual>(
124 ✗ state, ActivationModelFactory().create(activation_type, 17),
125 ✗ std::make_shared<crocoddyl::ResidualModelContactWrenchCone>(
126 ✗ state, frame_ids[i],
127 ✗ crocoddyl::WrenchCone(R, 1., Eigen::Vector2d(0.1, 0.1)), 0));
128 ✗ action->get_costs()->addCost("cost_" + std::to_string(i), cost, 0.01);
129 }
130 ✗ break;
131 ✗ default:
132 ✗ throw_pretty(__FILE__ ": Wrong ImpulseCostModelTypes::Type given");
133 break;
134 }
135
136 // Include the cost in the impulse model
137 ✗ action->get_costs()->addCost("cost", cost, 0.1);
138 ✗ return action;
139 }
140
141 } // namespace unittest
142 } // namespace crocoddyl
143