14 typedef pinocchio::urdf::details::
15 UrdfVisitor<double, 0, ::pinocchio::JointCollectionDefaultTpl>
19 static const std::array<std::string, 3>
ELEMENTS = {
"joint",
"geom",
"site"};
27 dst.put(ptree::path_type(
v.first),
v.second.data());
35 for (
const std::string & el :
ELEMENTS)
37 std::string
path = el +
".<xmlattr>";
42 const ptree & attr_current = current.get_child(
path, default_value);
47 current.put_child(
path, attr_parent);
54 auto n = el.get_optional<std::string>(
"<xmlattr>.name");
59 if (filePath.extension().empty())
60 throw std::invalid_argument(
"Cannot find extension for one of the mesh/texture");
62 auto st = filePath.stem();
66 throw std::invalid_argument(
"Cannot find a name for one of the mesh.texture");
72 const std::string & dir,
73 const std::string & modelPath,
76 namespace fs = boost::filesystem;
79 if (filePath.is_absolute() && !strippath)
88 if (meshPath.is_absolute())
93 return (mainPath.parent_path() / meshPath /
filename);
105 Eigen::Matrix3d aa1 =
107 Eigen::Matrix3d aa2 =
109 Eigen::Matrix3d aa3 =
112 return aa1 * aa2 * aa3;
115 template<
int Nq,
int Nv>
118 typedef UrdfVisitor::Vector Vector;
119 const double infty = std::numeric_limits<double>::infinity();
122 ret.
maxEffort = Vector::Constant(Nv, infty);
123 ret.
maxVel = Vector::Constant(Nv, infty);
124 ret.
maxConfig = Vector::Constant(Nq, 1.01);
125 ret.
minConfig = Vector::Constant(Nq, -1.01);
126 ret.
friction = Vector::Constant(Nv, 0.);
127 ret.
damping = Vector::Constant(Nv, 0.);
135 template<
int Nq,
int Nv>
168 if (!use_limits && el.get_optional<std::string>(
"<xmlattr>.range"))
169 throw std::invalid_argument(
"Range limit is specified but it was not specify to use it.");
172 auto type_s = el.get_optional<std::string>(
"<xmlattr>.type");
177 auto ax = el.get_optional<std::string>(
"<xmlattr>.axis");
179 axis = internal::getVectorFromStream<3>(*
ax);
182 auto range_ = el.get_optional<std::string>(
"<xmlattr>.range");
185 Eigen::Vector2d rangeT = internal::getVectorFromStream<2>(*range_);
190 range_ = el.get_optional<std::string>(
"<xmlattr>.actuatorfrcrange");
193 Eigen::Vector2d rangeT = internal::getVectorFromStream<2>(*range_);
198 auto value = el.get_optional<
double>(
"<xmlattr>.springref");
203 value = el.get_optional<
double>(
"<xmlattr>.damping");
207 value = el.get_optional<
double>(
"<xmlattr>.armature");
212 value = el.get_optional<
double>(
"<xmlattr>.frictionloss");
216 value = el.get_optional<
double>(
"<xmlattr>.ref");
224 throw std::invalid_argument(
225 "Reference position can only be used with hinge or slide joints.");
232 bool use_limit =
true;
234 auto name_s = el.get_optional<std::string>(
"<xmlattr>.name");
245 auto use_ls = el.get_optional<std::string>(
"<xmlattr>.limited");
246 use_limit = *use_ls ==
"true";
257 if (
auto joint_p = classE.
classElement.get_child_optional(
"joint"))
261 auto cl_s = el.get_optional<std::string>(
"<xmlattr>.class");
264 std::string className = *cl_s;
266 if (
auto joint_p = classE.
classElement.get_child_optional(
"joint"))
279 auto pos = el.get_optional<std::string>(
"<xmlattr>.pos");
281 placement.translation() = internal::getVectorFromStream<3>(*
pos);
285 auto rot_s = el.get_optional<std::string>(
"<xmlattr>.quat");
288 Eigen::Vector4d
quat = internal::getVectorFromStream<4>(*rot_s);
291 quaternion.normalize();
292 placement.rotation() = quaternion.toRotationMatrix();
295 else if ((rot_s = el.get_optional<std::string>(
"<xmlattr>.axisangle")))
297 Eigen::Vector4d axis_angle = internal::getVectorFromStream<4>(*rot_s);
299 double angle = axis_angle(3);
302 placement.rotation() = angleAxis.toRotationMatrix();
305 else if ((rot_s = el.get_optional<std::string>(
"<xmlattr>.euler")))
307 Eigen::Vector3d angles = internal::getVectorFromStream<3>(*rot_s);
311 else if ((rot_s = el.get_optional<std::string>(
"<xmlattr>.xyaxes")))
313 Eigen::Matrix<double, 6, 1> xyaxes = internal::getVectorFromStream<6>(*rot_s);
315 Eigen::Vector3d xAxis = xyaxes.head(3);
317 Eigen::Vector3d yAxis = xyaxes.tail(3);
320 double d = xAxis.dot(yAxis);
324 Eigen::Vector3d zAxis = xAxis.cross(yAxis);
334 else if ((rot_s = el.get_optional<std::string>(
"<xmlattr>.zaxis")))
336 Eigen::Vector3d zaxis = internal::getVectorFromStream<3>(*rot_s);
339 Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitZ(), zaxis).toRotationMatrix();
349 throw std::invalid_argument(
"Mass of body is not supposed to be negative");
352 auto com_s = el.get_optional<std::string>(
"<xmlattr>.pos");
354 com = internal::getVectorFromStream<3>(*com_s);
356 com = Inertia::Vector3::Zero();
360 Inertia::Matrix3 I = Eigen::Matrix3d::Identity();
362 auto inertia_s = el.get_optional<std::string>(
"<xmlattr>.diaginertia");
365 Eigen::Vector3d inertiaDiag = internal::getVectorFromStream<3>(*inertia_s);
366 I = inertiaDiag.asDiagonal();
369 else if ((inertia_s = el.get_optional<std::string>(
"<xmlattr>.fullinertia")))
373 inertiaStream >> I(0, 0);
374 inertiaStream >> I(1, 1);
375 inertiaStream >> I(2, 2);
376 inertiaStream >> I(0, 1);
377 inertiaStream >> I(0, 2);
378 inertiaStream >> I(1, 2);
386 for (
int i = 0;
i < 3;
i++)
394 const boost::optional<std::string> & childClass,
395 const std::string & parentName)
398 auto chcl_s = childClass;
401 throw std::invalid_argument(
"Cannot get inertia from geom and no inertia was found");
403 bool usegeominertia =
false;
405 usegeominertia =
true;
408 usegeominertia =
true;
413 if (
v.first ==
"<xmlattr>")
416 auto name_s =
v.second.get_optional<std::string>(
"name");
420 currentBody.
bodyName = parentName +
"Bis";
427 if (
auto chcl_st =
v.second.get_optional<std::string>(
"childclass"))
436 auto cl_s =
v.second.get_optional<std::string>(
"class");
443 if (
v.first ==
"inertial" && !usegeominertia)
447 if (
v.first ==
"geom")
450 currentGeom.
fill(
v.second, currentBody, *
this);
455 if (
v.first ==
"joint")
458 currentJoint.
fill(
v.second, currentBody, *
this);
461 if (
v.first ==
"freejoint")
465 auto jointName =
v.second.get_optional<std::string>(
"<xmlattr>.name");
473 if (
v.first ==
"site")
476 currentSite.
fill(
v.second, currentBody, *
this);
479 if (
v.first ==
"body")
491 inert_temp += geom.geomPlacement.act(geom.geomInertia);
500 namespace fs = boost::filesystem;
502 auto file = el.get_optional<std::string>(
"<xmlattr>.file");
504 throw std::invalid_argument(
"Only textures with files are supported");
512 auto str_v = el.get_optional<std::string>(
"<xmlattr>.type");
516 if ((str_v = el.get_optional<std::string>(
"<xmlattr>.gridsize")))
517 text.
gridsize = internal::getVectorFromStream<2>(*str_v);
526 auto n = el.get_optional<std::string>(
"<xmlattr>.name");
530 throw std::invalid_argument(
"Material was given without a name");
533 auto cl_s = el.get_optional<std::string>(
"<xmlattr>.class");
536 std::string className = *cl_s;
538 if (
auto mat_p = classE.
classElement.get_child_optional(
"material"))
539 mat.goThroughElement(*mat_p);
542 mat.goThroughElement(el);
549 namespace fs = boost::filesystem;
552 auto file = el.get_optional<std::string>(
"<xmlattr>.file");
554 throw std::invalid_argument(
"Only meshes with files are supported");
562 auto scale = el.get_optional<std::string>(
"<xmlattr>.scale");
564 mesh.scale = internal::getVectorFromStream<3>(*scale);
573 if (
v.first ==
"mesh")
576 if (
v.first ==
"material")
579 if (
v.first ==
"texture")
582 if (
v.first ==
"hfield")
583 throw std::invalid_argument(
"hfields are not supported yet");
589 boost::optional<std::string> nameClass;
592 if (
v.first ==
"<xmlattr>")
594 nameClass =
v.second.get_optional<std::string>(
"class");
601 mapOfClasses.insert(std::make_pair(*nameClass, classDefault));
604 throw std::invalid_argument(
"Class does not have a name. Cannot parse model.");
606 if (
v.first ==
"default")
614 auto auto_s = el.get_optional<std::string>(
"<xmlattr>.autolimits");
616 if (*auto_s ==
"true")
620 auto strip_s = el.get_optional<std::string>(
"<xmlattr>.strippath");
622 if (*strip_s ==
"true")
626 auto dir = el.get_optional<std::string>(
"<xmlattr>.assetdir");
633 if ((dir = el.get_optional<std::string>(
"<xmlattr>.meshdir")))
636 if ((dir = el.get_optional<std::string>(
"<xmlattr>.texturedir")))
639 auto value_v = el.get_optional<
double>(
"<xmlattr>.boundmass");
643 if ((value_v = el.get_optional<
double>(
"<xmlattr>.boundinertia")))
646 auto in_g = el.get_optional<std::string>(
"<xmlattr>.inertiafromgeom");
651 else if (*in_g ==
"false")
656 auto angle_s = el.get_optional<std::string>(
"<xmlattr>.angle");
658 if (*angle_s ==
"radian")
661 auto eulerS = el.get_optional<std::string>(
"<xmlattr>.eulerseq");
664 std::string eulerseq = *eulerS;
665 if (eulerseq.find_first_not_of(
"xyzXYZ") != std::string::npos || eulerseq.size() != 3)
666 throw std::invalid_argument(
667 "Model tried to use euler angles but euler sequence is wrong");
671 for (std::size_t
i = 0;
i < eulerseq.size();
i++)
695 throw std::invalid_argument(
"Euler Axis does not exist");
707 if (
v.first ==
"key")
709 auto nameKey =
v.second.get_optional<std::string>(
"<xmlattr>.name");
712 auto configVectorS =
v.second.get_optional<std::string>(
"<xmlattr>.qpos");
715 Eigen::VectorXd configVector =
717 mapOfConfigs.insert(std::make_pair(*nameKey, configVector));
727 if (
pt.get_child_optional(
"mujoco"))
728 el =
pt.get_child(
"mujoco");
730 throw std::invalid_argument(
"This is not a standard mujoco model. Cannot parse it.");
735 if (
v.first ==
"<xmlattr>")
737 auto n_s =
v.second.get_optional<std::string>(
"model");
741 throw std::invalid_argument(
"Model is missing a name. Cannot parse it");
744 if (
v.first ==
"compiler")
747 if (
v.first ==
"default")
750 if (
v.first ==
"asset")
753 if (
v.first ==
"keyframe")
756 if (
v.first ==
"worldbody")
758 boost::optional<std::string> childClass;
766 boost::property_tree::read_xml(xmlStr,
pt);
770 template<
typename TypeX,
typename TypeY,
typename TypeZ,
typename TypeUnaligned>
773 if (
axis.isApprox(Eigen::Vector3d::UnitX()))
775 else if (
axis.isApprox(Eigen::Vector3d::UnitY()))
777 else if (
axis.isApprox(Eigen::Vector3d::UnitZ()))
780 return TypeUnaligned(
axis.normalized());
792 UrdfVisitor::JointType jType;
799 jType = UrdfVisitor::FLOATING;
805 jType = UrdfVisitor::PRISMATIC;
811 jType = UrdfVisitor::SPHERICAL;
817 jType = UrdfVisitor::REVOLUTE;
820 throw std::invalid_argument(
"Unknown joint type");
823 jType, joint.
axis, parentFrameId, jointInParent, joint.
jointName, inert, bodyInJoint,
847 std::string jointName = nameOfBody +
"_fixed";
848 urdfVisitor << jointName <<
" being parsed." <<
'\n';
850 urdfVisitor.addFixedJointAndBody(parentFrameId, bodyPose, jointName, inert, nameOfBody);
854 bool composite =
false;
855 SE3 jointPlacement, firstJointPlacement, prevJointPlacement =
SE3::Identity();
859 std::string jointName;
866 jointName =
"Composite_" + firstOne.
jointName;
882 if (joint.jointType ==
"free")
883 throw std::invalid_argument(
"Joint Composite trying to be created with a freeFlyer");
885 SE3 jointInParent = bodyPose * joint.jointPlacement;
886 bodyInJoint = joint.jointPlacement.
inverse();
889 firstJointPlacement = jointInParent;
894 jointPlacement = prevJointPlacement.
inverse() * jointInParent;
895 if (joint.jointType ==
"slide")
898 createJoint<JointModelPX, JointModelPY, JointModelPZ, JointModelPrismaticUnaligned>(
902 rangeCompo = rangeCompo.
concatenate<1, 1>(joint.range);
904 else if (joint.jointType ==
"ball")
907 rangeCompo = rangeCompo.
concatenate<4, 3>(joint.range.setDimension<4, 3>());
909 else if (joint.jointType ==
"hinge")
912 createJoint<JointModelRX, JointModelRY, JointModelRZ, JointModelRevoluteUnaligned>(
915 rangeCompo = rangeCompo.
concatenate<1, 1>(joint.range);
918 throw std::invalid_argument(
"Unknown joint type trying to be parsed.");
920 prevJointPlacement = jointInParent;
925 frame.parentJoint, jointM,
frame.placement * firstJointPlacement, jointName,
929 urdfVisitor.appendBodyToJoint(jointFrameId, inert, bodyInJoint, nameOfBody);
947 if (joint.jointType ==
"free")
952 else if (joint.jointType ==
"ball")
957 else if (joint.jointType ==
"slide" || joint.jointType ==
"hinge")
969 if (keyframe.size() == model_nq)
971 Eigen::VectorXd qpos(model_nq);
975 int idx_q = joint.idx_q();
978 Eigen::VectorXd qpos_j = keyframe.segment(
idx_q,
nq);
980 if (joint.shortname() ==
"JointModelFreeFlyer")
982 Eigen::Vector4d new_quat(qpos_j(4), qpos_j(5), qpos_j(6), qpos_j(3));
983 qpos_j.tail(4) = new_quat;
985 else if (joint.shortname() ==
"JointModelSpherical")
987 Eigen::Vector4d new_quat(qpos_j(1), qpos_j(2), qpos_j(3), qpos_j(0));
990 else if (joint.shortname() ==
"JointModelComposite")
992 for (
const auto & joint_ : boost::get<JointModelComposite>(joint.toVariant()).joints)
994 int idx_q_ = joint_.idx_q() -
idx_q;
995 int nq_ = joint_.nq();
996 if (joint_.shortname() ==
"JointModelSpherical")
998 Eigen::Vector4d new_quat(
999 qpos_j(idx_q_ + 1), qpos_j(idx_q_ + 2), qpos_j(idx_q_ + 3), qpos_j(idx_q_));
1000 qpos_j.segment(idx_q_, nq_) = new_quat;
1003 qpos_j.segment(idx_q_, nq_) -=
q_ref.segment(idx_q_, nq_);
1009 qpos.segment(
idx_q,
nq) = qpos_j;
1012 urdfVisitor.model.referenceConfigurations.insert(std::make_pair(keyName, qpos));
1015 throw std::invalid_argument(
"Keyframe size does not match model size");
1029 if (entry != rootLinkName)