| Directory: | ./ |
|---|---|
| File: | include/pinocchio/parsers/urdf/model.hxx |
| Date: | 2025-04-30 16:14:33 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 172 | 259 | 66.4% |
| Branches: | 154 | 469 | 32.8% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2015-2021 CNRS INRIA | ||
| 3 | // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France. | ||
| 4 | // | ||
| 5 | |||
| 6 | #ifndef __pinocchio_multibody_parsers_urdf_model_hxx__ | ||
| 7 | #define __pinocchio_multibody_parsers_urdf_model_hxx__ | ||
| 8 | |||
| 9 | #include "pinocchio/math/matrix.hpp" | ||
| 10 | #include "pinocchio/parsers/config.hpp" | ||
| 11 | #include "pinocchio/parsers/urdf.hpp" | ||
| 12 | #include "pinocchio/multibody/model.hpp" | ||
| 13 | |||
| 14 | #include <sstream> | ||
| 15 | #include <boost/foreach.hpp> | ||
| 16 | #include <limits> | ||
| 17 | #include <iostream> | ||
| 18 | #include <boost/optional.hpp> | ||
| 19 | #include <boost/none.hpp> | ||
| 20 | |||
| 21 | namespace pinocchio | ||
| 22 | { | ||
| 23 | namespace urdf | ||
| 24 | { | ||
| 25 | namespace details | ||
| 26 | { | ||
| 27 | typedef double urdf_scalar_type; | ||
| 28 | |||
| 29 | template<typename _Scalar, int Options> | ||
| 30 | struct MimicInfo; | ||
| 31 | |||
| 32 | template<typename _Scalar, int Options> | ||
| 33 | class UrdfVisitorBaseTpl | ||
| 34 | { | ||
| 35 | public: | ||
| 36 | enum JointType | ||
| 37 | { | ||
| 38 | REVOLUTE, | ||
| 39 | CONTINUOUS, | ||
| 40 | PRISMATIC, | ||
| 41 | FLOATING, | ||
| 42 | PLANAR, | ||
| 43 | SPHERICAL, | ||
| 44 | MIMIC | ||
| 45 | }; | ||
| 46 | |||
| 47 | typedef enum ::pinocchio::FrameType FrameType; | ||
| 48 | typedef _Scalar Scalar; | ||
| 49 | typedef SE3Tpl<Scalar, Options> SE3; | ||
| 50 | typedef InertiaTpl<Scalar, Options> Inertia; | ||
| 51 | |||
| 52 | typedef Eigen::Matrix<Scalar, 3, 1> Vector3; | ||
| 53 | typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector; | ||
| 54 | typedef Eigen::Ref<Vector> VectorRef; | ||
| 55 | typedef Eigen::Ref<const Vector> VectorConstRef; | ||
| 56 | typedef MimicInfo<Scalar, Options> MimicInfo_; | ||
| 57 | |||
| 58 | virtual void setName(const std::string & name) = 0; | ||
| 59 | |||
| 60 | virtual void addRootJoint(const Inertia & Y, const std::string & body_name) = 0; | ||
| 61 | |||
| 62 | virtual void addJointAndBody( | ||
| 63 | JointType type, | ||
| 64 | const Vector3 & axis, | ||
| 65 | const FrameIndex & parentFrameId, | ||
| 66 | const SE3 & placement, | ||
| 67 | const std::string & joint_name, | ||
| 68 | const Inertia & Y, | ||
| 69 | const std::string & body_name, | ||
| 70 | const VectorConstRef & max_effort, | ||
| 71 | const VectorConstRef & max_velocity, | ||
| 72 | const VectorConstRef & min_config, | ||
| 73 | const VectorConstRef & max_config, | ||
| 74 | const VectorConstRef & friction, | ||
| 75 | const VectorConstRef & damping, | ||
| 76 | const boost::optional<MimicInfo_> & mimic_info = boost::none) = 0; | ||
| 77 | |||
| 78 | virtual void addJointAndBody( | ||
| 79 | JointType type, | ||
| 80 | const Vector3 & axis, | ||
| 81 | const FrameIndex & parentFrameId, | ||
| 82 | const SE3 & placement, | ||
| 83 | const std::string & joint_name, | ||
| 84 | const Inertia & Y, | ||
| 85 | const SE3 & frame_placement, | ||
| 86 | const std::string & body_name, | ||
| 87 | const VectorConstRef & max_effort, | ||
| 88 | const VectorConstRef & max_velocity, | ||
| 89 | const VectorConstRef & min_config, | ||
| 90 | const VectorConstRef & max_config, | ||
| 91 | const VectorConstRef & friction, | ||
| 92 | const VectorConstRef & damping, | ||
| 93 | const boost::optional<MimicInfo_> & mimic_info = boost::none) = 0; | ||
| 94 | |||
| 95 | virtual void addFixedJointAndBody( | ||
| 96 | const FrameIndex & parentFrameId, | ||
| 97 | const SE3 & joint_placement, | ||
| 98 | const std::string & joint_name, | ||
| 99 | const Inertia & Y, | ||
| 100 | const std::string & body_name) = 0; | ||
| 101 | |||
| 102 | virtual void appendBodyToJoint( | ||
| 103 | const FrameIndex fid, | ||
| 104 | const Inertia & Y, | ||
| 105 | const SE3 & placement, | ||
| 106 | const std::string & body_name) = 0; | ||
| 107 | |||
| 108 | virtual FrameIndex getBodyId(const std::string & frame_name) const = 0; | ||
| 109 | |||
| 110 | virtual JointIndex getJointId(const std::string & joint_name) const = 0; | ||
| 111 | |||
| 112 | virtual const std::string & getJointName(const JointIndex jointId) const = 0; | ||
| 113 | |||
| 114 | virtual Frame getBodyFrame(const std::string & frame_name) const = 0; | ||
| 115 | |||
| 116 | virtual JointIndex getParentId(const std::string & frame_name) const = 0; | ||
| 117 | |||
| 118 | virtual bool existFrame(const std::string & frame_name, const FrameType type) const = 0; | ||
| 119 | |||
| 120 | 127 | UrdfVisitorBaseTpl() | |
| 121 | 127 | : log(NULL) | |
| 122 | { | ||
| 123 | 127 | } | |
| 124 | |||
| 125 | template<typename T> | ||
| 126 | 125147 | UrdfVisitorBaseTpl & operator<<(const T & t) | |
| 127 | { | ||
| 128 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 38352 times.
|
125147 | if (log != NULL) |
| 129 | ✗ | *log << t; | |
| 130 | 125147 | return *this; | |
| 131 | } | ||
| 132 | |||
| 133 | std::ostream * log; | ||
| 134 | }; | ||
| 135 | |||
| 136 | template<typename _Scalar, int Options> | ||
| 137 | struct MimicInfo | ||
| 138 | { | ||
| 139 | typedef _Scalar Scalar; | ||
| 140 | typedef Eigen::Matrix<Scalar, 3, 1> Vector3; | ||
| 141 | |||
| 142 | std::string mimicked_name; | ||
| 143 | Scalar multiplier; | ||
| 144 | Scalar offset; | ||
| 145 | Vector3 axis; | ||
| 146 | |||
| 147 | // Use the JointType from UrdfVisitorBaseTpl | ||
| 148 | typedef typename UrdfVisitorBaseTpl<_Scalar, Options>::JointType JointType; | ||
| 149 | JointType jointType; | ||
| 150 | |||
| 151 | MimicInfo() = default; | ||
| 152 | |||
| 153 | ✗ | MimicInfo( | |
| 154 | std::string _mimicked_name, | ||
| 155 | Scalar _multiplier, | ||
| 156 | Scalar _offset, | ||
| 157 | Vector3 _axis, | ||
| 158 | JointType _jointType) | ||
| 159 | ✗ | : mimicked_name(_mimicked_name) | |
| 160 | ✗ | , multiplier(_multiplier) | |
| 161 | ✗ | , offset(_offset) | |
| 162 | ✗ | , axis(_axis) | |
| 163 | ✗ | , jointType(_jointType) | |
| 164 | { | ||
| 165 | } | ||
| 166 | }; | ||
| 167 | |||
| 168 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 169 | class UrdfVisitor : public UrdfVisitorBaseTpl<Scalar, Options> | ||
| 170 | { | ||
| 171 | public: | ||
| 172 | typedef UrdfVisitorBaseTpl<Scalar, Options> Base; | ||
| 173 | typedef typename Base::JointType JointType; | ||
| 174 | typedef typename Base::Vector3 Vector3; | ||
| 175 | typedef typename Base::VectorConstRef VectorConstRef; | ||
| 176 | typedef typename Base::SE3 SE3; | ||
| 177 | typedef typename Base::Inertia Inertia; | ||
| 178 | |||
| 179 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
| 180 | typedef typename Model::JointCollection JointCollection; | ||
| 181 | typedef typename Model::JointModel JointModel; | ||
| 182 | typedef typename Model::Frame Frame; | ||
| 183 | |||
| 184 | typedef MimicInfo<Scalar, Options> MimicInfo_; | ||
| 185 | |||
| 186 | Model & model; | ||
| 187 | std::string root_joint_name; | ||
| 188 | |||
| 189 | 69 | UrdfVisitor(Model & model) | |
| 190 | 69 | : model(model) | |
| 191 | { | ||
| 192 | 69 | } | |
| 193 | |||
| 194 | 58 | UrdfVisitor(Model & model, const std::string & rjn) | |
| 195 | 58 | : model(model) | |
| 196 | 58 | , root_joint_name(rjn) | |
| 197 | { | ||
| 198 | 58 | } | |
| 199 | |||
| 200 | 112 | void setName(const std::string & name) | |
| 201 | { | ||
| 202 | 112 | model.name = name; | |
| 203 | 112 | } | |
| 204 | |||
| 205 | 51 | virtual void addRootJoint(const Inertia & Y, const std::string & body_name) | |
| 206 | { | ||
| 207 | 51 | const Frame & parent_frame = model.frames[0]; | |
| 208 | |||
| 209 |
1/2✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
|
51 | model.addFrame( |
| 210 | 102 | Frame(body_name, parent_frame.parentJoint, 0, parent_frame.placement, BODY, Y)); | |
| 211 | 51 | } | |
| 212 | |||
| 213 | 2104 | void addJointAndBody( | |
| 214 | JointType type, | ||
| 215 | const Vector3 & axis, | ||
| 216 | const FrameIndex & parentFrameId, | ||
| 217 | const SE3 & placement, | ||
| 218 | const std::string & joint_name, | ||
| 219 | const Inertia & Y, | ||
| 220 | const std::string & body_name, | ||
| 221 | const VectorConstRef & max_effort, | ||
| 222 | const VectorConstRef & max_velocity, | ||
| 223 | const VectorConstRef & min_config, | ||
| 224 | const VectorConstRef & max_config, | ||
| 225 | const VectorConstRef & friction, | ||
| 226 | const VectorConstRef & damping, | ||
| 227 | const boost::optional<MimicInfo_> & mimic_info = boost::none) | ||
| 228 | { | ||
| 229 |
1/2✓ Branch 2 taken 2104 times.
✗ Branch 3 not taken.
|
2104 | addJointAndBody( |
| 230 | type, axis, parentFrameId, placement, joint_name, Y, SE3::Identity(), body_name, | ||
| 231 | max_effort, max_velocity, min_config, max_config, friction, damping, mimic_info); | ||
| 232 | 2104 | } | |
| 233 | |||
| 234 | 2268 | void addJointAndBody( | |
| 235 | JointType type, | ||
| 236 | const Vector3 & axis, | ||
| 237 | const FrameIndex & parentFrameId, | ||
| 238 | const SE3 & placement, | ||
| 239 | const std::string & joint_name, | ||
| 240 | const Inertia & Y, | ||
| 241 | const SE3 & frame_placement, | ||
| 242 | const std::string & body_name, | ||
| 243 | const VectorConstRef & max_effort, | ||
| 244 | const VectorConstRef & max_velocity, | ||
| 245 | const VectorConstRef & min_config, | ||
| 246 | const VectorConstRef & max_config, | ||
| 247 | const VectorConstRef & friction, | ||
| 248 | const VectorConstRef & damping, | ||
| 249 | const boost::optional<MimicInfo_> & mimic_info = boost::none) | ||
| 250 | { | ||
| 251 | JointIndex joint_id; | ||
| 252 | 2268 | const Frame & frame = model.frames[parentFrameId]; | |
| 253 |
5/8✓ Branch 0 taken 4 times.
✓ Branch 1 taken 2220 times.
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 34 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
|
2268 | switch (type) |
| 254 | { | ||
| 255 | 4 | case Base::FLOATING: | |
| 256 |
7/14✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 4 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 4 times.
✗ Branch 20 not taken.
|
16 | joint_id = model.addJoint( |
| 257 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
12 | frame.parentJoint, typename JointCollection::JointModelFreeFlyer(), |
| 258 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | frame.placement * placement, joint_name, max_effort, max_velocity, min_config, |
| 259 | max_config, friction, damping); | ||
| 260 | 4 | break; | |
| 261 | 2220 | case Base::REVOLUTE: | |
| 262 | 2220 | joint_id = addJoint< | |
| 263 | typename JointCollection::JointModelRX, typename JointCollection::JointModelRY, | ||
| 264 | typename JointCollection::JointModelRZ, | ||
| 265 |
1/2✓ Branch 1 taken 2220 times.
✗ Branch 2 not taken.
|
2220 | typename JointCollection::JointModelRevoluteUnaligned>( |
| 266 | axis, frame, placement, joint_name, max_effort, max_velocity, min_config, max_config, | ||
| 267 | friction, damping); | ||
| 268 | 2220 | break; | |
| 269 | 1 | case Base::CONTINUOUS: | |
| 270 | 1 | joint_id = addJoint< | |
| 271 | typename JointCollection::JointModelRUBX, typename JointCollection::JointModelRUBY, | ||
| 272 | typename JointCollection::JointModelRUBZ, | ||
| 273 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | typename JointCollection::JointModelRevoluteUnboundedUnaligned>( |
| 274 | axis, frame, placement, joint_name, max_effort, max_velocity, min_config, max_config, | ||
| 275 | friction, damping); | ||
| 276 | 1 | break; | |
| 277 | 34 | case Base::PRISMATIC: | |
| 278 | 34 | joint_id = addJoint< | |
| 279 | typename JointCollection::JointModelPX, typename JointCollection::JointModelPY, | ||
| 280 | typename JointCollection::JointModelPZ, | ||
| 281 |
1/2✓ Branch 1 taken 34 times.
✗ Branch 2 not taken.
|
34 | typename JointCollection::JointModelPrismaticUnaligned>( |
| 282 | axis, frame, placement, joint_name, max_effort, max_velocity, min_config, max_config, | ||
| 283 | friction, damping); | ||
| 284 | 34 | break; | |
| 285 | ✗ | case Base::PLANAR: | |
| 286 | ✗ | joint_id = model.addJoint( | |
| 287 | ✗ | frame.parentJoint, typename JointCollection::JointModelPlanar(), | |
| 288 | ✗ | frame.placement * placement, joint_name, max_effort, max_velocity, min_config, | |
| 289 | max_config, friction, damping); | ||
| 290 | ✗ | break; | |
| 291 | 9 | case Base::SPHERICAL: | |
| 292 |
7/14✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 9 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 9 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 9 times.
✗ Branch 20 not taken.
|
36 | joint_id = model.addJoint( |
| 293 |
2/4✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
|
27 | frame.parentJoint, typename JointCollection::JointModelSpherical(), |
| 294 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
9 | frame.placement * placement, joint_name, max_effort, max_velocity, min_config, |
| 295 | max_config, friction, damping); | ||
| 296 | 9 | break; | |
| 297 | ✗ | case Base::MIMIC: | |
| 298 | ✗ | if (mimic_info) | |
| 299 | ✗ | switch (mimic_info->jointType) | |
| 300 | { | ||
| 301 | ✗ | case Base::REVOLUTE: | |
| 302 | ✗ | joint_id = addMimicJoint< | |
| 303 | typename JointCollection::JointModelRX, typename JointCollection::JointModelRY, | ||
| 304 | typename JointCollection::JointModelRZ, | ||
| 305 | ✗ | typename JointCollection::JointModelRevoluteUnaligned>( | |
| 306 | frame, placement, joint_name, max_effort, max_velocity, min_config, max_config, | ||
| 307 | ✗ | friction, damping, *mimic_info); | |
| 308 | ✗ | break; | |
| 309 | ✗ | case Base::PRISMATIC: | |
| 310 | ✗ | joint_id = addMimicJoint< | |
| 311 | typename JointCollection::JointModelPX, typename JointCollection::JointModelPY, | ||
| 312 | typename JointCollection::JointModelPZ, | ||
| 313 | ✗ | typename JointCollection::JointModelPrismaticUnaligned>( | |
| 314 | frame, placement, joint_name, max_effort, max_velocity, min_config, max_config, | ||
| 315 | ✗ | friction, damping, *mimic_info); | |
| 316 | ✗ | break; | |
| 317 | ✗ | default: | |
| 318 | ✗ | PINOCCHIO_CHECK_INPUT_ARGUMENT( | |
| 319 | false, "Cannot mimic this type. Only revolute, prismatic and helicoidal can be " | ||
| 320 | "mimicked"); | ||
| 321 | break; | ||
| 322 | } | ||
| 323 | else | ||
| 324 | ✗ | PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "Cannot create mimic joint. Missing info.") | |
| 325 | ✗ | break; | |
| 326 | ✗ | default: | |
| 327 | ✗ | PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The joint type is not correct."); | |
| 328 | }; | ||
| 329 | |||
| 330 |
1/2✓ Branch 1 taken 2268 times.
✗ Branch 2 not taken.
|
2268 | FrameIndex jointFrameId = model.addJointFrame(joint_id, (int)parentFrameId); |
| 331 |
1/2✓ Branch 1 taken 2268 times.
✗ Branch 2 not taken.
|
2268 | appendBodyToJoint(jointFrameId, Y, frame_placement, body_name); |
| 332 | 2268 | } | |
| 333 | |||
| 334 | 1100 | void addFixedJointAndBody( | |
| 335 | const FrameIndex & parent_frame_id, | ||
| 336 | const SE3 & joint_placement, | ||
| 337 | const std::string & joint_name, | ||
| 338 | const Inertia & Y, | ||
| 339 | const std::string & body_name) | ||
| 340 | { | ||
| 341 | 1100 | const Frame & parent_frame = model.frames[parent_frame_id]; | |
| 342 | 1100 | const JointIndex parent_frame_parent = parent_frame.parentJoint; | |
| 343 | |||
| 344 |
1/2✓ Branch 1 taken 1100 times.
✗ Branch 2 not taken.
|
1100 | const SE3 placement = parent_frame.placement * joint_placement; |
| 345 |
1/2✓ Branch 1 taken 1100 times.
✗ Branch 2 not taken.
|
1100 | FrameIndex fid = model.addFrame(Frame( |
| 346 |
1/2✓ Branch 1 taken 1100 times.
✗ Branch 2 not taken.
|
1100 | joint_name, parent_frame.parentJoint, parent_frame_id, placement, FIXED_JOINT, Y)); |
| 347 | |||
| 348 |
1/2✓ Branch 1 taken 1100 times.
✗ Branch 2 not taken.
|
1100 | model.addBodyFrame(body_name, parent_frame_parent, placement, (int)fid); |
| 349 | 1100 | } | |
| 350 | |||
| 351 | 2332 | void appendBodyToJoint( | |
| 352 | const FrameIndex fid, | ||
| 353 | const Inertia & Y, | ||
| 354 | const SE3 & placement, | ||
| 355 | const std::string & body_name) | ||
| 356 | { | ||
| 357 | 2332 | const Frame & frame = model.frames[fid]; | |
| 358 |
1/2✓ Branch 1 taken 2332 times.
✗ Branch 2 not taken.
|
2332 | const SE3 & p = frame.placement * placement; |
| 359 | assert(frame.parentJoint >= 0); | ||
| 360 |
3/4✓ Branch 1 taken 2332 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1941 times.
✓ Branch 4 taken 391 times.
|
2332 | if (!Y.isZero(Scalar(0))) |
| 361 | { | ||
| 362 |
1/2✓ Branch 1 taken 1941 times.
✗ Branch 2 not taken.
|
1941 | model.appendBodyToJoint(frame.parentJoint, Y, p); |
| 363 | } | ||
| 364 | |||
| 365 |
1/2✓ Branch 1 taken 2332 times.
✗ Branch 2 not taken.
|
2332 | model.addBodyFrame(body_name, frame.parentJoint, p, (int)fid); |
| 366 | // Reference to model.frames[fid] can has changed because the vector | ||
| 367 | // may have been reallocated. | ||
| 368 | 2332 | assert(model.frames[fid].parentJoint >= 0); | |
| 369 | { | ||
| 370 |
4/8✓ Branch 4 taken 2332 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 2332 times.
✗ Branch 7 not taken.
✓ Branch 13 taken 2332 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 2332 times.
✗ Branch 16 not taken.
|
2332 | assert( |
| 371 | !hasNaN(model.inertias[model.frames[fid].parentJoint].lever()) | ||
| 372 | && !hasNaN(model.inertias[model.frames[fid].parentJoint].inertia().data())); | ||
| 373 | } | ||
| 374 | 2332 | } | |
| 375 | |||
| 376 | 3562 | FrameIndex getBodyId(const std::string & frame_name) const | |
| 377 | { | ||
| 378 | |||
| 379 |
2/4✓ Branch 1 taken 3562 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3562 times.
✗ Branch 4 not taken.
|
3562 | if (model.existFrame(frame_name, BODY)) |
| 380 | { | ||
| 381 |
1/2✓ Branch 1 taken 3562 times.
✗ Branch 2 not taken.
|
3562 | FrameIndex fid = model.getFrameId(frame_name, BODY); |
| 382 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 3562 times.
|
3562 | assert(model.frames[fid].type == BODY); |
| 383 | 3562 | return fid; | |
| 384 | } | ||
| 385 | else | ||
| 386 | ✗ | throw std::invalid_argument("Model does not have any body named " + frame_name); | |
| 387 | } | ||
| 388 | |||
| 389 | 164 | FrameIndex getJointId(const std::string & joint_name) const | |
| 390 | { | ||
| 391 | |||
| 392 |
1/2✓ Branch 1 taken 164 times.
✗ Branch 2 not taken.
|
164 | if (model.existJointName(joint_name)) |
| 393 | { | ||
| 394 | 164 | JointIndex jid = model.getJointId(joint_name); | |
| 395 | 164 | return jid; | |
| 396 | } | ||
| 397 | else | ||
| 398 | ✗ | throw std::invalid_argument("Model does not have any joint named " + joint_name); | |
| 399 | } | ||
| 400 | |||
| 401 | ✗ | const std::string & getJointName(const JointIndex jointId) const | |
| 402 | { | ||
| 403 | ✗ | return model.names[jointId]; | |
| 404 | } | ||
| 405 | |||
| 406 | 29 | Frame getBodyFrame(const std::string & frame_name) const | |
| 407 | { | ||
| 408 | |||
| 409 |
2/4✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 29 times.
✗ Branch 4 not taken.
|
29 | if (model.existFrame(frame_name, BODY)) |
| 410 | { | ||
| 411 |
1/2✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
|
29 | FrameIndex fid = model.getFrameId(frame_name, BODY); |
| 412 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 29 times.
|
29 | assert(model.frames[fid].type == BODY); |
| 413 | 29 | return model.frames[fid]; | |
| 414 | } | ||
| 415 | else | ||
| 416 | ✗ | throw std::invalid_argument("Model does not have any body named " + frame_name); | |
| 417 | } | ||
| 418 | |||
| 419 | 16 | JointIndex getParentId(const std::string & frame_name) const | |
| 420 | { | ||
| 421 | |||
| 422 |
2/4✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 16 times.
✗ Branch 4 not taken.
|
16 | if (model.existFrame(frame_name, BODY)) |
| 423 | { | ||
| 424 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
16 | FrameIndex fid = model.getFrameId(frame_name, BODY); |
| 425 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 16 times.
|
16 | assert(model.frames[fid].type == BODY); |
| 426 | 16 | return model.frames[fid].parentJoint; | |
| 427 | } | ||
| 428 | else | ||
| 429 | ✗ | throw std::invalid_argument("Model does not have any body named " + frame_name); | |
| 430 | } | ||
| 431 | |||
| 432 | ✗ | bool existFrame(const std::string & frame_name, const FrameType type) const | |
| 433 | { | ||
| 434 | ✗ | return model.existFrame(frame_name, type); | |
| 435 | } | ||
| 436 | |||
| 437 | template<typename TypeX, typename TypeY, typename TypeZ, typename TypeUnaligned> | ||
| 438 | 4510 | JointIndex addJoint( | |
| 439 | const Vector3 & axis, | ||
| 440 | const Frame & frame, | ||
| 441 | const SE3 & placement, | ||
| 442 | const std::string & joint_name, | ||
| 443 | const VectorConstRef & max_effort, | ||
| 444 | const VectorConstRef & max_velocity, | ||
| 445 | const VectorConstRef & min_config, | ||
| 446 | const VectorConstRef & max_config, | ||
| 447 | const VectorConstRef & friction, | ||
| 448 | const VectorConstRef & damping) | ||
| 449 | { | ||
| 450 | 4510 | CartesianAxis axisType = extractCartesianAxis(axis); | |
| 451 |
4/5✓ Branch 0 taken 547 times.
✓ Branch 1 taken 797 times.
✓ Branch 2 taken 598 times.
✓ Branch 3 taken 313 times.
✗ Branch 4 not taken.
|
4510 | switch (axisType) |
| 452 | { | ||
| 453 | 1094 | case AXIS_X: | |
| 454 |
6/12✓ Branch 2 taken 547 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 547 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 547 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 547 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 547 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 547 times.
✗ Branch 18 not taken.
|
4376 | return model.addJoint( |
| 455 |
3/6✓ Branch 1 taken 547 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 547 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 547 times.
✗ Branch 8 not taken.
|
3282 | frame.parentJoint, TypeX(), frame.placement * placement, joint_name, max_effort, |
| 456 | 1094 | max_velocity, min_config, max_config, friction, damping); | |
| 457 | break; | ||
| 458 | |||
| 459 | 1594 | case AXIS_Y: | |
| 460 |
6/12✓ Branch 2 taken 797 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 797 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 797 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 797 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 797 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 797 times.
✗ Branch 18 not taken.
|
6376 | return model.addJoint( |
| 461 |
3/6✓ Branch 1 taken 797 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 797 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 797 times.
✗ Branch 8 not taken.
|
4782 | frame.parentJoint, TypeY(), frame.placement * placement, joint_name, max_effort, |
| 462 | 1594 | max_velocity, min_config, max_config, friction, damping); | |
| 463 | break; | ||
| 464 | |||
| 465 | 1196 | case AXIS_Z: | |
| 466 |
6/12✓ Branch 2 taken 598 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 598 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 598 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 598 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 598 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 598 times.
✗ Branch 18 not taken.
|
4784 | return model.addJoint( |
| 467 |
3/6✓ Branch 1 taken 598 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 598 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 598 times.
✗ Branch 8 not taken.
|
3588 | frame.parentJoint, TypeZ(), frame.placement * placement, joint_name, max_effort, |
| 468 | 1196 | max_velocity, min_config, max_config, friction, damping); | |
| 469 | break; | ||
| 470 | |||
| 471 | 626 | case AXIS_UNALIGNED: | |
| 472 |
6/12✓ Branch 2 taken 313 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 313 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 313 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 313 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 313 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 313 times.
✗ Branch 18 not taken.
|
2504 | return model.addJoint( |
| 473 |
4/8✓ Branch 1 taken 313 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 313 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 313 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 313 times.
✗ Branch 11 not taken.
|
1878 | frame.parentJoint, TypeUnaligned(axis.normalized()), frame.placement * placement, |
| 474 | 626 | joint_name, max_effort, max_velocity, min_config, max_config, friction, damping); | |
| 475 | break; | ||
| 476 | ✗ | default: | |
| 477 | ✗ | PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The axis type of the joint is of wrong type."); | |
| 478 | break; | ||
| 479 | } | ||
| 480 | } | ||
| 481 | |||
| 482 | template<typename TypeX, typename TypeY, typename TypeZ, typename TypeUnaligned> | ||
| 483 | ✗ | JointIndex addMimicJoint( | |
| 484 | const Frame & frame, | ||
| 485 | const SE3 & placement, | ||
| 486 | const std::string & joint_name, | ||
| 487 | const VectorConstRef & max_effort, | ||
| 488 | const VectorConstRef & max_velocity, | ||
| 489 | const VectorConstRef & min_config, | ||
| 490 | const VectorConstRef & max_config, | ||
| 491 | const VectorConstRef & friction, | ||
| 492 | const VectorConstRef & damping, | ||
| 493 | const MimicInfo_ & mimic_info) | ||
| 494 | { | ||
| 495 | ✗ | if (!model.existJointName(mimic_info.mimicked_name)) | |
| 496 | ✗ | PINOCCHIO_CHECK_INPUT_ARGUMENT( | |
| 497 | false, "The parent joint of the mimic joint is not in the kinematic tree"); | ||
| 498 | |||
| 499 | ✗ | auto mimicked_joint = model.joints[getJointId(mimic_info.mimicked_name)]; | |
| 500 | |||
| 501 | ✗ | CartesianAxis axisType = extractCartesianAxis(mimic_info.axis); | |
| 502 | ✗ | switch (axisType) | |
| 503 | { | ||
| 504 | ✗ | case AXIS_X: | |
| 505 | ✗ | return model.addJoint( | |
| 506 | ✗ | frame.parentJoint, | |
| 507 | ✗ | typename JointCollection::JointModelMimic( | |
| 508 | ✗ | TypeX(), mimicked_joint, mimic_info.multiplier, mimic_info.offset), | |
| 509 | ✗ | frame.placement * placement, joint_name, max_effort, max_velocity, min_config, | |
| 510 | ✗ | max_config, friction, damping); | |
| 511 | break; | ||
| 512 | ✗ | case AXIS_Y: | |
| 513 | ✗ | return model.addJoint( | |
| 514 | ✗ | frame.parentJoint, | |
| 515 | ✗ | typename JointCollection::JointModelMimic( | |
| 516 | ✗ | TypeY(), mimicked_joint, mimic_info.multiplier, mimic_info.offset), | |
| 517 | ✗ | frame.placement * placement, joint_name, max_effort, max_velocity, min_config, | |
| 518 | ✗ | max_config, friction, damping); | |
| 519 | break; | ||
| 520 | |||
| 521 | ✗ | case AXIS_Z: | |
| 522 | ✗ | return model.addJoint( | |
| 523 | ✗ | frame.parentJoint, | |
| 524 | ✗ | typename JointCollection::JointModelMimic( | |
| 525 | ✗ | TypeZ(), mimicked_joint, mimic_info.multiplier, mimic_info.offset), | |
| 526 | ✗ | frame.placement * placement, joint_name, max_effort, max_velocity, min_config, | |
| 527 | ✗ | max_config, friction, damping); | |
| 528 | break; | ||
| 529 | |||
| 530 | ✗ | case AXIS_UNALIGNED: | |
| 531 | ✗ | return model.addJoint( | |
| 532 | ✗ | frame.parentJoint, | |
| 533 | ✗ | typename JointCollection::JointModelMimic( | |
| 534 | ✗ | TypeUnaligned(), mimicked_joint, mimic_info.multiplier, mimic_info.offset), | |
| 535 | ✗ | frame.placement * placement, joint_name, max_effort, max_velocity, min_config, | |
| 536 | ✗ | max_config, friction, damping); | |
| 537 | break; | ||
| 538 | |||
| 539 | ✗ | default: | |
| 540 | ✗ | PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The axis type of the joint is of wrong type."); | |
| 541 | break; | ||
| 542 | } | ||
| 543 | } | ||
| 544 | |||
| 545 | private: | ||
| 546 | /// | ||
| 547 | /// \brief The four possible cartesian types of an 3D axis. | ||
| 548 | /// | ||
| 549 | enum CartesianAxis | ||
| 550 | { | ||
| 551 | AXIS_X = 0, | ||
| 552 | AXIS_Y = 1, | ||
| 553 | AXIS_Z = 2, | ||
| 554 | AXIS_UNALIGNED | ||
| 555 | }; | ||
| 556 | |||
| 557 | /// | ||
| 558 | /// \brief Extract the cartesian property of a particular 3D axis. | ||
| 559 | /// | ||
| 560 | /// \param[in] axis The input URDF axis. | ||
| 561 | /// | ||
| 562 | /// \return The property of the particular axis pinocchio::urdf::CartesianAxis. | ||
| 563 | /// | ||
| 564 | 2255 | static inline CartesianAxis extractCartesianAxis(const Vector3 & axis) | |
| 565 | { | ||
| 566 |
4/6✓ Branch 2 taken 2255 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2255 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 547 times.
✓ Branch 8 taken 1708 times.
|
2255 | if (axis.isApprox(Vector3::UnitX())) |
| 567 | 547 | return AXIS_X; | |
| 568 |
4/6✓ Branch 2 taken 1708 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1708 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 797 times.
✓ Branch 8 taken 911 times.
|
1708 | else if (axis.isApprox(Vector3::UnitY())) |
| 569 | 797 | return AXIS_Y; | |
| 570 |
4/6✓ Branch 2 taken 911 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 911 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 598 times.
✓ Branch 8 taken 313 times.
|
911 | else if (axis.isApprox(Vector3::UnitZ())) |
| 571 | 598 | return AXIS_Z; | |
| 572 | else | ||
| 573 | 313 | return AXIS_UNALIGNED; | |
| 574 | } | ||
| 575 | }; | ||
| 576 | |||
| 577 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 578 | class UrdfVisitorWithRootJoint : public UrdfVisitor<Scalar, Options, JointCollectionTpl> | ||
| 579 | { | ||
| 580 | public: | ||
| 581 | typedef UrdfVisitor<Scalar, Options, JointCollectionTpl> Base; | ||
| 582 | typedef typename Base::Inertia Inertia; | ||
| 583 | using Base::appendBodyToJoint; | ||
| 584 | using Base::model; | ||
| 585 | |||
| 586 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
| 587 | typedef typename Model::JointCollection JointCollection; | ||
| 588 | typedef typename Model::JointModel JointModel; | ||
| 589 | |||
| 590 | JointModel root_joint; | ||
| 591 | |||
| 592 | 58 | UrdfVisitorWithRootJoint( | |
| 593 | Model & model, | ||
| 594 | const JointModelBase<JointModel> & root_joint, | ||
| 595 | const std::string & rjn = "root_joint") | ||
| 596 | : Base(model, rjn) | ||
| 597 |
1/2✓ Branch 3 taken 58 times.
✗ Branch 4 not taken.
|
58 | , root_joint(root_joint.derived()) |
| 598 | { | ||
| 599 | 58 | } | |
| 600 | |||
| 601 | 58 | void addRootJoint(const Inertia & Y, const std::string & body_name) | |
| 602 | { | ||
| 603 | 58 | const Frame & frame = model.frames[0]; | |
| 604 | |||
| 605 |
4/6✓ Branch 1 taken 58 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 57 times.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
58 | PINOCCHIO_THROW( |
| 606 | !model.existJointName(this->root_joint_name), std::invalid_argument, | ||
| 607 | "root_joint already exists as a joint in the kinematic tree."); | ||
| 608 | |||
| 609 | ✗ | JointIndex idx = model.addJoint( | |
| 610 |
2/4✓ Branch 1 taken 57 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 57 times.
✗ Branch 5 not taken.
|
57 | frame.parentJoint, root_joint, SE3::Identity(), this->root_joint_name |
| 611 | // TODO ,max_effort,max_velocity,min_config,max_config | ||
| 612 | ); | ||
| 613 | |||
| 614 |
1/2✓ Branch 1 taken 57 times.
✗ Branch 2 not taken.
|
57 | FrameIndex jointFrameId = model.addJointFrame(idx, 0); |
| 615 |
2/4✓ Branch 1 taken 57 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 57 times.
✗ Branch 5 not taken.
|
57 | appendBodyToJoint(jointFrameId, Y, SE3::Identity(), body_name); |
| 616 | 57 | } | |
| 617 | }; | ||
| 618 | |||
| 619 | typedef UrdfVisitorBaseTpl<double, 0> UrdfVisitorBase; | ||
| 620 | typedef MimicInfo<double, 0> MimicInfo_; | ||
| 621 | |||
| 622 | PINOCCHIO_PARSERS_DLLAPI void parseRootTree( | ||
| 623 | const ::urdf::ModelInterface * urdfTree, UrdfVisitorBase & model, const bool mimic = false); | ||
| 624 | |||
| 625 | PINOCCHIO_PARSERS_DLLAPI void parseRootTree( | ||
| 626 | const std::string & filename, UrdfVisitorBase & model, const bool mimic = false); | ||
| 627 | |||
| 628 | PINOCCHIO_PARSERS_DLLAPI void parseRootTreeFromXML( | ||
| 629 | const std::string & xmlString, UrdfVisitorBase & model, const bool mimic = false); | ||
| 630 | } // namespace details | ||
| 631 | |||
| 632 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 633 | 48 | ModelTpl<Scalar, Options, JointCollectionTpl> & buildModel( | |
| 634 | const std::string & filename, | ||
| 635 | const typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointModel & rootJoint, | ||
| 636 | const std::string & rootJointName, | ||
| 637 | ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 638 | const bool verbose, | ||
| 639 | const bool mimic) | ||
| 640 | { | ||
| 641 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 48 times.
|
48 | if (rootJointName.empty()) |
| 642 | ✗ | throw std::invalid_argument( | |
| 643 | "rootJoint was given without a name. Please fill the argument root_joint_name"); | ||
| 644 | |||
| 645 |
1/2✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
|
48 | details::UrdfVisitorWithRootJoint<Scalar, Options, JointCollectionTpl> visitor( |
| 646 | model, rootJoint, rootJointName); | ||
| 647 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 48 times.
|
48 | if (verbose) |
| 648 | ✗ | visitor.log = &std::cout; | |
| 649 |
1/2✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
|
48 | details::parseRootTree(filename, visitor, mimic); |
| 650 | 48 | return model; | |
| 651 | 48 | } | |
| 652 | |||
| 653 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 654 | 46 | ModelTpl<Scalar, Options, JointCollectionTpl> & buildModel( | |
| 655 | const std::string & filename, | ||
| 656 | const typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointModel & rootJoint, | ||
| 657 | ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 658 | const bool verbose, | ||
| 659 | const bool mimic) | ||
| 660 | { | ||
| 661 |
2/4✓ Branch 2 taken 46 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 46 times.
✗ Branch 6 not taken.
|
46 | return buildModel(filename, rootJoint, "root_joint", model, verbose, mimic); |
| 662 | } | ||
| 663 | |||
| 664 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 665 | 26 | ModelTpl<Scalar, Options, JointCollectionTpl> & buildModel( | |
| 666 | const std::string & filename, | ||
| 667 | ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 668 | const bool verbose, | ||
| 669 | const bool mimic) | ||
| 670 | { | ||
| 671 |
1/2✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
|
26 | details::UrdfVisitor<Scalar, Options, JointCollectionTpl> visitor(model); |
| 672 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 26 times.
|
26 | if (verbose) |
| 673 | ✗ | visitor.log = &std::cout; | |
| 674 |
1/2✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
|
26 | details::parseRootTree(filename, visitor, mimic); |
| 675 | 26 | return model; | |
| 676 | 26 | } | |
| 677 | |||
| 678 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 679 | 4 | ModelTpl<Scalar, Options, JointCollectionTpl> & buildModelFromXML( | |
| 680 | const std::string & xmlStream, | ||
| 681 | const typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointModel & rootJoint, | ||
| 682 | const std::string & rootJointName, | ||
| 683 | ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 684 | const bool verbose, | ||
| 685 | const bool mimic) | ||
| 686 | { | ||
| 687 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 4 times.
|
4 | if (rootJointName.empty()) |
| 688 | ✗ | throw std::invalid_argument( | |
| 689 | "rootJoint was given without a name. Please fill the argument rootJointName"); | ||
| 690 | |||
| 691 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | details::UrdfVisitorWithRootJoint<Scalar, Options, JointCollectionTpl> visitor( |
| 692 | model, rootJoint, rootJointName); | ||
| 693 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
|
4 | if (verbose) |
| 694 | ✗ | visitor.log = &std::cout; | |
| 695 |
2/2✓ Branch 1 taken 3 times.
✓ Branch 2 taken 1 times.
|
4 | details::parseRootTreeFromXML(xmlStream, visitor, mimic); |
| 696 | 3 | return model; | |
| 697 | 4 | } | |
| 698 | |||
| 699 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 700 | 4 | ModelTpl<Scalar, Options, JointCollectionTpl> & buildModelFromXML( | |
| 701 | const std::string & xmlStream, | ||
| 702 | const typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointModel & rootJoint, | ||
| 703 | ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 704 | const bool verbose, | ||
| 705 | const bool mimic) | ||
| 706 | { | ||
| 707 |
3/4✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✓ Branch 6 taken 1 times.
|
5 | return buildModelFromXML(xmlStream, rootJoint, "root_joint", model, verbose, mimic); |
| 708 | } | ||
| 709 | |||
| 710 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 711 | 5 | ModelTpl<Scalar, Options, JointCollectionTpl> & buildModelFromXML( | |
| 712 | const std::string & xmlStream, | ||
| 713 | ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 714 | const bool verbose, | ||
| 715 | const bool mimic) | ||
| 716 | { | ||
| 717 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | details::UrdfVisitor<Scalar, Options, JointCollectionTpl> visitor(model); |
| 718 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 5 times.
|
5 | if (verbose) |
| 719 | ✗ | visitor.log = &std::cout; | |
| 720 |
1/2✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
|
5 | details::parseRootTreeFromXML(xmlStream, visitor, mimic); |
| 721 | 5 | return model; | |
| 722 | 5 | } | |
| 723 | |||
| 724 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 725 | 1 | ModelTpl<Scalar, Options, JointCollectionTpl> & buildModel( | |
| 726 | const std::shared_ptr<::urdf::ModelInterface> urdfTree, | ||
| 727 | const typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointModel & rootJoint, | ||
| 728 | const std::string & rootJointName, | ||
| 729 | ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 730 | const bool verbose, | ||
| 731 | const bool mimic) | ||
| 732 | { | ||
| 733 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
|
1 | if (rootJointName.empty()) |
| 734 | ✗ | throw std::invalid_argument( | |
| 735 | "rootJoint was given without a name. Please fill the argument rootJointName"); | ||
| 736 | |||
| 737 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
1 | PINOCCHIO_CHECK_INPUT_ARGUMENT(urdfTree != NULL); |
| 738 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | details::UrdfVisitorWithRootJoint<Scalar, Options, JointCollectionTpl> visitor( |
| 739 | model, rootJoint, rootJointName); | ||
| 740 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
|
1 | if (verbose) |
| 741 | ✗ | visitor.log = &std::cout; | |
| 742 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | details::parseRootTree(urdfTree.get(), visitor, mimic); |
| 743 | 1 | return model; | |
| 744 | 1 | } | |
| 745 | |||
| 746 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 747 | 1 | ModelTpl<Scalar, Options, JointCollectionTpl> & buildModel( | |
| 748 | const std::shared_ptr<::urdf::ModelInterface> urdfTree, | ||
| 749 | const typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointModel & rootJoint, | ||
| 750 | ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 751 | const bool verbose, | ||
| 752 | const bool mimic) | ||
| 753 | { | ||
| 754 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
|
1 | return buildModel(urdfTree, rootJoint, "root_joint", model, verbose, mimic); |
| 755 | } | ||
| 756 | |||
| 757 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
| 758 | 1 | ModelTpl<Scalar, Options, JointCollectionTpl> & buildModel( | |
| 759 | const std::shared_ptr<::urdf::ModelInterface> urdfTree, | ||
| 760 | ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
| 761 | const bool verbose, | ||
| 762 | const bool mimic) | ||
| 763 | { | ||
| 764 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
1 | PINOCCHIO_CHECK_INPUT_ARGUMENT(urdfTree != NULL); |
| 765 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | details::UrdfVisitor<Scalar, Options, JointCollectionTpl> visitor(model); |
| 766 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
|
1 | if (verbose) |
| 767 | ✗ | visitor.log = &std::cout; | |
| 768 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | details::parseRootTree(urdfTree.get(), visitor, mimic); |
| 769 | 1 | return model; | |
| 770 | 1 | } | |
| 771 | |||
| 772 | } // namespace urdf | ||
| 773 | } // namespace pinocchio | ||
| 774 | |||
| 775 | #endif // ifndef __pinocchio_multibody_parsers_urdf_model_hxx__ | ||
| 776 |