Add toleranced waypoints to TrajOpt Solver (#403)

tesseract-robotics · Mar 8, 2024 · 47da448 · 47da448
1 parent 230020e
commit 47da448
Show file tree

Hide file tree

Showing 19 changed files with 690 additions and 148 deletions.
diff --git a/tesseract_command_language/src/cartesian_waypoint.cpp b/tesseract_command_language/src/cartesian_waypoint.cpp
@@ -8,12 +8,16 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_common/utils.h>
 #include <tesseract_command_language/cartesian_waypoint.h>
 
-tesseract_planning::CartesianWaypoint::CartesianWaypoint(const Eigen::Isometry3d& transform) : transform_(transform) {}
+tesseract_planning::CartesianWaypoint::CartesianWaypoint(
+    const Eigen::Isometry3d& transform)  // NOLINT(modernize-pass-by-value)
+  : transform_(transform)
+{
+}
 
 tesseract_planning::CartesianWaypoint::CartesianWaypoint(
-    const Eigen::Isometry3d& transform,
-    const Eigen::VectorXd& lower_tol,  // NOLINT(modernize-pass-by-value)
-    const Eigen::VectorXd& upper_tol)  // NOLINT(modernize-pass-by-value)
+    const Eigen::Isometry3d& transform,  // NOLINT(modernize-pass-by-value)
+    const Eigen::VectorXd& lower_tol,    // NOLINT(modernize-pass-by-value)
+    const Eigen::VectorXd& upper_tol)    // NOLINT(modernize-pass-by-value)
   : transform_(transform), lower_tolerance_(lower_tol), upper_tolerance_(upper_tol)
 {
 }

diff --git a/tesseract_examples/src/basic_cartesian_example.cpp b/tesseract_examples/src/basic_cartesian_example.cpp
@@ -233,8 +233,12 @@ bool BasicCartesianExample::run()
     profiles->addProfile<TrajOptCompositeProfile>(TRAJOPT_DEFAULT_NAMESPACE, "cartesian_program", composite_profile);
 
     auto plan_profile = std::make_shared<TrajOptDefaultPlanProfile>();
-    plan_profile->cartesian_coeff = Eigen::VectorXd::Ones(6);
-    plan_profile->joint_coeff = Eigen::VectorXd::Ones(7);
+    plan_profile->cartesian_cost_config.enabled = false;
+    plan_profile->cartesian_constraint_config.enabled = true;
+    plan_profile->cartesian_constraint_config.coeff = Eigen::VectorXd::Ones(6);
+    plan_profile->joint_cost_config.enabled = false;
+    plan_profile->joint_constraint_config.enabled = true;
+    plan_profile->joint_constraint_config.coeff = Eigen::VectorXd::Ones(7);
     profiles->addProfile<TrajOptPlanProfile>(TRAJOPT_DEFAULT_NAMESPACE, "RASTER", plan_profile);
     profiles->addProfile<TrajOptPlanProfile>(TRAJOPT_DEFAULT_NAMESPACE, "freespace_profile", plan_profile);
   }

diff --git a/tesseract_examples/src/glass_upright_example.cpp b/tesseract_examples/src/glass_upright_example.cpp
@@ -223,10 +223,12 @@ bool GlassUprightExample::run()
     profiles->addProfile<TrajOptCompositeProfile>(TRAJOPT_DEFAULT_NAMESPACE, "UPRIGHT", composite_profile);
 
     auto plan_profile = std::make_shared<TrajOptDefaultPlanProfile>();
-    plan_profile->cartesian_coeff = Eigen::VectorXd::Constant(6, 1, 5);
-    plan_profile->cartesian_coeff(0) = 0;
-    plan_profile->cartesian_coeff(1) = 0;
-    plan_profile->cartesian_coeff(2) = 0;
+    plan_profile->cartesian_cost_config.enabled = false;
+    plan_profile->cartesian_constraint_config.enabled = true;
+    plan_profile->cartesian_constraint_config.coeff = Eigen::VectorXd::Constant(6, 1, 5);
+    plan_profile->cartesian_constraint_config.coeff(0) = 0;
+    plan_profile->cartesian_constraint_config.coeff(1) = 0;
+    plan_profile->cartesian_constraint_config.coeff(2) = 0;
 
     // Add profile to Dictionary
     profiles->addProfile<TrajOptPlanProfile>(TRAJOPT_DEFAULT_NAMESPACE, "UPRIGHT", plan_profile);

diff --git a/tesseract_examples/src/pick_and_place_example.cpp b/tesseract_examples/src/pick_and_place_example.cpp
@@ -250,7 +250,9 @@ bool PickAndPlaceExample::run()
   {
     // Create TrajOpt Profile
     auto trajopt_plan_profile = std::make_shared<TrajOptDefaultPlanProfile>();
-    trajopt_plan_profile->cartesian_coeff = Eigen::VectorXd::Constant(6, 1, 10);
+    trajopt_plan_profile->cartesian_cost_config.enabled = false;
+    trajopt_plan_profile->cartesian_constraint_config.enabled = true;
+    trajopt_plan_profile->cartesian_constraint_config.coeff = Eigen::VectorXd::Constant(6, 1, 10);
 
     auto trajopt_composite_profile = std::make_shared<TrajOptDefaultCompositeProfile>();
     trajopt_composite_profile->longest_valid_segment_length = 0.05;

diff --git a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp
@@ -249,10 +249,12 @@ bool PuzzlePieceAuxillaryAxesExample::run()
   {
     // Create TrajOpt Profile
     auto trajopt_plan_profile = std::make_shared<tesseract_planning::TrajOptDefaultPlanProfile>();
-    trajopt_plan_profile->cartesian_coeff = Eigen::VectorXd::Constant(6, 1, 5);
-    trajopt_plan_profile->cartesian_coeff(3) = 2;
-    trajopt_plan_profile->cartesian_coeff(4) = 2;
-    trajopt_plan_profile->cartesian_coeff(5) = 0;
+    trajopt_plan_profile->cartesian_cost_config.enabled = false;
+    trajopt_plan_profile->cartesian_constraint_config.enabled = true;
+    trajopt_plan_profile->cartesian_constraint_config.coeff = Eigen::VectorXd::Constant(6, 1, 5);
+    trajopt_plan_profile->cartesian_constraint_config.coeff(3) = 2;
+    trajopt_plan_profile->cartesian_constraint_config.coeff(4) = 2;
+    trajopt_plan_profile->cartesian_constraint_config.coeff(5) = 0;
 
     auto trajopt_composite_profile = std::make_shared<TrajOptDefaultCompositeProfile>();
     trajopt_composite_profile->collision_constraint_config.enabled = false;

diff --git a/tesseract_examples/src/puzzle_piece_example.cpp b/tesseract_examples/src/puzzle_piece_example.cpp
@@ -240,8 +240,10 @@ bool PuzzlePieceExample::run()
   {
     // Create TrajOpt Profile
     auto trajopt_plan_profile = std::make_shared<TrajOptDefaultPlanProfile>();
-    trajopt_plan_profile->cartesian_coeff = Eigen::VectorXd::Constant(6, 1, 10);
-    trajopt_plan_profile->cartesian_coeff(5) = 0;
+    trajopt_plan_profile->cartesian_cost_config.enabled = false;
+    trajopt_plan_profile->cartesian_constraint_config.enabled = true;
+    trajopt_plan_profile->cartesian_constraint_config.coeff = Eigen::VectorXd::Constant(6, 1, 10);
+    trajopt_plan_profile->cartesian_constraint_config.coeff(5) = 0;
 
     auto trajopt_composite_profile = std::make_shared<TrajOptDefaultCompositeProfile>();
     trajopt_composite_profile->collision_constraint_config.enabled = false;

diff --git a/..._planners/descartes/include/tesseract_motion_planners/descartes/descartes_robot_sampler.h b/..._planners/descartes/include/tesseract_motion_planners/descartes/descartes_robot_sampler.h
@@ -54,12 +54,12 @@ class DescartesRobotSampler : public descartes_light::WaypointSampler<FloatType>
    * @param is_valid This is a user defined function to filter out solution
    */
   DescartesRobotSampler(std::string target_working_frame,
-                        const Eigen::Isometry3d& target_pose,
+                        const Eigen::Isometry3d& target_pose,  // NOLINT(modernize-pass-by-value)
                         PoseSamplerFn target_pose_sampler,
                         tesseract_kinematics::KinematicGroup::ConstPtr manip,
                         DescartesCollision::Ptr collision,
                         std::string tcp_frame,
-                        const Eigen::Isometry3d& tcp_offset,
+                        const Eigen::Isometry3d& tcp_offset,  // NOLINT(modernize-pass-by-value)
                         bool allow_collision,
                         DescartesVertexEvaluator::Ptr is_valid,
                         bool use_redundant_joint_solutions);

diff --git a/...s/include/tesseract_motion_planners/descartes/impl/descartes_collision_edge_evaluator.hpp b/...s/include/tesseract_motion_planners/descartes/impl/descartes_collision_edge_evaluator.hpp
@@ -54,7 +54,7 @@ DescartesCollisionEdgeEvaluator<FloatType>::DescartesCollisionEdgeEvaluator(
   if (discrete_contact_manager_ != nullptr)
   {
     discrete_contact_manager_->setActiveCollisionObjects(active_link_names_);
-    discrete_contact_manager_->applyContactManagerConfig(config.contact_manager_config);
+    discrete_contact_manager_->applyContactManagerConfig(collision_check_config_.contact_manager_config);
   }
   else if (collision_check_config_.type == tesseract_collision::CollisionEvaluatorType::DISCRETE ||
            collision_check_config_.type == tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE)
@@ -66,7 +66,7 @@ DescartesCollisionEdgeEvaluator<FloatType>::DescartesCollisionEdgeEvaluator(
   if (continuous_contact_manager_ != nullptr)
   {
     continuous_contact_manager_->setActiveCollisionObjects(active_link_names_);
-    continuous_contact_manager_->applyContactManagerConfig(config.contact_manager_config);
+    continuous_contact_manager_->applyContactManagerConfig(collision_check_config_.contact_manager_config);
   }
   else if (collision_check_config_.type == tesseract_collision::CollisionEvaluatorType::CONTINUOUS ||
            collision_check_config_.type == tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS)

diff --git a/...rs/descartes/include/tesseract_motion_planners/descartes/impl/descartes_robot_sampler.hpp b/...rs/descartes/include/tesseract_motion_planners/descartes/impl/descartes_robot_sampler.hpp
@@ -39,16 +39,17 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 namespace tesseract_planning
 {
 template <typename FloatType>
-DescartesRobotSampler<FloatType>::DescartesRobotSampler(std::string target_working_frame,
-                                                        const Eigen::Isometry3d& target_pose,
-                                                        PoseSamplerFn target_pose_sampler,
-                                                        tesseract_kinematics::KinematicGroup::ConstPtr manip,
-                                                        DescartesCollision::Ptr collision,
-                                                        std::string tcp_frame,
-                                                        const Eigen::Isometry3d& tcp_offset,
-                                                        bool allow_collision,
-                                                        DescartesVertexEvaluator::Ptr is_valid,
-                                                        bool use_redundant_joint_solutions)
+DescartesRobotSampler<FloatType>::DescartesRobotSampler(
+    std::string target_working_frame,
+    const Eigen::Isometry3d& target_pose,  // NOLINT(modernize-pass-by-value)
+    PoseSamplerFn target_pose_sampler,
+    tesseract_kinematics::KinematicGroup::ConstPtr manip,
+    DescartesCollision::Ptr collision,
+    std::string tcp_frame,
+    const Eigen::Isometry3d& tcp_offset,  // NOLINT(modernize-pass-by-value)
+    bool allow_collision,
+    DescartesVertexEvaluator::Ptr is_valid,
+    bool use_redundant_joint_solutions)
   : target_working_frame_(std::move(target_working_frame))
   , target_pose_(target_pose)
   , target_pose_sampler_(std::move(target_pose_sampler))

diff --git a/tesseract_motion_planners/descartes/src/deserialize.cpp b/tesseract_motion_planners/descartes/src/deserialize.cpp
@@ -42,7 +42,7 @@ namespace tesseract_planning
 DescartesDefaultPlanProfile<double> descartesPlanParser(const tinyxml2::XMLElement& xml_element)
 {
   const tinyxml2::XMLElement* descartes_plan_element = xml_element.FirstChildElement("DescartesPlanProfile");
-  return DescartesDefaultPlanProfile<double>(*descartes_plan_element);
+  return { *descartes_plan_element };
 }
 
 DescartesDefaultPlanProfile<double> descartesPlanFromXMLElement(const tinyxml2::XMLElement* profile_xml)

diff --git a/tesseract_motion_planners/descartes/src/serialize.cpp b/tesseract_motion_planners/descartes/src/serialize.cpp
@@ -74,7 +74,7 @@ std::string toXMLString(const DescartesPlanProfile<double>& plan_profile)
   std::shared_ptr<tinyxml2::XMLDocument> doc = toXMLDocument(plan_profile);
   tinyxml2::XMLPrinter printer;
   doc->Print(&printer);
-  return std::string(printer.CStr());
+  return { printer.CStr() };
 }
 
 }  // namespace tesseract_planning
diff --git a/tesseract_motion_planners/trajopt/CMakeLists.txt b/tesseract_motion_planners/trajopt/CMakeLists.txt
@@ -5,6 +5,7 @@ find_package(trajopt_sco REQUIRED)
 add_library(
   ${PROJECT_NAME}_trajopt
   src/trajopt_collision_config.cpp
+  src/trajopt_waypoint_config.cpp
   src/trajopt_motion_planner.cpp
   src/trajopt_utils.cpp
   src/profile/trajopt_default_plan_profile.cpp

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
@@ -33,6 +33,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <Eigen/Geometry>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
+#include <tesseract_motion_planners/trajopt/trajopt_waypoint_config.h>
 #include <tesseract_motion_planners/trajopt/profile/trajopt_profile.h>
 
 namespace tesseract_planning
@@ -51,9 +52,10 @@ class TrajOptDefaultPlanProfile : public TrajOptPlanProfile
   TrajOptDefaultPlanProfile(TrajOptDefaultPlanProfile&&) = default;
   TrajOptDefaultPlanProfile& operator=(TrajOptDefaultPlanProfile&&) = default;
 
-  Eigen::VectorXd cartesian_coeff{ Eigen::VectorXd::Constant(1, 1, 5) };
-  Eigen::VectorXd joint_coeff{ Eigen::VectorXd::Constant(1, 1, 5) };
-  trajopt::TermType term_type{ trajopt::TermType::TT_CNT };
+  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.
    *

diff --git a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_utils.h b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_utils.h
@@ -42,15 +42,20 @@ trajopt::TermInfo::Ptr createCartesianWaypointTermInfo(int index,
                                                        const std::string& tcp_frame,
                                                        const Eigen::Isometry3d& tcp_offset,
                                                        const Eigen::VectorXd& coeffs,
-                                                       trajopt::TermType type);
-
-trajopt::TermInfo::Ptr createDynamicCartesianWaypointTermInfo(int index,
-                                                              const std::string& working_frame,
-                                                              const Eigen::Isometry3d& c_wp,
-                                                              const std::string& tcp_frame,
-                                                              const Eigen::Isometry3d& tcp_offset,
-                                                              const Eigen::VectorXd& coeffs,
-                                                              trajopt::TermType type);
+                                                       trajopt::TermType type,
+                                                       const Eigen::VectorXd& lower_tolerance = Eigen::VectorXd(),
+                                                       const Eigen::VectorXd& upper_tolerance = Eigen::VectorXd());
+
+trajopt::TermInfo::Ptr
+createDynamicCartesianWaypointTermInfo(int index,
+                                       const std::string& working_frame,
+                                       const Eigen::Isometry3d& c_wp,
+                                       const std::string& tcp_frame,
+                                       const Eigen::Isometry3d& tcp_offset,
+                                       const Eigen::VectorXd& coeffs,
+                                       trajopt::TermType type,
+                                       const Eigen::VectorXd& lower_tolerance = Eigen::VectorXd(),
+                                       const Eigen::VectorXd& upper_tolerance = Eigen::VectorXd());
 
 trajopt::TermInfo::Ptr createNearJointStateTermInfo(const Eigen::VectorXd& target,
                                                     const std::vector<std::string>& joint_names,

diff --git a/...tion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_waypoint_config.h b/...tion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_waypoint_config.h
@@ -0,0 +1,104 @@
+/**
+ * @file trajopt_waypoint_config.h
+ * @brief TrajOpt waypoint configuration settings
+ *
+ * @author Tyler Marr
+ * @date November 2, 2023
+ * @version TODO
+ * @bug No known bugs
+ *
+ * @copyright Copyright (c) 2023, 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_CONFIG_TRAJOPT_WAYPOINT_CONFIG_H
+#define TESSERACT_MOTION_PLANNERS_TRAJOPT_CONFIG_TRAJOPT_WAYPOINT_CONFIG_H
+
+#include <tesseract_common/macros.h>
+#include <tesseract_common/utils.h>
+
+namespace tinyxml2
+{
+// NOLINTNEXTLINE(cppcoreguidelines-virtual-class-destructor)
+class XMLElement;
+class XMLDocument;
+}  // namespace tinyxml2
+
+namespace tesseract_planning
+{
+/**
+ * @brief Config settings for cartesian waypoints
+ */
+struct CartesianWaypointConfig
+{
+  // LCOV_EXCL_START
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  // LCOV_EXCL_STOP
+
+  CartesianWaypointConfig() = default;
+  CartesianWaypointConfig(const tinyxml2::XMLElement& xml_element);
+
+  /** @brief If true, a cost/constraint term will be added to the problem. Default: true*/
+  bool enabled{ true };
+
+  /** @brief If true, will override existing waypoint tolerance with described tolerance here. Default: false
+   * This is useful if you want to have a smaller tolerance for the cost than the constraint.*/
+  bool use_tolerance_override{ false };
+
+  /** @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::Matrix<double, 6, 1> lower_tolerance{ Eigen::VectorXd::Zero(6) };
+  /** @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::Matrix<double, 6, 1> upper_tolerance{ Eigen::VectorXd::Zero(6) };
+
+  /** @brief coefficients corresponsing to dx, dy, dz, rx, ry, rz*/
+  Eigen::Matrix<double, 6, 1> coeff{ Eigen::VectorXd::Constant(6, 5) };
+
+  tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const;
+};
+
+/**
+ * @brief Config settings for joint waypoints.
+ */
+struct JointWaypointConfig
+{
+  // LCOV_EXCL_START
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  // LCOV_EXCL_STOP
+
+  JointWaypointConfig() = default;
+  JointWaypointConfig(const tinyxml2::XMLElement& xml_element);
+
+  /** @brief If true, a cost/constraint term will be added to the problem. Default: true*/
+  bool enabled{ true };
+
+  /** @brief If true, will override existing waypoint tolerance with described tolerance here. Default: false
+   * This is useful if you want to have a smaller tolerance for the cost than the constraint.*/
+  bool use_tolerance_override{ false };
+
+  /** @brief Distance below waypoint that is allowed. Should be size of joints in a joint state*/
+  Eigen::VectorXd lower_tolerance;
+  /** @brief Distance above waypoint that is allowed. Should be size of joints in a joint state*/
+  Eigen::VectorXd upper_tolerance;
+
+  /** @brief coefficients corresponsing to joint values*/
+  Eigen::VectorXd coeff{ Eigen::VectorXd::Constant(1, 1, 5) };
+
+  tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const;
+};
+}  // namespace tesseract_planning
+
+#endif  // TESSERACT_MOTION_PLANNERS_TRAJOPT_CONFIG_TRAJOPT_WAYPOINT_CONFIG_H