Add new TrajOpt Profiles

tesseract-robotics · Dec 25, 2024 · 59ea53c · 59ea53c
1 parent ad440a9
commit 59ea53c
Show file tree

Hide file tree

Showing 30 changed files with 665 additions and 2,032 deletions.
diff --git a/tesseract_examples/src/car_seat_example.cpp b/tesseract_examples/src/car_seat_example.cpp
@@ -66,7 +66,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_task_composer/core/task_composer_plugin_factory.h>
 
 #include <tesseract_motion_planners/trajopt/profile/trajopt_default_composite_profile.h>
-#include <tesseract_motion_planners/trajopt/profile/trajopt_default_solver_profile.h>
+#include <tesseract_motion_planners/trajopt/profile/trajopt_osqp_solver_profile.h>
 #include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h>
 #include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h>
 #include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_solver_profile.h>
@@ -327,10 +327,10 @@ bool CarSeatExample::run()
     trajopt_composite_profile->collision_cost_config.safety_margin_buffer = 0.01;
     trajopt_composite_profile->collision_cost_config.coeff = 50;
 
-    auto trajopt_solver_profile = std::make_shared<TrajOptDefaultSolverProfile>();
-    trajopt_solver_profile->opt_info.max_iter = 200;
-    trajopt_solver_profile->opt_info.min_approx_improve = 1e-3;
-    trajopt_solver_profile->opt_info.min_trust_box_size = 1e-3;
+    auto trajopt_solver_profile = std::make_shared<TrajOptOSQPSolverProfile>();
+    trajopt_solver_profile->opt_params.max_iter = 200;
+    trajopt_solver_profile->opt_params.min_approx_improve = 1e-3;
+    trajopt_solver_profile->opt_params.min_trust_box_size = 1e-3;
 
     profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_composite_profile);
     profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_solver_profile);

diff --git a/tesseract_examples/src/pick_and_place_example.cpp b/tesseract_examples/src/pick_and_place_example.cpp
@@ -44,7 +44,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h>
 #include <tesseract_motion_planners/trajopt/profile/trajopt_default_composite_profile.h>
-#include <tesseract_motion_planners/trajopt/profile/trajopt_default_solver_profile.h>
+#include <tesseract_motion_planners/trajopt/profile/trajopt_osqp_solver_profile.h>
 #include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h>
 #include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h>
 #include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_solver_profile.h>
@@ -280,8 +280,8 @@ bool PickAndPlaceExample::run()
     trajopt_composite_profile->collision_cost_config.safety_margin_buffer = 0.01;
     trajopt_composite_profile->collision_cost_config.coeff = 50;
 
-    auto trajopt_solver_profile = std::make_shared<TrajOptDefaultSolverProfile>();
-    trajopt_solver_profile->opt_info.max_iter = 100;
+    auto trajopt_solver_profile = std::make_shared<TrajOptOSQPSolverProfile>();
+    trajopt_solver_profile->opt_params.max_iter = 100;
 
     profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_plan_profile);
     profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile);

diff --git a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp
@@ -53,7 +53,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h>
 #include <tesseract_motion_planners/trajopt/profile/trajopt_default_composite_profile.h>
-#include <tesseract_motion_planners/trajopt/profile/trajopt_default_solver_profile.h>
+#include <tesseract_motion_planners/trajopt/profile/trajopt_osqp_solver_profile.h>
 #include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h>
 #include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h>
 #include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_solver_profile.h>
@@ -279,14 +279,11 @@ bool PuzzlePieceAuxillaryAxesExample::run()
     trajopt_composite_profile->collision_cost_config.type = trajopt::CollisionEvaluatorType::SINGLE_TIMESTEP;
     trajopt_composite_profile->collision_cost_config.coeff = 1;
 
-    auto trajopt_solver_profile = std::make_shared<TrajOptDefaultSolverProfile>();
-    trajopt_solver_profile->convex_solver = sco::ModelType::OSQP;
-    auto convex_solver_config = std::make_shared<sco::OSQPModelConfig>();
-    convex_solver_config->settings.adaptive_rho = 0;
-    trajopt_solver_profile->convex_solver_config = convex_solver_config;
-    trajopt_solver_profile->opt_info.max_iter = 200;
-    trajopt_solver_profile->opt_info.min_approx_improve = 1e-3;
-    trajopt_solver_profile->opt_info.min_trust_box_size = 1e-3;
+    auto trajopt_solver_profile = std::make_shared<TrajOptOSQPSolverProfile>();
+    trajopt_solver_profile->settings.adaptive_rho = 0;
+    trajopt_solver_profile->opt_params.max_iter = 200;
+    trajopt_solver_profile->opt_params.min_approx_improve = 1e-3;
+    trajopt_solver_profile->opt_params.min_trust_box_size = 1e-3;
 
     profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_plan_profile);
     profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile);

diff --git a/tesseract_examples/src/puzzle_piece_example.cpp b/tesseract_examples/src/puzzle_piece_example.cpp
@@ -53,7 +53,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h>
 #include <tesseract_motion_planners/trajopt/profile/trajopt_default_composite_profile.h>
-#include <tesseract_motion_planners/trajopt/profile/trajopt_default_solver_profile.h>
+#include <tesseract_motion_planners/trajopt/profile/trajopt_osqp_solver_profile.h>
 #include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h>
 #include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h>
 #include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_solver_profile.h>
@@ -267,12 +267,11 @@ bool PuzzlePieceExample::run()
     trajopt_composite_profile->collision_cost_config.type = trajopt::CollisionEvaluatorType::SINGLE_TIMESTEP;
     trajopt_composite_profile->collision_cost_config.coeff = 20;
 
-    auto trajopt_solver_profile = std::make_shared<TrajOptDefaultSolverProfile>();
-    trajopt_solver_profile->convex_solver = sco::ModelType::OSQP;
-    trajopt_solver_profile->opt_info.num_threads = 0;
-    trajopt_solver_profile->opt_info.max_iter = 200;
-    trajopt_solver_profile->opt_info.min_approx_improve = 1e-3;
-    trajopt_solver_profile->opt_info.min_trust_box_size = 1e-3;
+    auto trajopt_solver_profile = std::make_shared<TrajOptOSQPSolverProfile>();
+    trajopt_solver_profile->opt_params.num_threads = 0;
+    trajopt_solver_profile->opt_params.max_iter = 200;
+    trajopt_solver_profile->opt_params.min_approx_improve = 1e-3;
+    trajopt_solver_profile->opt_params.min_trust_box_size = 1e-3;
 
     profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_plan_profile);
     profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile);

diff --git a/tesseract_motion_planners/CMakeLists.txt b/tesseract_motion_planners/CMakeLists.txt
@@ -15,6 +15,7 @@ find_package(Boost REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(Threads REQUIRED)
 find_package(tesseract_common REQUIRED)
+find_package(tesseract_kinematics REQUIRED)
 find_package(tesseract_environment REQUIRED)
 find_package(tesseract_command_language REQUIRED)
 

diff --git a/tesseract_motion_planners/core/src/utils.cpp b/tesseract_motion_planners/core/src/utils.cpp
@@ -253,14 +253,12 @@ bool formatProgramHelper(CompositeInstruction& composite_instructions,
       auto& base_instruction = i.as<MoveInstructionPoly>();
       tesseract_common::ManipulatorInfo mi = manip_info.getCombined(base_instruction.getManipulatorInfo());
 
-      tesseract_common::ManipulatorInfo combined_mi = mi.getCombined(base_instruction.getManipulatorInfo());
-
       std::vector<std::string> joint_names;
-      auto it = manip_joint_names.find(combined_mi.manipulator);
+      auto it = manip_joint_names.find(mi.manipulator);
       if (it == manip_joint_names.end())
       {
-        joint_names = env.getGroupJointNames(combined_mi.manipulator);
-        manip_joint_names[combined_mi.manipulator] = joint_names;
+        joint_names = env.getGroupJointNames(mi.manipulator);
+        manip_joint_names[mi.manipulator] = joint_names;
       }
       else
       {

diff --git a/tesseract_motion_planners/trajopt/CMakeLists.txt b/tesseract_motion_planners/trajopt/CMakeLists.txt
@@ -1,5 +1,6 @@
 find_package(trajopt REQUIRED)
 find_package(trajopt_sco REQUIRED)
+find_package(osqp REQUIRED)
 
 # Trajopt Planner
 add_library(
@@ -11,18 +12,19 @@ add_library(
   src/profile/trajopt_profile.cpp
   src/profile/trajopt_default_plan_profile.cpp
   src/profile/trajopt_default_composite_profile.cpp
-  src/profile/trajopt_default_solver_profile.cpp
-  src/serialize.cpp
-  src/deserialize.cpp)
+  src/profile/trajopt_osqp_solver_profile.cpp)
 target_link_libraries(
   ${PROJECT_NAME}_trajopt
   PUBLIC ${PROJECT_NAME}_core
          ${PROJECT_NAME}_simple
          trajopt::trajopt
          trajopt::trajopt_common
          trajopt::trajopt_sco
+         tesseract::tesseract_kinematics_core
+         tesseract::tesseract_environment
          Boost::boost
-         Eigen3::Eigen)
+         Eigen3::Eigen
+         osqp::osqp)
 target_compile_options(${PROJECT_NAME}_trajopt PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE})
 target_compile_options(${PROJECT_NAME}_trajopt PUBLIC ${TESSERACT_COMPILE_OPTIONS_PUBLIC})
 target_compile_definitions(${PROJECT_NAME}_trajopt PUBLIC ${TESSERACT_COMPILE_DEFINITIONS})
@@ -50,7 +52,11 @@ configure_component(
   COMPONENT trajopt
   NAMESPACE tesseract
   TARGETS ${PROJECT_NAME}_trajopt
-  DEPENDENCIES "tesseract_motion_planners COMPONENTS core simple" trajopt trajopt_sco)
+  DEPENDENCIES
+    "tesseract_motion_planners COMPONENTS core simple"
+    trajopt
+    trajopt_sco
+    osqp)
 
 if(TESSERACT_PACKAGE)
   cpack_component(

diff --git a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/deserialize.h b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/deserialize.h
diff --git a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/fwd.h b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/fwd.h
@@ -19,7 +19,7 @@ class TrajOptPlanProfile;
 class TrajOptCompositeProfile;
 class TrajOptSolverProfile;
 
-class TrajOptDefaultSolverProfile;
+class TrajOptOSQPSolverProfile;
 class TrajOptDefaultPlanProfile;
 class TrajOptDefaultCompositeProfile;
 }  // namespace tesseract_planning

diff --git a/...opt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_composite_profile.h b/...opt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_composite_profile.h
@@ -51,7 +51,6 @@ class TrajOptDefaultCompositeProfile : public TrajOptCompositeProfile
   using ConstPtr = std::shared_ptr<const TrajOptDefaultCompositeProfile>;
 
   TrajOptDefaultCompositeProfile() = default;
-  TrajOptDefaultCompositeProfile(const tinyxml2::XMLElement& xml_element);
 
   /** @brief The type of contact test to perform: FIRST, CLOSEST, ALL */
   tesseract_collision::ContactTestType contact_test_type{ tesseract_collision::ContactTestType::ALL };
@@ -98,56 +97,14 @@ class TrajOptDefaultCompositeProfile : public TrajOptCompositeProfile
   /**@brief Special link collision constraint distances */
   std::shared_ptr<trajopt_common::SafetyMarginData> special_collision_constraint{ nullptr };
 
-  void apply(trajopt::ProblemConstructionInfo& pci,
-             int start_index,
-             int end_index,
-             const tesseract_common::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;
+  TrajOptTermInfos create(const tesseract_common::ManipulatorInfo& composite_manip_info,
+                          const std::shared_ptr<const tesseract_environment::Environment>& env,
+                          const std::vector<int>& fixed_indices,
+                          int start_index,
+                          int end_index) const override;
 
 protected:
-  void addCollisionCost(trajopt::ProblemConstructionInfo& pci,
-                        int start_index,
-                        int end_index,
-                        const std::vector<int>& fixed_indices) const;
-
-  void addCollisionConstraint(trajopt::ProblemConstructionInfo& pci,
-                              int start_index,
-                              int end_index,
-                              const std::vector<int>& fixed_indices) const;
-
-  void addVelocitySmoothing(trajopt::ProblemConstructionInfo& pci,
-                            int start_index,
-                            int end_index,
-                            const std::vector<int>& fixed_indices) const;
-
-  void addAccelerationSmoothing(trajopt::ProblemConstructionInfo& pci,
-                                int start_index,
-                                int end_index,
-                                const std::vector<int>& fixed_indices) const;
-
-  void addJerkSmoothing(trajopt::ProblemConstructionInfo& pci,
-                        int start_index,
-                        int end_index,
-                        const std::vector<int>& fixed_indices) const;
-
-  void addConstraintErrorFunctions(trajopt::ProblemConstructionInfo& pci,
-                                   int start_index,
-                                   int end_index,
-                                   const std::vector<int>& fixed_indices) const;
-
-  void addAvoidSingularity(trajopt::ProblemConstructionInfo& pci,
-                           int start_index,
-                           int end_index,
-                           const std::string& link,
-                           const std::vector<int>& fixed_indices) const;
-
-  static void smoothMotionTerms(const tinyxml2::XMLElement& xml_element,
-                                bool& enabled,
-                                Eigen::VectorXd& coeff,
-                                std::size_t& length);
+  double computeLongestValidSegmentLength(const Eigen::MatrixX2d& joint_limits) const;
 
   friend class boost::serialization::access;
   template <class Archive>

diff --git a/.../trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h b/.../trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h
@@ -45,14 +45,21 @@ class TrajOptDefaultPlanProfile : public TrajOptPlanProfile
   using ConstPtr = std::shared_ptr<const TrajOptDefaultPlanProfile>;
 
   TrajOptDefaultPlanProfile() = default;
-  TrajOptDefaultPlanProfile(const tinyxml2::XMLElement& xml_element);
 
   CartesianWaypointConfig cartesian_cost_config;
   CartesianWaypointConfig cartesian_constraint_config;
   JointWaypointConfig joint_cost_config;
   JointWaypointConfig joint_constraint_config;
 
-  /** @brief Error function that is set as a constraint for each timestep.
+  TrajOptWaypointInfo create(const MoveInstructionPoly& move_instruction,
+                             const tesseract_common::ManipulatorInfo& composite_manip_info,
+                             const std::shared_ptr<const tesseract_environment::Environment>& env,
+                             const std::vector<std::string>& active_links,
+                             int index) const override;
+
+protected:
+  /**
+   * @brief Error function that is set as a constraint for each timestep.
    *
    * This is a vector of std::tuple<Error Function, Error Function Jacobian, Constraint Type, Coeff>, the error
    * function, constraint type, and coeff is required, but the jacobian is optional (nullptr).
@@ -70,29 +77,12 @@ class TrajOptDefaultPlanProfile : public TrajOptPlanProfile
    * Coefficients/Weights
    *
    */
-  std::vector<std::tuple<sco::VectorOfVector::func, sco::MatrixOfVector::func, sco::ConstraintType, Eigen::VectorXd>>
-      constraint_error_functions;
-
-  void apply(trajopt::ProblemConstructionInfo& pci,
-             const CartesianWaypointPoly& cartesian_waypoint,
-             const MoveInstructionPoly& parent_instruction,
-             const tesseract_common::ManipulatorInfo& manip_info,
-             const std::vector<std::string>& active_links,
-             int index) const override;
-
-  void apply(trajopt::ProblemConstructionInfo& pci,
-             const JointWaypointPoly& joint_waypoint,
-             const MoveInstructionPoly& parent_instruction,
-             const tesseract_common::ManipulatorInfo& manip_info,
-             const std::vector<std::string>& active_links,
-             int index) const override;
-
-  tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override;
-
-protected:
-  void addConstraintErrorFunctions(trajopt::ProblemConstructionInfo& pci, int index) const;
-
-  void addAvoidSingularity(trajopt::ProblemConstructionInfo& pci, const std::vector<int>& fixed_steps) const;
+  static std::shared_ptr<trajopt::TermInfo>
+  createConstraintFromErrorFunction(sco::VectorOfVector::func error_function,
+                                    sco::MatrixOfVector::func jacobian_function,
+                                    sco::ConstraintType type,
+                                    const Eigen::VectorXd& coeff,
+                                    int index);
 
   friend class boost::serialization::access;
   template <class Archive>