From b5102fbd39ba06459187eafe96c1178039f8e995 Mon Sep 17 00:00:00 2001 From: Matthew Powelson Date: Thu, 20 Jul 2023 15:40:50 -0600 Subject: [PATCH] Add basic planar joint support in the urdf parser --- .../urdf/lbr_iiwa_14_r820_with_planar.urdf | 193 +++++++++++++++++ .../urdf/lbr_iiwa_14_r820_with_planar.xacro | 5 + .../lbr_iiwa_14_r820_with_planar_macro.xacro | 195 ++++++++++++++++++ .../include/tesseract_urdf/urdf_parser.h | 28 +++ tesseract_urdf/src/urdf_parser.cpp | 136 ++++++++++-- .../test/tesseract_urdf_joint_unit.cpp | 7 + .../test/tesseract_urdf_urdf_unit.cpp | 26 +++ 7 files changed, 570 insertions(+), 20 deletions(-) create mode 100644 tesseract_support/urdf/lbr_iiwa_14_r820_with_planar.urdf create mode 100644 tesseract_support/urdf/lbr_iiwa_14_r820_with_planar.xacro create mode 100644 tesseract_support/urdf/lbr_iiwa_14_r820_with_planar_macro.xacro diff --git a/tesseract_support/urdf/lbr_iiwa_14_r820_with_planar.urdf b/tesseract_support/urdf/lbr_iiwa_14_r820_with_planar.urdf new file mode 100644 index 00000000000..2cfc093f99d --- /dev/null +++ b/tesseract_support/urdf/lbr_iiwa_14_r820_with_planar.urdf @@ -0,0 +1,193 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/tesseract_support/urdf/lbr_iiwa_14_r820_with_planar.xacro b/tesseract_support/urdf/lbr_iiwa_14_r820_with_planar.xacro new file mode 100644 index 00000000000..c2776232f41 --- /dev/null +++ b/tesseract_support/urdf/lbr_iiwa_14_r820_with_planar.xacro @@ -0,0 +1,5 @@ + + + + + diff --git a/tesseract_support/urdf/lbr_iiwa_14_r820_with_planar_macro.xacro b/tesseract_support/urdf/lbr_iiwa_14_r820_with_planar_macro.xacro new file mode 100644 index 00000000000..0f25a5d1b89 --- /dev/null +++ b/tesseract_support/urdf/lbr_iiwa_14_r820_with_planar_macro.xacro @@ -0,0 +1,195 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/tesseract_urdf/include/tesseract_urdf/urdf_parser.h b/tesseract_urdf/include/tesseract_urdf/urdf_parser.h index aa88dd8b783..0b92430198f 100644 --- a/tesseract_urdf/include/tesseract_urdf/urdf_parser.h +++ b/tesseract_urdf/include/tesseract_urdf/urdf_parser.h @@ -56,6 +56,34 @@ tesseract_scene_graph::SceneGraph::UPtr parseURDFFile(const std::string& path, void writeURDFFile(const tesseract_scene_graph::SceneGraph::ConstPtr& sg, const std::string& package_path, const std::string& urdf_name = ""); + +/** + * @brief Splits up floating and planar joints into revolute and prismatic joints + * @param joint Input joint + * @return Pair of resulting links and joints to be added + */ +std::pair, std::vector> +splitJoint(const tesseract_scene_graph::Joint::Ptr& joint); + +/** + * @brief Adds a link to the scene graph doing some error checking + * @param sg Input scene graph + * @param robot_name Input robot name + * @param l Input link + */ +void addLink(tesseract_scene_graph::SceneGraph::UPtr& sg, + const std::string& robot_name, + const tesseract_scene_graph::Link::Ptr& l); + +/** + * @brief Adds a joint to the scene graph doing some error checking + * @param sg Input scene graph + * @param robot_name Input robot name + * @param j Input joint + */ +void addJoint(tesseract_scene_graph::SceneGraph::UPtr& sg, + const std::string& robot_name, + const tesseract_scene_graph::Joint::Ptr& j); } // namespace tesseract_urdf #endif diff --git a/tesseract_urdf/src/urdf_parser.cpp b/tesseract_urdf/src/urdf_parser.cpp index c02ff55b695..9593a9e7d45 100644 --- a/tesseract_urdf/src/urdf_parser.cpp +++ b/tesseract_urdf/src/urdf_parser.cpp @@ -97,16 +97,7 @@ tesseract_scene_graph::SceneGraph::UPtr parseURDFString(const std::string& urdf_ { std::throw_with_nested(std::runtime_error("URDF: Error parsing 'link' element for robot '" + robot_name + "'!")); } - - // Check if link name is unique - if (sg->getLink(l->getName()) != nullptr) - std::throw_with_nested(std::runtime_error("URDF: Error link name '" + l->getName() + - "' is not unique for robot '" + robot_name + "'!")); - - // Add link to scene graph - if (!sg->addLink(*l)) - std::throw_with_nested(std::runtime_error("URDF: Error adding link '" + l->getName() + - "' to scene graph for robot '" + robot_name + "'!")); + addLink(sg, robot_name, l); } if (sg->getLinks().empty()) @@ -116,25 +107,27 @@ tesseract_scene_graph::SceneGraph::UPtr parseURDFString(const std::string& urdf_ joint = joint->NextSiblingElement("join" "t")) { - tesseract_scene_graph::Joint::Ptr j = nullptr; + tesseract_scene_graph::Joint::Ptr urdf_joint = nullptr; try { - j = parseJoint(joint, urdf_version); + urdf_joint = parseJoint(joint, urdf_version); } catch (...) { std::throw_with_nested(std::runtime_error("URDF: Error parsing 'joint' element for robot '" + robot_name + "'!")); } - // Check if joint name is unique - if (sg->getJoint(j->getName()) != nullptr) - std::throw_with_nested(std::runtime_error("URDF: Error joint name '" + j->getName() + - "' is not unique for robot '" + robot_name + "'!")); + // Split up any joints that require it + const auto split_joints = splitJoint(urdf_joint); - // Add joint to scene graph - if (!sg->addJoint(*j)) - std::throw_with_nested(std::runtime_error("URDF: Error adding joint '" + j->getName() + - "' to scene graph for robot '" + robot_name + "'!")); + for (const auto& l : split_joints.first) + { + addLink(sg, robot_name, l); + } + for (const auto& j : split_joints.second) + { + addJoint(sg, robot_name, j); + } } if (sg->getJoints().empty()) @@ -282,4 +275,107 @@ void writeURDFFile(const tesseract_scene_graph::SceneGraph::ConstPtr& sg, doc.SaveFile(full_filepath.c_str()); } +std::pair>, + std::vector>> +splitJoint(const tesseract_scene_graph::Joint::Ptr& joint) +{ + using namespace tesseract_scene_graph; + switch (joint->type) + { + case JointType::PLANAR: { + const std::string base_name = joint->getName(); + const std::string postfix = "planar"; + + auto j1 = std::make_shared(base_name + "_x_" + postfix); + j1->type = JointType::PRISMATIC; + j1->axis = Eigen::Vector3d(1.0, 0.0, 0.0); /// @todo Handle cases where joint->axis is not (0, 0, 1) + j1->parent_link_name = joint->parent_link_name; + j1->child_link_name = j1->getName() + "_link"; + j1->parent_to_joint_origin_transform = joint->parent_to_joint_origin_transform; + // j1->dynamics = joint->dynamics; + j1->limits = std::make_shared(std::numeric_limits::min(), + std::numeric_limits::max(), + std::numeric_limits::max(), + std::numeric_limits::max(), + std::numeric_limits::max()); + // j1->safety = joint->safety; + // j1->calibration = joint->calibration; + // j1->mimic = joint->mimic; + + auto l1 = std::make_shared(j1->getName() + "_link"); + + auto j2 = std::make_shared(base_name + "_y_" + postfix); + j2->type = JointType::PRISMATIC; + j2->axis = Eigen::Vector3d(0.0, 1.0, 0.0); /// @todo Handle cases where joint->axis is not (0, 0, 1) + j2->parent_link_name = j1->getName() + "_link"; + j2->child_link_name = j2->getName() + "_link"; + j2->parent_to_joint_origin_transform = Eigen::Isometry3d::Identity(); + // j2->dynamics = joint->dynamics; + j2->limits = std::make_shared(std::numeric_limits::min(), + std::numeric_limits::max(), + std::numeric_limits::max(), + std::numeric_limits::max(), + std::numeric_limits::max()); + // j2->safety = joint->safety; + // j2->calibration = joint->calibration; + // j2->mimic = joint->mimic; + + auto l2 = std::make_shared(j2->getName() + "_link"); + + auto j3 = std::make_shared(base_name + "_yaw_" + postfix); + j3->type = JointType::REVOLUTE; + j3->axis = Eigen::Vector3d(0.0, 0.0, 1.0); /// @todo Handle cases where joint->axis is not (0, 0, 1) + j3->parent_link_name = j2->getName() + "_link"; + j3->child_link_name = joint->child_link_name; + j3->parent_to_joint_origin_transform = Eigen::Isometry3d::Identity(); + // j3->dynamics = joint->dynamics; + j3->limits = std::make_shared(std::numeric_limits::min(), + std::numeric_limits::max(), + std::numeric_limits::max(), + std::numeric_limits::max(), + std::numeric_limits::max()); + // j3->safety = joint->safety; + // j3->calibration = joint->calibration; + // j3->mimic = joint->mimic; + + return { { l1, l2 }, { j1, j2, j3 } }; + } + case JointType::FLOATING: { + std::throw_with_nested(std::runtime_error("FLOATING joints are not yet supported")); + } + default: + return { {}, { joint } }; + } +} + +void addLink(tesseract_scene_graph::SceneGraph::UPtr& sg, + const std::string& robot_name, + const tesseract_scene_graph::Link::Ptr& l) +{ + // Check if link name is unique + if (sg->getLink(l->getName()) != nullptr) + std::throw_with_nested(std::runtime_error("URDF: Error link name '" + l->getName() + "' is not unique for robot '" + + robot_name + "'!")); + + // Add link to scene graph + if (!sg->addLink(*l)) + std::throw_with_nested(std::runtime_error("URDF: Error adding link '" + l->getName() + + "' to scene graph for robot '" + robot_name + "'!")); +} + +void addJoint(tesseract_scene_graph::SceneGraph::UPtr& sg, + const std::string& robot_name, + const tesseract_scene_graph::Joint::Ptr& j) +{ + // Check if joint name is unique + if (sg->getJoint(j->getName()) != nullptr) + std::throw_with_nested(std::runtime_error("URDF: Error joint name '" + j->getName() + + "' is not unique for robot '" + robot_name + "'!")); + + // Add joint to scene graph + if (!sg->addJoint(*j)) + std::throw_with_nested(std::runtime_error("URDF: Error adding joint '" + j->getName() + + "' to scene graph for robot '" + robot_name + "'!")); +} + } // namespace tesseract_urdf diff --git a/tesseract_urdf/test/tesseract_urdf_joint_unit.cpp b/tesseract_urdf/test/tesseract_urdf_joint_unit.cpp index 0e297c33649..0fb225c287a 100644 --- a/tesseract_urdf/test/tesseract_urdf_joint_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_joint_unit.cpp @@ -212,6 +212,13 @@ TEST(TesseractURDFUnit, parse_joint) // NOLINT EXPECT_TRUE(elem->limits == nullptr); EXPECT_TRUE(elem->safety == nullptr); EXPECT_TRUE(elem->mimic == nullptr); + + auto split_results = tesseract_urdf::splitJoint(elem); + EXPECT_EQ(split_results.first.size(), 2); + ASSERT_EQ(split_results.second.size(), 3); + EXPECT_EQ(split_results.second[0]->getName(), "my_joint_x_planar"); + EXPECT_EQ(split_results.second[1]->getName(), "my_joint_y_planar"); + EXPECT_EQ(split_results.second[2]->getName(), "my_joint_z_planar"); } { diff --git a/tesseract_urdf/test/tesseract_urdf_urdf_unit.cpp b/tesseract_urdf/test/tesseract_urdf_urdf_unit.cpp index d9086f10980..d6e9085b032 100644 --- a/tesseract_urdf/test/tesseract_urdf_urdf_unit.cpp +++ b/tesseract_urdf/test/tesseract_urdf_urdf_unit.cpp @@ -41,6 +41,32 @@ TEST(TesseractURDFUnit, parse_urdf) // NOLINT std::string str = R"( + + + + + + + + + + + + + )"; + tesseract_scene_graph::SceneGraph::Ptr sg = tesseract_urdf::parseURDFString(str, resource_locator); + EXPECT_TRUE(sg != nullptr); + EXPECT_TRUE(sg->getName() == "test"); + EXPECT_TRUE(sg->isTree()); + EXPECT_TRUE(sg->isAcyclic()); + EXPECT_TRUE(sg->getJoints().size() == 3); + EXPECT_TRUE(sg->getLinks().size() == 4); + } + + { + std::string str = + R"( +