GCC Code Coverage Report

Directory: ./
File: unittest/factory/contact_cost.cpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 85 94 90.4%
Branches: 88 174 50.6%
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 "contact_cost.hpp"
10
11 #include "crocoddyl/core/costs/residual.hpp"
12 #include "crocoddyl/multibody/residuals/contact-control-gravity.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 "diff_action.hpp"
18
19 namespace crocoddyl {
20 namespace unittest {
21
22 const std::vector<ContactCostModelTypes::Type> ContactCostModelTypes::all(
23 ContactCostModelTypes::init_all());
24
25 48 std::ostream& operator<<(std::ostream& os, ContactCostModelTypes::Type type) {
26
5/7
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 12 times.
✓ Branch 3 taken 6 times.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 48 switch (type) {
27 12 case ContactCostModelTypes::CostModelResidualContactForce:
28 12 os << "CostModelResidualContactForce";
29 12 break;
30 6 case ContactCostModelTypes::CostModelResidualContactCoPPosition:
31 6 os << "CostModelResidualContactCoPPosition";
32 6 break;
33 12 case ContactCostModelTypes::CostModelResidualContactFrictionCone:
34 12 os << "CostModelResidualContactFrictionCone";
35 12 break;
36 6 case ContactCostModelTypes::CostModelResidualContactWrenchCone:
37 6 os << "CostModelResidualContactWrenchCone";
38 6 break;
39 12 case ContactCostModelTypes::CostModelResidualContactControlGrav:
40 12 os << "CostModelResidualContactControlGrav";
41 12 break;
42 ✗ case ContactCostModelTypes::NbContactCostModelTypes:
43 ✗ os << "NbContactCostModelTypes";
44 ✗ break;
45 ✗ default:
46 ✗ break;
47 }
48 48 return os;
49 }
50
51 48 ContactCostModelFactory::ContactCostModelFactory() {}
52 48 ContactCostModelFactory::~ContactCostModelFactory() {}
53
54 std::shared_ptr<crocoddyl::DifferentialActionModelAbstract>
55 48 ContactCostModelFactory::create(
56 ContactCostModelTypes::Type cost_type, PinocchioModelTypes::Type model_type,
57 ActivationModelTypes::Type activation_type,
58 ActuationModelTypes::Type actuation_type) const {
59 // Create contact action model with no cost
60 48 std::shared_ptr<crocoddyl::DifferentialActionModelContactFwdDynamics> action;
61
2/3
✓ Branch 0 taken 30 times.
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 48 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 = DifferentialActionModelFactory().create_contactFwdDynamics(
64 30 StateModelTypes::StateMultibody_Talos, actuation_type, false);
65 30 break;
66 18 case PinocchioModelTypes::HyQ:
67
2/4
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
 36 action = DifferentialActionModelFactory().create_contactFwdDynamics(
68 18 StateModelTypes::StateMultibody_HyQ, actuation_type, false);
69 18 break;
70 ✗ default:
71 ✗ throw_pretty(__FILE__ ": Wrong PinocchioModelTypes::Type given");
72 break;
73 }
74
3/6
✓ Branch 2 taken 48 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 48 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 48 times.
✗ Branch 11 not taken.
 48 action->get_costs()->removeCost("state");
75
3/6
✓ Branch 2 taken 48 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 48 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 48 times.
✗ Branch 11 not taken.
 48 action->get_costs()->removeCost("control");
76
77 // Create cost
78 48 std::shared_ptr<crocoddyl::CostModelAbstract> cost;
79
1/2
✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
 96 PinocchioModelFactory model_factory(model_type);
80 std::shared_ptr<crocoddyl::StateMultibody> state =
81
1/2
✓ Branch 2 taken 48 times.
✗ Branch 3 not taken.
 96 std::static_pointer_cast<crocoddyl::StateMultibody>(action->get_state());
82
2/4
✓ Branch 2 taken 48 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 48 times.
✗ Branch 7 not taken.
 48 const std::size_t nu = action->get_actuation()->get_nu();
83
2/4
✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 48 times.
✗ Branch 5 not taken.
 48 Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
84
1/2
✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
 96 std::vector<std::size_t> frame_ids = model_factory.get_frame_ids();
85
5/6
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 12 times.
✓ Branch 3 taken 6 times.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 48 switch (cost_type) {
86 12 case ContactCostModelTypes::CostModelResidualContactForce:
87
2/2
✓ Branch 1 taken 36 times.
✓ Branch 2 taken 12 times.
 48 for (std::size_t i = 0; i < frame_ids.size(); ++i) {
88
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 72 cost = std::make_shared<crocoddyl::CostModelResidual>(
89 state,
90
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,
91 model_factory.get_contact_nc()),
92
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 72 std::make_shared<crocoddyl::ResidualModelContactForce>(
93 36 state, frame_ids[i], pinocchio::Force::Random(),
94
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(), nu));
95
4/8
✓ Branch 2 taken 36 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 36 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 36 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 36 times.
✗ Branch 14 not taken.
 36 action->get_costs()->addCost("cost_" + std::to_string(i), cost, 0.001);
96 }
97 12 break;
98 6 case ContactCostModelTypes::CostModelResidualContactCoPPosition:
99
2/2
✓ Branch 1 taken 12 times.
✓ Branch 2 taken 6 times.
 18 for (std::size_t i = 0; i < frame_ids.size(); ++i) {
100
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 24 cost = std::make_shared<crocoddyl::CostModelResidual>(
101
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),
102
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 24 std::make_shared<crocoddyl::ResidualModelContactCoPPosition>(
103 12 state, frame_ids[i],
104
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 36 crocoddyl::CoPSupport(R, Eigen::Vector2d(0.1, 0.1)), nu));
105
4/8
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 12 times.
✗ Branch 14 not taken.
 12 action->get_costs()->addCost("cost_" + std::to_string(i), cost, 0.001);
106 }
107 6 break;
108 12 case ContactCostModelTypes::CostModelResidualContactFrictionCone:
109
2/2
✓ Branch 1 taken 36 times.
✓ Branch 2 taken 12 times.
 48 for (std::size_t i = 0; i < frame_ids.size(); ++i) {
110
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 72 cost = std::make_shared<crocoddyl::CostModelResidual>(
111
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),
112
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 72 std::make_shared<crocoddyl::ResidualModelContactFrictionCone>(
113
1/2
✓ Branch 2 taken 36 times.
✗ Branch 3 not taken.
 108 state, frame_ids[i], crocoddyl::FrictionCone(R, 1.), nu));
114
4/8
✓ Branch 2 taken 36 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 36 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 36 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 36 times.
✗ Branch 14 not taken.
 36 action->get_costs()->addCost("cost_" + std::to_string(i), cost, 0.001);
115 }
116 12 break;
117 6 case ContactCostModelTypes::CostModelResidualContactWrenchCone:
118
2/2
✓ Branch 1 taken 12 times.
✓ Branch 2 taken 6 times.
 18 for (std::size_t i = 0; i < frame_ids.size(); ++i) {
119
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 24 cost = std::make_shared<crocoddyl::CostModelResidual>(
120
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),
121
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 24 std::make_shared<crocoddyl::ResidualModelContactWrenchCone>(
122 12 state, frame_ids[i],
123
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)), nu));
124
4/8
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 12 times.
✗ Branch 14 not taken.
 12 action->get_costs()->addCost("cost_" + std::to_string(i), cost, 0.001);
125 }
126 6 break;
127 12 case ContactCostModelTypes::CostModelResidualContactControlGrav:
128
2/2
✓ Branch 1 taken 36 times.
✓ Branch 2 taken 12 times.
 48 for (std::size_t i = 0; i < frame_ids.size(); ++i) {
129
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 72 cost = std::make_shared<crocoddyl::CostModelResidual>(
130 state,
131
3/6
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 36 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 36 times.
✗ Branch 9 not taken.
 72 ActivationModelFactory().create(activation_type, state->get_nv()),
132
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 72 std::make_shared<crocoddyl::ResidualModelContactControlGrav>(state,
133 36 nu));
134
4/8
✓ Branch 2 taken 36 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 36 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 36 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 36 times.
✗ Branch 14 not taken.
 36 action->get_costs()->addCost("cost_" + std::to_string(i), cost, 0.001);
135 }
136 12 break;
137 ✗ default:
138 ✗ throw_pretty(__FILE__ ": Wrong ContactCostModelTypes::Type given");
139 break;
140 }
141 // Include the cost in the contact model
142 96 return action;
143 48 }
144
145 } // namespace unittest
146 } // namespace crocoddyl
147