GCC Code Coverage Report

Directory: ./
File: unittest/factory/pinocchio_model.cpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 97 106 91.5%
Branches: 69 128 53.9%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-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 "pinocchio_model.hpp"
10
11 #include <pinocchio/fwd.hpp>
12 #include <pinocchio/parsers/sample-models.hpp>
13 #include <pinocchio/parsers/srdf.hpp>
14 #include <pinocchio/parsers/urdf.hpp>
15
16 #include "crocoddyl/core/utils/exception.hpp"
17
18 namespace crocoddyl {
19 namespace unittest {
20
21 const std::vector<PinocchioModelTypes::Type> PinocchioModelTypes::all(
22 PinocchioModelTypes::init_all());
23
24 195 std::ostream& operator<<(std::ostream& os, PinocchioModelTypes::Type type) {
25
5/7
✓ Branch 0 taken 16 times.
✓ Branch 1 taken 16 times.
✓ Branch 2 taken 51 times.
✓ Branch 3 taken 86 times.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 195 switch (type) {
26 16 case PinocchioModelTypes::Hector:
27 16 os << "Hector";
28 16 break;
29 16 case PinocchioModelTypes::TalosArm:
30 16 os << "TalosArm";
31 16 break;
32 51 case PinocchioModelTypes::HyQ:
33 51 os << "HyQ";
34 51 break;
35 86 case PinocchioModelTypes::Talos:
36 86 os << "Talos";
37 86 break;
38 26 case PinocchioModelTypes::RandomHumanoid:
39 26 os << "RandomHumanoid";
40 26 break;
41 ✗ case PinocchioModelTypes::NbPinocchioModelTypes:
42 ✗ os << "NbPinocchioModelTypes";
43 ✗ break;
44 ✗ default:
45 ✗ break;
46 }
47 195 return os;
48 }
49
50 11046 PinocchioModelFactory::PinocchioModelFactory(PinocchioModelTypes::Type type) {
51 11046 frame_name_.clear();
52 11046 frame_id_.clear();
53
5/7
✓ Branch 0 taken 714 times.
✓ Branch 1 taken 4244 times.
✓ Branch 2 taken 2748 times.
✓ Branch 3 taken 2457 times.
✓ Branch 4 taken 883 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 11046 switch (type) {
54 714 case PinocchioModelTypes::Hector:
55
3/6
✓ Branch 2 taken 714 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 714 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 714 times.
✗ Branch 10 not taken.
 714 construct_model(EXAMPLE_ROBOT_DATA_MODEL_DIR
56 "/hector_description/robots/quadrotor_base.urdf");
57
1/2
✓ Branch 1 taken 714 times.
✗ Branch 2 not taken.
 714 frame_name_.resize(1);
58
1/2
✓ Branch 1 taken 714 times.
✗ Branch 2 not taken.
 714 frame_id_.resize(1);
59
1/2
✓ Branch 2 taken 714 times.
✗ Branch 3 not taken.
 714 frame_name_[0] = "base_link";
60
1/2
✓ Branch 3 taken 714 times.
✗ Branch 4 not taken.
 714 frame_id_[0] = model_->getFrameId(frame_name_[0]);
61 714 contact_nc_ = 6;
62 714 break;
63 4244 case PinocchioModelTypes::TalosArm:
64
3/6
✓ Branch 2 taken 4244 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 4244 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 4244 times.
✗ Branch 10 not taken.
 4244 construct_model(
65 EXAMPLE_ROBOT_DATA_MODEL_DIR "/talos_data/robots/talos_left_arm.urdf",
66 EXAMPLE_ROBOT_DATA_MODEL_DIR "/talos_data/srdf/talos.srdf", false);
67
1/2
✓ Branch 1 taken 4244 times.
✗ Branch 2 not taken.
 4244 frame_name_.resize(1);
68
1/2
✓ Branch 1 taken 4244 times.
✗ Branch 2 not taken.
 4244 frame_id_.resize(1);
69
1/2
✓ Branch 2 taken 4244 times.
✗ Branch 3 not taken.
 4244 frame_name_[0] = "gripper_left_fingertip_1_link";
70
1/2
✓ Branch 3 taken 4244 times.
✗ Branch 4 not taken.
 4244 frame_id_[0] = model_->getFrameId(frame_name_[0]);
71 4244 contact_nc_ = 6;
72 4244 break;
73 2748 case PinocchioModelTypes::HyQ:
74
3/6
✓ Branch 2 taken 2748 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2748 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2748 times.
✗ Branch 10 not taken.
 2748 construct_model(EXAMPLE_ROBOT_DATA_MODEL_DIR
75 "/hyq_description/robots/hyq_no_sensors.urdf",
76 EXAMPLE_ROBOT_DATA_MODEL_DIR
77 "/hyq_description/srdf/hyq.srdf");
78
1/2
✓ Branch 1 taken 2748 times.
✗ Branch 2 not taken.
 2748 frame_name_.resize(4);
79
1/2
✓ Branch 1 taken 2748 times.
✗ Branch 2 not taken.
 2748 frame_id_.resize(4);
80
1/2
✓ Branch 2 taken 2748 times.
✗ Branch 3 not taken.
 2748 frame_name_[0] = "lf_foot";
81
1/2
✓ Branch 2 taken 2748 times.
✗ Branch 3 not taken.
 2748 frame_name_[1] = "rf_foot";
82
1/2
✓ Branch 2 taken 2748 times.
✗ Branch 3 not taken.
 2748 frame_name_[2] = "lh_foot";
83
1/2
✓ Branch 2 taken 2748 times.
✗ Branch 3 not taken.
 2748 frame_name_[3] = "rh_foot";
84
2/2
✓ Branch 1 taken 10992 times.
✓ Branch 2 taken 2748 times.
 13740 for (std::size_t i = 0; i < frame_name_.size(); ++i) {
85
1/2
✓ Branch 3 taken 10992 times.
✗ Branch 4 not taken.
 10992 frame_id_[i] = model_->getFrameId(frame_name_[i]);
86 }
87 2748 contact_nc_ = 3;
88 2748 break;
89 2457 case PinocchioModelTypes::Talos:
90
3/6
✓ Branch 2 taken 2457 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2457 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2457 times.
✗ Branch 10 not taken.
 2457 construct_model(
91 EXAMPLE_ROBOT_DATA_MODEL_DIR "/talos_data/robots/talos_reduced.urdf",
92 EXAMPLE_ROBOT_DATA_MODEL_DIR "/talos_data/srdf/talos.srdf");
93
1/2
✓ Branch 1 taken 2457 times.
✗ Branch 2 not taken.
 2457 frame_name_.resize(2);
94
1/2
✓ Branch 1 taken 2457 times.
✗ Branch 2 not taken.
 2457 frame_id_.resize(2);
95
1/2
✓ Branch 2 taken 2457 times.
✗ Branch 3 not taken.
 2457 frame_name_[0] = "left_sole_link";
96
1/2
✓ Branch 2 taken 2457 times.
✗ Branch 3 not taken.
 2457 frame_name_[1] = "right_sole_link";
97
2/2
✓ Branch 1 taken 4914 times.
✓ Branch 2 taken 2457 times.
 7371 for (std::size_t i = 0; i < frame_name_.size(); ++i) {
98
1/2
✓ Branch 3 taken 4914 times.
✗ Branch 4 not taken.
 4914 frame_id_[i] = model_->getFrameId(frame_name_[i]);
99 }
100 2457 contact_nc_ = 6;
101 2457 break;
102 883 case PinocchioModelTypes::RandomHumanoid:
103
3/6
✓ Branch 2 taken 883 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 883 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 883 times.
✗ Branch 10 not taken.
 883 construct_model();
104
1/2
✓ Branch 1 taken 883 times.
✗ Branch 2 not taken.
 883 frame_name_.resize(2);
105
1/2
✓ Branch 1 taken 883 times.
✗ Branch 2 not taken.
 883 frame_id_.resize(2);
106
1/2
✓ Branch 2 taken 883 times.
✗ Branch 3 not taken.
 883 frame_name_[0] = "rleg6_body";
107
1/2
✓ Branch 2 taken 883 times.
✗ Branch 3 not taken.
 883 frame_name_[1] = "lleg6_body";
108
2/2
✓ Branch 1 taken 1766 times.
✓ Branch 2 taken 883 times.
 2649 for (std::size_t i = 0; i < frame_name_.size(); ++i) {
109
1/2
✓ Branch 3 taken 1766 times.
✗ Branch 4 not taken.
 1766 frame_id_[i] = model_->getFrameId(frame_name_[i]);
110 }
111 883 contact_nc_ = 6;
112 883 break;
113 ✗ case PinocchioModelTypes::NbPinocchioModelTypes:
114 ✗ break;
115 ✗ default:
116 ✗ throw_pretty(__FILE__ ": Wrong PinocchioModelTypes::Type given");
117 break;
118 }
119 11046 }
120
121 11046 PinocchioModelFactory::~PinocchioModelFactory() {}
122
123 11046 void PinocchioModelFactory::construct_model(const std::string& urdf_file,
124 const std::string& srdf_file,
125 bool free_flyer) {
126 11046 model_ = std::make_shared<pinocchio::Model>();
127
2/2
✓ Branch 1 taken 10163 times.
✓ Branch 2 taken 883 times.
 11046 if (!urdf_file.empty()) {
128
2/2
✓ Branch 0 taken 5919 times.
✓ Branch 1 taken 4244 times.
 10163 if (free_flyer) {
129
2/4
✓ Branch 2 taken 5919 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5919 times.
✗ Branch 6 not taken.
 5919 pinocchio::urdf::buildModel(urdf_file, pinocchio::JointModelFreeFlyer(),
130 5919 *model_.get());
131
1/2
✓ Branch 3 taken 5919 times.
✗ Branch 4 not taken.
 5919 model_->lowerPositionLimit.segment<7>(0).fill(-1.);
132
1/2
✓ Branch 3 taken 5919 times.
✗ Branch 4 not taken.
 5919 model_->upperPositionLimit.segment<7>(0).fill(1.);
133
2/2
✓ Branch 1 taken 5205 times.
✓ Branch 2 taken 714 times.
 5919 if (!srdf_file.empty()) {
134 5205 pinocchio::srdf::loadReferenceConfigurations(*model_.get(), srdf_file,
135 false);
136 }
137 } else {
138 4244 pinocchio::urdf::buildModel(urdf_file, *model_.get());
139
1/2
✓ Branch 1 taken 4244 times.
✗ Branch 2 not taken.
 4244 if (!srdf_file.empty()) {
140 4244 pinocchio::srdf::loadReferenceConfigurations(*model_.get(), srdf_file,
141 false);
142 }
143 }
144 } else {
145 883 pinocchio::buildModels::humanoidRandom(*model_.get(), free_flyer);
146
1/2
✓ Branch 3 taken 883 times.
✗ Branch 4 not taken.
 883 model_->lowerPositionLimit.segment<7>(0).fill(-1.);
147
1/2
✓ Branch 3 taken 883 times.
✗ Branch 4 not taken.
 883 model_->upperPositionLimit.segment<7>(0).fill(1.);
148 }
149 11046 }
150
151 10931 std::shared_ptr<pinocchio::Model> PinocchioModelFactory::create() const {
152 10931 return model_;
153 }
154 25 std::vector<std::string> PinocchioModelFactory::get_frame_names() const {
155 25 return frame_name_;
156 }
157 643 std::vector<std::size_t> PinocchioModelFactory::get_frame_ids() const {
158 643 return frame_id_;
159 }
160 160 std::size_t PinocchioModelFactory::get_contact_nc() const {
161 160 return contact_nc_;
162 }
163
164 } // namespace unittest
165 } // namespace crocoddyl
166