GCC Code Coverage Report
 Directory: ./ Exec Total Coverage File: src/parsers/urdf/model.cpp Lines: 111 152 73.0 % Date: 2024-01-23 21:41:47 Branches: 179 463 38.7 %

 Line Branch Exec Source `1` `//` `2` `// Copyright (c) 2015-2020 CNRS` `3` `// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.` `4` `//` `5` `6` `#include "pinocchio/parsers/urdf.hpp"` `7` `#include "pinocchio/parsers/urdf/utils.hpp"` `8` `#include "pinocchio/parsers/urdf/model.hxx"` `9` `10` `#include ` `11` `#include ` `12` `13` `#include ` `14` `#include ` `15` `#include ` `16` `17` `namespace pinocchio` `18` `{` `19` ` namespace urdf` `20` ` {` `21` ` namespace details` `22` ` {` `23` ` ///` `24` ` /// \brief Convert URDF Inertial quantity to Spatial Inertia.` `25` ` ///` `26` ` /// \param[in] Y The input URDF Inertia.` `27` ` ///` `28` ` /// \return The converted Spatial Inertia pinocchio::Inertia.` `29` ` ///` `30` `1257` ` static Inertia convertFromUrdf(const ::urdf::Inertial & Y)` `31` ` {` `32` `1257` ` const ::urdf::Vector3 & p = Y.origin.position;` `33` `1257` ` const ::urdf::Rotation & q = Y.origin.rotation;` `34` `35` ✓✗ `1257` ` const Inertia::Vector3 com(p.x,p.y,p.z);` `36` ✓✗✓✗ `1257` ` const Inertia::Matrix3 & R = Eigen::Quaterniond(q.w,q.x,q.y,q.z).matrix();` `37` `38` ✓✗ `1257` ` Inertia::Matrix3 I;` `39` ✓✗✓✗✓✗ `2514` ` I << Y.ixx,Y.ixy,Y.ixz,` `40` ✓✗✓✗✓✗ `1257` ` Y.ixy,Y.iyy,Y.iyz,` `41` ✓✗✓✗✓✗ `1257` ` Y.ixz,Y.iyz,Y.izz;` `42` ✓✗✓✗✓✗✓✗✓✗ `1257` ` return Inertia(Y.mass,com,R*I*R.transpose());` `43` ` }` `44` `45` `1671` ` static Inertia convertFromUrdf(const ::urdf::InertialSharedPtr & Y)` `46` ` {` `47` ✓✓ `1671` ` if (Y) return convertFromUrdf(*Y);` `48` `414` ` return Inertia::Zero();` `49` ` }` `50` `51` `1619` ` static FrameIndex getParentLinkFrame(const ::urdf::LinkConstSharedPtr link,` `52` ` UrdfVisitorBase & model)` `53` ` {` `54` ✓✗✗✓✓✗✗✓✗✗ `1619` ` PINOCCHIO_CHECK_INPUT_ARGUMENT(link && link->getParent());` `55` ✓✗ `1619` ` FrameIndex id = model.getBodyId(link->getParent()->name);` `56` `1619` ` return id;` `57` ` }` `58` `59` ` ///` `60` ` /// \brief Recursive procedure for reading the URDF tree.` `61` ` /// The function returns an exception as soon as a necessary Inertia or Joint information are missing.` `62` ` ///` `63` ` /// \param[in] link The current URDF link.` `64` ` /// \param[in] model The model where the link must be added.` `65` ` ///` `66` `1619` ` void parseTree(::urdf::LinkConstSharedPtr link,` `67` ` UrdfVisitorBase & model)` `68` ` {` `69` ` typedef UrdfVisitorBase::Scalar Scalar;` `70` ` typedef UrdfVisitorBase::SE3 SE3;` `71` ` typedef UrdfVisitorBase::Vector Vector;` `72` ` typedef UrdfVisitorBase::Vector3 Vector3;` `73` ` typedef Model::FrameIndex FrameIndex;` `74` `75` ` // Parent joint of the current body` `76` ` const ::urdf::JointConstSharedPtr joint =` `77` `3238` ` ::urdf::const_pointer_cast< ::urdf::Joint>(link->parent_joint);` `78` `79` ✓✗ `1619` ` if(joint) // if the link is not the root of the tree` `80` ` {` `81` ✗✓✗✗ `1619` ` PINOCCHIO_CHECK_INPUT_ARGUMENT(link->getParent());` `82` `83` `1619` ` const std::string & joint_name = joint->name;` `84` `1619` ` const std::string & link_name = link->name;` `85` `1619` ` const std::string & parent_link_name = link->getParent()->name;` `86` ✓✗ `3238` ` std::ostringstream joint_info;` `87` `88` ✓✗ `1619` ` FrameIndex parentFrameId = getParentLinkFrame(link, model);` `89` `90` ` // Transformation from the parent link to the joint origin` `91` ` const SE3 jointPlacement` `92` ✓✗ `1619` ` = convertFromUrdf(joint->parent_to_joint_origin_transform);` `93` `94` ✓✗ `1619` ` const Inertia Y = convertFromUrdf(link->inertial);` `95` `96` ✓✗✓✗✓✗✓✗ `3238` ` Vector max_effort(1), max_velocity(1), min_config(1), max_config(1);` `97` ✓✗✓✗✓✗✓✗ `3238` ` Vector friction(Vector::Constant(1,0.)), damping(Vector::Constant(1,0.));` `98` ✓✗ `1619` ` Vector3 axis (joint->axis.x, joint->axis.y, joint->axis.z);` `99` `100` `1619` ` const Scalar infty = std::numeric_limits::infinity();` `101` `102` ✗✓✓✓✗✓✗ `1619` ` switch(joint->type)` `103` ` {` `104` ` case ::urdf::Joint::FLOATING:` `105` ` joint_info << "joint FreeFlyer";` `106` `107` ` max_effort = Vector::Constant(6, infty);` `108` ` max_velocity = Vector::Constant(6, infty);` `109` ` min_config = Vector::Constant(7,-infty);` `110` ` max_config = Vector::Constant(7, infty);` `111` ` min_config.tail<4>().setConstant(-1.01);` `112` ` max_config.tail<4>().setConstant( 1.01);` `113` `114` ` friction = Vector::Constant(6, 0.);` `115` ` damping = Vector::Constant(6, 0.);` `116` `117` ` model.addJointAndBody(UrdfVisitorBase::FLOATING, axis,` `118` ` parentFrameId,jointPlacement,joint->name,` `119` ` Y,link->name,` `120` ` max_effort,max_velocity,min_config,max_config,` `121` ` friction,damping);` `122` ` break;` `123` `124` `1047` ` case ::urdf::Joint::REVOLUTE:` `125` ✓✗ `1047` ` joint_info << "joint REVOLUTE with axis";` `126` `127` ` // TODO I think the URDF standard forbids REVOLUTE with no limits.` `128` ✗✓ `1047` ` assert(joint->limits);` `129` ✓✗ `1047` ` if(joint->limits)` `130` ` {` `131` ✓✗ `1047` ` max_effort << joint->limits->effort;` `132` ✓✗ `1047` ` max_velocity << joint->limits->velocity;` `133` ✓✗ `1047` ` min_config << joint->limits->lower;` `134` ✓✗ `1047` ` max_config << joint->limits->upper;` `135` ` }` `136` `137` ✓✓ `1047` ` if(joint->dynamics)` `138` ` {` `139` ✓✗ `197` ` friction << joint->dynamics->friction;` `140` ✓✗ `197` ` damping << joint->dynamics->damping;` `141` ` }` `142` `143` ✓✗✓✗✓✗✓✗✓✗ `2094` ` model.addJointAndBody(UrdfVisitorBase::REVOLUTE, axis,` `144` ✓✗ `1047` ` parentFrameId,jointPlacement,joint->name,` `145` `1047` ` Y,link->name,` `146` ` max_effort,max_velocity,min_config,max_config,` `147` ✓✗ `1047` ` friction,damping);` `148` `1047` ` break;` `149` `150` `1` ` case ::urdf::Joint::CONTINUOUS: // Revolute joint with no joint limits` `151` ✓✗ `1` ` joint_info << "joint CONTINUOUS with axis";` `152` `153` ✓✗ `1` ` min_config.resize(2);` `154` ✓✗ `1` ` max_config.resize(2);` `155` ✓✗✓✗ `1` ` min_config << -1.01, -1.01;` `156` ✓✗✓✗ `1` ` max_config << 1.01, 1.01;` `157` `158` ✓✗ `1` ` if(joint->limits)` `159` ` {` `160` ✓✗ `1` ` max_effort << joint->limits->effort;` `161` ✓✗ `1` ` max_velocity << joint->limits->velocity;` `162` ` }` `163` ` else` `164` ` {` `165` ` max_effort << infty;` `166` ` max_velocity << infty;` `167` ` }` `168` `169` ✗✓ `1` ` if(joint->dynamics)` `170` ` {` `171` ` friction << joint->dynamics->friction;` `172` ` damping << joint->dynamics->damping;` `173` ` }` `174` `175` ✓✗✓✗✓✗✓✗✓✗ `2` ` model.addJointAndBody(UrdfVisitorBase::CONTINUOUS, axis,` `176` ✓✗ `1` ` parentFrameId,jointPlacement,joint->name,` `177` `1` ` Y,link->name,` `178` ` max_effort,max_velocity,min_config,max_config,` `179` ✓✗ `1` ` friction,damping);` `180` `1` ` break;` `181` `182` `6` ` case ::urdf::Joint::PRISMATIC:` `183` ✓✗ `6` ` joint_info << "joint PRISMATIC with axis";` `184` `185` ` // TODO I think the URDF standard forbids REVOLUTE with no limits.` `186` ✗✓ `6` ` assert(joint->limits);` `187` ✓✗ `6` ` if (joint->limits)` `188` ` {` `189` ✓✗ `6` ` max_effort << joint->limits->effort;` `190` ✓✗ `6` ` max_velocity << joint->limits->velocity;` `191` ✓✗ `6` ` min_config << joint->limits->lower;` `192` ✓✗ `6` ` max_config << joint->limits->upper;` `193` ` }` `194` `195` ✓✓ `6` ` if(joint->dynamics)` `196` ` {` `197` ✓✗ `4` ` friction << joint->dynamics->friction;` `198` ✓✗ `4` ` damping << joint->dynamics->damping;` `199` ` }` `200` `201` ✓✗✓✗✓✗✓✗✓✗ `12` ` model.addJointAndBody(UrdfVisitorBase::PRISMATIC, axis,` `202` ✓✗ `6` ` parentFrameId,jointPlacement,joint->name,` `203` `6` ` Y,link->name,` `204` ` max_effort,max_velocity,min_config,max_config,` `205` ✓✗ `6` ` friction,damping);` `206` `6` ` break;` `207` `208` ` case ::urdf::Joint::PLANAR:` `209` ` joint_info << "joint PLANAR with normal axis along Z";` `210` `211` ` max_effort = Vector::Constant(3, infty);` `212` ` max_velocity = Vector::Constant(3, infty);` `213` ` min_config = Vector::Constant(4,-infty);` `214` ` max_config = Vector::Constant(4, infty);` `215` ` min_config.tail<2>().setConstant(-1.01);` `216` ` max_config.tail<2>().setConstant( 1.01);` `217` `218` ` friction = Vector::Constant(3, 0.);` `219` ` damping = Vector::Constant(3, 0.);` `220` `221` ` model.addJointAndBody(UrdfVisitorBase::PLANAR, axis,` `222` ` parentFrameId,jointPlacement,joint->name,` `223` ` Y,link->name,` `224` ` max_effort,max_velocity,min_config,max_config,` `225` ` friction,damping);` `226` ` break;` `227` `228` `565` ` case ::urdf::Joint::FIXED:` `229` ` // In case of fixed joint, if link has inertial tag:` `230` ` // -add the inertia of the link to his parent in the model` `231` ` // Otherwise do nothing.` `232` ` // In all cases:` `233` ` // -let all the children become children of parent` `234` ` // -inform the parser of the offset to apply` `235` ` // -add fixed body in model to display it in gepetto-viewer` `236` `237` ✓✗ `565` ` joint_info << "fixed joint";` `238` `565` ` model.addFixedJointAndBody(parentFrameId, jointPlacement,` `239` ✓✗ `565` ` joint_name, Y, link_name);` `240` `565` ` break;` `241` `242` ` default:` `243` ` throw std::invalid_argument("The type of joint " + joint_name + " is not supported.");` `244` ` break;` `245` ` }` `246` `247` ` model` `248` ✓✗✓✗ `1619` ` << "Adding Body" << '\n'` `249` ✓✗✓✗✓✗✓✗✓✗✓✗✓✗ `1619` ` << '\"' << link_name << "\" connected to \"" << parent_link_name << "\" through joint \"" << joint_name << "\"\n"` `250` ✓✗✓✗✓✗✓✗ `3238` ` << "joint type: " << joint_info.str() << '\n'` `251` ✓✗✓✗✓✗ `1619` ` << "joint placement:\n" << jointPlacement << '\n'` `252` ✓✗✓✗ `1619` ` << "body info: " << '\n'` `253` ✓✗✓✗✓✗ `1619` ` << " mass: " << Y.mass() << '\n'` `254` ✓✗✓✗✓✗✓✗ `1619` ` << " lever: " << Y.lever().transpose() << '\n'` `255` ✓✗✓✗✓✗✓✗✓✗ `1619` ` << " inertia elements (Ixx,Iyx,Iyy,Izx,Izy,Izz): " << Y.inertia().data().transpose() << '\n' << '\n';` `256` ` }` `257` ` else if (link->getParent())` `258` ` throw std::invalid_argument(link->name + " - joint information missing.");` `259` `260` ✓✗✓✗✓✗✓✗✓✗✓✗✓✗✓✗✓✓✓✓✓✗✓✗✓✓✓✗✓✗ `4677` ` BOOST_FOREACH(::urdf::LinkConstSharedPtr child, link->child_links)` `261` ` {` `262` ✓✗ `1529` ` parseTree(child, model);` `263` ` }` `264` `1619` ` }` `265` `266` ` ///` `267` ` /// \brief Parse a tree with a specific root joint linking the model to the environment.` `268` ` /// The function returns an exception as soon as a necessary Inertia or Joint information are missing.` `269` ` ///` `270` ` /// \param[in] link The current URDF link.` `271` ` /// \param[in] model The model where the link must be added.` `272` ` ///` `273` `52` ` void parseRootTree(const ::urdf::ModelInterface * urdfTree,` `274` ` UrdfVisitorBase& model)` `275` ` {` `276` ✓✗ `52` ` model.setName(urdfTree->getName());` `277` `278` `104` ` ::urdf::LinkConstSharedPtr root_link = urdfTree->getRoot();` `279` ✓✗✓✓ `52` ` model.addRootJoint(convertFromUrdf(root_link->inertial), root_link->name);` `280` `281` ✓✗✓✗✓✗✓✗✓✗✓✗✓✗✓✗✓✓✓✓✓✗✓✗✓✓✓✗✓✗ `231` ` BOOST_FOREACH(::urdf::LinkConstSharedPtr child, root_link->child_links)` `282` ` {` `283` ✓✗ `90` ` parseTree(child, model);` `284` ` }` `285` `51` ` }` `286` `287` `43` ` void parseRootTree(const std::string & filename,` `288` ` UrdfVisitorBase& model)` `289` ` {` `290` ✓✗ `43` ` ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile (filename);` `291` ✓✗ `43` ` if (urdfTree)` `292` ✓✗ `86` ` return parseRootTree (urdfTree.get(), model);` `293` ` else` `294` ` throw std::invalid_argument("The file " + filename + " does not "` `295` ` "contain a valid URDF model.");` `296` ` }` `297` `298` `7` ` void parseRootTreeFromXML(const std::string & xmlString,` `299` ` UrdfVisitorBase& model)` `300` ` {` `301` ✓✗ `8` ` ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDF(xmlString);` `302` ✓✗ `7` ` if (urdfTree)` `303` ✓✓ `13` ` return parseRootTree (urdfTree.get(), model);` `304` ` else` `305` ` throw std::invalid_argument("The XML stream does not contain a valid "` `306` ` "URDF model.");` `307` ` }` `308` ` } // namespace details` `309` ` } // namespace urdf` `310` `} // namespace pinocchio`

 Generated by: GCOVR (Version 4.2)