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"(
+