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