From a8a19f7e1ac3d08f36133a96b0955bc019b9721b Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Thu, 28 Oct 2021 23:12:04 -0500 Subject: [PATCH] Improve tesseract_environment code coverage (#655) * Improve tesseract_environment code coverage * Improve tesseract_common code coverage * Improve tesseract_geometry code coverage * Fix bug in tesseract_environment with populating contact managers * Fix issue in JointGroup and StateSolver getStaticLinkNames * Fix clang-tidy error * Fix issue with findTCPOffset and improve code coverage * Improve tesseract_environment code coverage * Add checkTrajectory unit tests * Improve checkTrajectory code coverage --- .../include/tesseract_common/types.h | 1 + .../include/tesseract_common/utils.h | 4 +- .../include/tesseract_common/yaml_utils.h | 23 +- tesseract_common/src/utils.cpp | 3 + .../test/tesseract_common_unit.cpp | 525 +++++++++- .../tesseract_environment/environment.h | 2 +- tesseract_environment/src/environment.cpp | 13 +- tesseract_environment/src/utils.cpp | 10 +- .../test/tesseract_environment_unit.cpp | 917 +++++++++++++++--- .../include/tesseract_geometry/impl/mesh.h | 8 +- .../tesseract_geometry/impl/sdf_mesh.h | 8 +- .../test/tesseract_geometry_unit.cpp | 30 + .../tesseract_kinematics/core/joint_group.h | 1 + tesseract_kinematics/core/src/joint_group.cpp | 11 +- .../test/kinematics_test_utils.h | 163 +++- tesseract_scene_graph/src/kdl_parser.cpp | 9 +- .../test/tesseract_scene_graph_unit.cpp | 8 + tesseract_support/urdf/lbr_iiwa_14_r820.srdf | 9 + .../urdf/lbr_iiwa_14_r820_plugins.yaml | 11 + 19 files changed, 1562 insertions(+), 194 deletions(-) diff --git a/tesseract_common/include/tesseract_common/types.h b/tesseract_common/include/tesseract_common/types.h index bbebce77961..81251cc554b 100644 --- a/tesseract_common/include/tesseract_common/types.h +++ b/tesseract_common/include/tesseract_common/types.h @@ -85,6 +85,7 @@ struct PairHash LinkNamesPair makeOrderedLinkPair(const std::string& link_name1, const std::string& link_name2); /** @brief The Plugin Information struct */ +// NOLINTNEXTLINE struct PluginInfo { /** @brief The plugin class name */ diff --git a/tesseract_common/include/tesseract_common/utils.h b/tesseract_common/include/tesseract_common/utils.h index 495bebf14bd..227e757ae03 100644 --- a/tesseract_common/include/tesseract_common/utils.h +++ b/tesseract_common/include/tesseract_common/utils.h @@ -137,8 +137,8 @@ Eigen::Vector3d calcRotationalError2(const Eigen::Ref& R) Eigen::VectorXd calcTransformError(const Eigen::Isometry3d& t1, const Eigen::Isometry3d& t2); /** - * @brief This computes a random color with alpha set to 1 - * @return A random color + * @brief This computes a random color RGBA [0, 1] with alpha set to 1 + * @return A random RGBA color */ Eigen::Vector4d computeRandomColor(); diff --git a/tesseract_common/include/tesseract_common/yaml_utils.h b/tesseract_common/include/tesseract_common/yaml_utils.h index 3a23ac27f54..9fd74839573 100644 --- a/tesseract_common/include/tesseract_common/yaml_utils.h +++ b/tesseract_common/include/tesseract_common/yaml_utils.h @@ -45,8 +45,10 @@ struct convert { Node node; node["class"] = rhs.class_name; + if (!rhs.config.IsNull()) node["config"] = rhs.config; + return node; } @@ -58,7 +60,7 @@ struct convert rhs.class_name = node["class"].as(); - if (node["config"]) + if (node["config"] != nullptr) rhs.config = node["config"]; return true; @@ -75,17 +77,16 @@ struct convert node["default"] = rhs.default_plugin; node["plugins"] = rhs.plugins; - ; return node; } static bool decode(const Node& node, tesseract_common::PluginInfoContainer& rhs) { - if (node["default"]) + if (node["default"] != nullptr) rhs.default_plugin = node["default"].as(); - if (!node["plugins"]) + if (node["plugins"] == nullptr) throw std::runtime_error("PluginInfoContainer, missing 'plugins' entry!"); const Node& plugins = node["plugins"]; @@ -98,7 +99,7 @@ struct convert } catch (const std::exception& e) { - throw std::runtime_error(std::string("ContactManagersPluginFactory: Constructor failed to cast 'plugins' to " + throw std::runtime_error(std::string("PluginInfoContainer: Constructor failed to cast 'plugins' to " "tesseract_common::PluginInfoMap! Details: ") + e.what()); } @@ -165,7 +166,7 @@ struct convert out.translation().z() = p["z"].as(); const YAML::Node& o = node["orientation"]; - if (o["x"] && o["y"] && o["z"] && o["w"]) + if (o["x"] && o["y"] && o["z"] && o["w"]) // NOLINT { Eigen::Quaterniond quat; quat.x() = o["x"].as(); @@ -175,11 +176,11 @@ struct convert out.linear() = quat.toRotationMatrix(); } - else if (o["r"] && o["p"] && o["y"]) + else if (o["r"] && o["p"] && o["y"]) // NOLINT { - double r = o["r"].as(); - double p = o["p"].as(); - double y = o["y"].as(); + auto r = o["r"].as(); + auto p = o["p"].as(); + auto y = o["y"].as(); Eigen::AngleAxisd rollAngle(r, Eigen::Vector3d::UnitX()); Eigen::AngleAxisd pitchAngle(p, Eigen::Vector3d::UnitY()); @@ -216,7 +217,7 @@ struct convert if (!node.IsSequence()) return false; - rhs.resize(node.size()); + rhs.resize(static_cast(node.size())); for (long i = 0; i < node.size(); ++i) rhs(i) = node[i].as(); diff --git a/tesseract_common/src/utils.cpp b/tesseract_common/src/utils.cpp index 0977bc249bd..d2ccb053aca 100644 --- a/tesseract_common/src/utils.cpp +++ b/tesseract_common/src/utils.cpp @@ -60,6 +60,8 @@ std::enable_if_t::value> my_rethrow_if_nested(const E& e) p->rethrow_nested(); } +// LCOV_EXCL_START +// These are tested in both tesseract_kinematics and tesseract_state_solver void twistChangeRefPoint(Eigen::Ref twist, const Eigen::Ref& ref_point) { twist(0) += twist(4) * ref_point(2) - twist(5) * ref_point(1); @@ -86,6 +88,7 @@ void jacobianChangeRefPoint(Eigen::Ref jacobian, const Eigen::R for (int i = 0; i < jacobian.cols(); i++) twistChangeRefPoint(jacobian.col(i), ref_point); } +// LCOV_EXCL_STOP Eigen::VectorXd concat(const Eigen::VectorXd& a, const Eigen::VectorXd& b) { diff --git a/tesseract_common/test/tesseract_common_unit.cpp b/tesseract_common/test/tesseract_common_unit.cpp index d85e55e4983..588282b28db 100644 --- a/tesseract_common/test/tesseract_common_unit.cpp +++ b/tesseract_common/test/tesseract_common_unit.cpp @@ -15,6 +15,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include TEST(TesseractCommonUnit, isNumeric) // NOLINT { @@ -1094,6 +1095,468 @@ TEST(TesseractCommonUnit, ContactManagersPluginInfoUnit) // NOLINT EXPECT_TRUE(cmpi.empty()); } +TEST(TesseractContactManagersFactoryUnit, KinematicsPluginInfoYamlUnit) // NOLINT +{ + std::string yaml_string = R"(kinematic_plugins: + search_paths: + - /usr/local/lib + search_libraries: + - tesseract_kinematics_kdl_factories + fwd_kin_plugins: + iiwa_manipulator: + default: KDLFwdKinChain + plugins: + KDLFwdKinChain: + class: KDLFwdKinChainFactory + config: + base_link: base_link + tip_link: tool0 + inv_kin_plugins: + iiwa_manipulator: + default: KDLInvKinChainLMA + plugins: + KDLInvKinChainLMA: + class: KDLInvKinChainLMAFactory + config: + base_link: base_link + tip_link: tool0 + KDLInvKinChainNR: + class: KDLInvKinChainNRFactory + config: + base_link: base_link + tip_link: tool0)"; + + { // Success + YAML::Node plugin_config = YAML::Load(yaml_string); + YAML::Node config = plugin_config[tesseract_common::KinematicsPluginInfo::CONFIG_KEY]; + auto cmpi = config.as(); + + const YAML::Node& plugin_info = plugin_config["kinematic_plugins"]; + const YAML::Node& search_paths = plugin_info["search_paths"]; + const YAML::Node& search_libraries = plugin_info["search_libraries"]; + const YAML::Node& fwd_kin_default_plugin = plugin_info["fwd_kin_plugins"]["iiwa_manipulator"]["default"]; + const YAML::Node& fwd_kin_plugins = plugin_info["fwd_kin_plugins"]["iiwa_manipulator"]["plugins"]; + const YAML::Node& inv_kin_default_plugin = plugin_info["inv_kin_plugins"]["iiwa_manipulator"]["default"]; + const YAML::Node& inv_kin_plugins = plugin_info["inv_kin_plugins"]["iiwa_manipulator"]["plugins"]; + + { + std::set sp = cmpi.search_paths; + EXPECT_EQ(sp.size(), 1); + + for (auto it = search_paths.begin(); it != search_paths.end(); ++it) + { + EXPECT_TRUE(std::find(sp.begin(), sp.end(), it->as()) != sp.end()); + } + } + + { + std::set sl = cmpi.search_libraries; + EXPECT_EQ(sl.size(), 1); + + for (auto it = search_libraries.begin(); it != search_libraries.end(); ++it) + { + EXPECT_TRUE(std::find(sl.begin(), sl.end(), it->as()) != sl.end()); + } + } + + EXPECT_EQ(fwd_kin_default_plugin.as(), cmpi.fwd_plugin_infos["iiwa_manipulator"].default_plugin); + EXPECT_EQ(fwd_kin_plugins.size(), cmpi.fwd_plugin_infos["iiwa_manipulator"].plugins.size()); + + EXPECT_EQ(inv_kin_default_plugin.as(), cmpi.inv_plugin_infos["iiwa_manipulator"].default_plugin); + EXPECT_EQ(inv_kin_plugins.size(), cmpi.inv_plugin_infos["iiwa_manipulator"].plugins.size()); + } + + { // search_paths failure + std::string yaml_string = R"(kinematic_plugins: + search_paths: + failure: issue + search_libraries: + - tesseract_kinematics_kdl_factories + fwd_kin_plugins: + iiwa_manipulator: + default: KDLFwdKinChain + plugins: + KDLFwdKinChain: + class: KDLFwdKinChainFactory + config: + base_link: base_link + tip_link: tool0 + inv_kin_plugins: + iiwa_manipulator: + default: KDLInvKinChainLMA + plugins: + KDLInvKinChainLMA: + class: KDLInvKinChainLMAFactory + config: + base_link: base_link + tip_link: tool0 + KDLInvKinChainNR: + class: KDLInvKinChainNRFactory + config: + base_link: base_link + tip_link: tool0)"; + + YAML::Node plugin_config = YAML::Load(yaml_string); + YAML::Node config = plugin_config[tesseract_common::KinematicsPluginInfo::CONFIG_KEY]; + EXPECT_ANY_THROW(config.as()); // NOLINT + } + + { // search_libraries failure + std::string yaml_string = R"(kinematic_plugins: + search_paths: + - /usr/local/lib + search_libraries: + failure: issue + fwd_kin_plugins: + iiwa_manipulator: + default: KDLFwdKinChain + plugins: + KDLFwdKinChain: + class: KDLFwdKinChainFactory + config: + base_link: base_link + tip_link: tool0 + inv_kin_plugins: + iiwa_manipulator: + default: KDLInvKinChainLMA + plugins: + KDLInvKinChainLMA: + class: KDLInvKinChainLMAFactory + config: + base_link: base_link + tip_link: tool0 + KDLInvKinChainNR: + class: KDLInvKinChainNRFactory + config: + base_link: base_link + tip_link: tool0)"; + + YAML::Node plugin_config = YAML::Load(yaml_string); + YAML::Node config = plugin_config[tesseract_common::KinematicsPluginInfo::CONFIG_KEY]; + EXPECT_ANY_THROW(config.as()); // NOLINT + } + + { // missing fwd plugins failure + std::string yaml_string = R"(kinematic_plugins: + search_paths: + - /usr/local/lib + search_libraries: + - tesseract_kinematics_kdl_factories + fwd_kin_plugins: + iiwa_manipulator: + default: KDLFwdKinChain + inv_kin_plugins: + iiwa_manipulator: + default: KDLInvKinChainLMA + plugins: + KDLInvKinChainLMA: + class: KDLInvKinChainLMAFactory + config: + base_link: base_link + tip_link: tool0 + KDLInvKinChainNR: + class: KDLInvKinChainNRFactory + config: + base_link: base_link + tip_link: tool0)"; + + YAML::Node plugin_config = YAML::Load(yaml_string); + YAML::Node config = plugin_config[tesseract_common::KinematicsPluginInfo::CONFIG_KEY]; + EXPECT_ANY_THROW(config.as()); // NOLINT + } + + { // fwd plugins is not map failure + + std::string yaml_string = R"(kinematic_plugins: + search_paths: + - /usr/local/lib + search_libraries: + - tesseract_kinematics_kdl_factories + fwd_kin_plugins: + iiwa_manipulator: + - tesseract_collision_bullet_factories + - tesseract_collision_fcl_factories + inv_kin_plugins: + iiwa_manipulator: + default: KDLInvKinChainLMA + plugins: + KDLInvKinChainLMA: + class: KDLInvKinChainLMAFactory + config: + base_link: base_link + tip_link: tool0 + KDLInvKinChainNR: + class: KDLInvKinChainNRFactory + config: + base_link: base_link + tip_link: tool0)"; + + YAML::Node plugin_config = YAML::Load(yaml_string); + YAML::Node config = plugin_config[tesseract_common::KinematicsPluginInfo::CONFIG_KEY]; + EXPECT_ANY_THROW(config.as()); // NOLINT + } + + { // missing inv plugins failure + std::string yaml_string = R"(kinematic_plugins: + search_paths: + - /usr/local/lib + search_libraries: + - tesseract_kinematics_kdl_factories + fwd_kin_plugins: + iiwa_manipulator: + default: KDLFwdKinChain + plugins: + KDLFwdKinChain: + class: KDLFwdKinChainFactory + config: + base_link: base_link + tip_link: tool0 + inv_kin_plugins: + iiwa_manipulator: + default: KDLInvKinChainLMA)"; + + YAML::Node plugin_config = YAML::Load(yaml_string); + YAML::Node config = plugin_config[tesseract_common::KinematicsPluginInfo::CONFIG_KEY]; + EXPECT_ANY_THROW(config.as()); // NOLINT + } + + { // inv plugins is not map failure + std::string yaml_string = R"(kinematic_plugins: + search_paths: + - /usr/local/lib + search_libraries: + - tesseract_kinematics_kdl_factories + fwd_kin_plugins: + iiwa_manipulator: + default: KDLFwdKinChain + plugins: + KDLFwdKinChain: + class: KDLFwdKinChainFactory + config: + base_link: base_link + tip_link: tool0 + inv_kin_plugins: + iiwa_manipulator: + - tesseract_collision_bullet_factories + - tesseract_collision_fcl_factories)"; + + YAML::Node plugin_config = YAML::Load(yaml_string); + YAML::Node config = plugin_config[tesseract_common::KinematicsPluginInfo::CONFIG_KEY]; + EXPECT_ANY_THROW(config.as()); // NOLINT + } +} + +TEST(TesseractContactManagersFactoryUnit, ContactManagersPluginInfoYamlUnit) // NOLINT +{ + std::string yaml_string = R"(contact_manager_plugins: + search_paths: + - /usr/local/lib + search_libraries: + - tesseract_collision_bullet_factories + - tesseract_collision_fcl_factories + discrete_plugins: + default: BulletDiscreteBVHManager + plugins: + BulletDiscreteBVHManager: + class: BulletDiscreteBVHManagerFactory + BulletDiscreteSimpleManager: + class: BulletDiscreteSimpleManagerFactory + FCLDiscreteBVHManager: + class: FCLDiscreteBVHManagerFactory + continuous_plugins: + default: BulletCastBVHManager + plugins: + BulletCastBVHManager: + class: BulletCastBVHManagerFactory + BulletCastSimpleManager: + class: BulletCastSimpleManagerFactory)"; + + { // Success + YAML::Node plugin_config = YAML::Load(yaml_string); + YAML::Node config = plugin_config[tesseract_common::ContactManagersPluginInfo::CONFIG_KEY]; + auto cmpi = config.as(); + + const YAML::Node& plugin_info = plugin_config["contact_manager_plugins"]; + const YAML::Node& search_paths = plugin_info["search_paths"]; + const YAML::Node& search_libraries = plugin_info["search_libraries"]; + const YAML::Node& discrete_default_plugin = plugin_info["discrete_plugins"]["default"]; + const YAML::Node& discrete_plugins = plugin_info["discrete_plugins"]["plugins"]; + const YAML::Node& continuous_default_plugin = plugin_info["continuous_plugins"]["default"]; + const YAML::Node& continuous_plugins = plugin_info["continuous_plugins"]["plugins"]; + + { + std::set sp = cmpi.search_paths; + EXPECT_EQ(sp.size(), 1); + + for (auto it = search_paths.begin(); it != search_paths.end(); ++it) + { + EXPECT_TRUE(std::find(sp.begin(), sp.end(), it->as()) != sp.end()); + } + } + + { + std::set sl = cmpi.search_libraries; + EXPECT_EQ(sl.size(), 2); + + for (auto it = search_libraries.begin(); it != search_libraries.end(); ++it) + { + EXPECT_TRUE(std::find(sl.begin(), sl.end(), it->as()) != sl.end()); + } + } + + EXPECT_EQ(discrete_default_plugin.as(), cmpi.discrete_plugin_infos.default_plugin); + EXPECT_EQ(discrete_plugins.size(), cmpi.discrete_plugin_infos.plugins.size()); + + EXPECT_EQ(continuous_default_plugin.as(), cmpi.continuous_plugin_infos.default_plugin); + EXPECT_EQ(continuous_plugins.size(), cmpi.continuous_plugin_infos.plugins.size()); + } + + { // search_paths failure + std::string yaml_string = R"(contact_manager_plugins: + search_paths: + failure: issue + search_libraries: + - tesseract_collision_bullet_factories + - tesseract_collision_fcl_factories + discrete_plugins: + default: BulletDiscreteBVHManager + plugins: + BulletDiscreteBVHManager: + class: BulletDiscreteBVHManagerFactory + BulletDiscreteSimpleManager: + class: BulletDiscreteSimpleManagerFactory + FCLDiscreteBVHManager: + class: FCLDiscreteBVHManagerFactory + continuous_plugins: + default: BulletCastBVHManager + plugins: + BulletCastBVHManager: + class: BulletCastBVHManagerFactory + BulletCastSimpleManager: + class: BulletCastSimpleManagerFactory)"; + YAML::Node plugin_config = YAML::Load(yaml_string); + YAML::Node config = plugin_config[tesseract_common::ContactManagersPluginInfo::CONFIG_KEY]; + EXPECT_ANY_THROW(config.as()); // NOLINT + } + + { // search_libraries failure + std::string yaml_string = R"(contact_manager_plugins: + search_paths: + - /usr/local/lib + search_libraries: + failure: issue + discrete_plugins: + default: BulletDiscreteBVHManager + plugins: + BulletDiscreteBVHManager: + class: BulletDiscreteBVHManagerFactory + BulletDiscreteSimpleManager: + class: BulletDiscreteSimpleManagerFactory + FCLDiscreteBVHManager: + class: FCLDiscreteBVHManagerFactory + continuous_plugins: + default: BulletCastBVHManager + plugins: + BulletCastBVHManager: + class: BulletCastBVHManagerFactory + BulletCastSimpleManager: + class: BulletCastSimpleManagerFactory)"; + YAML::Node plugin_config = YAML::Load(yaml_string); + YAML::Node config = plugin_config[tesseract_common::ContactManagersPluginInfo::CONFIG_KEY]; + EXPECT_ANY_THROW(config.as()); // NOLINT + } + + { // missing discrete plugins failure + std::string yaml_string = R"(contact_manager_plugins: + search_paths: + - /usr/local/lib + search_libraries: + - tesseract_collision_bullet_factories + - tesseract_collision_fcl_factories + discrete_plugins: + default: BulletDiscreteBVHManager + continuous_plugins: + default: BulletCastBVHManager + plugins: + BulletCastBVHManager: + class: BulletCastBVHManagerFactory + BulletCastSimpleManager: + class: BulletCastSimpleManagerFactory)"; + YAML::Node plugin_config = YAML::Load(yaml_string); + YAML::Node config = plugin_config[tesseract_common::ContactManagersPluginInfo::CONFIG_KEY]; + EXPECT_ANY_THROW(config.as()); // NOLINT + } + + { // discrete plugins is not map failure + std::string yaml_string = R"(contact_manager_plugins: + search_paths: + - /usr/local/lib + search_libraries: + - tesseract_collision_bullet_factories + - tesseract_collision_fcl_factories + discrete_plugins: + - tesseract_collision_bullet_factories + - tesseract_collision_fcl_factories + continuous_plugins: + default: BulletCastBVHManager + plugins: + BulletCastBVHManager: + class: BulletCastBVHManagerFactory + BulletCastSimpleManager: + class: BulletCastSimpleManagerFactory)"; + YAML::Node plugin_config = YAML::Load(yaml_string); + YAML::Node config = plugin_config[tesseract_common::ContactManagersPluginInfo::CONFIG_KEY]; + EXPECT_ANY_THROW(config.as()); // NOLINT + } + + { // missing continuous plugins failure + std::string yaml_string = R"(contact_manager_plugins: + search_paths: + - /usr/local/lib + search_libraries: + - tesseract_collision_bullet_factories + - tesseract_collision_fcl_factories + discrete_plugins: + default: BulletDiscreteBVHManager + plugins: + BulletDiscreteBVHManager: + class: BulletDiscreteBVHManagerFactory + BulletDiscreteSimpleManager: + class: BulletDiscreteSimpleManagerFactory + FCLDiscreteBVHManager: + class: FCLDiscreteBVHManagerFactory + continuous_plugins: + default: BulletCastBVHManager)"; + YAML::Node plugin_config = YAML::Load(yaml_string); + YAML::Node config = plugin_config[tesseract_common::ContactManagersPluginInfo::CONFIG_KEY]; + EXPECT_ANY_THROW(config.as()); // NOLINT + } + + { // continuous plugins is not map failure + std::string yaml_string = R"(contact_manager_plugins: + search_paths: + - /usr/local/lib + search_libraries: + - tesseract_collision_bullet_factories + - tesseract_collision_fcl_factories + discrete_plugins: + default: BulletDiscreteBVHManager + plugins: + BulletDiscreteBVHManager: + class: BulletDiscreteBVHManagerFactory + BulletDiscreteSimpleManager: + class: BulletDiscreteSimpleManagerFactory + FCLDiscreteBVHManager: + class: FCLDiscreteBVHManagerFactory + continuous_plugins: + - tesseract_collision_bullet_factories + - tesseract_collision_fcl_factories)"; + YAML::Node plugin_config = YAML::Load(yaml_string); + YAML::Node config = plugin_config[tesseract_common::ContactManagersPluginInfo::CONFIG_KEY]; + EXPECT_ANY_THROW(config.as()); // NOLINT + } +} + TEST(TesseractCommonUnit, linkNamesPairUnit) // NOLINT { tesseract_common::LinkNamesPair p1 = tesseract_common::makeOrderedLinkPair("link_1", "link_2"); @@ -1123,7 +1586,7 @@ TEST(TesseractCommonUnit, calcRotationalError) // NOLINT EXPECT_NEAR(rot_err.norm(), M_PI_2, 1e-6); EXPECT_TRUE(rot_err.normalized().isApprox(-Eigen::Vector3d::UnitZ(), 1e-6)); - // Test lessthan than -PI + // Test less than than -PI pi_rot = identity * Eigen::AngleAxisd(-3 * M_PI_2, Eigen::Vector3d::UnitZ()); rot_err = tesseract_common::calcRotationalError(pi_rot.rotation()); EXPECT_NEAR(rot_err.norm(), M_PI_2, 1e-6); @@ -1319,6 +1782,66 @@ TEST(TesseractCommonUnit, calcRotationalError2) // NOLINT } } +/** @brief Tests calcTransformError */ +TEST(TesseractCommonUnit, calcTransformError) // NOLINT +{ + Eigen::Isometry3d identity = Eigen::Isometry3d::Identity(); + + { // X-Axis + Eigen::Isometry3d pi_rot = identity * Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitX()); + Eigen::VectorXd err = tesseract_common::calcTransformError(identity, pi_rot); + EXPECT_TRUE(err.head(3).isApprox(Eigen::Vector3d::Zero())); + EXPECT_TRUE(err.tail(3).isApprox(Eigen::Vector3d(M_PI_2, 0, 0))); + } + + { // Y-Axis + Eigen::Isometry3d pi_rot = identity * Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitY()); + Eigen::VectorXd err = tesseract_common::calcTransformError(identity, pi_rot); + EXPECT_TRUE(err.head(3).isApprox(Eigen::Vector3d::Zero())); + EXPECT_TRUE(err.tail(3).isApprox(Eigen::Vector3d(0, M_PI_2, 0))); + } + + { // Z-Axis + Eigen::Isometry3d pi_rot = identity * Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitZ()); + Eigen::VectorXd err = tesseract_common::calcTransformError(identity, pi_rot); + EXPECT_TRUE(err.head(3).isApprox(Eigen::Vector3d::Zero())); + EXPECT_TRUE(err.tail(3).isApprox(Eigen::Vector3d(0, 0, M_PI_2))); + } + + { // Translation + Eigen::Isometry3d pi_rot = identity * Eigen::Translation3d(1, 2, 3); + Eigen::VectorXd err = tesseract_common::calcTransformError(identity, pi_rot); + EXPECT_TRUE(err.head(3).isApprox(Eigen::Vector3d(1, 2, 3))); + EXPECT_TRUE(err.tail(3).isApprox(Eigen::Vector3d::Zero())); + } +} + +/** @brief Tests calcTransformError */ +TEST(TesseractCommonUnit, computeRandomColor) // NOLINT +{ + Eigen::Vector4d color = tesseract_common::computeRandomColor(); + EXPECT_FALSE(color(0) < 0); + EXPECT_FALSE(color(1) < 0); + EXPECT_FALSE(color(2) < 0); + EXPECT_FALSE(color(3) < 0); + EXPECT_FALSE(color(0) > 1); + EXPECT_FALSE(color(1) > 1); + EXPECT_FALSE(color(2) > 1); + EXPECT_FALSE(color(3) > 1); +} + +/** @brief Tests calcTransformError */ +TEST(TesseractCommonUnit, concat) // NOLINT +{ + Eigen::Vector3d a(1, 2, 3); + Eigen::Vector3d b(4, 5, 6); + + Eigen::VectorXd c = tesseract_common::concat(a, b); + EXPECT_EQ(c.rows(), a.rows() + b.rows()); + EXPECT_TRUE(c.head(3).isApprox(a)); + EXPECT_TRUE(c.tail(3).isApprox(b)); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/tesseract_environment/include/tesseract_environment/environment.h b/tesseract_environment/include/tesseract_environment/environment.h index 4c4a9f9dbfb..784d95e1e4d 100644 --- a/tesseract_environment/include/tesseract_environment/environment.h +++ b/tesseract_environment/include/tesseract_environment/environment.h @@ -209,7 +209,7 @@ class Environment * * If manipulator information tcp is defined as a string it does the following * - First check if manipulator info is empty or already an Isometry3d, if so return identity - * - Next if not, it checks if the tcp offset name is a link in the environment if so it return identity + * - Next if not, it checks if the tcp offset name is a link in the environment if so throw an exception. * - Next if not found, it looks up the tcp name in the SRDF kinematics information * - Next if not found, it leverages the user defined callbacks to try an locate the tcp information. * - Next throw an exception, because no tcp information was located. diff --git a/tesseract_environment/src/environment.cpp b/tesseract_environment/src/environment.cpp index a7591635d57..da1c04f5256 100644 --- a/tesseract_environment/src/environment.cpp +++ b/tesseract_environment/src/environment.cpp @@ -369,10 +369,11 @@ Eigen::Isometry3d Environment::findTCPOffset(const tesseract_common::Manipulator if (manip_info.tcp_offset.index() != 0) return std::get<1>(manip_info.tcp_offset); - // Check if the tcp offset name is a link in the scene, if so return Identity + // Check if the tcp offset name is a link in the scene, if so throw an exception const std::string& tcp_offset_name = std::get<0>(manip_info.tcp_offset); if (state_solver_->hasLinkName(tcp_offset_name)) - return Eigen::Isometry3d::Identity(); + throw std::runtime_error("The tcp offset name '" + tcp_offset_name + + "' should not be an existing link in the scene. Assign it as the tcp_frame instead!"); // Check Manipulator Manager for TCP if (kinematics_information_.hasGroupTCP(manip_info.manipulator, tcp_offset_name)) @@ -392,7 +393,7 @@ Eigen::Isometry3d Environment::findTCPOffset(const tesseract_common::Manipulator } } - throw std::runtime_error("Could not find tcp by name " + tcp_offset_name + "' setting to Identity!"); + throw std::runtime_error("Could not find tcp by name " + tcp_offset_name + "'!"); } void Environment::addFindTCPOffsetCallback(const FindTCPOffsetCallbackFn& fn) @@ -709,6 +710,7 @@ bool Environment::setActiveDiscreteContactManagerHelper(const std::string& name) return false; } + contact_managers_plugin_info_.discrete_plugin_infos.default_plugin = name; discrete_manager_ = std::move(manager); // Update the current state information since the contact manager has been created/set @@ -732,6 +734,7 @@ bool Environment::setActiveContinuousContactManagerHelper(const std::string& nam return false; } + contact_managers_plugin_info_.continuous_plugin_infos.default_plugin = name; continuous_manager_ = std::move(manager); // Update the current state information since the contact manager has been created/set @@ -749,7 +752,7 @@ Environment::getDiscreteContactManagerHelper(const std::string& name) const return nullptr; manager->setIsContactAllowedFn(is_contact_allowed_fn_); - if (initialized_) + if (scene_graph_ != nullptr) { for (const auto& link : scene_graph_->getLinks()) { @@ -780,7 +783,7 @@ Environment::getContinuousContactManagerHelper(const std::string& name) const return nullptr; manager->setIsContactAllowedFn(is_contact_allowed_fn_); - if (initialized_) + if (scene_graph_ != nullptr) { for (const auto& link : scene_graph_->getLinks()) { diff --git a/tesseract_environment/src/utils.cpp b/tesseract_environment/src/utils.cpp index 14e03c59758..8db57f1b125 100644 --- a/tesseract_environment/src/utils.cpp +++ b/tesseract_environment/src/utils.cpp @@ -140,7 +140,7 @@ bool checkTrajectory(std::vector& contact throw std::runtime_error("checkTrajectory was given an CollisionEvaluatorType that is inconsistent with the " "ContactManager type"); - if (traj.rows() == 1) + if (traj.rows() < 2) throw std::runtime_error("checkTrajectory was given continuous contact manager with a trajectory that only has one " "state."); @@ -266,7 +266,7 @@ bool checkTrajectory(std::vector& contact throw std::runtime_error("checkTrajectory was given an CollisionEvaluatorType that is inconsistent with the " "ContactManager type"); - if (traj.rows() == 1) + if (traj.rows() < 2) throw std::runtime_error("checkTrajectory was given continuous contact manager with a trajectory that only has one " "state."); @@ -393,6 +393,9 @@ bool checkTrajectory(std::vector& contact throw std::runtime_error("checkTrajectory was given an CollisionEvaluatorType that is inconsistent with the " "ContactManager type"); + if (traj.rows() == 0) + throw std::runtime_error("checkTrajectory was given continuous contact manager with empty trajectory."); + if (traj.rows() == 1) { tesseract_scene_graph::SceneState state = state_solver.getState(joint_names, traj.row(0)); @@ -513,6 +516,9 @@ bool checkTrajectory(std::vector& contact throw std::runtime_error("checkTrajectory was given an CollisionEvaluatorType that is inconsistent with the " "ContactManager type"); + if (traj.rows() == 0) + throw std::runtime_error("checkTrajectory was given continuous contact manager with empty trajectory."); + if (traj.rows() == 1) { tesseract_common::TransformMap state = manip.calcFwdKin(traj.row(0)); diff --git a/tesseract_environment/test/tesseract_environment_unit.cpp b/tesseract_environment/test/tesseract_environment_unit.cpp index 408c3ffbe1b..754c05a73a6 100644 --- a/tesseract_environment/test/tesseract_environment_unit.cpp +++ b/tesseract_environment/test/tesseract_environment_unit.cpp @@ -14,6 +14,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include using namespace tesseract_scene_graph; using namespace tesseract_srdf; @@ -47,11 +48,13 @@ std::string locateResource(const std::string& url) return mod_url; } -Eigen::Isometry3d tcpCallback(const tesseract_common::ManipulatorInfo&) +Eigen::Isometry3d tcpCallback(const tesseract_common::ManipulatorInfo& mi) { - Eigen::Isometry3d tcp = Eigen::Isometry3d::Identity(); - tcp.translation() = Eigen::Vector3d(0, 0, 0.1); - return tcp; + const std::string& tcp_offset_name = std::get<0>(mi.tcp_offset); + if (tcp_offset_name == "laser") + return Eigen::Isometry3d::Identity() * Eigen::Translation3d(0, 0, 0.1); + + throw std::runtime_error("TCPCallback failed to find tcp!"); } SceneGraph::UPtr getSceneGraph() @@ -73,6 +76,26 @@ SRDFModel::Ptr getSRDFModel(const SceneGraph& scene_graph) return srdf; } +std::string getSceneGraphString() +{ + std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf"; + + std::ifstream f(path); + std::ostringstream ss; + ss << f.rdbuf(); + return ss.str(); +} + +std::string getSRDFModelString() +{ + std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.srdf"; + + std::ifstream f(path); + std::ostringstream ss; + ss << f.rdbuf(); + return ss.str(); +} + SceneGraph::Ptr getSubSceneGraph() { SceneGraph::Ptr subgraph = std::make_shared(); @@ -125,21 +148,15 @@ void runGetLinkTransformsTest(Environment& env) } } -Environment::Ptr getEnvironment() +enum class EnvironmentInitType { - tesseract_scene_graph::SceneGraph::Ptr scene_graph = getSceneGraph(); - EXPECT_TRUE(scene_graph != nullptr); - - // Check to make sure all links are enabled - for (const auto& link : scene_graph->getLinks()) - { - EXPECT_TRUE(scene_graph->getLinkCollisionEnabled(link->getName())); - EXPECT_TRUE(scene_graph->getLinkVisibility(link->getName())); - } - - auto srdf = getSRDFModel(*scene_graph); - EXPECT_TRUE(srdf != nullptr); + OBJECT, + STRING, + FILEPATH +}; +Environment::Ptr getEnvironment(EnvironmentInitType init_type = EnvironmentInitType::OBJECT) +{ auto env = std::make_shared(); EXPECT_TRUE(env != nullptr); EXPECT_EQ(0, env->getRevision()); @@ -148,17 +165,55 @@ Environment::Ptr getEnvironment() EXPECT_FALSE(env->isInitialized()); EXPECT_TRUE(env->clone() != nullptr); - bool success = env->init(*scene_graph, srdf); + bool success = false; + switch (init_type) + { + case EnvironmentInitType::OBJECT: + { + tesseract_scene_graph::SceneGraph::Ptr scene_graph = getSceneGraph(); + EXPECT_TRUE(scene_graph != nullptr); + + // Check to make sure all links are enabled + for (const auto& link : scene_graph->getLinks()) + { + EXPECT_TRUE(scene_graph->getLinkCollisionEnabled(link->getName())); + EXPECT_TRUE(scene_graph->getLinkVisibility(link->getName())); + } + + auto srdf = getSRDFModel(*scene_graph); + EXPECT_TRUE(srdf != nullptr); + + success = env->init(*scene_graph, srdf); + EXPECT_TRUE(env->getResourceLocator() == nullptr); + env->setResourceLocator(std::make_shared(locateResource)); + EXPECT_TRUE(env->getResourceLocator() != nullptr); + break; + } + case EnvironmentInitType::STRING: + { + std::string urdf_string = getSceneGraphString(); + std::string srdf_string = getSRDFModelString(); + success = env->init( + urdf_string, srdf_string, std::make_shared(locateResource)); + EXPECT_TRUE(env->getResourceLocator() != nullptr); + break; + } + case EnvironmentInitType::FILEPATH: + { + tesseract_common::fs::path urdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf"); + tesseract_common::fs::path srdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.srdf"); + + success = + env->init(urdf_path, srdf_path, std::make_shared(locateResource)); + EXPECT_TRUE(env->getResourceLocator() != nullptr); + break; + } + } + Environment::ConstPtr env_const = env; EXPECT_TRUE(success); EXPECT_EQ(3, env->getRevision()); EXPECT_TRUE(env->isInitialized()); - // EXPECT_TRUE(env->getManipulatorManager() != nullptr); - // EXPECT_TRUE(env_const->getManipulatorManager() != nullptr); - EXPECT_TRUE(env->getResourceLocator() == nullptr); - - env->setResourceLocator(std::make_shared(locateResource)); - EXPECT_TRUE(env->getResourceLocator() != nullptr); // Check to make sure all links are enabled for (const auto& link : env->getSceneGraph()->getLinks()) @@ -171,142 +226,292 @@ Environment::Ptr getEnvironment() env->addFindTCPOffsetCallback(tcpCallback); EXPECT_EQ(env->getFindTCPOffsetCallbacks().size(), 1); + // Check Group Names + tesseract_srdf::GroupNames group_names = env->getGroupNames(); + std::vector group_names_v(group_names.begin(), group_names.end()); + EXPECT_EQ(group_names_v.size(), 2); + EXPECT_EQ(group_names_v[0], "manipulator"); + EXPECT_EQ(group_names_v[1], "manipulator_joint_group"); + + // Check Group Joint Names + std::vector target_joint_names = { "joint_a1", "joint_a2", "joint_a3", "joint_a4", + "joint_a5", "joint_a6", "joint_a7" }; + { + std::vector joint_names = env->getGroupJointNames("manipulator"); + EXPECT_TRUE(tesseract_common::isIdentical(joint_names, target_joint_names)); + } + + { + std::vector joint_names = env->getGroupJointNames("manipulator_joint_group"); + EXPECT_TRUE(tesseract_common::isIdentical(joint_names, target_joint_names)); + } + + // Check Get Joint Group + auto jg = env->getJointGroup("manipulator"); + EXPECT_TRUE(jg != nullptr); + + EXPECT_ANY_THROW(env->getJointGroup("does_not_exist")); // NOLINT + + // Check Get Joint Group + auto kg = env->getKinematicGroup("manipulator"); + EXPECT_TRUE(kg != nullptr); + + EXPECT_ANY_THROW(env->getKinematicGroup("does_not_exist")); // NOLINT + + // Check Kinematics Information + tesseract_srdf::KinematicsInformation kin_info = env->getKinematicsInformation(); + std::vector group_names_ki(kin_info.group_names.begin(), kin_info.group_names.end()); + EXPECT_EQ(group_names_ki.size(), 2); + EXPECT_EQ(group_names_ki[0], "manipulator"); + EXPECT_EQ(group_names_ki[1], "manipulator_joint_group"); + + // Get active contact managers + { + tesseract_common::ContactManagersPluginInfo cm_info = env->getContactManagersPluginInfo(); + EXPECT_EQ(cm_info.discrete_plugin_infos.default_plugin, "BulletDiscreteBVHManager"); + EXPECT_EQ(cm_info.continuous_plugin_infos.default_plugin, "BulletCastBVHManager"); + EXPECT_EQ(env->getDiscreteContactManager()->getName(), "BulletDiscreteBVHManager"); + EXPECT_EQ(env->getContinuousContactManager()->getName(), "BulletCastBVHManager"); + EXPECT_EQ(env->getDiscreteContactManager()->getCollisionObjects().size(), 8); + EXPECT_EQ(env->getContinuousContactManager()->getCollisionObjects().size(), 8); + } + { + env->setActiveDiscreteContactManager("BulletDiscreteSimpleManager"); + env->setActiveContinuousContactManager("BulletCastSimpleManager"); + tesseract_common::ContactManagersPluginInfo cm_info = env->getContactManagersPluginInfo(); + EXPECT_EQ(cm_info.discrete_plugin_infos.default_plugin, "BulletDiscreteSimpleManager"); + EXPECT_EQ(cm_info.continuous_plugin_infos.default_plugin, "BulletCastSimpleManager"); + EXPECT_EQ(env->getDiscreteContactManager()->getName(), "BulletDiscreteSimpleManager"); + EXPECT_EQ(env->getContinuousContactManager()->getName(), "BulletCastSimpleManager"); + EXPECT_EQ(env->getDiscreteContactManager()->getCollisionObjects().size(), 8); + EXPECT_EQ(env->getContinuousContactManager()->getCollisionObjects().size(), 8); + } + { + env->setActiveDiscreteContactManager("does_not_exist"); + env->setActiveContinuousContactManager("does_not_exist"); + tesseract_common::ContactManagersPluginInfo cm_info = env->getContactManagersPluginInfo(); + EXPECT_EQ(cm_info.discrete_plugin_infos.default_plugin, "BulletDiscreteSimpleManager"); + EXPECT_EQ(cm_info.continuous_plugin_infos.default_plugin, "BulletCastSimpleManager"); + EXPECT_EQ(env->getDiscreteContactManager()->getName(), "BulletDiscreteSimpleManager"); + EXPECT_EQ(env->getContinuousContactManager()->getName(), "BulletCastSimpleManager"); + EXPECT_EQ(env->getDiscreteContactManager()->getCollisionObjects().size(), 8); + EXPECT_EQ(env->getContinuousContactManager()->getCollisionObjects().size(), 8); + } + { + env->setActiveDiscreteContactManager("BulletDiscreteBVHManager"); + env->setActiveContinuousContactManager("BulletCastBVHManager"); + tesseract_common::ContactManagersPluginInfo cm_info = env->getContactManagersPluginInfo(); + EXPECT_EQ(cm_info.discrete_plugin_infos.default_plugin, "BulletDiscreteBVHManager"); + EXPECT_EQ(cm_info.continuous_plugin_infos.default_plugin, "BulletCastBVHManager"); + EXPECT_EQ(env->getDiscreteContactManager()->getName(), "BulletDiscreteBVHManager"); + EXPECT_EQ(env->getContinuousContactManager()->getName(), "BulletCastBVHManager"); + EXPECT_EQ(env->getDiscreteContactManager()->getCollisionObjects().size(), 8); + EXPECT_EQ(env->getContinuousContactManager()->getCollisionObjects().size(), 8); + } + + // Test getting contact managers + { + EXPECT_EQ(env->getDiscreteContactManager("BulletDiscreteSimpleManager")->getName(), "BulletDiscreteSimpleManager"); + EXPECT_EQ(env->getContinuousContactManager("BulletCastSimpleManager")->getName(), "BulletCastSimpleManager"); + } + + // Failed + EXPECT_TRUE(env->getDiscreteContactManager("does_not_exist") == nullptr); + EXPECT_TRUE(env->getContinuousContactManager("does_not_exist") == nullptr); + return env; } -TEST(TesseractEnvironmentUnit, EnvInitFailuresUnit) // NOLINT +Environment::Ptr getEnvironmentURDFOnly(EnvironmentInitType init_type) { auto env = std::make_shared(); EXPECT_TRUE(env != nullptr); EXPECT_EQ(0, env->getRevision()); + EXPECT_EQ(0, env->getCommandHistory().size()); EXPECT_FALSE(env->reset()); EXPECT_FALSE(env->isInitialized()); EXPECT_TRUE(env->clone() != nullptr); - // Test Empty commands - Commands commands; - EXPECT_FALSE(env->init(commands)); - EXPECT_FALSE(env->isInitialized()); + bool success = false; + switch (init_type) + { + case EnvironmentInitType::OBJECT: + { + tesseract_scene_graph::SceneGraph::Ptr scene_graph = getSceneGraph(); + EXPECT_TRUE(scene_graph != nullptr); + + // Check to make sure all links are enabled + for (const auto& link : scene_graph->getLinks()) + { + EXPECT_TRUE(scene_graph->getLinkCollisionEnabled(link->getName())); + EXPECT_TRUE(scene_graph->getLinkVisibility(link->getName())); + } + + success = env->init(*scene_graph); + EXPECT_TRUE(env->getResourceLocator() == nullptr); + env->setResourceLocator(std::make_shared(locateResource)); + EXPECT_TRUE(env->getResourceLocator() != nullptr); + break; + } + case EnvironmentInitType::STRING: + { + std::string urdf_string = getSceneGraphString(); + success = env->init(urdf_string, std::make_shared(locateResource)); + EXPECT_TRUE(env->getResourceLocator() != nullptr); + break; + } + case EnvironmentInitType::FILEPATH: + { + tesseract_common::fs::path urdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf"); + success = env->init(urdf_path, std::make_shared(locateResource)); + EXPECT_TRUE(env->getResourceLocator() != nullptr); + break; + } + } - // Test scene graph is not first command - commands.clear(); - auto cmd = std::make_shared("joint_name", "parent_link"); - commands.push_back(cmd); - EXPECT_FALSE(env->init(commands)); - EXPECT_FALSE(env->isInitialized()); + Environment::ConstPtr env_const = env; + EXPECT_TRUE(success); + EXPECT_EQ(1, env->getRevision()); + EXPECT_TRUE(env->isInitialized()); + + // Check to make sure all links are enabled + for (const auto& link : env->getSceneGraph()->getLinks()) + { + EXPECT_TRUE(env->getSceneGraph()->getLinkCollisionEnabled(link->getName())); + EXPECT_TRUE(env->getSceneGraph()->getLinkVisibility(link->getName())); + } + + EXPECT_TRUE(env->getDiscreteContactManager() == nullptr); + EXPECT_TRUE(env->getContinuousContactManager() == nullptr); + + return env; } -TEST(TesseractEnvironmentUnit, EnvCloneContactManagerUnit) // NOLINT +TEST(TesseractEnvironmentUnit, EnvInitURDFOnlyUnit) // NOLINT { - { // Get the environment - auto env = getEnvironment(); + getEnvironmentURDFOnly(EnvironmentInitType::OBJECT); + getEnvironmentURDFOnly(EnvironmentInitType::STRING); + getEnvironmentURDFOnly(EnvironmentInitType::FILEPATH); +} - // Test after clone if active list correct - tesseract_collision::DiscreteContactManager::Ptr discrete_manager = env->getDiscreteContactManager(); - const std::vector& e_active_list = env->getActiveLinkNames(); - const std::vector& d_active_list = discrete_manager->getActiveCollisionObjects(); - EXPECT_TRUE(std::equal(e_active_list.begin(), e_active_list.end(), d_active_list.begin())); +TEST(TesseractEnvironmentUnit, EnvInitFailuresUnit) // NOLINT +{ + auto rl = std::make_shared(locateResource); + { + auto env = std::make_shared(); + EXPECT_TRUE(env != nullptr); + EXPECT_EQ(0, env->getRevision()); + EXPECT_FALSE(env->reset()); + EXPECT_FALSE(env->isInitialized()); + EXPECT_TRUE(env->clone() != nullptr); + } - tesseract_collision::ContinuousContactManager::Ptr cast_manager = env->getContinuousContactManager(); - const std::vector& c_active_list = cast_manager->getActiveCollisionObjects(); - EXPECT_TRUE(std::equal(e_active_list.begin(), e_active_list.end(), c_active_list.begin())); + { // Test Empty commands + Commands commands; + auto env = std::make_shared(); + + EXPECT_FALSE(env->init(commands)); + EXPECT_FALSE(env->isInitialized()); } - // { // Get the environment with registered default contact managers - // auto env = getEnvironment(EnvRegisterMethod::ON_CONSTRUCTION); + { // Test scene graph is not first command + Commands commands; + auto env = std::make_shared(); + auto cmd = std::make_shared("joint_name", "parent_link"); + commands.push_back(cmd); + EXPECT_FALSE(env->init(commands)); + EXPECT_FALSE(env->isInitialized()); + } - // // Test after clone if active list correct - // tesseract_collision::DiscreteContactManager::Ptr discrete_manager = env->getDiscreteContactManager(); - // const std::vector& e_active_list = env->getActiveLinkNames(); - // const std::vector& d_active_list = discrete_manager->getActiveCollisionObjects(); - // EXPECT_TRUE(std::equal(e_active_list.begin(), e_active_list.end(), d_active_list.begin())); + { // Test Empty URDF String + auto env = std::make_shared(); + std::string urdf_string; + EXPECT_FALSE(env->init(urdf_string, rl)); + EXPECT_FALSE(env->isInitialized()); + } - // tesseract_collision::ContinuousContactManager::Ptr cast_manager = env->getContinuousContactManager(); - // const std::vector& c_active_list = cast_manager->getActiveCollisionObjects(); - // EXPECT_TRUE(std::equal(e_active_list.begin(), e_active_list.end(), c_active_list.begin())); - // } + { // Test bad URDF file path + auto env = std::make_shared(); + tesseract_common::fs::path urdf_path("/usr/tmp/doesnotexist.urdf"); + EXPECT_FALSE(env->init(urdf_path, rl)); + EXPECT_FALSE(env->isInitialized()); + } - // { // Get the environment with registered default contact managers function - // auto env = getEnvironment(EnvRegisterMethod::CALL_DEFAULT_REGISTAR); + { // Test Empty URDF String with srdf + auto env = std::make_shared(); + std::string urdf_string; + std::string srdf_string = getSRDFModelString(); + EXPECT_FALSE(env->init(urdf_string, srdf_string, rl)); + EXPECT_FALSE(env->isInitialized()); + } - // // Test after clone if active list correct - // tesseract_collision::DiscreteContactManager::Ptr discrete_manager = env->getDiscreteContactManager(); - // const std::vector& e_active_list = env->getActiveLinkNames(); - // const std::vector& d_active_list = discrete_manager->getActiveCollisionObjects(); - // EXPECT_TRUE(std::equal(e_active_list.begin(), e_active_list.end(), d_active_list.begin())); + { // Test bad URDF file path with srdf + auto env = std::make_shared(); + tesseract_common::fs::path urdf_path("/usr/tmp/doesnotexist.urdf"); + tesseract_common::fs::path srdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.srdf"); + EXPECT_FALSE(env->init(urdf_path, srdf_path, rl)); + EXPECT_FALSE(env->isInitialized()); + } + + { // Test URDF String with empty srdf + auto env = std::make_shared(); + std::string urdf_string = getSceneGraphString(); + std::string srdf_string; + EXPECT_FALSE(env->init(urdf_string, srdf_string, rl)); + EXPECT_FALSE(env->isInitialized()); + } - // tesseract_collision::ContinuousContactManager::Ptr cast_manager = env->getContinuousContactManager(); - // const std::vector& c_active_list = cast_manager->getActiveCollisionObjects(); - // EXPECT_TRUE(std::equal(e_active_list.begin(), e_active_list.end(), c_active_list.begin())); - // } + { // Test URDF file path with bad srdf path + auto env = std::make_shared(); + tesseract_common::fs::path urdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf"); + tesseract_common::fs::path srdf_path("/usr/tmp/doesnotexist.srdf"); + EXPECT_FALSE(env->init(urdf_path, srdf_path, rl)); + EXPECT_FALSE(env->isInitialized()); + } } -void runFindTCPTest() +TEST(TesseractEnvironmentUnit, EnvCloneContactManagerUnit) // NOLINT { - // Get the environment - auto env = getEnvironment(); + { // Get the environment + auto env = getEnvironment(EnvironmentInitType::OBJECT); - // Add link to tip of kinematic chain - Eigen::Isometry3d tcp_link_tf = Eigen::Isometry3d::Identity(); - tcp_link_tf.translation() = Eigen::Vector3d(0, 0, 0.35); - Link tcp_link("tcp_link"); - Joint tcp_joint("tcp_joint"); - tcp_joint.parent_link_name = "tool0"; - tcp_joint.child_link_name = "tcp_link"; - tcp_joint.parent_to_joint_origin_transform = tcp_link_tf; - tcp_joint.type = JointType::FIXED; - env->applyCommand(std::make_shared(tcp_link, tcp_joint)); - - // Add external tcp - Eigen::Isometry3d external_tcp_link_tf = Eigen::Isometry3d::Identity(); - external_tcp_link_tf.translation() = Eigen::Vector3d(1, 0, 1); - Link external_tcp_link("external_tcp_link"); - Joint external_tcp_joint("external_tcp_joint"); - external_tcp_joint.parent_link_name = "base_link"; - external_tcp_joint.child_link_name = "external_tcp_link"; - external_tcp_joint.parent_to_joint_origin_transform = external_tcp_link_tf; - external_tcp_joint.type = JointType::FIXED; - env->applyCommand(std::make_shared(external_tcp_link, external_tcp_joint)); + // Test after clone if active list correct + tesseract_collision::DiscreteContactManager::Ptr discrete_manager = env->getDiscreteContactManager(); + const std::vector& e_active_list = env->getActiveLinkNames(); + const std::vector& d_active_list = discrete_manager->getActiveCollisionObjects(); + EXPECT_TRUE(std::equal(e_active_list.begin(), e_active_list.end(), d_active_list.begin())); - { // Should return the solution form the provided callback - Eigen::Isometry3d tcp = Eigen::Isometry3d::Identity(); - tcp.translation() = Eigen::Vector3d(0, 0, 0.1); - tesseract_common::ManipulatorInfo manip_info("manipulator", "unknown", "unknown", tcp); - Eigen::Isometry3d found_tcp = env->findTCPOffset(manip_info); - EXPECT_TRUE(std::get(manip_info.tcp_offset).isApprox(found_tcp, 1e-6)); + tesseract_collision::ContinuousContactManager::Ptr cast_manager = env->getContinuousContactManager(); + const std::vector& c_active_list = cast_manager->getActiveCollisionObjects(); + EXPECT_TRUE(std::equal(e_active_list.begin(), e_active_list.end(), c_active_list.begin())); } - { // Empty tcp should return identity - tesseract_common::ManipulatorInfo manip_info("manipulator", "", ""); + { // Get the environment + auto env = getEnvironment(EnvironmentInitType::STRING); - Eigen::Isometry3d tcp = Eigen::Isometry3d::Identity(); - Eigen::Isometry3d found_tcp = env->findTCPOffset(manip_info); - EXPECT_TRUE(tcp.isApprox(found_tcp, 1e-6)); - } + // Test after clone if active list correct + tesseract_collision::DiscreteContactManager::Ptr discrete_manager = env->getDiscreteContactManager(); + const std::vector& e_active_list = env->getActiveLinkNames(); + const std::vector& d_active_list = discrete_manager->getActiveCollisionObjects(); + EXPECT_TRUE(std::equal(e_active_list.begin(), e_active_list.end(), d_active_list.begin())); - { // The tcp is a link attached to the tip of the kinematic chain - tesseract_common::ManipulatorInfo manip_info("manipulator", "unknown", "tcp_link"); - Eigen::Isometry3d found_tcp = env->findTCPOffset(manip_info); - EXPECT_TRUE(tcp_link_tf.isApprox(found_tcp, 1e-6)); + tesseract_collision::ContinuousContactManager::Ptr cast_manager = env->getContinuousContactManager(); + const std::vector& c_active_list = cast_manager->getActiveCollisionObjects(); + EXPECT_TRUE(std::equal(e_active_list.begin(), e_active_list.end(), c_active_list.begin())); } - { // The tcp is external link name - tesseract_common::ManipulatorInfo manip_info("manipulator", "unknown", "external_tcp_link"); - Eigen::Isometry3d found_tcp = env->findTCPOffset(manip_info); - EXPECT_TRUE(external_tcp_link_tf.isApprox(found_tcp, 1e-6)); - } + { // Get the environment + auto env = getEnvironment(EnvironmentInitType::FILEPATH); - { // If the manipulator has a tcp transform then it should be returned - Eigen::Isometry3d tcp = Eigen::Isometry3d::Identity(); - tcp.translation() = Eigen::Vector3d(0, 0, 0.25); - tesseract_common::ManipulatorInfo manip_info("manipulator", "", "", tcp); - Eigen::Isometry3d found_tcp = env->findTCPOffset(manip_info); - EXPECT_TRUE(std::get(manip_info.tcp_offset).isApprox(found_tcp, 1e-6)); - } + // Test after clone if active list correct + tesseract_collision::DiscreteContactManager::Ptr discrete_manager = env->getDiscreteContactManager(); + const std::vector& e_active_list = env->getActiveLinkNames(); + const std::vector& d_active_list = discrete_manager->getActiveCollisionObjects(); + EXPECT_TRUE(std::equal(e_active_list.begin(), e_active_list.end(), d_active_list.begin())); - { // If the manipulator does not exist it should throw an exception - tesseract_common::ManipulatorInfo manip_info("missing_manipulator", "unknown", "unknown"); - EXPECT_ANY_THROW(env->findTCPOffset(manip_info)); // NOLINT + tesseract_collision::ContinuousContactManager::Ptr cast_manager = env->getContinuousContactManager(); + const std::vector& c_active_list = cast_manager->getActiveCollisionObjects(); + EXPECT_TRUE(std::equal(e_active_list.begin(), e_active_list.end(), c_active_list.begin())); } } @@ -925,6 +1130,46 @@ TEST(TesseractEnvironmentUnit, EnvChangeLinkVisibilityCommandUnit) // NOLINT EXPECT_FALSE(env->getSceneGraph()->getLinkVisibility(link_name)); } +TEST(TesseractEnvironmentUnit, EnvSetActiveContinuousContactManagerCommandUnit) // NOLINT +{ + // Get the environment + /** @todo update contact manager to have function to check collision object enabled state */ + auto env = getEnvironment(); + EXPECT_EQ(env->getRevision(), 3); + EXPECT_EQ(env->getCommandHistory().size(), 3); + + auto cmd = std::make_shared("BulletCastSimpleManager"); + EXPECT_TRUE(cmd != nullptr); + EXPECT_EQ(cmd->getType(), CommandType::SET_ACTIVE_CONTINUOUS_CONTACT_MANAGER); + EXPECT_EQ(cmd->getName(), "BulletCastSimpleManager"); + EXPECT_TRUE(env->applyCommand(cmd)); + EXPECT_EQ(env->getCommandHistory().back(), cmd); + + EXPECT_EQ(env->getRevision(), 4); + EXPECT_EQ(env->getCommandHistory().size(), 4); + EXPECT_EQ(env->getContinuousContactManager()->getName(), "BulletCastSimpleManager"); +} + +TEST(TesseractEnvironmentUnit, EnvSetActiveDiscreteContactManagerCommandUnit) // NOLINT +{ + // Get the environment + /** @todo update contact manager to have function to check collision object enabled state */ + auto env = getEnvironment(); + EXPECT_EQ(env->getRevision(), 3); + EXPECT_EQ(env->getCommandHistory().size(), 3); + + auto cmd = std::make_shared("BulletDiscreteSimpleManager"); + EXPECT_TRUE(cmd != nullptr); + EXPECT_EQ(cmd->getType(), CommandType::SET_ACTIVE_DISCRETE_CONTACT_MANAGER); + EXPECT_EQ(cmd->getName(), "BulletDiscreteSimpleManager"); + EXPECT_TRUE(env->applyCommand(cmd)); + EXPECT_EQ(env->getCommandHistory().back(), cmd); + + EXPECT_EQ(env->getRevision(), 4); + EXPECT_EQ(env->getCommandHistory().size(), 4); + EXPECT_EQ(env->getDiscreteContactManager()->getName(), "BulletDiscreteSimpleManager"); +} + TEST(TesseractEnvironmentUnit, EnvChangeCollisionMarginsCommandUnit) // NOLINT { { // MODIFY_PAIR_MARGIN and OVERRIDE_PAIR_MARGIN Unit Test @@ -1212,6 +1457,9 @@ TEST(TesseractEnvironmentUnit, EnvCurrentStatePreservedWhenEnvChanges) // NOLIN EXPECT_TRUE(env->getLinkVisibility(link_name)); } + // Get current timestamp + auto d1 = env->getCurrentStateTimestamp(); + // Set the initial state of the robot std::unordered_map joint_states; joint_states["joint_a1"] = 0.0; @@ -1223,6 +1471,11 @@ TEST(TesseractEnvironmentUnit, EnvCurrentStatePreservedWhenEnvChanges) // NOLIN joint_states["joint_a7"] = 0.0; env->setState(joint_states); + // Get new timestamp + auto d2 = env->getCurrentStateTimestamp(); + + EXPECT_TRUE(d2.count() > d1.count()); + SceneState state = env->getState(); for (auto& joint_state : joint_states) { @@ -2088,10 +2341,420 @@ TEST(TesseractEnvironmentUnit, EnvSetState2) // NOLINT } } -// TEST(TesseractEnvironmentUnit, EnvFindTCPUnit) // NOLINT -//{ -// runFindTCPTest(); -//} +TEST(TesseractEnvironmentUnit, EnvFindTCPUnit) // NOLINT +{ + // Get the environment + auto env = getEnvironment(); + + { // Should return the solution form the provided callback + Eigen::Isometry3d tcp = Eigen::Isometry3d::Identity(); + tcp.translation() = Eigen::Vector3d(0, 0, 0.1); + tesseract_common::ManipulatorInfo manip_info("manipulator", "unknown", "unknown", tcp); + Eigen::Isometry3d found_tcp = env->findTCPOffset(manip_info); + EXPECT_TRUE(std::get(manip_info.tcp_offset).isApprox(found_tcp, 1e-6)); + } + + { // If the manipulator has a tcp transform then it should be returned + Eigen::Isometry3d tcp = Eigen::Isometry3d::Identity(); + tcp.translation() = Eigen::Vector3d(0, 0, 0.25); + tesseract_common::ManipulatorInfo manip_info("manipulator", "", ""); + manip_info.tcp_offset = "laser"; + Eigen::Isometry3d found_tcp = env->findTCPOffset(manip_info); + EXPECT_TRUE(found_tcp.isApprox(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0, 0, 0.1), 1e-6)); + } + + { // The tcp offset name is a link in the environment so it should throw an exception + tesseract_common::ManipulatorInfo manip_info("manipulator", "unknown", "unknown"); + manip_info.tcp_offset = "tool0"; + EXPECT_ANY_THROW(env->findTCPOffset(manip_info)); // NOLINT + } + + { // If the tcp offset name does not exist it should throw an exception + tesseract_common::ManipulatorInfo manip_info("manipulator", "unknown", "unknown"); + manip_info.tcp_offset = "unknown"; + EXPECT_ANY_THROW(env->findTCPOffset(manip_info)); // NOLINT + } +} + +TEST(TesseractEnvironmentUnit, getActiveLinkNamesRecursiveUnit) // NOLINT +{ + // Get the environment + auto env = getEnvironment(); + + std::vector active_links; + getActiveLinkNamesRecursive(active_links, *env->getSceneGraph(), env->getRootLinkName(), false); + std::vector target_active_links = env->getActiveLinkNames(); + EXPECT_TRUE(tesseract_common::isIdentical(active_links, target_active_links, false)); +} + +TEST(TesseractEnvironmentUnit, checkTrajectoryUnit) // NOLINT +{ + // Get the environment + auto env = getEnvironment(); + + // Add sphere to environment + Link link_sphere("sphere_attached"); + + Visual::Ptr visual = std::make_shared(); + visual->origin = Eigen::Isometry3d::Identity(); + visual->origin.translation() = Eigen::Vector3d(0.5, 0, 0.55); + visual->geometry = std::make_shared(0.15); + link_sphere.visual.push_back(visual); + + Collision::Ptr collision = std::make_shared(); + collision->origin = visual->origin; + collision->geometry = visual->geometry; + link_sphere.collision.push_back(collision); + + Joint joint_sphere("joint_sphere_attached"); + joint_sphere.parent_link_name = "base_link"; + joint_sphere.child_link_name = link_sphere.getName(); + joint_sphere.type = JointType::FIXED; + + auto cmd = std::make_shared(link_sphere, joint_sphere); + + EXPECT_TRUE(env->applyCommand(cmd)); + + // Set the robot initial state + std::vector joint_names; + joint_names.emplace_back("joint_a1"); + joint_names.emplace_back("joint_a2"); + joint_names.emplace_back("joint_a3"); + joint_names.emplace_back("joint_a4"); + joint_names.emplace_back("joint_a5"); + joint_names.emplace_back("joint_a6"); + joint_names.emplace_back("joint_a7"); + + Eigen::VectorXd joint_start_pos(7); + joint_start_pos(0) = -0.4; + joint_start_pos(1) = 0.2762; + joint_start_pos(2) = 0.0; + joint_start_pos(3) = -1.3348; + joint_start_pos(4) = 0.0; + joint_start_pos(5) = 1.4959; + joint_start_pos(6) = 0.0; + + Eigen::VectorXd joint_end_pos(7); + joint_end_pos(0) = 0.4; + joint_end_pos(1) = 0.2762; + joint_end_pos(2) = 0.0; + joint_end_pos(3) = -1.3348; + joint_end_pos(4) = 0.0; + joint_end_pos(5) = 1.4959; + joint_end_pos(6) = 0.0; + + tesseract_common::TrajArray traj(5, joint_start_pos.size()); + + for (int i = 0; i < joint_start_pos.size(); ++i) + traj.col(i) = Eigen::VectorXd::LinSpaced(5, joint_start_pos(i), joint_end_pos(i)); + + auto discrete_manager = env->getDiscreteContactManager(); + auto continuous_manager = env->getContinuousContactManager(); + auto state_solver = env->getStateSolver(); + auto joint_group = env->getJointGroup("manipulator"); + + { + tesseract_collision::CollisionCheckConfig config; + config.type = CollisionEvaluatorType::DISCRETE; + std::vector contacts; + EXPECT_TRUE( + tesseract_environment::checkTrajectory(contacts, *discrete_manager, *state_solver, joint_names, traj, config)); + EXPECT_EQ(contacts.size(), 3); + EXPECT_EQ(contacts[0].size(), 2); + EXPECT_EQ(contacts[1].size(), 2); + EXPECT_EQ(contacts[2].size(), 2); + } + + { + tesseract_collision::CollisionCheckConfig config; + config.type = CollisionEvaluatorType::DISCRETE; + config.contact_request.type = tesseract_collision::ContactTestType::FIRST; + std::vector contacts; + EXPECT_TRUE( + tesseract_environment::checkTrajectory(contacts, *discrete_manager, *state_solver, joint_names, traj, config)); + EXPECT_EQ(contacts.size(), 1); + EXPECT_EQ(contacts[0].size(), 1); + } + + { + tesseract_collision::CollisionCheckConfig config; + config.type = CollisionEvaluatorType::DISCRETE; + std::vector contacts; + EXPECT_FALSE(tesseract_environment::checkTrajectory( + contacts, *discrete_manager, *state_solver, joint_names, joint_start_pos.transpose(), config)); + } + + { + tesseract_collision::CollisionCheckConfig config; + config.type = CollisionEvaluatorType::LVS_DISCRETE; + config.longest_valid_segment_length = std::numeric_limits::max(); + std::vector contacts; + EXPECT_TRUE( + tesseract_environment::checkTrajectory(contacts, *discrete_manager, *state_solver, joint_names, traj, config)); + EXPECT_EQ(contacts.size(), 3); + EXPECT_EQ(contacts[0].size(), 2); + EXPECT_EQ(contacts[1].size(), 2); + EXPECT_EQ(contacts[2].size(), 2); + } + + { + tesseract_collision::CollisionCheckConfig config; + config.type = CollisionEvaluatorType::LVS_DISCRETE; + config.contact_request.type = tesseract_collision::ContactTestType::FIRST; + config.longest_valid_segment_length = std::numeric_limits::max(); + std::vector contacts; + EXPECT_TRUE( + tesseract_environment::checkTrajectory(contacts, *discrete_manager, *state_solver, joint_names, traj, config)); + EXPECT_EQ(contacts.size(), 1); + EXPECT_EQ(contacts[0].size(), 1); + } + + { + tesseract_collision::CollisionCheckConfig config; + config.type = CollisionEvaluatorType::LVS_DISCRETE; + std::vector contacts; + EXPECT_TRUE( + tesseract_environment::checkTrajectory(contacts, *discrete_manager, *state_solver, joint_names, traj, config)); + EXPECT_EQ(contacts.size(), 134); + } + + { + tesseract_collision::CollisionCheckConfig config; + config.type = CollisionEvaluatorType::DISCRETE; + std::vector contacts; + EXPECT_TRUE(tesseract_environment::checkTrajectory(contacts, *discrete_manager, *joint_group, traj, config)); + EXPECT_EQ(contacts.size(), 3); + EXPECT_EQ(contacts[0].size(), 2); + EXPECT_EQ(contacts[1].size(), 2); + EXPECT_EQ(contacts[2].size(), 2); + } + + { + tesseract_collision::CollisionCheckConfig config; + config.type = CollisionEvaluatorType::DISCRETE; + config.contact_request.type = tesseract_collision::ContactTestType::FIRST; + std::vector contacts; + EXPECT_TRUE(tesseract_environment::checkTrajectory(contacts, *discrete_manager, *joint_group, traj, config)); + EXPECT_EQ(contacts.size(), 1); + EXPECT_EQ(contacts[0].size(), 1); + } + + { + tesseract_collision::CollisionCheckConfig config; + config.type = CollisionEvaluatorType::DISCRETE; + std::vector contacts; + EXPECT_FALSE(tesseract_environment::checkTrajectory( + contacts, *discrete_manager, *joint_group, joint_start_pos.transpose(), config)); + } + + { + tesseract_collision::CollisionCheckConfig config; + config.type = CollisionEvaluatorType::LVS_DISCRETE; + config.longest_valid_segment_length = std::numeric_limits::max(); + std::vector contacts; + EXPECT_TRUE(tesseract_environment::checkTrajectory(contacts, *discrete_manager, *joint_group, traj, config)); + EXPECT_EQ(contacts.size(), 3); + EXPECT_EQ(contacts[0].size(), 2); + EXPECT_EQ(contacts[1].size(), 2); + EXPECT_EQ(contacts[2].size(), 2); + } + + { + tesseract_collision::CollisionCheckConfig config; + config.type = CollisionEvaluatorType::LVS_DISCRETE; + config.contact_request.type = tesseract_collision::ContactTestType::FIRST; + config.longest_valid_segment_length = std::numeric_limits::max(); + std::vector contacts; + EXPECT_TRUE(tesseract_environment::checkTrajectory(contacts, *discrete_manager, *joint_group, traj, config)); + EXPECT_EQ(contacts.size(), 1); + EXPECT_EQ(contacts[0].size(), 1); + } + + { + tesseract_collision::CollisionCheckConfig config; + config.type = CollisionEvaluatorType::LVS_DISCRETE; + std::vector contacts; + EXPECT_TRUE(tesseract_environment::checkTrajectory(contacts, *discrete_manager, *joint_group, traj, config)); + EXPECT_EQ(contacts.size(), 134); + } + + { + tesseract_collision::CollisionCheckConfig config; + config.type = CollisionEvaluatorType::CONTINUOUS; + std::vector contacts; + EXPECT_TRUE(tesseract_environment::checkTrajectory( + contacts, *continuous_manager, *state_solver, joint_names, traj, config)); + EXPECT_EQ(contacts.size(), 4); + EXPECT_EQ(contacts[0].size(), 2); + EXPECT_EQ(contacts[1].size(), 2); + EXPECT_EQ(contacts[2].size(), 3); + EXPECT_EQ(contacts[3].size(), 2); + } + + { + tesseract_collision::CollisionCheckConfig config; + config.type = CollisionEvaluatorType::CONTINUOUS; + config.contact_request.type = tesseract_collision::ContactTestType::FIRST; + std::vector contacts; + EXPECT_TRUE(tesseract_environment::checkTrajectory( + contacts, *continuous_manager, *state_solver, joint_names, traj, config)); + EXPECT_EQ(contacts.size(), 1); + EXPECT_EQ(contacts[0].size(), 1); + } + + { + tesseract_collision::CollisionCheckConfig config; + config.type = CollisionEvaluatorType::LVS_CONTINUOUS; + config.longest_valid_segment_length = std::numeric_limits::max(); + std::vector contacts; + EXPECT_TRUE(tesseract_environment::checkTrajectory( + contacts, *continuous_manager, *state_solver, joint_names, traj, config)); + EXPECT_EQ(contacts.size(), 4); + EXPECT_EQ(contacts[0].size(), 2); + EXPECT_EQ(contacts[1].size(), 2); + EXPECT_EQ(contacts[2].size(), 3); + EXPECT_EQ(contacts[3].size(), 2); + } + + { + tesseract_collision::CollisionCheckConfig config; + config.type = CollisionEvaluatorType::LVS_CONTINUOUS; + config.contact_request.type = tesseract_collision::ContactTestType::FIRST; + config.longest_valid_segment_length = std::numeric_limits::max(); + std::vector contacts; + EXPECT_TRUE(tesseract_environment::checkTrajectory( + contacts, *continuous_manager, *state_solver, joint_names, traj, config)); + EXPECT_EQ(contacts.size(), 1); + EXPECT_EQ(contacts[0].size(), 1); + } + + { + tesseract_collision::CollisionCheckConfig config; + config.type = CollisionEvaluatorType::LVS_CONTINUOUS; + std::vector contacts; + EXPECT_TRUE(tesseract_environment::checkTrajectory( + contacts, *continuous_manager, *state_solver, joint_names, traj, config)); + EXPECT_EQ(contacts.size(), 135); + } + + { + tesseract_collision::CollisionCheckConfig config; + config.type = CollisionEvaluatorType::CONTINUOUS; + std::vector contacts; + EXPECT_TRUE(tesseract_environment::checkTrajectory(contacts, *continuous_manager, *joint_group, traj, config)); + EXPECT_EQ(contacts.size(), 4); + EXPECT_EQ(contacts[0].size(), 2); + EXPECT_EQ(contacts[1].size(), 2); + EXPECT_EQ(contacts[2].size(), 3); + EXPECT_EQ(contacts[3].size(), 2); + } + + { + tesseract_collision::CollisionCheckConfig config; + config.type = CollisionEvaluatorType::CONTINUOUS; + config.contact_request.type = tesseract_collision::ContactTestType::FIRST; + std::vector contacts; + EXPECT_TRUE(tesseract_environment::checkTrajectory(contacts, *continuous_manager, *joint_group, traj, config)); + EXPECT_EQ(contacts.size(), 1); + EXPECT_EQ(contacts[0].size(), 1); + } + + { + tesseract_collision::CollisionCheckConfig config; + config.type = CollisionEvaluatorType::LVS_CONTINUOUS; + config.longest_valid_segment_length = std::numeric_limits::max(); + std::vector contacts; + EXPECT_TRUE(tesseract_environment::checkTrajectory(contacts, *continuous_manager, *joint_group, traj, config)); + EXPECT_EQ(contacts.size(), 4); + EXPECT_EQ(contacts[0].size(), 2); + EXPECT_EQ(contacts[1].size(), 2); + EXPECT_EQ(contacts[2].size(), 3); + EXPECT_EQ(contacts[3].size(), 2); + } + + { + tesseract_collision::CollisionCheckConfig config; + config.type = CollisionEvaluatorType::LVS_CONTINUOUS; + config.contact_request.type = tesseract_collision::ContactTestType::FIRST; + config.longest_valid_segment_length = std::numeric_limits::max(); + std::vector contacts; + EXPECT_TRUE(tesseract_environment::checkTrajectory(contacts, *continuous_manager, *joint_group, traj, config)); + EXPECT_EQ(contacts.size(), 1); + EXPECT_EQ(contacts[0].size(), 1); + } + + { + tesseract_collision::CollisionCheckConfig config; + config.type = CollisionEvaluatorType::LVS_CONTINUOUS; + std::vector contacts; + EXPECT_TRUE(tesseract_environment::checkTrajectory(contacts, *continuous_manager, *joint_group, traj, config)); + EXPECT_EQ(contacts.size(), 135); + } + + // Failures + { + tesseract_collision::CollisionCheckConfig config; + config.type = CollisionEvaluatorType::CONTINUOUS; + std::vector contacts; + // NOLINTNEXTLINE + EXPECT_ANY_THROW( + tesseract_environment::checkTrajectory(contacts, *discrete_manager, *state_solver, joint_names, traj, config)); + } + { + tesseract_collision::CollisionCheckConfig config; + config.type = CollisionEvaluatorType::CONTINUOUS; + std::vector contacts; + // NOLINTNEXTLINE + EXPECT_ANY_THROW(tesseract_environment::checkTrajectory(contacts, *discrete_manager, *joint_group, traj, config)); + } + { + tesseract_collision::CollisionCheckConfig config; + config.type = CollisionEvaluatorType::DISCRETE; + std::vector contacts; + // NOLINTNEXTLINE + EXPECT_ANY_THROW(tesseract_environment::checkTrajectory( + contacts, *discrete_manager, *state_solver, joint_names, tesseract_common::TrajArray(), config)); + } + { + tesseract_collision::CollisionCheckConfig config; + config.type = CollisionEvaluatorType::DISCRETE; + std::vector contacts; + // NOLINTNEXTLINE + EXPECT_ANY_THROW(tesseract_environment::checkTrajectory( + contacts, *discrete_manager, *joint_group, tesseract_common::TrajArray(), config)); + } + { + tesseract_collision::CollisionCheckConfig config; + config.type = CollisionEvaluatorType::DISCRETE; + std::vector contacts; + // NOLINTNEXTLINE + EXPECT_ANY_THROW(tesseract_environment::checkTrajectory( + contacts, *continuous_manager, *state_solver, joint_names, traj, config)); + } + { + tesseract_collision::CollisionCheckConfig config; + config.type = CollisionEvaluatorType::DISCRETE; + std::vector contacts; + // NOLINTNEXTLINE + EXPECT_ANY_THROW(tesseract_environment::checkTrajectory(contacts, *continuous_manager, *joint_group, traj, config)); + } + { + tesseract_collision::CollisionCheckConfig config; + config.type = CollisionEvaluatorType::CONTINUOUS; + std::vector contacts; + // NOLINTNEXTLINE + EXPECT_ANY_THROW(tesseract_environment::checkTrajectory( + contacts, *continuous_manager, *state_solver, joint_names, tesseract_common::TrajArray(), config)); + } + { + tesseract_collision::CollisionCheckConfig config; + config.type = CollisionEvaluatorType::CONTINUOUS; + std::vector contacts; + // NOLINTNEXTLINE + EXPECT_ANY_THROW(tesseract_environment::checkTrajectory( + contacts, *continuous_manager, *joint_group, tesseract_common::TrajArray(), config)); + } +} int main(int argc, char** argv) { diff --git a/tesseract_geometry/include/tesseract_geometry/impl/mesh.h b/tesseract_geometry/include/tesseract_geometry/impl/mesh.h index 9cf5666ecb8..e55902121ec 100644 --- a/tesseract_geometry/include/tesseract_geometry/impl/mesh.h +++ b/tesseract_geometry/include/tesseract_geometry/impl/mesh.h @@ -88,9 +88,7 @@ class Mesh : public PolygonMesh GeometryType::MESH) { if ((getFaceCount() * 4) != getFaces()->size()) - { - std::throw_with_nested(std::runtime_error("Mesh is not triangular")); - } + std::throw_with_nested(std::runtime_error("Mesh is not triangular")); // LCOV_EXCL_LINE } /** @@ -129,9 +127,7 @@ class Mesh : public PolygonMesh GeometryType::MESH) { if ((getFaceCount() * 4) != getFaces()->size()) - { - std::throw_with_nested(std::runtime_error("Mesh is not triangular")); - } + std::throw_with_nested(std::runtime_error("Mesh is not triangular")); // LCOV_EXCL_LINE } ~Mesh() override = default; diff --git a/tesseract_geometry/include/tesseract_geometry/impl/sdf_mesh.h b/tesseract_geometry/include/tesseract_geometry/impl/sdf_mesh.h index 22c2d4b15b2..2f55d966a04 100644 --- a/tesseract_geometry/include/tesseract_geometry/impl/sdf_mesh.h +++ b/tesseract_geometry/include/tesseract_geometry/impl/sdf_mesh.h @@ -86,9 +86,7 @@ class SDFMesh : public PolygonMesh GeometryType::SDF_MESH) { if ((getFaceCount() * 4) != getFaces()->size()) - { - std::throw_with_nested(std::runtime_error("Mesh is not triangular")); - } + std::throw_with_nested(std::runtime_error("Mesh is not triangular")); // LCOV_EXCL_LINE } /** @@ -127,9 +125,7 @@ class SDFMesh : public PolygonMesh GeometryType::SDF_MESH) { if ((getFaceCount() * 4) != getFaces()->size()) - { - std::throw_with_nested(std::runtime_error("Mesh is not triangular")); - } + std::throw_with_nested(std::runtime_error("Mesh is not triangular")); // LCOV_EXCL_LINE } ~SDFMesh() override = default; diff --git a/tesseract_geometry/test/tesseract_geometry_unit.cpp b/tesseract_geometry/test/tesseract_geometry_unit.cpp index ba718974be1..8fa61c74396 100644 --- a/tesseract_geometry/test/tesseract_geometry_unit.cpp +++ b/tesseract_geometry/test/tesseract_geometry_unit.cpp @@ -121,6 +121,36 @@ TEST(TesseractGeometryUnit, Plane) // NOLINT EXPECT_NEAR(std::static_pointer_cast(geom_clone)->getD(), 1, 1e-5); } +TEST(TesseractGeometryUnit, PolygonMesh) // NOLINT +{ + auto vertices = std::make_shared(); + vertices->emplace_back(1, 1, 0); + vertices->emplace_back(1, -1, 0); + vertices->emplace_back(-1, -1, 0); + vertices->emplace_back(1, -1, 0); + + auto faces = std::make_shared(); + faces->resize(5); + (*faces)(0) = 4; + (*faces)(1) = 0; + (*faces)(2) = 1; + (*faces)(3) = 2; + (*faces)(4) = 3; + + using T = tesseract_geometry::PolygonMesh; + auto geom = std::make_shared(vertices, faces); + EXPECT_TRUE(geom->getVertices() != nullptr); + EXPECT_TRUE(geom->getFaces() != nullptr); + EXPECT_TRUE(geom->getVertexCount() == 4); + EXPECT_TRUE(geom->getFaceCount() == 1); + + auto geom_clone = geom->clone(); + EXPECT_TRUE(std::static_pointer_cast(geom_clone)->getVertices() != nullptr); + EXPECT_TRUE(std::static_pointer_cast(geom_clone)->getFaces() != nullptr); + EXPECT_TRUE(std::static_pointer_cast(geom_clone)->getVertexCount() == 4); + EXPECT_TRUE(std::static_pointer_cast(geom_clone)->getFaceCount() == 1); +} + TEST(TesseractGeometryUnit, ConvexMesh) // NOLINT { auto vertices = std::make_shared(); diff --git a/tesseract_kinematics/core/include/tesseract_kinematics/core/joint_group.h b/tesseract_kinematics/core/include/tesseract_kinematics/core/joint_group.h index df0ebbbfaf1..89206b803ba 100644 --- a/tesseract_kinematics/core/include/tesseract_kinematics/core/joint_group.h +++ b/tesseract_kinematics/core/include/tesseract_kinematics/core/joint_group.h @@ -204,6 +204,7 @@ class JointGroup tesseract_scene_graph::SceneState state_; tesseract_scene_graph::StateSolver::UPtr state_solver_; std::vector joint_names_; + std::vector link_names_; std::vector static_link_names_; tesseract_common::TransformMap static_link_transforms_; tesseract_common::KinematicLimits limits_; diff --git a/tesseract_kinematics/core/src/joint_group.cpp b/tesseract_kinematics/core/src/joint_group.cpp index 39404b01211..7c40d2c49aa 100644 --- a/tesseract_kinematics/core/src/joint_group.cpp +++ b/tesseract_kinematics/core/src/joint_group.cpp @@ -59,6 +59,7 @@ JointGroup::JointGroup(std::string name, std::vector active_link_names = state_solver_->getActiveLinkNames(); for (const auto& link : scene_graph.getLinks()) { + link_names_.push_back(link->getName()); auto it = std::find(active_link_names.begin(), active_link_names.end(), link->getName()); if (it == active_link_names.end()) { @@ -99,6 +100,7 @@ JointGroup& JointGroup::operator=(const JointGroup& other) state_ = other.state_; state_solver_ = other.state_solver_->clone(); joint_names_ = other.joint_names_; + link_names_ = other.link_names_; static_link_names_ = other.static_link_names_; static_link_transforms_ = other.static_link_transforms_; limits_ = other.limits_; @@ -250,18 +252,21 @@ bool JointGroup::checkJoints(const Eigen::Ref& vec) const std::vector JointGroup::getJointNames() const { return joint_names_; } -std::vector JointGroup::getLinkNames() const { return state_solver_->getLinkNames(); } +std::vector JointGroup::getLinkNames() const { return link_names_; } std::vector JointGroup::getActiveLinkNames() const { return state_solver_->getActiveLinkNames(); } -std::vector JointGroup::getStaticLinkNames() const { return state_solver_->getStaticLinkNames(); } +std::vector JointGroup::getStaticLinkNames() const { return static_link_names_; } bool JointGroup::isActiveLinkName(const std::string& link_name) const { return state_solver_->isActiveLinkName(link_name); } -bool JointGroup::hasLinkName(const std::string& link_name) const { return state_solver_->hasLinkName(link_name); } +bool JointGroup::hasLinkName(const std::string& link_name) const +{ + return (std::find(link_names_.begin(), link_names_.end(), link_name) != link_names_.end()); +} tesseract_common::KinematicLimits JointGroup::getLimits() const { return limits_; } diff --git a/tesseract_kinematics/test/kinematics_test_utils.h b/tesseract_kinematics/test/kinematics_test_utils.h index fdbd3f05952..d7ec6bb2168 100644 --- a/tesseract_kinematics/test/kinematics_test_utils.h +++ b/tesseract_kinematics/test/kinematics_test_utils.h @@ -641,15 +641,35 @@ inline void runActiveLinkNamesIIWATest(const tesseract_kinematics::KinematicGrou std::vector target_active_link_names = { "link_1", "link_2", "link_3", "link_4", "link_5", "link_6", "link_7", "tool0" }; + std::vector target_static_link_names = { "base_link", "base" }; + std::vector target_link_names = target_active_link_names; - target_link_names.emplace_back("base_link"); - target_link_names.emplace_back("base"); + target_link_names.insert(target_link_names.end(), target_static_link_names.begin(), target_static_link_names.end()); + + for (const auto& l : target_active_link_names) + { + EXPECT_TRUE(kin_group.isActiveLinkName(l)); + } + + for (const auto& l : target_link_names) + { + EXPECT_TRUE(kin_group.hasLinkName(l)); + } - std::vector link_names = kin_group.getActiveLinkNames(); - runStringVectorEqualTest(link_names, target_active_link_names); + { + std::vector link_names = kin_group.getActiveLinkNames(); + runStringVectorEqualTest(link_names, target_active_link_names); + } - link_names = kin_group.getLinkNames(); - runStringVectorEqualTest(link_names, target_link_names); + { + std::vector link_names = kin_group.getStaticLinkNames(); + runStringVectorEqualTest(link_names, target_static_link_names); + } + + { + std::vector link_names = kin_group.getLinkNames(); + runStringVectorEqualTest(link_names, target_link_names); + } } inline void runActiveLinkNamesABBTest(const tesseract_kinematics::KinematicGroup& kin_group) @@ -661,14 +681,36 @@ inline void runActiveLinkNamesABBTest(const tesseract_kinematics::KinematicGroup std::vector target_active_link_names = { "link_1", "link_2", "link_3", "link_4", "link_5", "link_6", "tool0" }; + + std::vector target_static_link_names = { "base_link" }; + std::vector target_link_names = target_active_link_names; - target_link_names.emplace_back("base_link"); + target_link_names.insert(target_link_names.end(), target_static_link_names.begin(), target_static_link_names.end()); - std::vector link_names = kin_group.getActiveLinkNames(); - runStringVectorEqualTest(link_names, target_active_link_names); + for (const auto& l : target_active_link_names) + { + EXPECT_TRUE(kin_group.isActiveLinkName(l)); + } - link_names = kin_group.getLinkNames(); - runStringVectorEqualTest(link_names, target_link_names); + for (const auto& l : target_link_names) + { + EXPECT_TRUE(kin_group.hasLinkName(l)); + } + + { + std::vector link_names = kin_group.getActiveLinkNames(); + runStringVectorEqualTest(link_names, target_active_link_names); + } + + { + std::vector link_names = kin_group.getStaticLinkNames(); + runStringVectorEqualTest(link_names, target_static_link_names); + } + + { + std::vector link_names = kin_group.getLinkNames(); + runStringVectorEqualTest(link_names, target_link_names); + } } inline void runActiveLinkNamesURTest(const tesseract_kinematics::KinematicGroup& kin_group) @@ -681,14 +723,36 @@ inline void runActiveLinkNamesURTest(const tesseract_kinematics::KinematicGroup& std::vector target_active_link_names = { "shoulder_link", "upper_arm_link", "forearm_link", "wrist_1_link", "wrist_2_link", "wrist_3_link", "tool0" }; + + std::vector target_static_link_names = { "base_link" }; + std::vector target_link_names = target_active_link_names; - target_link_names.emplace_back("base_link"); + target_link_names.insert(target_link_names.end(), target_static_link_names.begin(), target_static_link_names.end()); + + for (const auto& l : target_active_link_names) + { + EXPECT_TRUE(kin_group.isActiveLinkName(l)); + } + + for (const auto& l : target_link_names) + { + EXPECT_TRUE(kin_group.hasLinkName(l)); + } - std::vector link_names = kin_group.getActiveLinkNames(); - runStringVectorEqualTest(link_names, target_active_link_names); + { + std::vector link_names = kin_group.getActiveLinkNames(); + runStringVectorEqualTest(link_names, target_active_link_names); + } + + { + std::vector link_names = kin_group.getStaticLinkNames(); + runStringVectorEqualTest(link_names, target_static_link_names); + } - link_names = kin_group.getLinkNames(); - runStringVectorEqualTest(link_names, target_link_names); + { + std::vector link_names = kin_group.getLinkNames(); + runStringVectorEqualTest(link_names, target_link_names); + } } inline void runKinGroupJacobianABBOnPositionerTest(tesseract_kinematics::KinematicGroup& kin_group) @@ -747,14 +811,36 @@ inline void runActiveLinkNamesABBOnPositionerTest(const tesseract_kinematics::Ki std::vector target_active_link_names = { "positioner_tool0", "base_link", "base", "link_1", "link_2", "link_3", "link_4", "link_5", "link_6", "tool0" }; + + std::vector target_static_link_names = { "positioner_base_link" }; + std::vector target_link_names = target_active_link_names; - target_link_names.emplace_back("positioner_base_link"); + target_link_names.insert(target_link_names.end(), target_static_link_names.begin(), target_static_link_names.end()); - std::vector link_names = kin_group.getActiveLinkNames(); - runStringVectorEqualTest(link_names, target_active_link_names); + for (const auto& l : target_active_link_names) + { + EXPECT_TRUE(kin_group.isActiveLinkName(l)); + } - link_names = kin_group.getLinkNames(); - runStringVectorEqualTest(link_names, target_link_names); + for (const auto& l : target_link_names) + { + EXPECT_TRUE(kin_group.hasLinkName(l)); + } + + { + std::vector link_names = kin_group.getActiveLinkNames(); + runStringVectorEqualTest(link_names, target_active_link_names); + } + + { + std::vector link_names = kin_group.getStaticLinkNames(); + runStringVectorEqualTest(link_names, target_static_link_names); + } + + { + std::vector link_names = kin_group.getLinkNames(); + runStringVectorEqualTest(link_names, target_link_names); + } } inline void runKinGroupJacobianABBExternalPositionerTest(tesseract_kinematics::KinematicGroup& kin_group) @@ -815,17 +901,36 @@ inline void runActiveLinkNamesABBExternalPositionerTest(const tesseract_kinemati std::vector target_active_link_names = { "positioner_tool0", "positioner_link_1", "link_1", "link_2", "link_3", "link_4", "link_5", "link_6", "tool0" }; + + std::vector target_static_link_names = { "world", "base_link", "base", "positioner_base_link" }; + std::vector target_link_names = target_active_link_names; - target_link_names.emplace_back("world"); - target_link_names.emplace_back("base_link"); - target_link_names.emplace_back("base"); - target_link_names.emplace_back("positioner_base_link"); + target_link_names.insert(target_link_names.end(), target_static_link_names.begin(), target_static_link_names.end()); + + for (const auto& l : target_active_link_names) + { + EXPECT_TRUE(kin_group.isActiveLinkName(l)); + } + + for (const auto& l : target_link_names) + { + EXPECT_TRUE(kin_group.hasLinkName(l)); + } - std::vector link_names = kin_group.getActiveLinkNames(); - runStringVectorEqualTest(link_names, target_active_link_names); + { + std::vector link_names = kin_group.getActiveLinkNames(); + runStringVectorEqualTest(link_names, target_active_link_names); + } - link_names = kin_group.getLinkNames(); - runStringVectorEqualTest(link_names, target_link_names); + { + std::vector link_names = kin_group.getStaticLinkNames(); + runStringVectorEqualTest(link_names, target_static_link_names); + } + + { + std::vector link_names = kin_group.getLinkNames(); + runStringVectorEqualTest(link_names, target_link_names); + } } inline void runInvKinIIWATest(const tesseract_kinematics::KinematicsPluginFactory& factory, diff --git a/tesseract_scene_graph/src/kdl_parser.cpp b/tesseract_scene_graph/src/kdl_parser.cpp index 798678a86c5..0b19b844ee4 100644 --- a/tesseract_scene_graph/src/kdl_parser.cpp +++ b/tesseract_scene_graph/src/kdl_parser.cpp @@ -264,8 +264,12 @@ struct kdl_sub_tree_builder : public boost::dfs_visitor<> std::size_t num_v = boost::num_vertices(graph); std::size_t num_e = boost::num_edges(graph); data_.link_names.reserve(num_v); + data_.static_link_names.reserve(num_v); data_.active_link_names.reserve(num_v); data_.active_joint_names.reserve(num_e); + + data_.link_names.push_back(link->getName()); + data_.static_link_names.push_back(link->getName()); return; } @@ -298,10 +302,10 @@ struct kdl_sub_tree_builder : public boost::dfs_visitor<> segment_transforms_[parent_link_name], KDL::RigidBodyInertia(0)); data_.tree.addSegment(world_sgm, data_.base_link_name); - data_.link_names.push_back(data_.base_link_name); } link_names_.push_back(parent_link_name); link_names_.push_back(link->getName()); + data_.static_link_names.push_back(parent_link_name); data_.link_names.push_back(parent_link_name); data_.link_names.push_back(link->getName()); data_.active_link_names.push_back(link->getName()); @@ -324,6 +328,7 @@ struct kdl_sub_tree_builder : public boost::dfs_visitor<> { data_.link_names.push_back(parent_link_name); link_names_.push_back(parent_link_name); + data_.static_link_names.push_back(parent_link_name); KDL::Frame new_tree_parent_to_joint = segment_transforms_[data_.base_link_name].Inverse() * segment_transforms_[parent_joint->parent_link_name]; @@ -354,6 +359,8 @@ struct kdl_sub_tree_builder : public boost::dfs_visitor<> auto active_it = std::find(data_.active_link_names.begin(), data_.active_link_names.end(), parent_link_name); if (active_it != data_.active_link_names.end() || kdl_jnt.getType() != KDL::Joint::None) data_.active_link_names.push_back(link->getName()); + else + data_.static_link_names.push_back(link->getName()); if (kdl_jnt.getType() != KDL::Joint::None) data_.active_joint_names.push_back(parent_joint->getName()); diff --git a/tesseract_scene_graph/test/tesseract_scene_graph_unit.cpp b/tesseract_scene_graph/test/tesseract_scene_graph_unit.cpp index ae082c41dd5..9f3b7845e29 100644 --- a/tesseract_scene_graph/test/tesseract_scene_graph_unit.cpp +++ b/tesseract_scene_graph/test/tesseract_scene_graph_unit.cpp @@ -1052,11 +1052,13 @@ TEST(TesseractSceneGraphUnit, LoadKDLUnit) // NOLINT std::vector joint_names{ "joint_1", "joint_2", "joint_3", "joint_4" }; std::vector link_names{ "base_link", "link_1", "link_2", "link_3", "link_4", "link_5" }; + std::vector static_link_names{ "base_link", "link_1" }; std::vector active_link_names{ "link_2", "link_3", "link_4", "link_5" }; { KDLTreeData data = parseSceneGraph(g); EXPECT_TRUE(tesseract_common::isIdentical(data.link_names, link_names, false)); + EXPECT_TRUE(tesseract_common::isIdentical(data.static_link_names, static_link_names, false)); EXPECT_TRUE(tesseract_common::isIdentical(data.active_joint_names, joint_names, false)); EXPECT_TRUE(tesseract_common::isIdentical(data.active_link_names, active_link_names, false)); @@ -1086,6 +1088,7 @@ TEST(TesseractSceneGraphUnit, LoadKDLUnit) // NOLINT KDLTreeData data = parseSceneGraph(*g_clone); EXPECT_TRUE(tesseract_common::isIdentical(data.link_names, link_names, false)); + EXPECT_TRUE(tesseract_common::isIdentical(data.static_link_names, static_link_names, false)); EXPECT_TRUE(tesseract_common::isIdentical(data.active_joint_names, joint_names, false)); EXPECT_TRUE(tesseract_common::isIdentical(data.active_link_names, active_link_names, false)); @@ -1113,6 +1116,9 @@ TEST(TesseractSceneGraphUnit, LoadSubKDLUnit) // NOLINT "right_joint_2", "right_joint_3", "right_joint_4" }; std::vector link_names{ "world", "left_link_2", "left_link_3", "left_link_4", "left_link_5", "right_link_2", "right_link_3", "right_link_4", "right_link_5" }; + + std::vector static_link_names{ "world", "left_link_2", "right_link_2" }; + std::vector active_link_names{ "left_link_3", "left_link_4", "left_link_5", "right_link_3", "right_link_4", "right_link_5" }; @@ -1129,6 +1135,7 @@ TEST(TesseractSceneGraphUnit, LoadSubKDLUnit) // NOLINT KDLTreeData data = parseSceneGraph(g, sub_joint_names, joint_values); EXPECT_TRUE(tesseract_common::isIdentical(data.link_names, link_names, false)); + EXPECT_TRUE(tesseract_common::isIdentical(data.static_link_names, static_link_names, false)); EXPECT_TRUE(tesseract_common::isIdentical(data.active_joint_names, sub_joint_names, false)); EXPECT_TRUE(tesseract_common::isIdentical(data.active_link_names, active_link_names, false)); @@ -1159,6 +1166,7 @@ TEST(TesseractSceneGraphUnit, LoadSubKDLUnit) // NOLINT KDLTreeData data = parseSceneGraph(*g_clone, sub_joint_names, joint_values); EXPECT_TRUE(tesseract_common::isIdentical(data.link_names, link_names, false)); + EXPECT_TRUE(tesseract_common::isIdentical(data.static_link_names, static_link_names, false)); EXPECT_TRUE(tesseract_common::isIdentical(data.active_joint_names, sub_joint_names, false)); EXPECT_TRUE(tesseract_common::isIdentical(data.active_link_names, active_link_names, false)); diff --git a/tesseract_support/urdf/lbr_iiwa_14_r820.srdf b/tesseract_support/urdf/lbr_iiwa_14_r820.srdf index f5b63e90126..08dc33f4cb6 100644 --- a/tesseract_support/urdf/lbr_iiwa_14_r820.srdf +++ b/tesseract_support/urdf/lbr_iiwa_14_r820.srdf @@ -12,6 +12,15 @@ + + + + + + + + + diff --git a/tesseract_support/urdf/lbr_iiwa_14_r820_plugins.yaml b/tesseract_support/urdf/lbr_iiwa_14_r820_plugins.yaml index 43b2914bed6..1e93c86bfd3 100644 --- a/tesseract_support/urdf/lbr_iiwa_14_r820_plugins.yaml +++ b/tesseract_support/urdf/lbr_iiwa_14_r820_plugins.yaml @@ -1,6 +1,17 @@ kinematic_plugins: + search_paths: + - /usr/local/lib search_libraries: - tesseract_kinematics_kdl_factories + fwd_kin_plugins: + manipulator: + default: KDLFwdKinChain + plugins: + KDLFwdKinChain: + class: KDLFwdKinChainFactory + config: + base_link: base_link + tip_link: tool0 inv_kin_plugins: manipulator: default: KDLInvKinChainLMA