Merge pull request #388 from tiffanyec/motion_plan_exercise

new motion planning with tesseract exercise

exercises/8.0/solution/snp_motion_planning/CMakeLists.txt

@@ -0,0 +1,55 @@
+cmake_minimum_required(VERSION 3.5)
+project(snp_motion_planning)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+set(ROS2_DEPS
+    rclcpp
+    snp_msgs
+    std_srvs
+    tesseract_monitoring)
+
+find_package(ament_cmake REQUIRED)
+foreach(dep ${ROS2_DEPS})
+  find_package(${dep} REQUIRED)
+endforeach()
+
+find_package(descartes_light REQUIRED)
+find_package(tesseract_command_language REQUIRED)
+find_package(tesseract_task_composer REQUIRED)
+find_package(tesseract_kinematics REQUIRED)
+find_package(LAPACK REQUIRED) # Requried for ikfast
+
+# Planning server
+add_executable(${PROJECT_NAME}_node src/planning_server.cpp)
+ament_target_dependencies(${PROJECT_NAME}_node ${ROS2_DEPS})
+target_link_libraries(${PROJECT_NAME}_node tesseract::tesseract_command_language tesseract::tesseract_task_composer)
+
+# Plugin Library
+add_library(${PROJECT_NAME}_plugins SHARED src/motoman_hc10_ikfast_plugin.cpp)
+target_link_libraries(${PROJECT_NAME}_plugins PUBLIC ${LAPACK_LIBRARIES} tesseract::tesseract_common
+                                                     tesseract::tesseract_kinematics_ikfast)
+target_include_directories(${PROJECT_NAME}_plugins SYSTEM PUBLIC ${LAPACK_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS})
+
+# Install the executable(s)
+install(TARGETS ${PROJECT_NAME}_node DESTINATION lib/${PROJECT_NAME})
+
+# Install the library(ies)
+install(TARGETS ${PROJECT_NAME}_plugins EXPORT ${PROJECT_NAME}-targets DESTINATION lib)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_export_dependencies(${ROS2_DEPS})
+ament_export_targets(${PROJECT_NAME}-targets HAS_LIBRARY_TARGET)
+ament_package()
+
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/)

exercises/8.0/solution/snp_motion_planning/launch/planning_server.launch.xml

@@ -0,0 +1,15 @@
+<?xml version="1.0" ?>
+<launch>
+  <arg name="robot_description"/>
+  <arg name="robot_description_semantic"/>
+  <arg name="verbose" default="False"/>
+  <arg name="touch_links" default="[]"/>
+
+  <node pkg="snp_motion_planning" exec="snp_motion_planning_node" output="screen">
+    <param name="robot_description" value="$(var robot_description)"/>
+    <param name="robot_description_semantic" value="$(var robot_description_semantic)"/>
+    <param name="verbose" value="$(var verbose)"/>
+    <param name="touch_links" value="$(var touch_links)"/>
+  </node>
+
+</launch>

exercises/8.0/solution/snp_motion_planning/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>snp_motion_planning</name>
+  <version>0.1.0</version>
+  <description>SNP Motion Planning</description>
+  <maintainer email="michael.ripperger@swri.org">Michael Ripperger</maintainer>
+  <license>Apache 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <depend>rclcpp</depend>
+  <depend>snp_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>tesseract_command_language</depend>
+  <depend>tesseract_monitoring</depend>
+  <depend>tesseract_support</depend>
+  <depend>tesseract_urdf</depend>
+  <depend>tesseract_kinematics</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

exercises/8.0/solution/snp_motion_planning/src/motoman_hc10_ikfast_plugin.cpp

@@ -0,0 +1,122 @@
+/*
+ * Software License Agreement (Apache License)
+ *
+ * Copyright (c) 2016, Southwest Research Institute
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <tesseract_common/macros.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <boost/core/demangle.hpp>
+#include <Eigen/Geometry>
+#include <tesseract_kinematics/ikfast/impl/ikfast_inv_kin.hpp>
+#include <tesseract_kinematics/core/kinematics_plugin_factory.h>
+#include "motoman_hc10_ikfast_solver.hpp"
+#include <vector>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+namespace tesseract_kinematics
+{
+template <typename T>
+T get(const YAML::Node& node, const std::string& key)
+{
+  try
+  {
+    return node[key].as<T>();
+  }
+  catch (const YAML::Exception&)
+  {
+    std::stringstream ss;
+    ss << "Failed to extract '" << key << "' from configuration with type '" << boost::core::demangle(typeid(T).name());
+    throw std::runtime_error(ss.str());
+  }
+}
+
+class IKFastInvKinFactory : public InvKinFactory
+{
+public:
+  virtual InverseKinematics::UPtr create(const std::string& solver_name,
+                                         const tesseract_scene_graph::SceneGraph& scene_graph,
+                                         const tesseract_scene_graph::SceneState& /*scene_state*/,
+                                         const KinematicsPluginFactory& /*plugin_factory*/,
+                                         const YAML::Node& config) const
+  {
+    auto base_link = get<std::string>(config, "base_link");
+    auto tip_link = get<std::string>(config, "tip_link");
+    auto n_joints = get<std::size_t>(config, "n_joints");
+
+    // Get the free joint states
+    std::map<std::size_t, std::vector<double>> free_joint_states_map;
+    try
+    {
+      const YAML::Node& free_joint_states_node = config["free_joint_states"];
+      for (auto it = free_joint_states_node.begin(); it != free_joint_states_node.end(); ++it)
+      {
+        free_joint_states_map[it->first.as<std::size_t>()] = it->second.as<std::vector<double>>();
+      }
+    }
+    catch (const std::exception&)
+    {
+      CONSOLE_BRIDGE_logDebug("No free joint states specified for IKFast plugin");
+    }
+
+    // Get the active joints in between the base link and tip link
+    tesseract_scene_graph::ShortestPath path = scene_graph.getShortestPath(base_link, tip_link);
+
+    // Check the joints specification
+    if (path.active_joints.size() != n_joints + free_joint_states_map.size())
+    {
+      std::stringstream ss;
+      ss << "Number of active joints (" << path.active_joints.size()
+         << ") must equal the sum of the number of nominal joints (" << n_joints << ") and the number of free joints ("
+         << free_joint_states_map.size() << ")";
+      throw std::runtime_error(ss.str());
+    }
+
+    // Get the redundancy capable joint indices
+    std::vector<Eigen::Index> redundancy_capable_joint_indices;
+    try
+    {
+      redundancy_capable_joint_indices = config["redundancy_capable_joint_indices"].as<std::vector<Eigen::Index>>();
+    }
+    catch (const std::exception&)
+    {
+      CONSOLE_BRIDGE_logDebug("Using default redundancy capable joints for IKFast plugin");
+
+      for (std::size_t i = 0; i < path.active_joints.size(); ++i)
+      {
+        if (free_joint_states_map.find(i) == free_joint_states_map.end())
+        {
+          const auto& joint = scene_graph.getJoint(path.active_joints.at(i));
+          if (joint->limits->upper - joint->limits->lower > 2 * M_PI)
+          {
+            redundancy_capable_joint_indices.push_back(static_cast<Eigen::Index>(i));
+          }
+        }
+      }
+    }
+
+    std::vector<std::vector<double>> free_joint_states;
+    free_joint_states.reserve(free_joint_states_map.size());
+    std::transform(free_joint_states_map.begin(), free_joint_states_map.end(), std::back_inserter(free_joint_states),
+                   [](const std::pair<const std::size_t, std::vector<double>>& pair) { return pair.second; });
+
+    return std::make_unique<IKFastInvKin>(base_link, tip_link, path.active_joints, redundancy_capable_joint_indices,
+                                          solver_name, free_joint_states);
+  }
+};
+
+}  // namespace tesseract_kinematics
+
+TESSERACT_ADD_PLUGIN(tesseract_kinematics::IKFastInvKinFactory, MotomanHC10InvKinFactory)