Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/factory/pinocchio_model.cpp	Lines:	113	122	92.6 %
Date:	2024-02-13 11:12:33	Branches:	86	162	53.1 %


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2022, University of Edinburgh, Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#include "pinocchio_model.hpp"

#include <pinocchio/algorithm/center-of-mass.hpp>
#include <pinocchio/algorithm/centroidal-derivatives.hpp>
#include <pinocchio/algorithm/centroidal.hpp>
#include <pinocchio/algorithm/frames.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
#include <pinocchio/algorithm/kinematics-derivatives.hpp>
#include <pinocchio/algorithm/kinematics.hpp>
#include <pinocchio/algorithm/rnea-derivatives.hpp>
#include <pinocchio/fwd.hpp>
#include <pinocchio/parsers/sample-models.hpp>
#include <pinocchio/parsers/srdf.hpp>
#include <pinocchio/parsers/urdf.hpp>

#include "crocoddyl/core/utils/exception.hpp"

namespace crocoddyl {
namespace unittest {

const std::vector<PinocchioModelTypes::Type> PinocchioModelTypes::all(
    PinocchioModelTypes::init_all());

std::ostream& operator<<(std::ostream& os, PinocchioModelTypes::Type type) {
  switch (type) {
    case PinocchioModelTypes::Hector:
      os << "Hector";
      break;
    case PinocchioModelTypes::TalosArm:
      os << "TalosArm";
      break;
    case PinocchioModelTypes::HyQ:
      os << "HyQ";
      break;
    case PinocchioModelTypes::Talos:
      os << "Talos";
      break;
    case PinocchioModelTypes::RandomHumanoid:
      os << "RandomHumanoid";
      break;
    case PinocchioModelTypes::NbPinocchioModelTypes:
      os << "NbPinocchioModelTypes";
      break;
    default:
      break;
  }
  return os;
}

PinocchioModelFactory::PinocchioModelFactory(PinocchioModelTypes::Type type) {
  frame_name_.clear();
  frame_id_.clear();
  switch (type) {
    case PinocchioModelTypes::Hector:
      construct_model(EXAMPLE_ROBOT_DATA_MODEL_DIR
                      "/hector_description/robots/quadrotor_base.urdf");
      frame_name_.resize(1);
      frame_id_.resize(1);
      frame_name_[0] = "base_link";
      frame_id_[0] = model_->getFrameId(frame_name_[0]);
      contact_nc_ = 6;
      break;
    case PinocchioModelTypes::TalosArm:
      construct_model(
          EXAMPLE_ROBOT_DATA_MODEL_DIR "/talos_data/robots/talos_left_arm.urdf",
          EXAMPLE_ROBOT_DATA_MODEL_DIR "/talos_data/srdf/talos.srdf", false);
      frame_name_.resize(1);
      frame_id_.resize(1);
      frame_name_[0] = "gripper_left_fingertip_1_link";
      frame_id_[0] = model_->getFrameId(frame_name_[0]);
      contact_nc_ = 6;
      break;
    case PinocchioModelTypes::HyQ:
      construct_model(EXAMPLE_ROBOT_DATA_MODEL_DIR
                      "/hyq_description/robots/hyq_no_sensors.urdf",
                      EXAMPLE_ROBOT_DATA_MODEL_DIR
                      "/hyq_description/srdf/hyq.srdf");
      frame_name_.resize(4);
      frame_id_.resize(4);
      frame_name_[0] = "lf_foot";
      frame_name_[1] = "rf_foot";
      frame_name_[2] = "lh_foot";
      frame_name_[3] = "rh_foot";
      for (std::size_t i = 0; i < frame_name_.size(); ++i) {
        frame_id_[i] = model_->getFrameId(frame_name_[i]);
      }
      contact_nc_ = 3;
      break;
    case PinocchioModelTypes::Talos:
      construct_model(
          EXAMPLE_ROBOT_DATA_MODEL_DIR "/talos_data/robots/talos_reduced.urdf",
          EXAMPLE_ROBOT_DATA_MODEL_DIR "/talos_data/srdf/talos.srdf");
      frame_name_.resize(2);
      frame_id_.resize(2);
      frame_name_[0] = "left_sole_link";
      frame_name_[1] = "right_sole_link";
      for (std::size_t i = 0; i < frame_name_.size(); ++i) {
        frame_id_[i] = model_->getFrameId(frame_name_[i]);
      }
      contact_nc_ = 6;
      break;
    case PinocchioModelTypes::RandomHumanoid:
      construct_model();
      frame_name_.resize(2);
      frame_id_.resize(2);
      frame_name_[0] = "rleg6_body";
      frame_name_[1] = "lleg6_body";
      for (std::size_t i = 0; i < frame_name_.size(); ++i) {
        frame_id_[i] = model_->getFrameId(frame_name_[i]);
      }
      contact_nc_ = 6;
      break;
    case PinocchioModelTypes::NbPinocchioModelTypes:
      break;
    default:
      throw_pretty(__FILE__ ": Wrong PinocchioModelTypes::Type given");
      break;
  }
}

PinocchioModelFactory::~PinocchioModelFactory() {}

void PinocchioModelFactory::construct_model(const std::string& urdf_file,
                                            const std::string& srdf_file,
                                            bool free_flyer) {
  model_ = boost::make_shared<pinocchio::Model>();
  if (!urdf_file.empty()) {
    if (free_flyer) {
      pinocchio::urdf::buildModel(urdf_file, pinocchio::JointModelFreeFlyer(),
                                  *model_.get());
      model_->lowerPositionLimit.segment<7>(0).fill(-1.);
      model_->upperPositionLimit.segment<7>(0).fill(1.);
      if (!srdf_file.empty()) {
        pinocchio::srdf::loadReferenceConfigurations(*model_.get(), srdf_file,
                                                     false);
      }
    } else {
      pinocchio::urdf::buildModel(urdf_file, *model_.get());
      if (!srdf_file.empty()) {
        pinocchio::srdf::loadReferenceConfigurations(*model_.get(), srdf_file,
                                                     false);
      }
    }
  } else {
    pinocchio::buildModels::humanoidRandom(*model_.get(), free_flyer);
    model_->lowerPositionLimit.segment<7>(0).fill(-1.);
    model_->upperPositionLimit.segment<7>(0).fill(1.);
  }
}

boost::shared_ptr<pinocchio::Model> PinocchioModelFactory::create() const {
  return model_;
}
std::vector<std::string> PinocchioModelFactory::get_frame_names() const {
  return frame_name_;
}
std::vector<std::size_t> PinocchioModelFactory::get_frame_ids() const {
  return frame_id_;
}
std::size_t PinocchioModelFactory::get_contact_nc() const {
  return contact_nc_;
}

/**
 * @brief Compute all the pinocchio data needed for the numerical
 * differentiation. We use the address of the object to avoid a copy from the
 * "boost::bind".
 *
 * @param model is the rigid body robot model.
 * @param data contains the results of the computations.
 * @param x is the state vector.
 */
void updateAllPinocchio(pinocchio::Model* const model, pinocchio::Data* data,
                        const Eigen::VectorXd& x, const Eigen::VectorXd&) {
  const Eigen::VectorXd& q = x.segment(0, model->nq);
  const Eigen::VectorXd& v = x.segment(model->nq, model->nv);
  Eigen::VectorXd a = Eigen::VectorXd::Zero(model->nv);
  Eigen::Matrix<double, 6, Eigen::Dynamic> tmp;
  tmp.resize(6, model->nv);
  pinocchio::forwardKinematics(*model, *data, q, v, a);
  pinocchio::computeForwardKinematicsDerivatives(*model, *data, q, v, a);
  pinocchio::computeJointJacobians(*model, *data, q);
  pinocchio::updateFramePlacements(*model, *data);
  pinocchio::centerOfMass(*model, *data, q, v, a);
  pinocchio::jacobianCenterOfMass(*model, *data, q);
  pinocchio::computeCentroidalMomentum(*model, *data, q, v);
  pinocchio::computeCentroidalDynamicsDerivatives(*model, *data, q, v, a, tmp,
                                                  tmp, tmp, tmp);
  pinocchio::computeRNEADerivatives(*model, *data, q, v, a);
}

}  // namespace unittest
}  // namespace crocoddyl


Generated by: GCOVR (Version 4.2)

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	✓✓✓✓ ✓✗✗	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		11144	PinocchioModelFactory::PinocchioModelFactory(PinocchioModelTypes::Type type) {
59		11144	frame_name_.clear();
60		11144	frame_id_.clear();
61	✓✓✓✓ ✓✗✗	11144	switch (type) {
62		714	case PinocchioModelTypes::Hector:
63	✓✗✓✗ ✓✗	714	construct_model(EXAMPLE_ROBOT_DATA_MODEL_DIR
64			"/hector_description/robots/quadrotor_base.urdf");
65	✓✗	714	frame_name_.resize(1);
66	✓✗	714	frame_id_.resize(1);
67	✓✗	714	frame_name_[0] = "base_link";
68	✓✗	714	frame_id_[0] = model_->getFrameId(frame_name_[0]);
69		714	contact_nc_ = 6;
70		714	break;
71		4280	case PinocchioModelTypes::TalosArm:
72	✓✗✓✗ ✓✗	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	✓✗	4280	frame_name_.resize(1);
76	✓✗	4280	frame_id_.resize(1);
77	✓✗	4280	frame_name_[0] = "gripper_left_fingertip_1_link";
78	✓✗	4280	frame_id_[0] = model_->getFrameId(frame_name_[0]);
79		4280	contact_nc_ = 6;
80		4280	break;
81		2765	case PinocchioModelTypes::HyQ:
82	✓✗✓✗ ✓✗	2765	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	✓✗	2765	frame_name_.resize(4);
87	✓✗	2765	frame_id_.resize(4);
88	✓✗	2765	frame_name_[0] = "lf_foot";
89	✓✗	2765	frame_name_[1] = "rf_foot";
90	✓✗	2765	frame_name_[2] = "lh_foot";
91	✓✗	2765	frame_name_[3] = "rh_foot";
92	✓✓	13825	for (std::size_t i = 0; i < frame_name_.size(); ++i) {
93	✓✗	11060	frame_id_[i] = model_->getFrameId(frame_name_[i]);
94			}
95		2765	contact_nc_ = 3;
96		2765	break;
97		2473	case PinocchioModelTypes::Talos:
98	✓✗✓✗ ✓✗	2473	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	✓✗	2473	frame_name_.resize(2);
102	✓✗	2473	frame_id_.resize(2);
103	✓✗	2473	frame_name_[0] = "left_sole_link";
104	✓✗	2473	frame_name_[1] = "right_sole_link";
105	✓✓	7419	for (std::size_t i = 0; i < frame_name_.size(); ++i) {
106	✓✗	4946	frame_id_[i] = model_->getFrameId(frame_name_[i]);
107			}
108		2473	contact_nc_ = 6;
109		2473	break;
110		912	case PinocchioModelTypes::RandomHumanoid:
111	✓✗✓✗ ✓✗	912	construct_model();
112	✓✗	912	frame_name_.resize(2);
113	✓✗	912	frame_id_.resize(2);
114	✓✗	912	frame_name_[0] = "rleg6_body";
115	✓✗	912	frame_name_[1] = "lleg6_body";
116	✓✓	2736	for (std::size_t i = 0; i < frame_name_.size(); ++i) {
117	✓✗	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		11144	}
128
129		11144	PinocchioModelFactory::~PinocchioModelFactory() {}
130
131		11144	void PinocchioModelFactory::construct_model(const std::string& urdf_file,
132			const std::string& srdf_file,
133			bool free_flyer) {
134		11144	model_ = boost::make_shared<pinocchio::Model>();
135	✓✓	11144	if (!urdf_file.empty()) {
136	✓✓	10232	if (free_flyer) {
137		5952	pinocchio::urdf::buildModel(urdf_file, pinocchio::JointModelFreeFlyer(),
138	✓✗✓✗	11904	*model_.get());
139	✓✗	5952	model_->lowerPositionLimit.segment<7>(0).fill(-1.);
140	✓✗	5952	model_->upperPositionLimit.segment<7>(0).fill(1.);
141	✓✓	5952	if (!srdf_file.empty()) {
142		5238	pinocchio::srdf::loadReferenceConfigurations(*model_.get(), srdf_file,
143			false);
144			}
145			} else {
146		4280	pinocchio::urdf::buildModel(urdf_file, *model_.get());
147	✓✗	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	✓✗	912	model_->lowerPositionLimit.segment<7>(0).fill(-1.);
155	✓✗	912	model_->upperPositionLimit.segment<7>(0).fill(1.);
156			}
157		11144	}
158
159		11029	boost::shared_ptr<pinocchio::Model> PinocchioModelFactory::create() const {
160		11029	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	✓✗✓✗	129078	const Eigen::VectorXd& q = x.segment(0, model->nq);
184	✓✗✓✗	129078	const Eigen::VectorXd& v = x.segment(model->nq, model->nv);
185	✓✗✓✗	129078	Eigen::VectorXd a = Eigen::VectorXd::Zero(model->nv);
186	✓✗	129078	Eigen::Matrix<double, 6, Eigen::Dynamic> tmp;
187	✓✗	64539	tmp.resize(6, model->nv);
188	✓✗	64539	pinocchio::forwardKinematics(model, data, q, v, a);
189	✓✗	64539	pinocchio::computeForwardKinematicsDerivatives(model, data, q, v, a);
190	✓✗	64539	pinocchio::computeJointJacobians(model, data, q);
191	✓✗	64539	pinocchio::updateFramePlacements(model, data);
192	✓✗	64539	pinocchio::centerOfMass(model, data, q, v, a);
193	✓✗	64539	pinocchio::jacobianCenterOfMass(model, data, q);
194	✓✗	64539	pinocchio::computeCentroidalMomentum(model, data, q, v);
195	✓✗	64539	pinocchio::computeCentroidalDynamicsDerivatives(model, data, q, v, a, tmp,
196			tmp, tmp, tmp);
197	✓✗	64539	pinocchio::computeRNEADerivatives(model, data, q, v, a);
198		64539	}
199
200			} // namespace unittest
201			} // namespace crocoddyl