GCC Code Coverage Report

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		✗	std::ostream& operator<<(std::ostream& os, PinocchioModelTypes::Type type) {
25		✗	switch (type) {
26		✗	case PinocchioModelTypes::Hector:
27		✗	os << "Hector";
28		✗	break;
29		✗	case PinocchioModelTypes::TalosArm:
30		✗	os << "TalosArm";
31		✗	break;
32		✗	case PinocchioModelTypes::HyQ:
33		✗	os << "HyQ";
34		✗	break;
35		✗	case PinocchioModelTypes::Talos:
36		✗	os << "Talos";
37		✗	break;
38		✗	case PinocchioModelTypes::RandomHumanoid:
39		✗	os << "RandomHumanoid";
40		✗	break;
41		✗	case PinocchioModelTypes::NbPinocchioModelTypes:
42		✗	os << "NbPinocchioModelTypes";
43		✗	break;
44		✗	default:
45		✗	break;
46			}
47		✗	return os;
48			}
49
50		✗	PinocchioModelFactory::PinocchioModelFactory(PinocchioModelTypes::Type type) {
51		✗	frame_name_.clear();
52		✗	frame_id_.clear();
53		✗	switch (type) {
54		✗	case PinocchioModelTypes::Hector:
55		✗	construct_model(EXAMPLE_ROBOT_DATA_MODEL_DIR
56			"/hector_description/robots/quadrotor_base.urdf");
57		✗	frame_name_.resize(1);
58		✗	frame_id_.resize(1);
59		✗	frame_name_[0] = "base_link";
60		✗	frame_id_[0] = model_->getFrameId(frame_name_[0]);
61		✗	contact_nc_ = 6;
62		✗	break;
63		✗	case PinocchioModelTypes::TalosArm:
64		✗	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		✗	frame_name_.resize(1);
68		✗	frame_id_.resize(1);
69		✗	frame_name_[0] = "gripper_left_fingertip_1_link";
70		✗	frame_id_[0] = model_->getFrameId(frame_name_[0]);
71		✗	contact_nc_ = 6;
72		✗	break;
73		✗	case PinocchioModelTypes::HyQ:
74		✗	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		✗	frame_name_.resize(4);
79		✗	frame_id_.resize(4);
80		✗	frame_name_[0] = "lf_foot";
81		✗	frame_name_[1] = "rf_foot";
82		✗	frame_name_[2] = "lh_foot";
83		✗	frame_name_[3] = "rh_foot";
84		✗	for (std::size_t i = 0; i < frame_name_.size(); ++i) {
85		✗	frame_id_[i] = model_->getFrameId(frame_name_[i]);
86			}
87		✗	contact_nc_ = 3;
88		✗	break;
89		✗	case PinocchioModelTypes::Talos:
90		✗	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		✗	frame_name_.resize(2);
94		✗	frame_id_.resize(2);
95		✗	frame_name_[0] = "left_sole_link";
96		✗	frame_name_[1] = "right_sole_link";
97		✗	for (std::size_t i = 0; i < frame_name_.size(); ++i) {
98		✗	frame_id_[i] = model_->getFrameId(frame_name_[i]);
99			}
100		✗	contact_nc_ = 6;
101		✗	break;
102		✗	case PinocchioModelTypes::RandomHumanoid:
103		✗	construct_model();
104		✗	frame_name_.resize(2);
105		✗	frame_id_.resize(2);
106		✗	frame_name_[0] = "rleg6_body";
107		✗	frame_name_[1] = "lleg6_body";
108		✗	for (std::size_t i = 0; i < frame_name_.size(); ++i) {
109		✗	frame_id_[i] = model_->getFrameId(frame_name_[i]);
110			}
111		✗	contact_nc_ = 6;
112		✗	break;
113		✗	case PinocchioModelTypes::NbPinocchioModelTypes:
114		✗	break;
115		✗	default:
116		✗	throw_pretty(__FILE__ ": Wrong PinocchioModelTypes::Type given");
117			break;
118			}
119		✗	}
120
121		✗	PinocchioModelFactory::~PinocchioModelFactory() {}
122
123		✗	void PinocchioModelFactory::construct_model(const std::string& urdf_file,
124			const std::string& srdf_file,
125			bool free_flyer) {
126		✗	model_ = std::make_shared<pinocchio::Model>();
127		✗	if (!urdf_file.empty()) {
128		✗	if (free_flyer) {
129		✗	pinocchio::urdf::buildModel(urdf_file, pinocchio::JointModelFreeFlyer(),
130		✗	*model_.get());
131		✗	model_->lowerPositionLimit.segment<7>(0).fill(-1.);
132		✗	model_->upperPositionLimit.segment<7>(0).fill(1.);
133		✗	if (!srdf_file.empty()) {
134		✗	pinocchio::srdf::loadReferenceConfigurations(*model_.get(), srdf_file,
135			false);
136			}
137			} else {
138		✗	pinocchio::urdf::buildModel(urdf_file, *model_.get());
139		✗	if (!srdf_file.empty()) {
140		✗	pinocchio::srdf::loadReferenceConfigurations(*model_.get(), srdf_file,
141			false);
142			}
143			}
144			} else {
145		✗	pinocchio::buildModels::humanoidRandom(*model_.get(), free_flyer);
146		✗	model_->lowerPositionLimit.segment<7>(0).fill(-1.);
147		✗	model_->upperPositionLimit.segment<7>(0).fill(1.);
148			}
149		✗	}
150
151		✗	std::shared_ptr<pinocchio::Model> PinocchioModelFactory::create() const {
152		✗	return model_;
153			}
154		✗	std::vector<std::string> PinocchioModelFactory::get_frame_names() const {
155		✗	return frame_name_;
156			}
157		✗	std::vector<std::size_t> PinocchioModelFactory::get_frame_ids() const {
158		✗	return frame_id_;
159			}
160		✗	std::size_t PinocchioModelFactory::get_contact_nc() const {
161		✗	return contact_nc_;
162			}
163
164			} // namespace unittest
165			} // namespace crocoddyl
166