GCC Code Coverage Report

Directory: ./
File: unittest/factory/pinocchio_model.cpp
Date: 2025-01-16 08:47:40
Exec Total Coverage
Lines: 113 122 92.6%
Branches: 86 162 53.1%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-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 "pinocchio_model.hpp"
10
11 #include <pinocchio/algorithm/center-of-mass.hpp>
12 #include <pinocchio/algorithm/centroidal-derivatives.hpp>
13 #include <pinocchio/algorithm/centroidal.hpp>
14 #include <pinocchio/algorithm/frames.hpp>
15 #include <pinocchio/algorithm/jacobian.hpp>
16 #include <pinocchio/algorithm/kinematics-derivatives.hpp>
17 #include <pinocchio/algorithm/kinematics.hpp>
18 #include <pinocchio/algorithm/rnea-derivatives.hpp>
19 #include <pinocchio/fwd.hpp>
20 #include <pinocchio/parsers/sample-models.hpp>
21 #include <pinocchio/parsers/srdf.hpp>
22 #include <pinocchio/parsers/urdf.hpp>
23
24 #include "crocoddyl/core/utils/exception.hpp"
25
26 namespace crocoddyl {
27 namespace unittest {
28
29 const std::vector<PinocchioModelTypes::Type> PinocchioModelTypes::all(
30 PinocchioModelTypes::init_all());
31
32 195 std::ostream& operator<<(std::ostream& os, PinocchioModelTypes::Type type) {
33
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) {
34 16 case PinocchioModelTypes::Hector:
35 16 os << "Hector";
36 16 break;
37 16 case PinocchioModelTypes::TalosArm:
38 16 os << "TalosArm";
39 16 break;
40 51 case PinocchioModelTypes::HyQ:
41 51 os << "HyQ";
42 51 break;
43 86 case PinocchioModelTypes::Talos:
44 86 os << "Talos";
45 86 break;
46 26 case PinocchioModelTypes::RandomHumanoid:
47 26 os << "RandomHumanoid";
48 26 break;
49 ✗ case PinocchioModelTypes::NbPinocchioModelTypes:
50 ✗ os << "NbPinocchioModelTypes";
51 ✗ break;
52 ✗ default:
53 ✗ break;
54 }
55 195 return os;
56 }
57
58 11164 PinocchioModelFactory::PinocchioModelFactory(PinocchioModelTypes::Type type) {
59 11164 frame_name_.clear();
60 11164 frame_id_.clear();
61
5/7
✓ Branch 0 taken 714 times.
✓ Branch 1 taken 4280 times.
✓ Branch 2 taken 2777 times.
✓ Branch 3 taken 2481 times.
✓ Branch 4 taken 912 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 11164 switch (type) {
62 714 case PinocchioModelTypes::Hector:
63
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
64 "/hector_description/robots/quadrotor_base.urdf");
65
1/2
✓ Branch 1 taken 714 times.
✗ Branch 2 not taken.
 714 frame_name_.resize(1);
66
1/2
✓ Branch 1 taken 714 times.
✗ Branch 2 not taken.
 714 frame_id_.resize(1);
67
1/2
✓ Branch 2 taken 714 times.
✗ Branch 3 not taken.
 714 frame_name_[0] = "base_link";
68
1/2
✓ Branch 3 taken 714 times.
✗ Branch 4 not taken.
 714 frame_id_[0] = model_->getFrameId(frame_name_[0]);
69 714 contact_nc_ = 6;
70 714 break;
71 4280 case PinocchioModelTypes::TalosArm:
72
3/6
✓ Branch 2 taken 4280 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 4280 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 4280 times.
✗ Branch 10 not taken.
 4280 construct_model(
73 EXAMPLE_ROBOT_DATA_MODEL_DIR "/talos_data/robots/talos_left_arm.urdf",
74 EXAMPLE_ROBOT_DATA_MODEL_DIR "/talos_data/srdf/talos.srdf", false);
75
1/2
✓ Branch 1 taken 4280 times.
✗ Branch 2 not taken.
 4280 frame_name_.resize(1);
76
1/2
✓ Branch 1 taken 4280 times.
✗ Branch 2 not taken.
 4280 frame_id_.resize(1);
77
1/2
✓ Branch 2 taken 4280 times.
✗ Branch 3 not taken.
 4280 frame_name_[0] = "gripper_left_fingertip_1_link";
78
1/2
✓ Branch 3 taken 4280 times.
✗ Branch 4 not taken.
 4280 frame_id_[0] = model_->getFrameId(frame_name_[0]);
79 4280 contact_nc_ = 6;
80 4280 break;
81 2777 case PinocchioModelTypes::HyQ:
82
3/6
✓ Branch 2 taken 2777 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2777 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2777 times.
✗ Branch 10 not taken.
 2777 construct_model(EXAMPLE_ROBOT_DATA_MODEL_DIR
83 "/hyq_description/robots/hyq_no_sensors.urdf",
84 EXAMPLE_ROBOT_DATA_MODEL_DIR
85 "/hyq_description/srdf/hyq.srdf");
86
1/2
✓ Branch 1 taken 2777 times.
✗ Branch 2 not taken.
 2777 frame_name_.resize(4);
87
1/2
✓ Branch 1 taken 2777 times.
✗ Branch 2 not taken.
 2777 frame_id_.resize(4);
88
1/2
✓ Branch 2 taken 2777 times.
✗ Branch 3 not taken.
 2777 frame_name_[0] = "lf_foot";
89
1/2
✓ Branch 2 taken 2777 times.
✗ Branch 3 not taken.
 2777 frame_name_[1] = "rf_foot";
90
1/2
✓ Branch 2 taken 2777 times.
✗ Branch 3 not taken.
 2777 frame_name_[2] = "lh_foot";
91
1/2
✓ Branch 2 taken 2777 times.
✗ Branch 3 not taken.
 2777 frame_name_[3] = "rh_foot";
92
2/2
✓ Branch 1 taken 11108 times.
✓ Branch 2 taken 2777 times.
 13885 for (std::size_t i = 0; i < frame_name_.size(); ++i) {
93
1/2
✓ Branch 3 taken 11108 times.
✗ Branch 4 not taken.
 11108 frame_id_[i] = model_->getFrameId(frame_name_[i]);
94 }
95 2777 contact_nc_ = 3;
96 2777 break;
97 2481 case PinocchioModelTypes::Talos:
98
3/6
✓ Branch 2 taken 2481 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2481 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2481 times.
✗ Branch 10 not taken.
 2481 construct_model(
99 EXAMPLE_ROBOT_DATA_MODEL_DIR "/talos_data/robots/talos_reduced.urdf",
100 EXAMPLE_ROBOT_DATA_MODEL_DIR "/talos_data/srdf/talos.srdf");
101
1/2
✓ Branch 1 taken 2481 times.
✗ Branch 2 not taken.
 2481 frame_name_.resize(2);
102
1/2
✓ Branch 1 taken 2481 times.
✗ Branch 2 not taken.
 2481 frame_id_.resize(2);
103
1/2
✓ Branch 2 taken 2481 times.
✗ Branch 3 not taken.
 2481 frame_name_[0] = "left_sole_link";
104
1/2
✓ Branch 2 taken 2481 times.
✗ Branch 3 not taken.
 2481 frame_name_[1] = "right_sole_link";
105
2/2
✓ Branch 1 taken 4962 times.
✓ Branch 2 taken 2481 times.
 7443 for (std::size_t i = 0; i < frame_name_.size(); ++i) {
106
1/2
✓ Branch 3 taken 4962 times.
✗ Branch 4 not taken.
 4962 frame_id_[i] = model_->getFrameId(frame_name_[i]);
107 }
108 2481 contact_nc_ = 6;
109 2481 break;
110 912 case PinocchioModelTypes::RandomHumanoid:
111
3/6
✓ Branch 2 taken 912 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 912 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 912 times.
✗ Branch 10 not taken.
 912 construct_model();
112
1/2
✓ Branch 1 taken 912 times.
✗ Branch 2 not taken.
 912 frame_name_.resize(2);
113
1/2
✓ Branch 1 taken 912 times.
✗ Branch 2 not taken.
 912 frame_id_.resize(2);
114
1/2
✓ Branch 2 taken 912 times.
✗ Branch 3 not taken.
 912 frame_name_[0] = "rleg6_body";
115
1/2
✓ Branch 2 taken 912 times.
✗ Branch 3 not taken.
 912 frame_name_[1] = "lleg6_body";
116
2/2
✓ Branch 1 taken 1824 times.
✓ Branch 2 taken 912 times.
 2736 for (std::size_t i = 0; i < frame_name_.size(); ++i) {
117
1/2
✓ Branch 3 taken 1824 times.
✗ Branch 4 not taken.
 1824 frame_id_[i] = model_->getFrameId(frame_name_[i]);
118 }
119 912 contact_nc_ = 6;
120 912 break;
121 ✗ case PinocchioModelTypes::NbPinocchioModelTypes:
122 ✗ break;
123 ✗ default:
124 ✗ throw_pretty(__FILE__ ": Wrong PinocchioModelTypes::Type given");
125 break;
126 }
127 11164 }
128
129 11164 PinocchioModelFactory::~PinocchioModelFactory() {}
130
131 11164 void PinocchioModelFactory::construct_model(const std::string& urdf_file,
132 const std::string& srdf_file,
133 bool free_flyer) {
134 11164 model_ = boost::make_shared<pinocchio::Model>();
135
2/2
✓ Branch 1 taken 10252 times.
✓ Branch 2 taken 912 times.
 11164 if (!urdf_file.empty()) {
136
2/2
✓ Branch 0 taken 5972 times.
✓ Branch 1 taken 4280 times.
 10252 if (free_flyer) {
137
2/4
✓ Branch 2 taken 5972 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5972 times.
✗ Branch 6 not taken.
 5972 pinocchio::urdf::buildModel(urdf_file, pinocchio::JointModelFreeFlyer(),
138 5972 *model_.get());
139
1/2
✓ Branch 3 taken 5972 times.
✗ Branch 4 not taken.
 5972 model_->lowerPositionLimit.segment<7>(0).fill(-1.);
140
1/2
✓ Branch 3 taken 5972 times.
✗ Branch 4 not taken.
 5972 model_->upperPositionLimit.segment<7>(0).fill(1.);
141
2/2
✓ Branch 1 taken 5258 times.
✓ Branch 2 taken 714 times.
 5972 if (!srdf_file.empty()) {
142 5258 pinocchio::srdf::loadReferenceConfigurations(*model_.get(), srdf_file,
143 false);
144 }
145 } else {
146 4280 pinocchio::urdf::buildModel(urdf_file, *model_.get());
147
1/2
✓ Branch 1 taken 4280 times.
✗ Branch 2 not taken.
 4280 if (!srdf_file.empty()) {
148 4280 pinocchio::srdf::loadReferenceConfigurations(*model_.get(), srdf_file,
149 false);
150 }
151 }
152 } else {
153 912 pinocchio::buildModels::humanoidRandom(*model_.get(), free_flyer);
154
1/2
✓ Branch 3 taken 912 times.
✗ Branch 4 not taken.
 912 model_->lowerPositionLimit.segment<7>(0).fill(-1.);
155
1/2
✓ Branch 3 taken 912 times.
✗ Branch 4 not taken.
 912 model_->upperPositionLimit.segment<7>(0).fill(1.);
156 }
157 11164 }
158
159 11049 boost::shared_ptr<pinocchio::Model> PinocchioModelFactory::create() const {
160 11049 return model_;
161 }
162 25 std::vector<std::string> PinocchioModelFactory::get_frame_names() const {
163 25 return frame_name_;
164 }
165 643 std::vector<std::size_t> PinocchioModelFactory::get_frame_ids() const {
166 643 return frame_id_;
167 }
168 160 std::size_t PinocchioModelFactory::get_contact_nc() const {
169 160 return contact_nc_;
170 }
171
172 /**
173 * @brief Compute all the pinocchio data needed for the numerical
174 * differentiation. We use the address of the object to avoid a copy from the
175 * "boost::bind".
176 *
177 * @param model is the rigid body robot model.
178 * @param data contains the results of the computations.
179 * @param x is the state vector.
180 */
181 64539 void updateAllPinocchio(pinocchio::Model* const model, pinocchio::Data* data,
182 const Eigen::VectorXd& x, const Eigen::VectorXd&) {
183
2/4
✓ Branch 1 taken 64539 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 64539 times.
✗ Branch 5 not taken.
 64539 const Eigen::VectorXd& q = x.segment(0, model->nq);
184
2/4
✓ Branch 1 taken 64539 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 64539 times.
✗ Branch 5 not taken.
 64539 const Eigen::VectorXd& v = x.segment(model->nq, model->nv);
185
2/4
✓ Branch 1 taken 64539 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 64539 times.
✗ Branch 5 not taken.
 64539 Eigen::VectorXd a = Eigen::VectorXd::Zero(model->nv);
186
1/2
✓ Branch 1 taken 64539 times.
✗ Branch 2 not taken.
 64539 Eigen::Matrix<double, 6, Eigen::Dynamic> tmp;
187
1/2
✓ Branch 1 taken 64539 times.
✗ Branch 2 not taken.
 64539 tmp.resize(6, model->nv);
188
1/2
✓ Branch 1 taken 64539 times.
✗ Branch 2 not taken.
 64539 pinocchio::forwardKinematics(*model, *data, q, v, a);
189
1/2
✓ Branch 1 taken 64539 times.
✗ Branch 2 not taken.
 64539 pinocchio::computeForwardKinematicsDerivatives(*model, *data, q, v, a);
190
1/2
✓ Branch 1 taken 64539 times.
✗ Branch 2 not taken.
 64539 pinocchio::computeJointJacobians(*model, *data, q);
191
1/2
✓ Branch 1 taken 64539 times.
✗ Branch 2 not taken.
 64539 pinocchio::updateFramePlacements(*model, *data);
192
1/2
✓ Branch 1 taken 64539 times.
✗ Branch 2 not taken.
 64539 pinocchio::centerOfMass(*model, *data, q, v, a);
193
1/2
✓ Branch 1 taken 64539 times.
✗ Branch 2 not taken.
 64539 pinocchio::jacobianCenterOfMass(*model, *data, q);
194
1/2
✓ Branch 1 taken 64539 times.
✗ Branch 2 not taken.
 64539 pinocchio::computeCentroidalMomentum(*model, *data, q, v);
195
1/2
✓ Branch 1 taken 64539 times.
✗ Branch 2 not taken.
 64539 pinocchio::computeCentroidalDynamicsDerivatives(*model, *data, q, v, a, tmp,
196 tmp, tmp, tmp);
197
1/2
✓ Branch 1 taken 64539 times.
✗ Branch 2 not taken.
 64539 pinocchio::computeRNEADerivatives(*model, *data, q, v, a);
198 64539 }
199
200 } // namespace unittest
201 } // namespace crocoddyl
202