Add TrajOpt Ifopt planner (#443)

.github/workflows/windows_noetic_build.yml

@@ -37,6 +37,6 @@ jobs:
 
         rosdep install -q --from-paths . --ignore-src -y
 
-        catkin_make_isolated --install --force-cmake --only-pkg-with-deps tesseract_collision tesseract_common tesseract_environment tesseract_geometry tesseract_kinematics tesseract_scene_graph tesseract_support tesseract_urdf tesseract_visualization tesseract_motion_planners tesseract_process_managers tesseract_time_parameterization tesseract_python --cmake-args -DCMAKE_BUILD_TYPE=Release -DINSTALL_OMPL=ON -DINSTALL_OMPL_TAG=master
+        catkin_make_isolated --install --force-cmake --only-pkg-with-deps tesseract_collision tesseract_common tesseract_environment tesseract_geometry tesseract_kinematics tesseract_scene_graph tesseract_support tesseract_urdf tesseract_visualization tesseract_motion_planners tesseract_process_managers tesseract_time_parameterization tesseract_python --cmake-args -DCMAKE_BUILD_TYPE=Release -DINSTALL_OMPL=ON -DINSTALL_OMPL_TAG=master -DBUILD_IPOPT=OFF -DBUILD_SNOPT=OFF
         call "D:\a\tesseract\tesseract\install_isolated\setup.bat"
-        catkin_make_isolated --install --force-cmake --pkg tesseract_collision tesseract_common tesseract_environment tesseract_geometry tesseract_kinematics tesseract_scene_graph tesseract_support tesseract_urdf tesseract_visualization tesseract_motion_planners tesseract_process_managers tesseract_time_parameterization --cmake-args -DCMAKE_BUILD_TYPE=Release -DTESSERACT_ENABLE_TESTING=ON -DINSTALL_OMPL=ON -DINSTALL_OMPL_TAG=master
+        catkin_make_isolated --install --force-cmake --pkg tesseract_collision tesseract_common tesseract_environment tesseract_geometry tesseract_kinematics tesseract_scene_graph tesseract_support tesseract_urdf tesseract_visualization tesseract_motion_planners tesseract_process_managers tesseract_time_parameterization --cmake-args -DCMAKE_BUILD_TYPE=Release -DTESSERACT_ENABLE_TESTING=ON -DINSTALL_OMPL=ON -DINSTALL_OMPL_TAG=master -DBUILD_IPOPT=OFF -DBUILD_SNOPT=OFF

tesseract/tesseract_collision/include/tesseract_collision/core/types.h

@@ -384,6 +384,7 @@ struct ContactTestData
  */
 enum class CollisionEvaluatorType
 {
+  NONE,
   DISCRETE,
   LVS_DISCRETE,
   CONTINUOUS,

tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/cartesian_waypoint.h

@@ -264,6 +264,24 @@ class CartesianWaypoint
 
   /** @brief The Cartesian Waypoint */
   Eigen::Isometry3d waypoint;
+  /** @brief Distance below waypoint that is allowed. Should be size = 6. First 3 elements are dx, dy, dz. The last 3
+   * elements are angle axis error allowed (Eigen::AngleAxisd.axis() * Eigen::AngleAxisd.angle()) */
+  Eigen::VectorXd lower_tolerance;
+  /** @brief Distance above waypoint that is allowed. Should be size = 6. First 3 elements are dx, dy, dz. The last 3
+   * elements are angle axis error allowed (Eigen::AngleAxisd.axis() * Eigen::AngleAxisd.angle())*/
+  Eigen::VectorXd upper_tolerance;
+
+  bool isToleranced() const
+  {
+    // Check if they are empty
+    if (lower_tolerance.size() == 0 || upper_tolerance.size() == 0)
+      return false;
+
+    // Check if they are close to 0
+    Eigen::VectorXd range = upper_tolerance - lower_tolerance;
+    double sum = range.sum();
+    return (sum < 1e-5);
+  }
 };
 
 }  // namespace tesseract_planning

tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/joint_waypoint.h

@@ -265,13 +265,29 @@ class JointWaypoint
   inline operator Eigen::Ref<Eigen::VectorXd>() { return Eigen::Ref<Eigen::VectorXd>(waypoint); }
 
   //////////////////////////////////
-  // Cartesian Waypoint Container //
+  // Joint Waypoint Container //
   //////////////////////////////////
 
 #endif  // SWIG
 
   Eigen::VectorXd waypoint;
   std::vector<std::string> joint_names;
+  /** @brief Joint distance below waypoint that is allowed. Each element should be <= 0 */
+  Eigen::VectorXd lower_tolerance;
+  /** @brief Joint distance above waypoint that is allowed. Each element should be >= 0 */
+  Eigen::VectorXd upper_tolerance;
+
+  bool isToleranced() const
+  {
+    // Check if they are empty
+    if (lower_tolerance.size() == 0 || upper_tolerance.size() == 0)
+      return false;
+
+    // Check if they are close to 0
+    Eigen::VectorXd range = upper_tolerance - lower_tolerance;
+    double sum = range.sum();
+    return (sum < 1e-5);
+  }
 };
 }  // namespace tesseract_planning
 

tesseract/tesseract_planning/tesseract_motion_planners/CMakeLists.txt

@@ -18,7 +18,10 @@ find_package(tesseract_environment REQUIRED)
 find_package(tesseract_common REQUIRED)
 find_package(tesseract_command_language REQUIRED)
 find_package(trajopt REQUIRED)
+find_package(trajopt_ifopt REQUIRED)
 find_package(trajopt_sco REQUIRED)
+find_package(trajopt_sqp REQUIRED)
+find_package(cmake_common_scripts REQUIRED)
 # serialization was required because ompl does not include find_dependency for its required dependencies
 find_package(Boost REQUIRED COMPONENTS serialization system filesystem program_options)
 
@@ -166,12 +169,35 @@ target_include_directories(${PROJECT_NAME}_trajopt SYSTEM PUBLIC
     ${EIGEN3_INCLUDE_DIRS}
     ${Boost_INCLUDE_DIRS})
 
+# Trajopt_ifopt Planner
+add_library(${PROJECT_NAME}_trajopt_ifopt SHARED
+  src/trajopt_ifopt/trajopt_ifopt_motion_planner.cpp
+  src/trajopt_ifopt/trajopt_ifopt_utils.cpp
+  src/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.cpp
+  src/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.cpp
+  src/trajopt_ifopt/problem_generators/default_problem_generator.cpp)
+target_link_libraries(${PROJECT_NAME}_trajopt_ifopt PUBLIC ${Boost_LIBRARIES} ${PROJECT_NAME}_core trajopt::trajopt_ifopt trajopt::trajopt_sqp)
+target_compile_options(${PROJECT_NAME}_trajopt_ifopt PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE})
+target_compile_options(${PROJECT_NAME}_trajopt_ifopt PUBLIC ${TESSERACT_COMPILE_OPTIONS_PUBLIC})
+target_compile_definitions(${PROJECT_NAME}_trajopt_ifopt PUBLIC ${TESSERACT_COMPILE_DEFINITIONS})
+target_clang_tidy(${PROJECT_NAME}_trajopt_ifopt ARGUMENTS ${TESSERACT_CLANG_TIDY_ARGS} ENABLE ${TESSERACT_ENABLE_CLANG_TIDY})
+target_cxx_version(${PROJECT_NAME}_trajopt_ifopt PUBLIC VERSION ${TESSERACT_CXX_VERSION})
+target_code_coverage(${PROJECT_NAME}_trajopt_ifopt ALL EXCLUDE ${COVERAGE_EXCLUDE} ENABLE ${TESSERACT_ENABLE_TESTING})
+target_include_directories(${PROJECT_NAME}_trajopt_ifopt PUBLIC
+    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+    "$<INSTALL_INTERFACE:include>")
+target_include_directories(${PROJECT_NAME}_trajopt_ifopt SYSTEM PUBLIC
+    ${EIGEN3_INCLUDE_DIRS}
+    ${Boost_INCLUDE_DIRS})
+
 configure_package(NAMESPACE tesseract
                   TARGETS ${PROJECT_NAME}_core
                           ${PROJECT_NAME}_descartes
                           ${PROJECT_NAME}_ompl
                           ${PROJECT_NAME}_simple
-                          ${PROJECT_NAME}_trajopt)
+                          ${PROJECT_NAME}_trajopt
+                          ${PROJECT_NAME}_trajopt_ifopt
+                  )
 
 # Mark cpp header files for installation
 install(DIRECTORY include/${PROJECT_NAME}

tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h

@@ -33,6 +33,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <Eigen/Geometry>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
+#include <tesseract_command_language/cartesian_waypoint.h>
+#include <tesseract_command_language/joint_waypoint.h>
 #include <tesseract_motion_planners/trajopt/profile/trajopt_profile.h>
 
 #ifdef SWIG
@@ -81,14 +83,14 @@ class TrajOptDefaultPlanProfile : public TrajOptPlanProfile
       constraint_error_functions;
 
   void apply(trajopt::ProblemConstructionInfo& pci,
-             const Eigen::Isometry3d& cartesian_waypoint,
+             const CartesianWaypoint& cartesian_waypoint,
              const Instruction& parent_instruction,
              const ManipulatorInfo& manip_info,
              const std::vector<std::string>& active_links,
              int index) const override;
 
   void apply(trajopt::ProblemConstructionInfo& pci,
-             const Eigen::VectorXd& joint_waypoint,
+             const JointWaypoint& joint_waypoint,
              const Instruction& parent_instruction,
              const ManipulatorInfo& manip_info,
              const std::vector<std::string>& active_links,

tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h

@@ -34,6 +34,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_command_language/core/instruction.h>
+#include <tesseract_command_language/joint_waypoint.h>
+#include <tesseract_command_language/cartesian_waypoint.h>
 #include <tesseract_command_language/types.h>
 
 #ifdef SWIG
@@ -58,14 +60,14 @@ class TrajOptPlanProfile
   TrajOptPlanProfile& operator=(TrajOptPlanProfile&&) = default;
 
   virtual void apply(trajopt::ProblemConstructionInfo& pci,
-                     const Eigen::Isometry3d& cartesian_waypoint,
+                     const CartesianWaypoint& cartesian_waypoint,
                      const Instruction& parent_instruction,
                      const ManipulatorInfo& manip_info,
                      const std::vector<std::string>& active_links,
                      int index) const = 0;
 
   virtual void apply(trajopt::ProblemConstructionInfo& pci,
-                     const Eigen::VectorXd& joint_waypoint,
+                     const JointWaypoint& joint_waypoint,
                      const Instruction& parent_instruction,
                      const ManipulatorInfo& manip_info,
                      const std::vector<std::string>& active_links,

tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/trajopt_utils.h

@@ -63,6 +63,13 @@ trajopt::TermInfo::Ptr createJointWaypointTermInfo(const Eigen::VectorXd& j_wp,
                                                    const Eigen::VectorXd& coeffs,
                                                    trajopt::TermType type);
 
+trajopt::TermInfo::Ptr createTolerancedJointWaypointTermInfo(const Eigen::VectorXd& j_wp,
+                                                             const Eigen::VectorXd& lower_tol,
+                                                             const Eigen::VectorXd& upper_tol,
+                                                             int index,
+                                                             const Eigen::VectorXd& coeffs,
+                                                             trajopt::TermType type);
+
 trajopt::TermInfo::Ptr createCollisionTermInfo(
     int start_index,
     int end_index,

tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/problem_generators/default_problem_generator.h

@@ -0,0 +1,46 @@
+/**
+ * @file default_problem_generator.h
+ * @brief Generates a trajopt problem from a planner request
+ *
+ * @author Levi Armstrong
+ * @date April 18, 2018
+ * @version TODO
+ * @bug No known bugs
+ *
+ * @copyright Copyright (c) 2020, Southwest Research Institute
+ *
+ * @par License
+ * Software License Agreement (Apache License)
+ * @par
+ * 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
+ * @par
+ * 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.
+ */
+#ifndef TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_PROBLEM_GENERATOR_H
+#define TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_PROBLEM_GENERATOR_H
+
+#include <tesseract_common/macros.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <ifopt/problem.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+#include <tesseract_motion_planners/core/types.h>
+#include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h>
+
+namespace tesseract_planning
+{
+std::shared_ptr<TrajOptIfoptProblem>
+DefaultTrajoptProblemGenerator(const std::string& name,
+                               const PlannerRequest& request,
+                               const TrajOptIfoptPlanProfileMap& plan_profiles,
+                               const TrajOptIfoptCompositeProfileMap& composite_profiles);
+
+}  // namespace tesseract_planning
+#endif

tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h

@@ -0,0 +1,98 @@
+/**
+ * @file trajopt_default_composite_profile.h
+ * @brief
+ *
+ * @author Levi Armstrong
+ * @date June 18, 2020
+ * @version TODO
+ * @bug No known bugs
+ *
+ * @copyright Copyright (c) 2020, Southwest Research Institute
+ *
+ * @par License
+ * Software License Agreement (Apache License)
+ * @par
+ * 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
+ * @par
+ * 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.
+ */
+
+#ifndef TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_COMPOSITE_PROFILE_H
+#define TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_COMPOSITE_PROFILE_H
+
+#include <tesseract_common/macros.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <vector>
+#include <Eigen/Geometry>
+#include <Eigen/Core>
+#include <trajopt_ifopt/constraints/collision_evaluators.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+#include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h>
+
+namespace tesseract_planning
+{
+class TrajOptIfoptDefaultCompositeProfile : public TrajOptIfoptCompositeProfile
+{
+public:
+  TrajOptIfoptDefaultCompositeProfile() = default;
+  TrajOptIfoptDefaultCompositeProfile(const tinyxml2::XMLElement& xml_element);
+
+  /** @brief Configuration info for collisions that are modeled as costs */
+  trajopt::TrajOptCollisionConfig::Ptr collision_cost_config;
+  /** @brief Configuration info for collisions that are modeled as constraints */
+  trajopt::TrajOptCollisionConfig::Ptr collision_constraint_config;
+  /** @brief If true, a joint velocity cost with a target of 0 will be applied for all timesteps Default: true*/
+  bool smooth_velocities = true;
+  /** @brief This default to all ones, but allows you to weight different joints */
+  Eigen::VectorXd velocity_coeff;
+  /** @brief If true, a joint acceleration cost with a target of 0 will be applied for all timesteps Default: false*/
+  bool smooth_accelerations = true;
+  /** @brief This default to all ones, but allows you to weight different joints */
+  Eigen::VectorXd acceleration_coeff;
+  /** @brief If true, a joint jerk cost with a target of 0 will be applied for all timesteps Default: false*/
+  bool smooth_jerks = true;
+  /** @brief This default to all ones, but allows you to weight different joints */
+  Eigen::VectorXd jerk_coeff;
+
+  /** @brief Set the resolution at which state validity needs to be verified in order for a motion between two states
+   * to be considered valid in post checking of trajectory returned by TrajOptIfopt.
+   *
+   * The resolution is equal to longest_valid_segment_fraction * state_space.getMaximumExtent()
+   *
+   * Note: The planner takes the conservative of either longest_valid_segment_fraction or longest_valid_segment_length.
+   */
+  double longest_valid_segment_fraction = 0.01;  // 1%
+
+  /** @brief Set the resolution at which state validity needs to be verified in order for a motion between two states
+   * to be considered valid. If norm(state1 - state0) > longest_valid_segment_length.
+   *
+   * Note: This gets converted to longest_valid_segment_fraction.
+   *       longest_valid_segment_fraction = longest_valid_segment_length / state_space.getMaximumExtent()
+   */
+  double longest_valid_segment_length = 0.5;
+
+  /** @brief Special link collision cost distances */
+  trajopt::SafetyMarginData::Ptr special_collision_cost{ nullptr };
+  /** @brief Special link collision constraint distances */
+  trajopt::SafetyMarginData::Ptr special_collision_constraint{ nullptr };
+
+  void apply(TrajOptIfoptProblem& problem,
+             int start_index,
+             int end_index,
+             const ManipulatorInfo& manip_info,
+             const std::vector<std::string>& active_links,
+             const std::vector<int>& fixed_indices) const override;
+
+  tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override;
+};
+}  // namespace tesseract_planning
+
+#endif  // TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_COMPOSITE_PROFILE_H