GCC Code Coverage Report

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