diff --git a/tesseract_examples/src/car_seat_example.cpp b/tesseract_examples/src/car_seat_example.cpp index ade2c1d4e03..f318817fc97 100644 --- 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 #include -#include +#include #include #include #include @@ -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(); - 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(); + 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 index 1e013eafda2..19932ee202b 100644 --- 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 #include -#include +#include #include #include #include @@ -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(); - trajopt_solver_profile->opt_info.max_iter = 100; + auto trajopt_solver_profile = std::make_shared(); + 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 index 3ab5620abd8..7825df54b56 100644 --- 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 #include -#include +#include #include #include #include @@ -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(); - trajopt_solver_profile->convex_solver = sco::ModelType::OSQP; - auto convex_solver_config = std::make_shared(); - 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(); + 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 index 94a1e1fea31..1f90282945e 100644 --- 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 #include -#include +#include #include #include #include @@ -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(); - 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(); + 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 index 596aa85b098..8d6d7a9c5f5 100644 --- 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/trajopt/CMakeLists.txt b/tesseract_motion_planners/trajopt/CMakeLists.txt index cd8c92b8847..68b0e4de0f4 100644 --- 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,9 +12,7 @@ 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 @@ -21,8 +20,11 @@ target_link_libraries( 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 deleted file mode 100644 index b06613f0938..00000000000 --- a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/deserialize.h +++ /dev/null @@ -1,67 +0,0 @@ -/** - * @file deserialize.h - * @brief Provide methods for deserialize instructions to xml and deserialization - * - * @author Tyler Marr - * @date August 20, 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_DESERIALIZE_H -#define TESSERACT_MOTION_PLANNERS_TRAJOPT_DESERIALIZE_H - -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -namespace tinyxml2 -{ -class XMLElement; // NOLINT -class XMLDocument; -} // namespace tinyxml2 - -namespace tesseract_planning -{ -class TrajOptDefaultPlanProfile; -class TrajOptDefaultCompositeProfile; - -TrajOptDefaultPlanProfile trajOptPlanParser(const tinyxml2::XMLElement& xml_element); - -TrajOptDefaultPlanProfile trajOptPlanFromXMLElement(const tinyxml2::XMLElement* profile_xml); - -TrajOptDefaultPlanProfile trajOptPlanFromXMLDocument(const tinyxml2::XMLDocument& xml_doc); - -TrajOptDefaultPlanProfile trajOptPlanFromXMLFile(const std::string& file_path); - -TrajOptDefaultPlanProfile trajOptPlanFromXMLString(const std::string& xml_string); - -TrajOptDefaultCompositeProfile trajOptCompositeParser(const tinyxml2::XMLElement& xml_element); - -TrajOptDefaultCompositeProfile trajOptCompositeFromXMLElement(const tinyxml2::XMLElement* profile_xml); - -TrajOptDefaultCompositeProfile trajOptCompositeFromXMLDocument(const tinyxml2::XMLDocument& xml_doc); - -TrajOptDefaultCompositeProfile trajOptCompositeFromXMLFile(const std::string& file_path); - -TrajOptDefaultCompositeProfile trajOptCompositeFromXMLString(const std::string& xml_string); - -} // namespace tesseract_planning - -#endif // 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 index 047f372fc50..af026c35e6c 100644 --- 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/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_composite_profile.h b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_composite_profile.h index 1058890e86e..b0732ab45ee 100644 --- a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_composite_profile.h +++ b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_composite_profile.h @@ -51,7 +51,6 @@ class TrajOptDefaultCompositeProfile : public TrajOptCompositeProfile using ConstPtr = std::shared_ptr; 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 special_collision_constraint{ nullptr }; - void apply(trajopt::ProblemConstructionInfo& pci, - int start_index, - int end_index, - const tesseract_common::ManipulatorInfo& manip_info, - const std::vector& active_links, - const std::vector& fixed_indices) const override; - - tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + TrajOptTermInfos createCostAndConstraints(const tesseract_common::ManipulatorInfo& composite_manip_info, + const std::shared_ptr& env, + const std::vector& 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& fixed_indices) const; - - void addCollisionConstraint(trajopt::ProblemConstructionInfo& pci, - int start_index, - int end_index, - const std::vector& fixed_indices) const; - - void addVelocitySmoothing(trajopt::ProblemConstructionInfo& pci, - int start_index, - int end_index, - const std::vector& fixed_indices) const; - - void addAccelerationSmoothing(trajopt::ProblemConstructionInfo& pci, - int start_index, - int end_index, - const std::vector& fixed_indices) const; - - void addJerkSmoothing(trajopt::ProblemConstructionInfo& pci, - int start_index, - int end_index, - const std::vector& fixed_indices) const; - - void addConstraintErrorFunctions(trajopt::ProblemConstructionInfo& pci, - int start_index, - int end_index, - const std::vector& fixed_indices) const; - - void addAvoidSingularity(trajopt::ProblemConstructionInfo& pci, - int start_index, - int end_index, - const std::string& link, - const std::vector& 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 diff --git a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h index affec26a22c..2380e1a0dc0 100644 --- a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h +++ b/tesseract_motion_planners/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; 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 createCostAndConstraints(const MoveInstructionPoly& move_instruction, + const tesseract_common::ManipulatorInfo& composite_manip_info, + const std::shared_ptr& env, + const std::vector& 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, 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> - 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& 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& 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& fixed_steps) const; + static std::shared_ptr + 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 diff --git a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_solver_profile.h b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_solver_profile.h deleted file mode 100644 index c69e3add39b..00000000000 --- a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_solver_profile.h +++ /dev/null @@ -1,66 +0,0 @@ -/** - * @file trajopt_default_solver_profile.h - * @brief - * - * @author Levi Armstrong - * @date December 13, 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_DEFAULT_SOLVER_PROFILE_H -#define TESSERACT_MOTION_PLANNERS_TRAJOPT_DEFAULT_SOLVER_PROFILE_H - -#include -#include - -namespace tesseract_planning -{ -/** @brief The contains the default solver parameters available for setting up TrajOpt */ -class TrajOptDefaultSolverProfile : public TrajOptSolverProfile -{ -public: - using Ptr = std::shared_ptr; - using ConstPtr = std::shared_ptr; - - /** @brief The Convex solver to use */ - sco::ModelType convex_solver{ sco::ModelType::OSQP }; - - /** @brief The convex solver config to use, if nullptr the default settings are used */ - sco::ModelConfig::Ptr convex_solver_config{ nullptr }; - - /** @brief Optimization paramters */ - sco::BasicTrustRegionSQPParameters opt_info; - - /** @brief Optimization callbacks */ - std::vector callbacks; - - void apply(trajopt::ProblemConstructionInfo& pci) const override; - - tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; - -protected: - friend class boost::serialization::access; - template - void serialize(Archive&, const unsigned int); // NOLINT -}; -} // namespace tesseract_planning - -BOOST_CLASS_EXPORT_KEY(tesseract_planning::TrajOptDefaultSolverProfile) - -#endif // TESSERACT_MOTION_PLANNERS_TRAJOPT_DEFAULT_SOLVER_PROFILE_H diff --git a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_osqp_solver_profile.h b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_osqp_solver_profile.h new file mode 100644 index 00000000000..96e720105cc --- /dev/null +++ b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_osqp_solver_profile.h @@ -0,0 +1,63 @@ +/** + * @file trajopt_osqp_solver_profile.h + * @brief + * + * @author Levi Armstrong + * @date December 13, 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_OSQP_SOLVER_PROFILE_H +#define TESSERACT_MOTION_PLANNERS_TRAJOPT_OSQP_SOLVER_PROFILE_H + +#include +#include + +namespace boost::serialization +{ +template +void serialize(Archive& ar, OSQPSettings& settings, const unsigned int version); // NOLINT + +} // namespace boost::serialization + +namespace tesseract_planning +{ +/** @brief The contains the OSQP Solver and Trust Region Parameters available for setting up TrajOpt */ +class TrajOptOSQPSolverProfile : public TrajOptSolverProfile +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + OSQPSettings settings{}; + + sco::ModelType getSolverType() const override; + + std::unique_ptr createSolverConfig() const override; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT +}; +} // namespace tesseract_planning + +BOOST_CLASS_EXPORT_KEY(tesseract_planning::TrajOptOSQPSolverProfile) + +#endif // TESSERACT_MOTION_PLANNERS_TRAJOPT_OSQP_SOLVER_PROFILE_H diff --git a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h index 6a1f07a46f5..0d1b26316e8 100644 --- a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h +++ b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h @@ -32,20 +32,39 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include #include #include +#include +#include -namespace tinyxml2 +namespace boost::serialization { -class XMLElement; // NOLINT -class XMLDocument; -} // namespace tinyxml2 +template +void serialize(Archive& ar, sco::BasicTrustRegionSQPParameters& params, const unsigned int version); // NOLINT +} // namespace boost::serialization namespace tesseract_planning { +/** @brief Structure to store TrajOpt cost and constrant term infos */ +struct TrajOptTermInfos +{ + std::vector> costs; + std::vector> constraints; +}; + +/** @brief Structure to store TrajOpt waypoint cost and constrant term infos */ +struct TrajOptWaypointInfo +{ + TrajOptTermInfos term_infos; + Eigen::VectorXd seed; + bool fixed{ false }; +}; + class TrajOptPlanProfile : public Profile { public: @@ -54,28 +73,19 @@ class TrajOptPlanProfile : public Profile TrajOptPlanProfile(); + virtual TrajOptWaypointInfo + createCostAndConstraints(const MoveInstructionPoly& move_instruction, + const tesseract_common::ManipulatorInfo& composite_manip_info, + const std::shared_ptr& env, + const std::vector& active_links, + int index) const = 0; + /** * @brief A utility function for getting profile ID * @return The profile ID used when storing in profile dictionary */ static std::size_t getStaticKey(); - virtual void apply(trajopt::ProblemConstructionInfo& pci, - const CartesianWaypointPoly& cartesian_waypoint, - const MoveInstructionPoly& parent_instruction, - const tesseract_common::ManipulatorInfo& manip_info, - const std::vector& active_links, - int index) const = 0; - - virtual void apply(trajopt::ProblemConstructionInfo& pci, - const JointWaypointPoly& joint_waypoint, - const MoveInstructionPoly& parent_instruction, - const tesseract_common::ManipulatorInfo& manip_info, - const std::vector& active_links, - int index) const = 0; - - virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0; - protected: friend class boost::serialization::access; template @@ -90,21 +100,19 @@ class TrajOptCompositeProfile : public Profile TrajOptCompositeProfile(); + virtual TrajOptTermInfos + createCostAndConstraints(const tesseract_common::ManipulatorInfo& composite_manip_info, + const std::shared_ptr& env, + const std::vector& fixed_indices, + int start_index, + int end_index) const = 0; + /** * @brief A utility function for getting profile ID * @return The profile ID used when storing in profile dictionary */ static std::size_t getStaticKey(); - virtual void apply(trajopt::ProblemConstructionInfo& pci, - int start_index, - int end_index, - const tesseract_common::ManipulatorInfo& manip_info, - const std::vector& active_links, - const std::vector& fixed_indices) const = 0; - - virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0; - protected: friend class boost::serialization::access; template @@ -119,16 +127,27 @@ class TrajOptSolverProfile : public Profile TrajOptSolverProfile(); + /** @brief Optimization paramters */ + sco::BasicTrustRegionSQPParameters opt_params; + + /** @brief Get the convex solver to use */ + virtual sco::ModelType getSolverType() const = 0; + + /** @brief The convex solver config to use, if nullptr the default settings are used */ + virtual std::unique_ptr createSolverConfig() const = 0; + + /** @brief Optimization paramters */ + virtual sco::BasicTrustRegionSQPParameters createOptimizationParameters() const; + + /** @brief Optimization callbacks */ + virtual std::vector createOptimizationCallbacks() const; + /** * @brief A utility function for getting profile ID * @return The profile ID used when storing in profile dictionary */ static std::size_t getStaticKey(); - virtual void apply(trajopt::ProblemConstructionInfo& pci) const = 0; - - virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0; - protected: friend class boost::serialization::access; template diff --git a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/serialize.h b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/serialize.h deleted file mode 100644 index 788acfec326..00000000000 --- a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/serialize.h +++ /dev/null @@ -1,59 +0,0 @@ -/** - * @file serialize.h - * @brief Provide methods for serializing trajopt plans to xml - * - * @author Tyler Marr - * @date August 20, 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_SERIALIZE_H -#define TESSERACT_MOTION_PLANNERS_TRAJOPT_SERIALIZE_H - -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -namespace tinyxml2 -{ -class XMLElement; // NOLINT -class XMLDocument; -} // namespace tinyxml2 - -namespace tesseract_planning -{ -class TrajOptPlanProfile; -class TrajOptCompositeProfile; - -std::shared_ptr toXMLDocument(const TrajOptPlanProfile& plan_profile); - -bool toXMLFile(const TrajOptPlanProfile& plan_profile, const std::string& file_path); - -std::string toXMLString(const TrajOptPlanProfile& plan_profile); - -std::shared_ptr toXMLDocument(const TrajOptCompositeProfile& composite_profile); - -bool toXMLFile(const TrajOptCompositeProfile& composite_profile, const std::string& file_path); - -std::string toXMLString(const TrajOptCompositeProfile& composite_profile); - -} // namespace tesseract_planning -#endif // TESSERACT_MOTION_PLANNERS_TRAJOPT_SERIALIZE_H diff --git a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_collision_config.h b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_collision_config.h index c26929f7938..b1372cb6162 100644 --- a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_collision_config.h +++ b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_collision_config.h @@ -34,12 +34,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -namespace tinyxml2 -{ -class XMLElement; // NOLINT -class XMLDocument; -} // namespace tinyxml2 - namespace tesseract_planning { /** @@ -48,7 +42,6 @@ namespace tesseract_planning struct CollisionCostConfig { CollisionCostConfig() = default; - CollisionCostConfig(const tinyxml2::XMLElement& xml_element); /** @brief If true, a collision cost term will be added to the problem. Default: true*/ bool enabled = true; @@ -71,8 +64,6 @@ struct CollisionCostConfig /** @brief The collision coeff/weight */ double coeff = 20; - tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const; - protected: friend class boost::serialization::access; template @@ -85,7 +76,6 @@ struct CollisionCostConfig struct CollisionConstraintConfig { CollisionConstraintConfig() = default; - CollisionConstraintConfig(const tinyxml2::XMLElement& xml_element); /** @brief If true, a collision cost term will be added to the problem. Default: true*/ bool enabled = true; @@ -103,8 +93,6 @@ struct CollisionConstraintConfig /** @brief The collision coeff/weight */ double coeff = 20; - tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const; - protected: friend class boost::serialization::access; template 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 index 332a201f745..c191df27f6a 100644 --- 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 @@ -38,6 +38,10 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_planning { +std::unique_ptr +createKinematicGroup(const tesseract_common::ManipulatorInfo& manip_info, + const tesseract_environment::Environment& env); + std::shared_ptr createCartesianWaypointTermInfo(int index, const std::string& working_frame, diff --git a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_waypoint_config.h b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_waypoint_config.h index e09acb3ae89..ab0b204add9 100644 --- a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_waypoint_config.h +++ b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_waypoint_config.h @@ -33,13 +33,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -namespace tinyxml2 -{ -// NOLINTNEXTLINE(cppcoreguidelines-virtual-class-destructor) -class XMLElement; -class XMLDocument; -} // namespace tinyxml2 - namespace tesseract_planning { /** @@ -52,7 +45,6 @@ struct CartesianWaypointConfig // 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 }; @@ -71,8 +63,6 @@ struct CartesianWaypointConfig /** @brief coefficients corresponsing to dx, dy, dz, rx, ry, rz*/ Eigen::Matrix coeff{ Eigen::VectorXd::Constant(6, 5) }; - tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const; - protected: friend class boost::serialization::access; template @@ -89,7 +79,6 @@ struct JointWaypointConfig // 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 }; @@ -106,8 +95,6 @@ struct JointWaypointConfig /** @brief coefficients corresponsing to joint values*/ Eigen::VectorXd coeff{ Eigen::VectorXd::Constant(1, 1, 5) }; - tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const; - protected: friend class boost::serialization::access; template diff --git a/tesseract_motion_planners/trajopt/src/deserialize.cpp b/tesseract_motion_planners/trajopt/src/deserialize.cpp deleted file mode 100644 index 623fa637bc7..00000000000 --- a/tesseract_motion_planners/trajopt/src/deserialize.cpp +++ /dev/null @@ -1,212 +0,0 @@ -/** - * @file deserialize.cpp - * @brief Provide methods for deserialize instructions to xml and deserialization - * - * @author Tyler Marr - * @date August 21, 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. - */ - -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -#include -#include -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include -#include -#include - -namespace tesseract_planning -{ -TrajOptDefaultPlanProfile trajOptPlanParser(const tinyxml2::XMLElement& xml_element) -{ - const tinyxml2::XMLElement* trajopt_plan_element = xml_element.FirstChildElement("TrajOptDefaultPlanProfile"); - return TrajOptDefaultPlanProfile{ *trajopt_plan_element }; -} - -TrajOptDefaultPlanProfile trajOptPlanFromXMLElement(const tinyxml2::XMLElement* profile_xml) -{ - std::array version{ 0, 0, 0 }; - std::string version_string; - int status = tesseract_common::QueryStringAttribute(profile_xml, "version", version_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("fromXML: Error parsing robot attribute 'version'"); - - if (status != tinyxml2::XML_NO_ATTRIBUTE) - { - std::vector tokens; - boost::split(tokens, version_string, boost::is_any_of("."), boost::token_compress_on); - if (tokens.size() < 2 || tokens.size() > 3 || !tesseract_common::isNumeric(tokens)) - throw std::runtime_error("fromXML: Error parsing robot attribute 'version'"); - - tesseract_common::toNumeric(tokens[0], version[0]); - tesseract_common::toNumeric(tokens[1], version[1]); - if (tokens.size() == 3) - tesseract_common::toNumeric(tokens[2], version[2]); - else - version[2] = 0; - } - else - { - CONSOLE_BRIDGE_logWarn("No version number was provided so latest parser will be used."); - } - - const tinyxml2::XMLElement* planner_xml = profile_xml->FirstChildElement("Planner"); - if (planner_xml == nullptr) - throw std::runtime_error("fromXML: Could not find the 'Planner' element in the xml file."); - - int type{ 0 }; - status = planner_xml->QueryIntAttribute("type", &type); - if (status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("fromXML: Failed to parse instruction type attribute."); - - return trajOptPlanParser(*planner_xml); -} - -TrajOptDefaultPlanProfile trajOptPlanFromXMLDocument(const tinyxml2::XMLDocument& xml_doc) -{ - const tinyxml2::XMLElement* planner_xml = xml_doc.FirstChildElement("Profile"); - if (planner_xml == nullptr) - throw std::runtime_error("Could not find the 'Profile' element in the xml file"); - - return trajOptPlanFromXMLElement(planner_xml); -} - -TrajOptDefaultPlanProfile trajOptPlanFromXMLString(const std::string& xml_string) -{ - tinyxml2::XMLDocument xml_doc; - int status = xml_doc.Parse(xml_string.c_str()); - if (status != tinyxml2::XMLError::XML_SUCCESS) - throw std::runtime_error("Could not parse the Planner Profile XML File."); - - return trajOptPlanFromXMLDocument(xml_doc); -} - -TrajOptDefaultPlanProfile trajOptPlanFromXMLFile(const std::string& file_path) -{ - // get the entire file - std::string xml_string; - std::fstream xml_file(file_path.c_str(), std::fstream::in); - if (xml_file.is_open()) - { - while (xml_file.good()) - { - std::string line; - std::getline(xml_file, line); - xml_string += (line + "\n"); - } - xml_file.close(); - return trajOptPlanFromXMLString(xml_string); - } - - throw std::runtime_error("Could not open file " + file_path + "for parsing."); -} - -TrajOptDefaultCompositeProfile trajOptCompositeParser(const tinyxml2::XMLElement& xml_element) -{ - const tinyxml2::XMLElement* trajopt_composite_element = xml_element.FirstChildElement("TrajoptCompositeProfile"); - return TrajOptDefaultCompositeProfile{ *trajopt_composite_element }; -} - -TrajOptDefaultCompositeProfile trajOptCompositeFromXMLElement(const tinyxml2::XMLElement* profile_xml) -{ - std::array version{ 0, 0, 0 }; - std::string version_string; - int status = tesseract_common::QueryStringAttribute(profile_xml, "version", version_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("fromXML: Error parsing robot attribute 'version'"); - - if (status != tinyxml2::XML_NO_ATTRIBUTE) - { - std::vector tokens; - boost::split(tokens, version_string, boost::is_any_of("."), boost::token_compress_on); - if (tokens.size() < 2 || tokens.size() > 3 || !tesseract_common::isNumeric(tokens)) - throw std::runtime_error("fromXML: Error parsing robot attribute 'version'"); - - tesseract_common::toNumeric(tokens[0], version[0]); - tesseract_common::toNumeric(tokens[1], version[1]); - if (tokens.size() == 3) - tesseract_common::toNumeric(tokens[2], version[2]); - else - version[2] = 0; - } - else - { - CONSOLE_BRIDGE_logWarn("No version number was provided so latest parser will be used."); - } - - const tinyxml2::XMLElement* planner_xml = profile_xml->FirstChildElement("Planner"); - if (planner_xml == nullptr) - throw std::runtime_error("fromXML: Could not find the 'Planner' element in the xml file."); - - int type{ 0 }; - status = planner_xml->QueryIntAttribute("type", &type); - if (status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("fromXML: Failed to parse instruction type attribute."); - - return trajOptCompositeParser(*planner_xml); -} - -TrajOptDefaultCompositeProfile trajOptCompositeFromXMLDocument(const tinyxml2::XMLDocument& xml_doc) -{ - const tinyxml2::XMLElement* planner_xml = xml_doc.FirstChildElement("Profile"); - if (planner_xml == nullptr) - throw std::runtime_error("Could not find the 'Profile' element in the xml file"); - - return trajOptCompositeFromXMLElement(planner_xml); -} - -TrajOptDefaultCompositeProfile trajOptCompositeFromXMLString(const std::string& xml_string) -{ - tinyxml2::XMLDocument xml_doc; - int status = xml_doc.Parse(xml_string.c_str()); - if (status != tinyxml2::XMLError::XML_SUCCESS) - throw std::runtime_error("Could not parse the Planner Profile XML File."); - - return trajOptCompositeFromXMLDocument(xml_doc); -} - -TrajOptDefaultCompositeProfile trajOptCompositeFromXMLFile(const std::string& file_path) -{ - // get the entire file - std::string xml_string; - std::fstream xml_file(file_path.c_str(), std::fstream::in); - if (xml_file.is_open()) - { - while (xml_file.good()) - { - std::string line; - std::getline(xml_file, line); - xml_string += (line + "\n"); - } - xml_file.close(); - return trajOptCompositeFromXMLString(xml_string); - } - - throw std::runtime_error("Could not open file " + file_path + "for parsing."); -} - -} // namespace tesseract_planning diff --git a/tesseract_motion_planners/trajopt/src/profile/trajopt_default_composite_profile.cpp b/tesseract_motion_planners/trajopt/src/profile/trajopt_default_composite_profile.cpp index 199942a75de..bff6c739a2b 100644 --- a/tesseract_motion_planners/trajopt/src/profile/trajopt_default_composite_profile.cpp +++ b/tesseract_motion_planners/trajopt/src/profile/trajopt_default_composite_profile.cpp @@ -27,7 +27,6 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include -#include #include #include #include @@ -43,429 +42,130 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -#include #include +#include +#include #include static const double LONGEST_VALID_SEGMENT_FRACTION_DEFAULT = 0.01; namespace tesseract_planning { -TrajOptDefaultCompositeProfile::TrajOptDefaultCompositeProfile(const tinyxml2::XMLElement& xml_element) +TrajOptTermInfos TrajOptDefaultCompositeProfile::createCostAndConstraints( + const tesseract_common::ManipulatorInfo& composite_manip_info, + const std::shared_ptr& env, + const std::vector& fixed_indices, + int start_index, + int end_index) const { - const tinyxml2::XMLElement* contact_test_type_element = xml_element.FirstChildElement("ContactTestType"); - const tinyxml2::XMLElement* collision_cost_config_element = xml_element.FirstChildElement("CollisionCostConfig"); - const tinyxml2::XMLElement* collision_cnt_config_element = xml_element.FirstChildElement("CollisionConstraintConfig"); - const tinyxml2::XMLElement* smooth_velocities_element = xml_element.FirstChildElement("SmoothVelocities"); - const tinyxml2::XMLElement* smooth_accelerations_element = xml_element.FirstChildElement("SmoothAccelerations"); - const tinyxml2::XMLElement* smooth_jerks_element = xml_element.FirstChildElement("SmoothJerks"); - const tinyxml2::XMLElement* avoid_singularities_element = xml_element.FirstChildElement("AvoidSingularity"); - const tinyxml2::XMLElement* longest_valid_seg_fraction_element = xml_element.FirstChildElement("LongestValidSegmentFr" - "action"); - const tinyxml2::XMLElement* longest_valid_seg_length_element = xml_element.FirstChildElement("LongestValidSegmentLeng" - "th"); - - int status{ tinyxml2::XMLError::XML_SUCCESS }; - - if (contact_test_type_element != nullptr) - { - auto type = static_cast(tesseract_collision::ContactTestType::ALL); - status = contact_test_type_element->QueryIntAttribute("type", &type); - if (status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("TrajoptCompositeProfile: Error parsing ContactTest type attribute."); - - contact_test_type = static_cast(type); - } - - if (collision_cost_config_element != nullptr) - { - collision_cost_config = CollisionCostConfig(*collision_cost_config_element); - } - - if (collision_cnt_config_element != nullptr) - { - collision_constraint_config = CollisionConstraintConfig(*collision_cnt_config_element); - } - - std::size_t coeff_length = 0; - if (smooth_velocities_element != nullptr) - { - TrajOptDefaultCompositeProfile::smoothMotionTerms( - *smooth_velocities_element, smooth_velocities, velocity_coeff, coeff_length); - } + // -------- Construct the problem ------------ + // ------------------------------------------- + TrajOptTermInfos term_infos; + tesseract_kinematics::JointGroup::UPtr joint_group = env->getJointGroup(composite_manip_info.manipulator); + const Eigen::Index dof = joint_group->numJoints(); + const Eigen::MatrixX2d joint_limits = joint_group->getLimits().joint_limits; + const double lvs_length = computeLongestValidSegmentLength(joint_limits); - if (smooth_accelerations_element != nullptr) + if (collision_constraint_config.enabled) { - TrajOptDefaultCompositeProfile::smoothMotionTerms( - *smooth_accelerations_element, smooth_accelerations, acceleration_coeff, coeff_length); - } + trajopt::TermInfo::Ptr ti = createCollisionTermInfo(start_index, + end_index, + collision_constraint_config.safety_margin, + collision_constraint_config.safety_margin_buffer, + collision_constraint_config.type, + collision_constraint_config.use_weighted_sum, + collision_constraint_config.coeff, + contact_test_type, + lvs_length, + trajopt::TermType::TT_CNT); + + // Update the term info with the (possibly) new start and end state indices for which to apply this cost + std::shared_ptr ct = std::static_pointer_cast(ti); + if (special_collision_constraint) + { + for (auto& info : ct->info) + { + info = special_collision_constraint; + } + } + ct->fixed_steps = fixed_indices; - if (smooth_jerks_element != nullptr) - { - TrajOptDefaultCompositeProfile::smoothMotionTerms(*smooth_jerks_element, smooth_jerks, jerk_coeff, coeff_length); + term_infos.constraints.push_back(ct); } - if (avoid_singularities_element != nullptr) + if (collision_cost_config.enabled) { - const tinyxml2::XMLElement* enabled_element = avoid_singularities_element->FirstChildElement("Enabled"); - const tinyxml2::XMLElement* coeff_element = avoid_singularities_element->FirstChildElement("Coefficient"); - - if (enabled_element == nullptr) - throw std::runtime_error("TrajoptCompositeProfile: Avoid singularity element must have Enabled element."); - - status = enabled_element->QueryBoolText(&avoid_singularity); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("TrajoptCompositeProfile: Error parsing Enabled string"); - - if (coeff_element != nullptr) + // Create a default collision term info + trajopt::TermInfo::Ptr ti = createCollisionTermInfo(start_index, + end_index, + collision_cost_config.safety_margin, + collision_cost_config.safety_margin_buffer, + collision_cost_config.type, + collision_cost_config.use_weighted_sum, + collision_cost_config.coeff, + contact_test_type, + lvs_length, + trajopt::TermType::TT_COST); + + // Update the term info with the (possibly) new start and end state indices for which to apply this cost + std::shared_ptr ct = std::static_pointer_cast(ti); + if (special_collision_cost) { - std::string coeff_string; - status = tesseract_common::QueryStringText(coeff_element, coeff_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("TrajoptCompositeProfile: AvoidSingularity: Error parsing Coefficient string"); - - if (!tesseract_common::isNumeric(coeff_string)) - throw std::runtime_error("TrajoptCompositeProfile: AvoidSingularity: Coefficient is not a numeric values."); - - tesseract_common::toNumeric(coeff_string, avoid_singularity_coeff); + for (auto& info : ct->info) + { + info = special_collision_cost; + } } - } - - if (longest_valid_seg_fraction_element != nullptr) - { - std::string long_valid_seg_frac_string; - status = tesseract_common::QueryStringText(longest_valid_seg_fraction_element, long_valid_seg_frac_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("TrajoptCompositeProfile: Error parsing LongestValidSegmentFraction string"); - - if (!tesseract_common::isNumeric(long_valid_seg_frac_string)) - throw std::runtime_error("TrajoptCompositeProfile: LongestValidSegmentFraction is not a numeric values."); + ct->fixed_steps = fixed_indices; - tesseract_common::toNumeric(long_valid_seg_frac_string, longest_valid_segment_fraction); + term_infos.costs.push_back(ct); } - if (longest_valid_seg_length_element != nullptr) + if (smooth_velocities) { - std::string long_valid_seg_len_string; - status = tesseract_common::QueryStringText(longest_valid_seg_length_element, long_valid_seg_len_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("TrajoptCompositeProfile: Error parsing LongestValidSegmentLength string"); - - if (!tesseract_common::isNumeric(long_valid_seg_len_string)) - throw std::runtime_error("TrajoptCompositeProfile: LongestValidSegmentLength is not a numeric values."); - - tesseract_common::toNumeric(long_valid_seg_len_string, longest_valid_segment_length); + if (velocity_coeff.size() == 0) + term_infos.costs.push_back(createSmoothVelocityTermInfo(start_index, end_index, static_cast(dof))); + else + term_infos.costs.push_back(createSmoothVelocityTermInfo(start_index, end_index, velocity_coeff)); } -} - -void TrajOptDefaultCompositeProfile::smoothMotionTerms(const tinyxml2::XMLElement& xml_element, - bool& enabled, - Eigen::VectorXd& coeff, - std::size_t& length) -{ - const tinyxml2::XMLElement* enabled_element = xml_element.FirstChildElement("Enabled"); - const tinyxml2::XMLElement* coeff_element = xml_element.FirstChildElement("Coefficients"); - - if (enabled_element == nullptr) - throw std::runtime_error("TrajoptCompositeProfile: All motion smoothing types must have Enabled element."); - - int status = enabled_element->QueryBoolText(&enabled); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("TrajoptCompositeProfile: Error parsing Enabled string"); - if (coeff_element != nullptr) + if (smooth_accelerations) { - std::vector coeff_tokens; - std::string coeff_string; - status = tesseract_common::QueryStringText(coeff_element, coeff_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("TrajoptCompositeProfile: Error parsing motion smoothing Coefficients string"); - - boost::split(coeff_tokens, coeff_string, boost::is_any_of(" "), boost::token_compress_on); - if (length == 0) - length = coeff_tokens.size(); - else if (length != coeff_tokens.size()) - throw std::runtime_error("TrajoptCompositeProfile: Motion smoothing Coefficients are inconsistent sizes."); - - if (!tesseract_common::isNumeric(coeff_tokens)) - throw std::runtime_error("TrajoptCompositeProfile: Motion smoothing Coefficients are not all numeric values."); - - coeff.resize(static_cast(length)); - for (std::size_t i = 0; i < coeff_tokens.size(); ++i) - tesseract_common::toNumeric(coeff_tokens[i], coeff[static_cast(i)]); + if (acceleration_coeff.size() == 0) + term_infos.costs.push_back(createSmoothAccelerationTermInfo(start_index, end_index, static_cast(dof))); + else + term_infos.costs.push_back(createSmoothAccelerationTermInfo(start_index, end_index, acceleration_coeff)); } -} - -void TrajOptDefaultCompositeProfile::apply(trajopt::ProblemConstructionInfo& pci, - int start_index, - int end_index, - const tesseract_common::ManipulatorInfo& manip_info, - const std::vector& /*active_links*/, - const std::vector& fixed_indices) const -{ - // -------- Construct the problem ------------ - // ------------------------------------------- - - if (manip_info.manipulator.empty()) - throw std::runtime_error("TrajOpt, manipulator is empty!"); - - if (manip_info.tcp_frame.empty()) - throw std::runtime_error("TrajOpt, tcp_frame is empty!"); - - if (manip_info.working_frame.empty()) - throw std::runtime_error("TrajOpt, working_frame is empty!"); - - if (collision_constraint_config.enabled) - addCollisionConstraint(pci, start_index, end_index, fixed_indices); - - if (collision_cost_config.enabled) - addCollisionCost(pci, start_index, end_index, fixed_indices); - - if (smooth_velocities) - addVelocitySmoothing(pci, start_index, end_index, fixed_indices); - - if (smooth_accelerations) - addAccelerationSmoothing(pci, start_index, end_index, fixed_indices); if (smooth_jerks) - addJerkSmoothing(pci, start_index, end_index, fixed_indices); - - // if (!constraint_error_functions.empty()) - // addConstraintErrorFunctions(pci, start_index, end_index, fixed_indices); - - if (avoid_singularity) - addAvoidSingularity(pci, start_index, end_index, manip_info.tcp_frame, fixed_indices); -} - -tinyxml2::XMLElement* TrajOptDefaultCompositeProfile::toXML(tinyxml2::XMLDocument& doc) const -{ - Eigen::IOFormat eigen_format(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", " "); - - tinyxml2::XMLElement* xml_planner = doc.NewElement("Planner"); - xml_planner->SetAttribute("type", std::to_string(1).c_str()); - - tinyxml2::XMLElement* xml_trajopt = doc.NewElement("TrajoptCompositeProfile"); - - tinyxml2::XMLElement* xml_contact_test_type = doc.NewElement("ContactTest"); - xml_contact_test_type->SetAttribute("type", std::to_string(static_cast(contact_test_type)).c_str()); - xml_trajopt->InsertEndChild(xml_contact_test_type); - - tinyxml2::XMLElement* xml_collision_cost_info = collision_cost_config.toXML(doc); - xml_trajopt->InsertEndChild(xml_collision_cost_info); - - tinyxml2::XMLElement* xml_collision_constraint_info = collision_constraint_config.toXML(doc); - xml_trajopt->InsertEndChild(xml_collision_constraint_info); - - tinyxml2::XMLElement* xml_smooth_velocities = doc.NewElement("SmoothVelocities"); - - tinyxml2::XMLElement* xml_sv_enabled = doc.NewElement("Enabled"); - xml_sv_enabled->SetText(smooth_velocities); - xml_smooth_velocities->InsertEndChild(xml_sv_enabled); - - tinyxml2::XMLElement* xml_sv_coeff = doc.NewElement("Coefficients"); - std::stringstream sv_coeff; - sv_coeff << velocity_coeff.format(eigen_format); - xml_sv_coeff->SetText(sv_coeff.str().c_str()); - xml_smooth_velocities->InsertEndChild(xml_sv_coeff); - xml_trajopt->InsertEndChild(xml_smooth_velocities); - - tinyxml2::XMLElement* xml_smooth_accelerations = doc.NewElement("SmoothAccelerations"); - - tinyxml2::XMLElement* xml_sa_enabled = doc.NewElement("Enabled"); - xml_sa_enabled->SetText(smooth_accelerations); - xml_smooth_accelerations->InsertEndChild(xml_sa_enabled); - - tinyxml2::XMLElement* xml_sa_coeff = doc.NewElement("Coefficients"); - std::stringstream sa_coeff; - sa_coeff << acceleration_coeff.format(eigen_format); - xml_sa_coeff->SetText(sa_coeff.str().c_str()); - xml_smooth_accelerations->InsertEndChild(xml_sa_coeff); - xml_trajopt->InsertEndChild(xml_smooth_accelerations); - - tinyxml2::XMLElement* xml_smooth_jerks = doc.NewElement("SmoothJerks"); - - tinyxml2::XMLElement* xml_sj_enabled = doc.NewElement("Enabled"); - xml_sj_enabled->SetText(smooth_jerks); - xml_smooth_jerks->InsertEndChild(xml_sj_enabled); - - tinyxml2::XMLElement* xml_sj_coeff = doc.NewElement("Coefficients"); - std::stringstream sj_coeff; - sj_coeff << jerk_coeff.format(eigen_format); - xml_sj_coeff->SetText(sj_coeff.str().c_str()); - xml_smooth_jerks->InsertEndChild(xml_sj_coeff); - xml_trajopt->InsertEndChild(xml_smooth_jerks); - - tinyxml2::XMLElement* xml_avoid_singularity = doc.NewElement("AvoidSingularity"); - - tinyxml2::XMLElement* xml_as_enabled = doc.NewElement("Enabled"); - xml_as_enabled->SetText(avoid_singularity); - xml_avoid_singularity->InsertEndChild(xml_as_enabled); - - tinyxml2::XMLElement* xml_as_coeff = doc.NewElement("Coefficient"); - xml_as_coeff->SetText(avoid_singularity_coeff); - xml_avoid_singularity->InsertEndChild(xml_as_coeff); - xml_trajopt->InsertEndChild(xml_avoid_singularity); - - tinyxml2::XMLElement* xml_long_valid_seg_frac = doc.NewElement("LongestValidSegmentFraction"); - xml_long_valid_seg_frac->SetText(longest_valid_segment_fraction); - xml_trajopt->InsertEndChild(xml_long_valid_seg_frac); - - tinyxml2::XMLElement* xml_long_valid_seg_len = doc.NewElement("LongestValidSegmentLength"); - xml_long_valid_seg_len->SetText(longest_valid_segment_length); - xml_trajopt->InsertEndChild(xml_long_valid_seg_len); - - xml_planner->InsertEndChild(xml_trajopt); - - return xml_planner; -} - -void TrajOptDefaultCompositeProfile::addCollisionCost(trajopt::ProblemConstructionInfo& pci, - int start_index, - int end_index, - const std::vector& fixed_indices) const -{ - // Calculate longest valid segment length - const Eigen::MatrixX2d& limits = pci.kin->getLimits().joint_limits; - double length = 0; - double extent = (limits.col(1) - limits.col(0)).norm(); - if (longest_valid_segment_fraction > 0 && longest_valid_segment_length > 0) - { - length = std::min(longest_valid_segment_fraction * extent, longest_valid_segment_length); - } - else if (longest_valid_segment_fraction > 0) - { - length = longest_valid_segment_fraction * extent; - } - else if (longest_valid_segment_length > 0) { - length = longest_valid_segment_length; - } - else - { - length = LONGEST_VALID_SEGMENT_FRACTION_DEFAULT * extent; + if (jerk_coeff.size() == 0) + term_infos.costs.push_back(createSmoothJerkTermInfo(start_index, end_index, static_cast(dof))); + else + term_infos.costs.push_back(createSmoothJerkTermInfo(start_index, end_index, jerk_coeff)); } - // Create a default collision term info - trajopt::TermInfo::Ptr ti = createCollisionTermInfo(start_index, - end_index, - collision_cost_config.safety_margin, - collision_cost_config.safety_margin_buffer, - collision_cost_config.type, - collision_cost_config.use_weighted_sum, - collision_cost_config.coeff, - contact_test_type, - length, - trajopt::TermType::TT_COST); - - // Update the term info with the (possibly) new start and end state indices for which to apply this cost - std::shared_ptr ct = std::static_pointer_cast(ti); - if (special_collision_cost) - { - for (auto& info : ct->info) - { - info = special_collision_cost; - } - } - ct->fixed_steps = fixed_indices; + if (avoid_singularity) + term_infos.costs.push_back(createAvoidSingularityTermInfo( + start_index, end_index, composite_manip_info.tcp_frame, avoid_singularity_coeff)); - pci.cost_infos.push_back(ct); + return term_infos; } -void TrajOptDefaultCompositeProfile::addCollisionConstraint(trajopt::ProblemConstructionInfo& pci, - int start_index, - int end_index, - const std::vector& fixed_indices) const +double TrajOptDefaultCompositeProfile::computeLongestValidSegmentLength(const Eigen::MatrixX2d joint_limits) const { // Calculate longest valid segment length - const Eigen::MatrixX2d& limits = pci.kin->getLimits().joint_limits; - double length = 0; - double extent = (limits.col(1) - limits.col(0)).norm(); + double extent = (joint_limits.col(1) - joint_limits.col(0)).norm(); if (longest_valid_segment_fraction > 0 && longest_valid_segment_length > 0) - { - length = std::min(longest_valid_segment_fraction * extent, longest_valid_segment_length); - } - else if (longest_valid_segment_fraction > 0) - { - length = longest_valid_segment_fraction * extent; - } - else if (longest_valid_segment_length > 0) - { - length = longest_valid_segment_length; - } - else - { - length = LONGEST_VALID_SEGMENT_FRACTION_DEFAULT * extent; - } + return std::min(longest_valid_segment_fraction * extent, longest_valid_segment_length); - // Create a default collision term info - trajopt::TermInfo::Ptr ti = createCollisionTermInfo(start_index, - end_index, - collision_constraint_config.safety_margin, - collision_constraint_config.safety_margin_buffer, - collision_constraint_config.type, - collision_constraint_config.use_weighted_sum, - collision_constraint_config.coeff, - contact_test_type, - length, - trajopt::TermType::TT_CNT); + if (longest_valid_segment_fraction > 0) + return longest_valid_segment_fraction * extent; - // Update the term info with the (possibly) new start and end state indices for which to apply this cost - std::shared_ptr ct = std::static_pointer_cast(ti); - if (special_collision_constraint) - { - for (auto& info : ct->info) - { - info = special_collision_constraint; - } - } - ct->fixed_steps = fixed_indices; - - pci.cnt_infos.push_back(ct); -} - -void TrajOptDefaultCompositeProfile::addVelocitySmoothing(trajopt::ProblemConstructionInfo& pci, - int start_index, - int end_index, - const std::vector& /*fixed_indices*/) const -{ - if (velocity_coeff.size() == 0) - pci.cost_infos.push_back( - createSmoothVelocityTermInfo(start_index, end_index, static_cast(pci.kin->numJoints()))); - else - pci.cost_infos.push_back(createSmoothVelocityTermInfo(start_index, end_index, velocity_coeff)); -} - -void TrajOptDefaultCompositeProfile::addAccelerationSmoothing(trajopt::ProblemConstructionInfo& pci, - int start_index, - int end_index, - const std::vector& /*fixed_indices*/) const -{ - if (acceleration_coeff.size() == 0) - pci.cost_infos.push_back( - createSmoothAccelerationTermInfo(start_index, end_index, static_cast(pci.kin->numJoints()))); - else - pci.cost_infos.push_back(createSmoothAccelerationTermInfo(start_index, end_index, acceleration_coeff)); -} + if (longest_valid_segment_length > 0) + return longest_valid_segment_length; -void TrajOptDefaultCompositeProfile::addJerkSmoothing(trajopt::ProblemConstructionInfo& pci, - int start_index, - int end_index, - const std::vector& /*fixed_indices*/) const -{ - if (jerk_coeff.size() == 0) - pci.cost_infos.push_back(createSmoothJerkTermInfo(start_index, end_index, static_cast(pci.kin->numJoints()))); - else - pci.cost_infos.push_back(createSmoothJerkTermInfo(start_index, end_index, jerk_coeff)); -} - -void TrajOptDefaultCompositeProfile::addAvoidSingularity(trajopt::ProblemConstructionInfo& pci, - int start_index, - int end_index, - const std::string& link, - const std::vector& /*fixed_indices*/) const -{ - trajopt::TermInfo::Ptr ti = createAvoidSingularityTermInfo(start_index, end_index, link, avoid_singularity_coeff); - pci.cost_infos.push_back(ti); + return LONGEST_VALID_SEGMENT_FRACTION_DEFAULT * extent; } template diff --git a/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp b/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp index 63b96df67f6..582203a19b9 100644 --- a/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp +++ b/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp @@ -36,282 +36,231 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include +#include #include namespace tesseract_planning { -TrajOptDefaultPlanProfile::TrajOptDefaultPlanProfile(const tinyxml2::XMLElement& xml_element) +TrajOptWaypointInfo TrajOptDefaultPlanProfile::createCostAndConstraints( + const MoveInstructionPoly& move_instruction, + const tesseract_common::ManipulatorInfo& composite_manip_info, + const std::shared_ptr& env, + const std::vector& active_links, + int index) const { - const tinyxml2::XMLElement* cartesian_cost_element = xml_element.FirstChildElement("CartesianCostConfig"); - const tinyxml2::XMLElement* cartesian_constraint_element = xml_element.FirstChildElement("CartesianConstraintConfig"); - const tinyxml2::XMLElement* joint_cost_element = xml_element.FirstChildElement("JointCostConfig"); - const tinyxml2::XMLElement* joint_constraint_element = xml_element.FirstChildElement("JointConstraintConfig"); - const tinyxml2::XMLElement* cnt_error_fn_element = xml_element.FirstChildElement("ConstraintErrorFunctions"); + assert(!(composite_manip_info.empty() && move_instruction.getManipulatorInfo().empty())); + tesseract_common::ManipulatorInfo mi = composite_manip_info.getCombined(move_instruction.getManipulatorInfo()); + std::vector joint_names = env->getGroupJointNames(mi.manipulator); + assert(checkJointPositionFormat(joint_names, move_instruction.getWaypoint())); - int status{ tinyxml2::XMLError::XML_SUCCESS }; - - if (cartesian_cost_element != nullptr) - { - const tinyxml2::XMLElement* cartesian_waypoint_config = cartesian_cost_element->FirstChildElement("CartesianWaypoin" - "tConfig"); - cartesian_cost_config = CartesianWaypointConfig(*cartesian_waypoint_config); - } - - if (cartesian_constraint_element != nullptr) + TrajOptWaypointInfo info; + if (move_instruction.getWaypoint().isCartesianWaypoint()) { - const tinyxml2::XMLElement* cartesian_waypoint_config = cartesian_constraint_element->FirstChildElement("CartesianW" - "aypointCon" - "fig"); - cartesian_constraint_config = CartesianWaypointConfig(*cartesian_waypoint_config); - } + if (mi.empty()) + throw std::runtime_error("TrajOptPlanProfile, manipulator info is empty!"); - if (joint_cost_element != nullptr) - { - const tinyxml2::XMLElement* cartesian_waypoint_config = joint_cost_element->FirstChildElement("JointWaypointConfi" - "g"); - joint_cost_config = JointWaypointConfig(*cartesian_waypoint_config); - } + const auto& cwp = move_instruction.getWaypoint().as(); - if (joint_constraint_element != nullptr) - { - const tinyxml2::XMLElement* cartesian_waypoint_config = joint_constraint_element->FirstChildElement("JointWaypointC" - "onfig"); - joint_constraint_config = JointWaypointConfig(*cartesian_waypoint_config); - } - - if (cnt_error_fn_element != nullptr) - { - std::string error_fn_name; - status = tesseract_common::QueryStringAttribute(cnt_error_fn_element, "type", error_fn_name); - if (status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("TrajOptPlanProfile: Error parsing ConstraintErrorFunctions plugin attribute."); - - // TODO: Implement plugin capabilities - } -} -void TrajOptDefaultPlanProfile::apply(trajopt::ProblemConstructionInfo& pci, - const CartesianWaypointPoly& cartesian_waypoint, - const MoveInstructionPoly& parent_instruction, - const tesseract_common::ManipulatorInfo& manip_info, - const std::vector& active_links, - int index) const -{ - assert(!(manip_info.empty() && parent_instruction.getManipulatorInfo().empty())); - tesseract_common::ManipulatorInfo mi = manip_info.getCombined(parent_instruction.getManipulatorInfo()); + Eigen::Isometry3d tcp_offset = env->findTCPOffset(mi); - if (mi.manipulator.empty()) - throw std::runtime_error("TrajOptPlanProfile, manipulator is empty!"); + /* Check if this cartesian waypoint is dynamic + * (i.e. defined relative to a frame that will move with the kinematic chain) + */ + bool is_active_tcp_frame = + (std::find(active_links.begin(), active_links.end(), mi.tcp_frame) != active_links.end()); + bool is_static_working_frame = + (std::find(active_links.begin(), active_links.end(), mi.working_frame) == active_links.end()); - if (mi.tcp_frame.empty()) - throw std::runtime_error("TrajOptPlanProfile, tcp_frame is empty!"); - - if (mi.working_frame.empty()) - throw std::runtime_error("TrajOptPlanProfile, working_frame is empty!"); - - Eigen::Isometry3d tcp_offset = pci.env->findTCPOffset(mi); - - /* Check if this cartesian waypoint is dynamic - * (i.e. defined relative to a frame that will move with the kinematic chain) - */ - bool is_active_tcp_frame = (std::find(active_links.begin(), active_links.end(), mi.tcp_frame) != active_links.end()); - bool is_static_working_frame = - (std::find(active_links.begin(), active_links.end(), mi.working_frame) == active_links.end()); - - // Override cost tolerances if the profile specifies that they should be overrided. - Eigen::VectorXd lower_tolerance_cost = cartesian_waypoint.getLowerTolerance(); - Eigen::VectorXd upper_tolerance_cost = cartesian_waypoint.getUpperTolerance(); - if (cartesian_cost_config.use_tolerance_override) - { - lower_tolerance_cost = cartesian_cost_config.lower_tolerance; - upper_tolerance_cost = cartesian_cost_config.upper_tolerance; - } - Eigen::VectorXd lower_tolerance_cnt = cartesian_waypoint.getLowerTolerance(); - Eigen::VectorXd upper_tolerance_cnt = cartesian_waypoint.getUpperTolerance(); - if (cartesian_constraint_config.use_tolerance_override) - { - lower_tolerance_cnt = cartesian_constraint_config.lower_tolerance; - upper_tolerance_cnt = cartesian_constraint_config.upper_tolerance; - } - - if ((is_static_working_frame && is_active_tcp_frame) || (!is_active_tcp_frame && !is_static_working_frame)) - { - if (cartesian_cost_config.enabled) + // Override cost tolerances if the profile specifies that they should be overrided. + Eigen::VectorXd lower_tolerance_cost = cwp.getLowerTolerance(); + Eigen::VectorXd upper_tolerance_cost = cwp.getUpperTolerance(); + if (cartesian_cost_config.use_tolerance_override) { - trajopt::TermInfo::Ptr ti = createCartesianWaypointTermInfo(index, - mi.working_frame, - cartesian_waypoint.getTransform(), - mi.tcp_frame, - tcp_offset, - cartesian_cost_config.coeff, - trajopt::TermType::TT_COST, - lower_tolerance_cost, - upper_tolerance_cost); - pci.cost_infos.push_back(ti); + lower_tolerance_cost = cartesian_cost_config.lower_tolerance; + upper_tolerance_cost = cartesian_cost_config.upper_tolerance; } - if (cartesian_constraint_config.enabled) + Eigen::VectorXd lower_tolerance_cnt = cwp.getLowerTolerance(); + Eigen::VectorXd upper_tolerance_cnt = cwp.getUpperTolerance(); + if (cartesian_constraint_config.use_tolerance_override) { - trajopt::TermInfo::Ptr ti = createCartesianWaypointTermInfo(index, - mi.working_frame, - cartesian_waypoint.getTransform(), - mi.tcp_frame, - tcp_offset, - cartesian_constraint_config.coeff, - trajopt::TermType::TT_CNT, - lower_tolerance_cnt, - upper_tolerance_cnt); - pci.cnt_infos.push_back(ti); + lower_tolerance_cnt = cartesian_constraint_config.lower_tolerance; + upper_tolerance_cnt = cartesian_constraint_config.upper_tolerance; } - } - else if (!is_static_working_frame && is_active_tcp_frame) - { - if (cartesian_cost_config.enabled) + + if ((is_static_working_frame && is_active_tcp_frame) || (!is_active_tcp_frame && !is_static_working_frame)) { - trajopt::TermInfo::Ptr ti = createDynamicCartesianWaypointTermInfo(index, - mi.working_frame, - cartesian_waypoint.getTransform(), - mi.tcp_frame, - tcp_offset, - cartesian_cost_config.coeff, - trajopt::TermType::TT_COST, - lower_tolerance_cost, - upper_tolerance_cost); - pci.cost_infos.push_back(ti); + if (cartesian_cost_config.enabled) + { + trajopt::TermInfo::Ptr ti = createCartesianWaypointTermInfo(index, + mi.working_frame, + cwp.getTransform(), + mi.tcp_frame, + tcp_offset, + cartesian_cost_config.coeff, + trajopt::TermType::TT_COST, + lower_tolerance_cost, + upper_tolerance_cost); + info.term_infos.costs.push_back(ti); + } + if (cartesian_constraint_config.enabled) + { + trajopt::TermInfo::Ptr ti = createCartesianWaypointTermInfo(index, + mi.working_frame, + cwp.getTransform(), + mi.tcp_frame, + tcp_offset, + cartesian_constraint_config.coeff, + trajopt::TermType::TT_CNT, + lower_tolerance_cnt, + upper_tolerance_cnt); + info.term_infos.constraints.push_back(ti); + } } - if (cartesian_constraint_config.enabled) + else if (!is_static_working_frame && is_active_tcp_frame) { - trajopt::TermInfo::Ptr ti = createDynamicCartesianWaypointTermInfo(index, - mi.working_frame, - cartesian_waypoint.getTransform(), - mi.tcp_frame, - tcp_offset, - cartesian_constraint_config.coeff, - trajopt::TermType::TT_CNT, - lower_tolerance_cnt, - upper_tolerance_cnt); - pci.cnt_infos.push_back(ti); + if (cartesian_cost_config.enabled) + { + trajopt::TermInfo::Ptr ti = createDynamicCartesianWaypointTermInfo(index, + mi.working_frame, + cwp.getTransform(), + mi.tcp_frame, + tcp_offset, + cartesian_cost_config.coeff, + trajopt::TermType::TT_COST, + lower_tolerance_cost, + upper_tolerance_cost); + info.term_infos.costs.push_back(ti); + } + if (cartesian_constraint_config.enabled) + { + trajopt::TermInfo::Ptr ti = createDynamicCartesianWaypointTermInfo(index, + mi.working_frame, + cwp.getTransform(), + mi.tcp_frame, + tcp_offset, + cartesian_constraint_config.coeff, + trajopt::TermType::TT_CNT, + lower_tolerance_cnt, + upper_tolerance_cnt); + info.term_infos.constraints.push_back(ti); + } + } + else + { + throw std::runtime_error("TrajOpt, both tcp_frame and working_frame are both static!"); } - } - else - { - throw std::runtime_error("TrajOpt, both tcp_frame and working_frame are both static!"); - } - // Add constraints from error functions if available. - addConstraintErrorFunctions(pci, index); -} + if (cwp.hasSeed()) + info.seed = cwp.getSeed().position; + else + info.seed = env->getCurrentJointValues(joint_names); -void TrajOptDefaultPlanProfile::apply(trajopt::ProblemConstructionInfo& pci, - const JointWaypointPoly& joint_waypoint, - const MoveInstructionPoly& /*parent_instruction*/, - const tesseract_common::ManipulatorInfo& /*manip_info*/, - const std::vector& /*active_links*/, - int index) const -{ - // Override cost tolerances if the profile specifies that they should be overrided. - Eigen::VectorXd lower_tolerance_cost = joint_waypoint.getLowerTolerance(); - Eigen::VectorXd upper_tolerance_cost = joint_waypoint.getUpperTolerance(); - if (joint_cost_config.use_tolerance_override) - { - lower_tolerance_cost = joint_cost_config.lower_tolerance; - upper_tolerance_cost = joint_cost_config.upper_tolerance; + /** @todo If fixed cartesian and not term_type cost add as fixed */ + info.fixed = false; } - Eigen::VectorXd lower_tolerance_cnt = joint_waypoint.getLowerTolerance(); - Eigen::VectorXd upper_tolerance_cnt = joint_waypoint.getUpperTolerance(); - if (joint_constraint_config.use_tolerance_override) + else if (move_instruction.getWaypoint().isJointWaypoint() || move_instruction.getWaypoint().isStateWaypoint()) { - lower_tolerance_cnt = joint_constraint_config.lower_tolerance; - upper_tolerance_cnt = joint_constraint_config.upper_tolerance; - } - if (joint_waypoint.isToleranced()) - { - if (joint_cost_config.enabled) + JointWaypointPoly jwp; + if (move_instruction.getWaypoint().isStateWaypoint()) { - trajopt::TermInfo::Ptr ti = createTolerancedJointWaypointTermInfo(joint_waypoint.getPosition(), - lower_tolerance_cost, - upper_tolerance_cost, - index, - joint_cost_config.coeff, - trajopt::TermType::TT_COST); - pci.cost_infos.push_back(ti); + const auto& swp = move_instruction.getWaypoint().as(); + JointWaypointPoly jwp = move_instruction.createJointWaypoint(); + jwp.setNames(swp.getNames()); + jwp.setPosition(swp.getPosition()); + info.fixed = true; } - if (joint_constraint_config.enabled) + else { - trajopt::TermInfo::Ptr ti = createTolerancedJointWaypointTermInfo(joint_waypoint.getPosition(), - lower_tolerance_cnt, - upper_tolerance_cnt, - index, - joint_constraint_config.coeff, - trajopt::TermType::TT_CNT); - pci.cnt_infos.push_back(ti); + jwp = move_instruction.getWaypoint().as(); + if (jwp.isConstrained()) + { + // Add to fixed indices + if (!jwp.isToleranced()) /** @todo Should not make fixed if term_type is cost */ + info.fixed = true; + } } - } - else - { - if (joint_cost_config.enabled) - { - trajopt::TermInfo::Ptr ti = createJointWaypointTermInfo( - joint_waypoint.getPosition(), index, joint_cost_config.coeff, trajopt::TermType::TT_COST); - pci.cost_infos.push_back(ti); - } - if (joint_constraint_config.enabled) + + // Set seed state + info.seed = jwp.getPosition(); + + if (jwp.isConstrained()) { - trajopt::TermInfo::Ptr ti = createJointWaypointTermInfo( - joint_waypoint.getPosition(), index, joint_constraint_config.coeff, trajopt::TermType::TT_CNT); - pci.cnt_infos.push_back(ti); + // Override cost tolerances if the profile specifies that they should be overrided. + Eigen::VectorXd lower_tolerance_cost = jwp.getLowerTolerance(); + Eigen::VectorXd upper_tolerance_cost = jwp.getUpperTolerance(); + if (joint_cost_config.use_tolerance_override) + { + lower_tolerance_cost = joint_cost_config.lower_tolerance; + upper_tolerance_cost = joint_cost_config.upper_tolerance; + } + Eigen::VectorXd lower_tolerance_cnt = jwp.getLowerTolerance(); + Eigen::VectorXd upper_tolerance_cnt = jwp.getUpperTolerance(); + if (joint_constraint_config.use_tolerance_override) + { + lower_tolerance_cnt = joint_constraint_config.lower_tolerance; + upper_tolerance_cnt = joint_constraint_config.upper_tolerance; + } + if (jwp.isToleranced()) + { + if (joint_cost_config.enabled) + { + trajopt::TermInfo::Ptr ti = createTolerancedJointWaypointTermInfo(jwp.getPosition(), + lower_tolerance_cost, + upper_tolerance_cost, + index, + joint_cost_config.coeff, + trajopt::TermType::TT_COST); + info.term_infos.costs.push_back(ti); + } + if (joint_constraint_config.enabled) + { + trajopt::TermInfo::Ptr ti = createTolerancedJointWaypointTermInfo(jwp.getPosition(), + lower_tolerance_cnt, + upper_tolerance_cnt, + index, + joint_constraint_config.coeff, + trajopt::TermType::TT_CNT); + info.term_infos.constraints.push_back(ti); + } + } + else + { + if (joint_cost_config.enabled) + { + trajopt::TermInfo::Ptr ti = createJointWaypointTermInfo( + jwp.getPosition(), index, joint_cost_config.coeff, trajopt::TermType::TT_COST); + info.term_infos.costs.push_back(ti); + } + if (joint_constraint_config.enabled) + { + trajopt::TermInfo::Ptr ti = createJointWaypointTermInfo( + jwp.getPosition(), index, joint_constraint_config.coeff, trajopt::TermType::TT_CNT); + info.term_infos.constraints.push_back(ti); + } + } } } - // Add constraints from error functions if available. - addConstraintErrorFunctions(pci, index); + return info; } -void TrajOptDefaultPlanProfile::addConstraintErrorFunctions(trajopt::ProblemConstructionInfo& pci, int index) const +std::shared_ptr +TrajOptDefaultPlanProfile::createConstraintFromErrorFunction(sco::VectorOfVector::func error_function, + sco::MatrixOfVector::func jacobian_function, + sco::ConstraintType type, + const Eigen::VectorXd& coeff, + int index) { - for (const auto& c : constraint_error_functions) - { - trajopt::TermInfo::Ptr ti = - createUserDefinedTermInfo(index, index, std::get<0>(c), std::get<1>(c), trajopt::TermType::TT_CNT); - - // Update the term info with the (possibly) new start and end state indices for which to apply this cost - std::shared_ptr ef = std::static_pointer_cast(ti); - ef->constraint_type = std::get<2>(c); - ef->coeff = std::get<3>(c); - - pci.cnt_infos.push_back(ef); - } -} - -tinyxml2::XMLElement* TrajOptDefaultPlanProfile::toXML(tinyxml2::XMLDocument& doc) const -{ - tinyxml2::XMLElement* xml_planner = doc.NewElement("Planner"); - xml_planner->SetAttribute("type", std::to_string(1).c_str()); - - tinyxml2::XMLElement* xml_trajopt = doc.NewElement("TrajOptDefaultPlanProfile"); - - tinyxml2::XMLElement* xml_cart_cost_parent = doc.NewElement("CartesianCostConfig"); - tinyxml2::XMLElement* xml_cart_cost = cartesian_cost_config.toXML(doc); - xml_cart_cost_parent->InsertEndChild(xml_cart_cost); - xml_trajopt->InsertEndChild(xml_cart_cost_parent); - - tinyxml2::XMLElement* xml_cart_cnt_parent = doc.NewElement("CartesianConstraintConfig"); - tinyxml2::XMLElement* xml_cart_cnt = cartesian_constraint_config.toXML(doc); - xml_cart_cnt_parent->InsertEndChild(xml_cart_cnt); - xml_trajopt->InsertEndChild(xml_cart_cnt_parent); - - tinyxml2::XMLElement* xml_joint_cost_parent = doc.NewElement("JointCostConfig"); - tinyxml2::XMLElement* xml_joint_cost = joint_cost_config.toXML(doc); - xml_joint_cost_parent->InsertEndChild(xml_joint_cost); - xml_trajopt->InsertEndChild(xml_joint_cost_parent); - - tinyxml2::XMLElement* xml_joint_cnt_parent = doc.NewElement("JointConstraintConfig"); - tinyxml2::XMLElement* xml_joint_cnt = joint_constraint_config.toXML(doc); - xml_joint_cnt_parent->InsertEndChild(xml_joint_cnt); - xml_trajopt->InsertEndChild(xml_joint_cnt_parent); - - xml_planner->InsertEndChild(xml_trajopt); - - return xml_planner; + trajopt::TermInfo::Ptr ti = createUserDefinedTermInfo( + index, index, std::move(error_function), std::move(jacobian_function), trajopt::TermType::TT_CNT); + + // Update the term info with the (possibly) new start and end state indices for which to apply this cost + std::shared_ptr ef = std::static_pointer_cast(ti); + ef->constraint_type = type; + ef->coeff = coeff; + return ef; } template @@ -322,7 +271,6 @@ void TrajOptDefaultPlanProfile::serialize(Archive& ar, const unsigned int /*vers ar& BOOST_SERIALIZATION_NVP(cartesian_constraint_config); ar& BOOST_SERIALIZATION_NVP(joint_cost_config); ar& BOOST_SERIALIZATION_NVP(joint_constraint_config); - // ar& BOOST_SERIALIZATION_NVP(constraint_error_functions); /** @todo FIX */ } } // namespace tesseract_planning diff --git a/tesseract_motion_planners/trajopt/src/profile/trajopt_default_solver_profile.cpp b/tesseract_motion_planners/trajopt/src/profile/trajopt_default_solver_profile.cpp deleted file mode 100644 index b38cbbdccd1..00000000000 --- a/tesseract_motion_planners/trajopt/src/profile/trajopt_default_solver_profile.cpp +++ /dev/null @@ -1,64 +0,0 @@ -/** - * @file trajopt_default_solver_profile.cpp - * @brief - * - * @author Levi Armstrong - * @date December 13, 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. - */ - -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include - -namespace tesseract_planning -{ -void TrajOptDefaultSolverProfile::apply(trajopt::ProblemConstructionInfo& pci) const -{ - pci.basic_info.convex_solver = convex_solver; - pci.basic_info.convex_solver_config = convex_solver_config; - pci.opt_info = opt_info; - pci.callbacks = callbacks; -} - -tinyxml2::XMLElement* TrajOptDefaultSolverProfile::toXML(tinyxml2::XMLDocument& /*doc*/) const { return nullptr; } - -template -void TrajOptDefaultSolverProfile::serialize(Archive& ar, const unsigned int /*version*/) -{ - ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TrajOptSolverProfile); - /** @todo FIX */ - // ar& BOOST_SERIALIZATION_NVP(convex_solver); - // ar& BOOST_SERIALIZATION_NVP(convex_solver_config); - // ar& BOOST_SERIALIZATION_NVP(opt_info); - // ar& BOOST_SERIALIZATION_NVP(callbacks); -} - -} // namespace tesseract_planning - -#include -TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TrajOptDefaultSolverProfile) -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TrajOptDefaultSolverProfile) diff --git a/tesseract_motion_planners/trajopt/src/profile/trajopt_osqp_solver_profile.cpp b/tesseract_motion_planners/trajopt/src/profile/trajopt_osqp_solver_profile.cpp new file mode 100644 index 00000000000..24e1a57c378 --- /dev/null +++ b/tesseract_motion_planners/trajopt/src/profile/trajopt_osqp_solver_profile.cpp @@ -0,0 +1,89 @@ +/** + * @file trajopt_osqp_solver_profile.cpp + * @brief + * + * @author Levi Armstrong + * @date December 13, 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. + */ + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include + +namespace boost::serialization +{ +template +void serialize(Archive& ar, OSQPSettings& settings, const unsigned int /*version*/) +{ + ar& boost::serialization::make_nvp("rho", settings.rho); + ar& boost::serialization::make_nvp("sigma", settings.sigma); + ar& boost::serialization::make_nvp("scaling", settings.scaling); + ar& boost::serialization::make_nvp("adaptive_rho", settings.adaptive_rho); + ar& boost::serialization::make_nvp("adaptive_rho_interval", settings.adaptive_rho_interval); + ar& boost::serialization::make_nvp("adaptive_rho_tolerance", settings.adaptive_rho_tolerance); + ar& boost::serialization::make_nvp("adaptive_rho_fraction", settings.adaptive_rho_fraction); + ar& boost::serialization::make_nvp("max_iter", settings.max_iter); + ar& boost::serialization::make_nvp("eps_abs", settings.eps_abs); + ar& boost::serialization::make_nvp("eps_rel", settings.eps_rel); + ar& boost::serialization::make_nvp("eps_prim_inf", settings.eps_prim_inf); + ar& boost::serialization::make_nvp("eps_dual_inf", settings.eps_dual_inf); + ar& boost::serialization::make_nvp("alpha", settings.alpha); + ar& boost::serialization::make_nvp("linsys_solver", settings.linsys_solver); + ar& boost::serialization::make_nvp("delta", settings.delta); + ar& boost::serialization::make_nvp("polish", settings.polish); + ar& boost::serialization::make_nvp("polish_refine_iter", settings.polish_refine_iter); + ar& boost::serialization::make_nvp("verbose", settings.verbose); + ar& boost::serialization::make_nvp("scaled_termination", settings.scaled_termination); + ar& boost::serialization::make_nvp("check_termination", settings.check_termination); + ar& boost::serialization::make_nvp("warm_start", settings.warm_start); + ar& boost::serialization::make_nvp("time_limit", settings.time_limit); +} +} // namespace boost::serialization + +namespace tesseract_planning +{ +sco::ModelType TrajOptOSQPSolverProfile::getSolverType() const { return sco::ModelType::OSQP; } + +std::unique_ptr TrajOptOSQPSolverProfile::createSolverConfig() const +{ + auto config = std::make_unique(); + config->settings = settings; + return config; +} + +template +void TrajOptOSQPSolverProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TrajOptSolverProfile); + ar& BOOST_SERIALIZATION_NVP(settings); +} + +} // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_FREE_ARCHIVES_INSTANTIATE(OSQPSettings) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TrajOptOSQPSolverProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TrajOptOSQPSolverProfile) diff --git a/tesseract_motion_planners/trajopt/src/profile/trajopt_profile.cpp b/tesseract_motion_planners/trajopt/src/profile/trajopt_profile.cpp index 190c23d1e47..5a9acb1af6b 100644 --- a/tesseract_motion_planners/trajopt/src/profile/trajopt_profile.cpp +++ b/tesseract_motion_planners/trajopt/src/profile/trajopt_profile.cpp @@ -28,6 +28,32 @@ #include #include +namespace boost::serialization +{ +template +void serialize(Archive& ar, sco::BasicTrustRegionSQPParameters& params, const unsigned int /*version*/) +{ + ar& boost::serialization::make_nvp("improve_ratio_threshold", params.improve_ratio_threshold); + ar& boost::serialization::make_nvp("min_trust_box_size", params.min_trust_box_size); + ar& boost::serialization::make_nvp("min_approx_improve", params.min_approx_improve); + ar& boost::serialization::make_nvp("min_approx_improve_frac", params.min_approx_improve_frac); + ar& boost::serialization::make_nvp("max_iter", params.max_iter); + ar& boost::serialization::make_nvp("trust_shrink_ratio", params.trust_shrink_ratio); + ar& boost::serialization::make_nvp("trust_expand_ratio", params.trust_expand_ratio); + ar& boost::serialization::make_nvp("cnt_tolerance", params.cnt_tolerance); + ar& boost::serialization::make_nvp("max_merit_coeff_increases", params.max_merit_coeff_increases); + ar& boost::serialization::make_nvp("max_qp_solver_failures", params.max_qp_solver_failures); + ar& boost::serialization::make_nvp("merit_coeff_increase_ratio", params.merit_coeff_increase_ratio); + ar& boost::serialization::make_nvp("max_time", params.max_time); + ar& boost::serialization::make_nvp("initial_merit_error_coeff", params.initial_merit_error_coeff); + ar& boost::serialization::make_nvp("inflate_constraints_individually", params.inflate_constraints_individually); + ar& boost::serialization::make_nvp("trust_box_size", params.trust_box_size); + ar& boost::serialization::make_nvp("log_results", params.log_results); + ar& boost::serialization::make_nvp("log_dir", params.log_dir); + ar& boost::serialization::make_nvp("num_threads", params.num_threads); +} +} // namespace boost::serialization + namespace tesseract_planning { TrajOptPlanProfile::TrajOptPlanProfile() : Profile(TrajOptPlanProfile::getStaticKey()) {} @@ -57,15 +83,21 @@ TrajOptSolverProfile::TrajOptSolverProfile() : Profile(TrajOptSolverProfile::get std::size_t TrajOptSolverProfile::getStaticKey() { return std::type_index(typeid(TrajOptSolverProfile)).hash_code(); } +sco::BasicTrustRegionSQPParameters TrajOptSolverProfile::createOptimizationParameters() const { return opt_params; } + +std::vector TrajOptSolverProfile::createOptimizationCallbacks() const { return {}; } + template void TrajOptSolverProfile::serialize(Archive& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); + ar& BOOST_SERIALIZATION_NVP(opt_params); } } // namespace tesseract_planning #include +TESSERACT_SERIALIZE_FREE_ARCHIVES_INSTANTIATE(sco::BasicTrustRegionSQPParameters) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TrajOptPlanProfile) BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TrajOptPlanProfile) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TrajOptCompositeProfile) diff --git a/tesseract_motion_planners/trajopt/src/serialize.cpp b/tesseract_motion_planners/trajopt/src/serialize.cpp deleted file mode 100644 index 5285e751807..00000000000 --- a/tesseract_motion_planners/trajopt/src/serialize.cpp +++ /dev/null @@ -1,120 +0,0 @@ -/** - * @file serialize.cpp - * @brief - * - * @author Tyler Marr - * @date August 20, 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. - */ - -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -#include -#include -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include -#include - -static const std::array VERSION{ { 1, 0, 0 } }; -static const Eigen::IOFormat eigen_format(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", " "); - -namespace tesseract_planning -{ -std::shared_ptr toXMLDocument(const TrajOptPlanProfile& plan_profile) -{ - auto doc = std::make_shared(); - tinyxml2::XMLElement* xml_root = doc->NewElement("Profile"); - xml_root->SetAttribute("name", "TrajoptDefaultPlanProfile"); - xml_root->SetAttribute( - "version", - (std::to_string(VERSION[0]) + "." + std::to_string(VERSION[1]) + "." + std::to_string(VERSION[2])).c_str()); - - tinyxml2::XMLElement* xml_plan_profile = plan_profile.toXML(*doc); - xml_root->InsertEndChild(xml_plan_profile); - doc->InsertFirstChild(xml_root); - - return doc; -} - -bool toXMLFile(const TrajOptPlanProfile& plan_profile, const std::string& file_path) -{ - std::shared_ptr doc = toXMLDocument(plan_profile); - tinyxml2::XMLError status = doc->SaveFile(file_path.c_str()); - if (status != tinyxml2::XMLError::XML_SUCCESS) - { - CONSOLE_BRIDGE_logError("Failed to save Plan Profile XML File: %s", file_path.c_str()); - return false; - } - - return true; -} - -std::string toXMLString(const TrajOptPlanProfile& plan_profile) -{ - std::shared_ptr doc = toXMLDocument(plan_profile); - tinyxml2::XMLPrinter printer; - doc->Print(&printer); - return std::string{ printer.CStr() }; -} - -std::shared_ptr toXMLDocument(const TrajOptCompositeProfile& composite_profile) -{ - auto doc = std::make_shared(); - tinyxml2::XMLElement* xml_root = doc->NewElement("Profile"); - xml_root->SetAttribute("name", "TrajoptDefaultCompositeProfile"); - xml_root->SetAttribute( - "version", - (std::to_string(VERSION[0]) + "." + std::to_string(VERSION[1]) + "." + std::to_string(VERSION[2])).c_str()); - - tinyxml2::XMLElement* xml_composite_profile = composite_profile.toXML(*doc); - xml_root->InsertEndChild(xml_composite_profile); - doc->InsertFirstChild(xml_root); - - return doc; -} - -bool toXMLFile(const TrajOptCompositeProfile& composite_profile, const std::string& file_path) -{ - std::shared_ptr doc = toXMLDocument(composite_profile); - tinyxml2::XMLError status = doc->SaveFile(file_path.c_str()); - if (status != tinyxml2::XMLError::XML_SUCCESS) - { - CONSOLE_BRIDGE_logError("Failed to save Composite Profile XML File: %s", file_path.c_str()); - return false; - } - - return true; -} - -std::string toXMLString(const TrajOptCompositeProfile& composite_profile) -{ - std::shared_ptr doc = toXMLDocument(composite_profile); - tinyxml2::XMLPrinter printer; - doc->Print(&printer); - return std::string{ printer.CStr() }; -} - -} // namespace tesseract_planning diff --git a/tesseract_motion_planners/trajopt/src/trajopt_collision_config.cpp b/tesseract_motion_planners/trajopt/src/trajopt_collision_config.cpp index 8a12453479a..0542ef1b517 100644 --- a/tesseract_motion_planners/trajopt/src/trajopt_collision_config.cpp +++ b/tesseract_motion_planners/trajopt/src/trajopt_collision_config.cpp @@ -28,7 +28,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include -#include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP @@ -37,110 +36,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_planning { -CollisionCostConfig::CollisionCostConfig(const tinyxml2::XMLElement& xml_element) -{ - const tinyxml2::XMLElement* enabled_element = xml_element.FirstChildElement("Enabled"); - const tinyxml2::XMLElement* use_weighted_sum_element = xml_element.FirstChildElement("UseWeightedSum"); - const tinyxml2::XMLElement* type_element = xml_element.FirstChildElement("CollisionEvaluator"); - const tinyxml2::XMLElement* safety_margin_element = xml_element.FirstChildElement("SafetyMargin"); - const tinyxml2::XMLElement* safety_margin_buffer_element = xml_element.FirstChildElement("SafetyMarginBuffer"); - const tinyxml2::XMLElement* coeff_element = xml_element.FirstChildElement("Coefficient"); - - if (enabled_element == nullptr) - throw std::runtime_error("CollisionCostConfig: Must have Enabled element."); - - int status = enabled_element->QueryBoolText(&enabled); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("CollisionCostConfig: Error parsing Enabled string"); - - if (use_weighted_sum_element != nullptr) - { - status = use_weighted_sum_element->QueryBoolText(&use_weighted_sum); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("CollisionCostConfig: Error parsing UseWeightedSum string"); - } - - if (type_element != nullptr) - { - auto coll_type = static_cast(trajopt::CollisionEvaluatorType::CAST_CONTINUOUS); - status = type_element->QueryIntAttribute("type", &coll_type); - if (status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("CollisionCostConfig: Error parsing CollisionEvaluator type attribute."); - - type = static_cast(coll_type); - } - - if (safety_margin_element != nullptr) - { - std::string safety_margin_string; - status = tesseract_common::QueryStringText(safety_margin_element, safety_margin_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("CollisionCostConfig: Error parsing SafetyMargin string"); - - if (!tesseract_common::isNumeric(safety_margin_string)) - throw std::runtime_error("CollisionCostConfig: SafetyMargin is not a numeric values."); - - tesseract_common::toNumeric(safety_margin_string, safety_margin); - } - - if (safety_margin_buffer_element != nullptr) - { - std::string safety_margin_buffer_string; - status = tesseract_common::QueryStringText(safety_margin_buffer_element, safety_margin_buffer_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("CollisionCostConfig: Error parsing SafetyMarginBuffer string"); - - if (!tesseract_common::isNumeric(safety_margin_buffer_string)) - throw std::runtime_error("CollisionCostConfig: SafetyMarginBuffer is not a numeric values."); - - tesseract_common::toNumeric(safety_margin_buffer_string, safety_margin_buffer); - } - - if (coeff_element != nullptr) - { - std::string coeff_string; - status = tesseract_common::QueryStringText(coeff_element, coeff_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("CollisionCostConfig: Error parsing Coefficient string"); - - if (!tesseract_common::isNumeric(coeff_string)) - throw std::runtime_error("CollisionCostConfig: Coefficient is not a numeric values."); - - tesseract_common::toNumeric(coeff_string, coeff); - } -} - -tinyxml2::XMLElement* CollisionCostConfig::toXML(tinyxml2::XMLDocument& doc) const -{ - tinyxml2::XMLElement* xml_coll_cost_config = doc.NewElement("CollisionCostConfig"); - - tinyxml2::XMLElement* xml_enabled = doc.NewElement("Enabled"); - xml_enabled->SetText(enabled); - xml_coll_cost_config->InsertEndChild(xml_enabled); - - tinyxml2::XMLElement* xml_use_weighted_sum = doc.NewElement("UseWeightedSum"); - xml_use_weighted_sum->SetText(use_weighted_sum); - xml_coll_cost_config->InsertEndChild(xml_use_weighted_sum); - - tinyxml2::XMLElement* xml_type = doc.NewElement("CollisionEvaluator"); - xml_type->SetAttribute("type", std::to_string(static_cast(type)).c_str()); - xml_coll_cost_config->InsertEndChild(xml_type); - - tinyxml2::XMLElement* xml_safety_margin = doc.NewElement("SafetyMargin"); - xml_safety_margin->SetText(safety_margin); - xml_coll_cost_config->InsertEndChild(xml_safety_margin); - - tinyxml2::XMLElement* xml_safety_margin_buffer = doc.NewElement("SafetyMarginBuffer"); - xml_safety_margin_buffer->SetText(safety_margin_buffer); - xml_coll_cost_config->InsertEndChild(xml_safety_margin_buffer); - - tinyxml2::XMLElement* xml_coeff = doc.NewElement("Coefficient"); - xml_coeff->SetText(coeff); - xml_coll_cost_config->InsertEndChild(xml_coeff); - - return xml_coll_cost_config; -} - template void CollisionCostConfig::serialize(Archive& ar, const unsigned int /*version*/) { @@ -152,110 +47,6 @@ void CollisionCostConfig::serialize(Archive& ar, const unsigned int /*version*/) ar& BOOST_SERIALIZATION_NVP(coeff); } -CollisionConstraintConfig::CollisionConstraintConfig(const tinyxml2::XMLElement& xml_element) -{ - const tinyxml2::XMLElement* enabled_element = xml_element.FirstChildElement("Enabled"); - const tinyxml2::XMLElement* use_weighted_sum_element = xml_element.FirstChildElement("UseWeightedSum"); - const tinyxml2::XMLElement* type_element = xml_element.FirstChildElement("CollisionEvaluator"); - const tinyxml2::XMLElement* safety_margin_element = xml_element.FirstChildElement("SafetyMargin"); - const tinyxml2::XMLElement* safety_margin_buffer_element = xml_element.FirstChildElement("SafetyMarginBuffer"); - const tinyxml2::XMLElement* coeff_element = xml_element.FirstChildElement("Coefficient"); - - if (enabled_element == nullptr) - throw std::runtime_error("CollisionConstraintConfig: Must have Enabled element."); - - int status = enabled_element->QueryBoolText(&enabled); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("CollisionConstraintConfig: Error parsing Enabled string"); - - if (use_weighted_sum_element != nullptr) - { - status = use_weighted_sum_element->QueryBoolText(&use_weighted_sum); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("CollisionConstraintConfig: Error parsing UseWeightedSum string"); - } - - if (type_element != nullptr) - { - auto coll_type = static_cast(trajopt::CollisionEvaluatorType::CAST_CONTINUOUS); - status = type_element->QueryIntAttribute("type", &coll_type); - if (status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("CollisionConstraintConfig: Error parsing CollisionEvaluator type attribute."); - - type = static_cast(coll_type); - } - - if (safety_margin_element != nullptr) - { - std::string safety_margin_string; - status = tesseract_common::QueryStringText(safety_margin_element, safety_margin_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("CollisionConstraintConfig: Error parsing SafetyMargin string"); - - if (!tesseract_common::isNumeric(safety_margin_string)) - throw std::runtime_error("CollisionConstraintConfig: SafetyMargin is not a numeric values."); - - tesseract_common::toNumeric(safety_margin_string, safety_margin); - } - - if (safety_margin_buffer_element != nullptr) - { - std::string safety_margin_buffer_string; - status = tesseract_common::QueryStringText(safety_margin_buffer_element, safety_margin_buffer_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("CollisionConstraintConfig: Error parsing SafetyMarginBuffer string"); - - if (!tesseract_common::isNumeric(safety_margin_buffer_string)) - throw std::runtime_error("CollisionConstraintConfig: SafetyMarginBuffer is not a numeric values."); - - tesseract_common::toNumeric(safety_margin_buffer_string, safety_margin_buffer); - } - - if (coeff_element != nullptr) - { - std::string coeff_string; - status = tesseract_common::QueryStringText(coeff_element, coeff_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("CollisionConstraintConfig: Error parsing Coefficient string"); - - if (!tesseract_common::isNumeric(coeff_string)) - throw std::runtime_error("CollisionConstraintConfig: Coefficient is not a numeric values."); - - tesseract_common::toNumeric(coeff_string, coeff); - } -} - -tinyxml2::XMLElement* CollisionConstraintConfig::toXML(tinyxml2::XMLDocument& doc) const -{ - tinyxml2::XMLElement* xml_coll_cnt_config = doc.NewElement("CollisionConstraintConfig"); - - tinyxml2::XMLElement* xml_enabled = doc.NewElement("Enabled"); - xml_enabled->SetText(enabled); - xml_coll_cnt_config->InsertEndChild(xml_enabled); - - tinyxml2::XMLElement* xml_use_weighted_sum = doc.NewElement("UseWeightedSum"); - xml_use_weighted_sum->SetText(use_weighted_sum); - xml_coll_cnt_config->InsertEndChild(xml_use_weighted_sum); - - tinyxml2::XMLElement* xml_type = doc.NewElement("CollisionEvaluator"); - xml_type->SetAttribute("type", std::to_string(static_cast(type)).c_str()); - xml_coll_cnt_config->InsertEndChild(xml_type); - - tinyxml2::XMLElement* xml_safety_margin = doc.NewElement("SafetyMargin"); - xml_safety_margin->SetText(safety_margin); - xml_coll_cnt_config->InsertEndChild(xml_safety_margin); - - tinyxml2::XMLElement* xml_safety_margin_buffer = doc.NewElement("SafetyMarginBuffer"); - xml_safety_margin_buffer->SetText(safety_margin_buffer); - xml_coll_cnt_config->InsertEndChild(xml_safety_margin_buffer); - - tinyxml2::XMLElement* xml_coeff = doc.NewElement("Coefficient"); - xml_coeff->SetText(coeff); - xml_coll_cnt_config->InsertEndChild(xml_coeff); - - return xml_coll_cnt_config; -} - template void CollisionConstraintConfig::serialize(Archive& ar, const unsigned int /*version*/) { diff --git a/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp index 4959dac7b79..c9aa258107a 100644 --- a/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp +++ b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp @@ -36,10 +36,11 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include #include #include #include -#include +#include #include #include @@ -180,43 +181,18 @@ TrajOptMotionPlanner::createProblem(const PlannerRequest& request) const // Store fixed steps std::vector fixed_steps; - // Create the problem - auto pci = std::make_shared(request.env); - // Assume all the plan instructions have the same manipulator as the composite assert(!request.instructions.getManipulatorInfo().empty()); const tesseract_common::ManipulatorInfo& composite_mi = request.instructions.getManipulatorInfo(); - // Assign Kinematics object - try - { - tesseract_kinematics::KinematicGroup::Ptr kin_group; - std::string error_msg; - if (composite_mi.manipulator_ik_solver.empty()) - { - kin_group = request.env->getKinematicGroup(composite_mi.manipulator); - error_msg = "Failed to find kinematic group for manipulator '" + composite_mi.manipulator + "'"; - } - else - { - kin_group = request.env->getKinematicGroup(composite_mi.manipulator, composite_mi.manipulator_ik_solver); - error_msg = "Failed to find kinematic group for manipulator '" + composite_mi.manipulator + "' with solver '" + - composite_mi.manipulator_ik_solver + "'"; - } - - if (kin_group == nullptr) - { - CONSOLE_BRIDGE_logError("%s", error_msg.c_str()); - throw std::runtime_error(error_msg); - } + // Flatten the input for planning + auto move_instructions = request.instructions.flatten(&moveFilter); - pci->kin = kin_group; - } - catch (...) - { - pci->kin = request.env->getJointGroup(composite_mi.manipulator); - } + // Create the problem + auto pci = std::make_shared(request.env); + // Assign Kinematics object + pci->kin = createKinematicGroup(composite_mi, *request.env); if (pci->kin == nullptr) { std::string error_msg = "In TrajOpt problem generator, manipulator does not exist!"; @@ -224,24 +200,24 @@ TrajOptMotionPlanner::createProblem(const PlannerRequest& request) const throw std::runtime_error(error_msg); } + // Setup Basic Info + pci->basic_info.n_steps = static_cast(move_instructions.size()); + pci->basic_info.manip = composite_mi.manipulator; + pci->basic_info.use_time = false; + // Apply Solver parameters - TrajOptSolverProfile::ConstPtr solver_profile = - getProfile(name_, - request.instructions.getProfile(name_), - *request.profiles, - std::make_shared()); + TrajOptSolverProfile::ConstPtr solver_profile = getProfile( + name_, request.instructions.getProfile(name_), *request.profiles, std::make_shared()); if (!solver_profile) throw std::runtime_error("TrajOptMotionPlanner: Invalid profile"); - solver_profile->apply(*pci); + pci->basic_info.convex_solver = solver_profile->getSolverType(); + pci->basic_info.convex_solver_config = solver_profile->createSolverConfig(); + pci->opt_info = solver_profile->createOptimizationParameters(); + pci->callbacks = solver_profile->createOptimizationCallbacks(); // Get kinematics information - tesseract_environment::Environment::ConstPtr env = request.env; std::vector active_links = pci->kin->getActiveLinkNames(); - std::vector joint_names = pci->kin->getJointNames(); - - // Flatten the input for planning - auto move_instructions = request.instructions.flatten(&moveFilter); // Create a temp seed storage. std::vector seed_states; @@ -251,88 +227,29 @@ TrajOptMotionPlanner::createProblem(const PlannerRequest& request) const { const auto& move_instruction = move_instructions[static_cast(i)].get().as(); - // If plan instruction has manipulator information then use it over the one provided by the composite. - tesseract_common::ManipulatorInfo mi = composite_mi.getCombined(move_instruction.getManipulatorInfo()); - - if (mi.manipulator.empty()) - throw std::runtime_error("TrajOpt, manipulator is empty!"); - - if (mi.tcp_frame.empty()) - throw std::runtime_error("TrajOpt, tcp_frame is empty!"); - - if (mi.working_frame.empty()) - throw std::runtime_error("TrajOpt, working_frame is empty!"); - // Get Plan Profile TrajOptPlanProfile::ConstPtr cur_plan_profile = getProfile( name_, move_instruction.getProfile(name_), *request.profiles, std::make_shared()); if (!cur_plan_profile) throw std::runtime_error("TrajOptMotionPlanner: Invalid profile"); - if (move_instruction.getWaypoint().isCartesianWaypoint()) - { - const auto& cwp = move_instruction.getWaypoint().as(); - cur_plan_profile->apply(*pci, cwp, move_instruction, composite_mi, active_links, i); - - // Seed state - if (cwp.hasSeed()) - { - assert(checkJointPositionFormat(joint_names, move_instruction.getWaypoint())); - seed_states.push_back(cwp.getSeed().position); - } - else - { - seed_states.push_back(request.env->getCurrentJointValues(joint_names)); - } - - /** @todo If fixed cartesian and not term_type cost add as fixed */ - } - else if (move_instruction.getWaypoint().isJointWaypoint()) - { - assert(checkJointPositionFormat(joint_names, move_instruction.getWaypoint())); - - const auto& jwp = move_instruction.getWaypoint().as(); - if (jwp.isConstrained()) - { - cur_plan_profile->apply(*pci, jwp, move_instruction, composite_mi, active_links, i); + TrajOptWaypointInfo wp_info = + cur_plan_profile->createCostAndConstraints(move_instruction, composite_mi, request.env, active_links, i); - // Add to fixed indices - if (!jwp.isToleranced()) /** @todo Should not make fixed if term_type is cost */ - fixed_steps.push_back(i); - } + if (wp_info.seed.rows() != pci->kin->numJoints()) + throw std::runtime_error("TrajOptMotionPlanner, profile returned invalid seed"); - // Add seed state - seed_states.push_back(jwp.getPosition()); - } - else if (move_instruction.getWaypoint().isStateWaypoint()) - { - assert(checkJointPositionFormat(joint_names, move_instruction.getWaypoint())); - const auto& swp = move_instruction.getWaypoint().as(); - JointWaypointPoly jwp = move_instruction.createJointWaypoint(); - jwp.setNames(swp.getNames()); - jwp.setPosition(swp.getPosition()); - cur_plan_profile->apply(*pci, jwp, move_instruction, composite_mi, active_links, i); - - // Add seed state - seed_states.push_back(swp.getPosition()); - - // Add to fixed indices - fixed_steps.push_back(i); /** @todo Should not make fixed if term_type is cost */ - } - else - { - throw std::runtime_error("TrajOptMotionPlanner: unknown waypoint type"); - } - } + if (!wp_info.term_infos.costs.empty()) + pci->cost_infos.insert(pci->cost_infos.end(), wp_info.term_infos.costs.begin(), wp_info.term_infos.costs.end()); - // ---------------- - // Create Problem - // ---------------- + if (!wp_info.term_infos.constraints.empty()) + pci->cnt_infos.insert( + pci->cnt_infos.end(), wp_info.term_infos.constraints.begin(), wp_info.term_infos.constraints.end()); - // Setup Basic Info - pci->basic_info.n_steps = static_cast(move_instructions.size()); - pci->basic_info.manip = composite_mi.manipulator; - pci->basic_info.use_time = false; + seed_states.push_back(wp_info.seed); + if (wp_info.fixed) + fixed_steps.push_back(i); + } // Add the fixed timesteps. TrajOpt will constrain the optimization such that any costs applied at these timesteps // will be ignored. Costs applied to variables at fixed timesteps generally causes solver failures @@ -345,16 +262,23 @@ TrajOptMotionPlanner::createProblem(const PlannerRequest& request) const for (long i = 0; i < pci->basic_info.n_steps; ++i) pci->init_info.data.row(i) = seed_states[static_cast(i)]; - TrajOptCompositeProfile::ConstPtr cur_composite_profile = + // Get composite cost and constraints + TrajOptCompositeProfile::ConstPtr composite_profile = getProfile(name_, request.instructions.getProfile(name_), *request.profiles, std::make_shared()); - if (!cur_composite_profile) - throw std::runtime_error("TrajOptMotionPlanner: Invalid profile"); + if (!composite_profile) + throw std::runtime_error("TrajOptMotionPlanner: Invalid composite profile"); + + TrajOptTermInfos c_term_infos = composite_profile->createCostAndConstraints( + composite_mi, request.env, fixed_steps, 0, pci->basic_info.n_steps - 1); + if (!c_term_infos.costs.empty()) + pci->cost_infos.insert(pci->cost_infos.end(), c_term_infos.costs.begin(), c_term_infos.costs.end()); - cur_composite_profile->apply(*pci, 0, pci->basic_info.n_steps - 1, composite_mi, active_links, fixed_steps); + if (!c_term_infos.constraints.empty()) + pci->cnt_infos.insert(pci->cnt_infos.end(), c_term_infos.constraints.begin(), c_term_infos.constraints.end()); return pci; } diff --git a/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp b/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp index acdca0f7015..ec54299d768 100644 --- a/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp +++ b/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp @@ -24,10 +24,48 @@ * limitations under the License. */ #include +#include +#include +#include #include +#include namespace tesseract_planning { +std::unique_ptr +createKinematicGroup(const tesseract_common::ManipulatorInfo& manip_info, const tesseract_environment::Environment& env) +{ + // Assign Kinematics object + try + { + tesseract_kinematics::KinematicGroup::UPtr kin_group; + std::string error_msg; + if (manip_info.manipulator_ik_solver.empty()) + { + kin_group = env.getKinematicGroup(manip_info.manipulator); + error_msg = "Failed to find kinematic group for manipulator '" + manip_info.manipulator + "'"; + } + else + { + kin_group = env.getKinematicGroup(manip_info.manipulator, manip_info.manipulator_ik_solver); + error_msg = "Failed to find kinematic group for manipulator '" + manip_info.manipulator + "' with solver '" + + manip_info.manipulator_ik_solver + "'"; + } + + if (kin_group == nullptr) + { + CONSOLE_BRIDGE_logError("%s", error_msg.c_str()); + throw std::runtime_error(error_msg); + } + + return kin_group; + } + catch (...) + { + return env.getJointGroup(manip_info.manipulator); + } +} + std::shared_ptr createCartesianWaypointTermInfo(int index, const std::string& working_frame, const Eigen::Isometry3d& c_wp, diff --git a/tesseract_motion_planners/trajopt/src/trajopt_waypoint_config.cpp b/tesseract_motion_planners/trajopt/src/trajopt_waypoint_config.cpp index f2db1b9fb65..db35ff7ccd9 100644 --- a/tesseract_motion_planners/trajopt/src/trajopt_waypoint_config.cpp +++ b/tesseract_motion_planners/trajopt/src/trajopt_waypoint_config.cpp @@ -27,7 +27,6 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include -#include #include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP @@ -38,123 +37,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_planning { -CartesianWaypointConfig::CartesianWaypointConfig(const tinyxml2::XMLElement& xml_element) -{ - const tinyxml2::XMLElement* enabled_element = xml_element.FirstChildElement("Enabled"); - const tinyxml2::XMLElement* use_tolerance_override_element = xml_element.FirstChildElement("UseToleranceOverride"); - const tinyxml2::XMLElement* lower_tolerance_element = xml_element.FirstChildElement("LowerTolerance"); - const tinyxml2::XMLElement* upper_tolerance_element = xml_element.FirstChildElement("UpperTolerance"); - const tinyxml2::XMLElement* coefficients_element = xml_element.FirstChildElement("Coefficients"); - - if (enabled_element == nullptr) - throw std::runtime_error("CartesianWaypointConfig: Must have Enabled element."); - int status = enabled_element->QueryBoolText(&enabled); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("CartesianWaypointConfig: Error parsing Enabled string"); - - if (use_tolerance_override_element != nullptr) - { - status = use_tolerance_override_element->QueryBoolText(&use_tolerance_override); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("CartesianWaypointConfig: Error parsing UseToleranceOverride string"); - } - - if (lower_tolerance_element != nullptr) - { - std::vector lower_tolerance_tokens; - std::string lower_tolerance_string; - status = tesseract_common::QueryStringText(lower_tolerance_element, lower_tolerance_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("CartesianWaypointConfig: Error parsing LowerTolerance string"); - - boost::split(lower_tolerance_tokens, lower_tolerance_string, boost::is_any_of(" "), boost::token_compress_on); - - if (!tesseract_common::isNumeric(lower_tolerance_tokens)) - throw std::runtime_error("CartesianWaypointConfig: LowerTolerance are not all numeric values."); - - lower_tolerance.resize(static_cast(lower_tolerance_tokens.size())); - for (std::size_t i = 0; i < lower_tolerance_tokens.size(); ++i) - tesseract_common::toNumeric(lower_tolerance_tokens[i], lower_tolerance[static_cast(i)]); - } - - if (upper_tolerance_element != nullptr) - { - std::vector upper_tolerance_tokens; - std::string upper_tolerance_string; - status = tesseract_common::QueryStringText(upper_tolerance_element, upper_tolerance_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("CartesianWaypointConfig: Error parsing UpperTolerance string"); - - boost::split(upper_tolerance_tokens, upper_tolerance_string, boost::is_any_of(" "), boost::token_compress_on); - - if (!tesseract_common::isNumeric(upper_tolerance_tokens)) - throw std::runtime_error("CartesianWaypointConfig: UpperTolerance are not all numeric values."); - - upper_tolerance.resize(static_cast(upper_tolerance_tokens.size())); - for (std::size_t i = 0; i < upper_tolerance_tokens.size(); ++i) - tesseract_common::toNumeric(upper_tolerance_tokens[i], upper_tolerance[static_cast(i)]); - } - - if (coefficients_element != nullptr) - { - std::vector coefficients_tokens; - std::string coefficients_string; - status = tesseract_common::QueryStringText(coefficients_element, coefficients_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("CartesianWaypointConfig: Error parsing Coefficients string"); - - boost::split(coefficients_tokens, coefficients_string, boost::is_any_of(" "), boost::token_compress_on); - - if (!tesseract_common::isNumeric(coefficients_tokens)) - throw std::runtime_error("CartesianWaypointConfig: Coefficients are not all numeric values."); - - coeff.resize(static_cast(coefficients_tokens.size())); - for (std::size_t i = 0; i < coefficients_tokens.size(); ++i) - tesseract_common::toNumeric(coefficients_tokens[i], coeff[static_cast(i)]); - } -} - -tinyxml2::XMLElement* CartesianWaypointConfig::toXML(tinyxml2::XMLDocument& doc) const -{ - Eigen::IOFormat eigen_format(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", " "); - - tinyxml2::XMLElement* xml_cartesian_waypoint_config = doc.NewElement("CartesianWaypointConfig"); - - tinyxml2::XMLElement* xml_enabled = doc.NewElement("Enabled"); - xml_enabled->SetText(enabled); - xml_cartesian_waypoint_config->InsertEndChild(xml_enabled); - - tinyxml2::XMLElement* xml_use_tolerance_override = doc.NewElement("UseToleranceOverride"); - xml_use_tolerance_override->SetText(use_tolerance_override); - xml_cartesian_waypoint_config->InsertEndChild(xml_use_tolerance_override); - - if (lower_tolerance.size() > 0) - { - tinyxml2::XMLElement* xml_lower_tolerance = doc.NewElement("LowerTolerance"); - std::stringstream lower_tolerance_ss; - lower_tolerance_ss << lower_tolerance.format(eigen_format); - xml_lower_tolerance->SetText(lower_tolerance_ss.str().c_str()); - xml_cartesian_waypoint_config->InsertEndChild(xml_lower_tolerance); - } - - if (upper_tolerance.size() > 0) - { - tinyxml2::XMLElement* xml_upper_tolerance = doc.NewElement("UpperTolerance"); - std::stringstream upper_tolerance_ss; - upper_tolerance_ss << upper_tolerance.format(eigen_format); - xml_upper_tolerance->SetText(upper_tolerance_ss.str().c_str()); - xml_cartesian_waypoint_config->InsertEndChild(xml_upper_tolerance); - } - - tinyxml2::XMLElement* xml_coeff = doc.NewElement("Coefficients"); - std::stringstream coeff_ss; - coeff_ss << coeff.format(eigen_format); - xml_coeff->SetText(coeff_ss.str().c_str()); - xml_cartesian_waypoint_config->InsertEndChild(xml_coeff); - - return xml_cartesian_waypoint_config; -} - template void CartesianWaypointConfig::serialize(Archive& ar, const unsigned int /*version*/) { @@ -165,124 +47,6 @@ void CartesianWaypointConfig::serialize(Archive& ar, const unsigned int /*versio ar& BOOST_SERIALIZATION_NVP(coeff); } -JointWaypointConfig::JointWaypointConfig(const tinyxml2::XMLElement& xml_element) -{ - const tinyxml2::XMLElement* enabled_element = xml_element.FirstChildElement("Enabled"); - const tinyxml2::XMLElement* use_tolerance_override_element = xml_element.FirstChildElement("UseToleranceOverride"); - const tinyxml2::XMLElement* lower_tolerance_element = xml_element.FirstChildElement("LowerTolerance"); - const tinyxml2::XMLElement* upper_tolerance_element = xml_element.FirstChildElement("UpperTolerance"); - const tinyxml2::XMLElement* coefficients_element = xml_element.FirstChildElement("Coefficients"); - - if (enabled_element == nullptr) - throw std::runtime_error("JointWaypointConfig: Must have Enabled element."); - - int status = enabled_element->QueryBoolText(&enabled); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("JointWaypointConfig: Error parsing Enabled string"); - - if (use_tolerance_override_element != nullptr) - { - status = use_tolerance_override_element->QueryBoolText(&use_tolerance_override); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("JointWaypointConfig: Error parsing UseToleranceOverride string"); - } - - if (lower_tolerance_element != nullptr) - { - std::vector lower_tolerance_tokens; - std::string lower_tolerance_string; - status = tesseract_common::QueryStringText(lower_tolerance_element, lower_tolerance_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("JointWaypointConfig: Error parsing LowerTolerance string"); - - boost::split(lower_tolerance_tokens, lower_tolerance_string, boost::is_any_of(" "), boost::token_compress_on); - - if (!tesseract_common::isNumeric(lower_tolerance_tokens)) - throw std::runtime_error("JointWaypointConfig: LowerTolerance are not all numeric values."); - - lower_tolerance.resize(static_cast(lower_tolerance_tokens.size())); - for (std::size_t i = 0; i < lower_tolerance_tokens.size(); ++i) - tesseract_common::toNumeric(lower_tolerance_tokens[i], lower_tolerance[static_cast(i)]); - } - - if (upper_tolerance_element != nullptr) - { - std::vector upper_tolerance_tokens; - std::string upper_tolerance_string; - status = tesseract_common::QueryStringText(upper_tolerance_element, upper_tolerance_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("JointWaypointConfig: Error parsing UpperTolerance string"); - - boost::split(upper_tolerance_tokens, upper_tolerance_string, boost::is_any_of(" "), boost::token_compress_on); - - if (!tesseract_common::isNumeric(upper_tolerance_tokens)) - throw std::runtime_error("JointWaypointConfig: UpperTolerance are not all numeric values."); - - upper_tolerance.resize(static_cast(upper_tolerance_tokens.size())); - for (std::size_t i = 0; i < upper_tolerance_tokens.size(); ++i) - tesseract_common::toNumeric(upper_tolerance_tokens[i], upper_tolerance[static_cast(i)]); - } - - if (coefficients_element != nullptr) - { - std::vector coefficients_tokens; - std::string coefficients_string; - status = tesseract_common::QueryStringText(coefficients_element, coefficients_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("JointWaypointConfig: Error parsing Coefficients string"); - - boost::split(coefficients_tokens, coefficients_string, boost::is_any_of(" "), boost::token_compress_on); - - if (!tesseract_common::isNumeric(coefficients_tokens)) - throw std::runtime_error("JointWaypointConfig: Coefficients are not all numeric values."); - - coeff.resize(static_cast(coefficients_tokens.size())); - for (std::size_t i = 0; i < coefficients_tokens.size(); ++i) - tesseract_common::toNumeric(coefficients_tokens[i], coeff[static_cast(i)]); - } -} - -tinyxml2::XMLElement* JointWaypointConfig::toXML(tinyxml2::XMLDocument& doc) const -{ - Eigen::IOFormat eigen_format(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", " "); - - tinyxml2::XMLElement* xml_cartesian_waypoint_config = doc.NewElement("JointWaypointConfig"); - - tinyxml2::XMLElement* xml_enabled = doc.NewElement("Enabled"); - xml_enabled->SetText(enabled); - xml_cartesian_waypoint_config->InsertEndChild(xml_enabled); - - tinyxml2::XMLElement* xml_use_tolerance_override = doc.NewElement("UseToleranceOverride"); - xml_use_tolerance_override->SetText(use_tolerance_override); - xml_cartesian_waypoint_config->InsertEndChild(xml_use_tolerance_override); - - if (lower_tolerance.size() > 0) - { - tinyxml2::XMLElement* xml_lower_tolerance = doc.NewElement("LowerTolerance"); - std::stringstream lower_tolerance_ss; - lower_tolerance_ss << lower_tolerance.format(eigen_format); - xml_lower_tolerance->SetText(lower_tolerance_ss.str().c_str()); - xml_cartesian_waypoint_config->InsertEndChild(xml_lower_tolerance); - } - - if (upper_tolerance.size() > 0) - { - tinyxml2::XMLElement* xml_upper_tolerance = doc.NewElement("UpperTolerance"); - std::stringstream upper_tolerance_ss; - upper_tolerance_ss << upper_tolerance.format(eigen_format); - xml_upper_tolerance->SetText(upper_tolerance_ss.str().c_str()); - xml_cartesian_waypoint_config->InsertEndChild(xml_upper_tolerance); - } - - tinyxml2::XMLElement* xml_coeff = doc.NewElement("Coefficients"); - std::stringstream coeff_ss; - coeff_ss << coeff.format(eigen_format); - xml_coeff->SetText(coeff_ss.str().c_str()); - xml_cartesian_waypoint_config->InsertEndChild(xml_coeff); - - return xml_cartesian_waypoint_config; -} - template void JointWaypointConfig::serialize(Archive& ar, const unsigned int /*version*/) { diff --git a/tesseract_motion_planners/trajopt/test/trajopt_planner_tests.cpp b/tesseract_motion_planners/trajopt/test/trajopt_planner_tests.cpp index 75bd1867e35..331869807b3 100644 --- a/tesseract_motion_planners/trajopt/test/trajopt_planner_tests.cpp +++ b/tesseract_motion_planners/trajopt/test/trajopt_planner_tests.cpp @@ -46,9 +46,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include -#include -#include +#include #include #include #include @@ -147,7 +145,7 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptPlannerBooleanFlagsJointJoint) // N // Create Profiles auto plan_profile = std::make_shared(); auto composite_profile = std::make_shared(); - auto solver_profile = std::make_shared(); + auto solver_profile = std::make_shared(); // Profile Dictionary auto profiles = std::make_shared(); @@ -221,7 +219,7 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceJointJoint) // NOLINT // Create Profiles auto plan_profile = std::make_shared(); auto composite_profile = std::make_shared(); - auto solver_profile = std::make_shared(); + auto solver_profile = std::make_shared(); // Profile Dictionary auto profiles = std::make_shared(); @@ -307,7 +305,7 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceJointCart) // NOLINT // Create Profiles auto plan_profile = std::make_shared(); auto composite_profile = std::make_shared(); - auto solver_profile = std::make_shared(); + auto solver_profile = std::make_shared(); // Profile Dictionary auto profiles = std::make_shared(); @@ -394,7 +392,7 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartJoint) // NOLINT // Create Profiles auto plan_profile = std::make_shared(); auto composite_profile = std::make_shared(); - auto solver_profile = std::make_shared(); + auto solver_profile = std::make_shared(); // Profile Dictionary auto profiles = std::make_shared(); @@ -479,7 +477,7 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartCart) // NOLINT // Create Profiles auto plan_profile = std::make_shared(); auto composite_profile = std::make_shared(); - auto solver_profile = std::make_shared(); + auto solver_profile = std::make_shared(); // Profile Dictionary auto profiles = std::make_shared(); @@ -564,7 +562,7 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptPlannerBooleanFlagsCartCart) // NOL // Create Profiles auto plan_profile = std::make_shared(); auto composite_profile = std::make_shared(); - auto solver_profile = std::make_shared(); + auto solver_profile = std::make_shared(); // Profile Dictionary auto profiles = std::make_shared(); @@ -651,7 +649,7 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayJointConstraint) // NOLINT plan_profile->joint_cost_config.enabled = false; plan_profile->joint_constraint_config.enabled = true; auto composite_profile = std::make_shared(); - auto solver_profile = std::make_shared(); + auto solver_profile = std::make_shared(); // Profile Dictionary auto profiles = std::make_shared(); @@ -712,7 +710,7 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayJointCost) // NOLINT plan_profile->joint_constraint_config.enabled = false; auto composite_profile = std::make_shared(); - auto solver_profile = std::make_shared(); + auto solver_profile = std::make_shared(); // Profile Dictionary auto profiles = std::make_shared(); @@ -742,79 +740,6 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayJointCost) // NOLINT (tesseract_tests::vectorContainsType(problem->getCosts()))); } -TEST(TesseractPlanningTrajoptSerializeUnit, SerializeTrajoptDefaultCompositeToXml) // NOLINT -{ - // Write program to file - TrajOptDefaultCompositeProfile comp_profile; - CollisionCostConfig collision_cost_config; - comp_profile.collision_cost_config = collision_cost_config; - - CollisionConstraintConfig collision_constraint_config; - comp_profile.collision_constraint_config = collision_constraint_config; - - comp_profile.velocity_coeff = Eigen::VectorXd::Ones(6) * 10; - comp_profile.acceleration_coeff = Eigen::VectorXd::Ones(6) * 10; - comp_profile.jerk_coeff = Eigen::VectorXd::Ones(6) * 10; - - comp_profile.smooth_velocities = false; - - EXPECT_TRUE(toXMLFile(comp_profile, tesseract_common::getTempPath() + "trajopt_default_composite_example_input.xml")); - - // Import file - TrajOptDefaultCompositeProfile imported_comp_profile = - trajOptCompositeFromXMLFile(tesseract_common::getTempPath() + "trajopt_default_composite_example_input.xml"); - - // Re-write file and compare changed from default term - EXPECT_TRUE(toXMLFile(imported_comp_profile, - tesseract_common::getTempPath() + "trajopt_default_composite_example_input2.xml")); - EXPECT_TRUE(comp_profile.smooth_velocities == imported_comp_profile.smooth_velocities); -} - -TEST(TesseractPlanningTrajoptSerializeUnit, SerializeTrajoptDefaultPlanToXml) // NOLINT -{ - // Write program to file - TrajOptDefaultPlanProfile plan_profile; - plan_profile.cartesian_cost_config.coeff = Eigen::VectorXd::Ones(6) * 10; - plan_profile.joint_cost_config.coeff = Eigen::VectorXd::Ones(6) * 9; - plan_profile.cartesian_cost_config.enabled = true; - plan_profile.cartesian_constraint_config.enabled = false; - plan_profile.joint_cost_config.enabled = true; - plan_profile.joint_constraint_config.enabled = false; - plan_profile.cartesian_constraint_config.lower_tolerance = Eigen::VectorXd::Ones(6) * -0.01; - plan_profile.cartesian_constraint_config.upper_tolerance = Eigen::VectorXd::Ones(6) * 0.02; - plan_profile.joint_constraint_config.lower_tolerance = Eigen::VectorXd::Ones(6) * -0.03; - plan_profile.joint_constraint_config.upper_tolerance = Eigen::VectorXd::Ones(6) * 0.04; - - EXPECT_TRUE(toXMLFile(plan_profile, tesseract_common::getTempPath() + "trajopt_default_plan_example_input.xml")); - - // Import file - TrajOptDefaultPlanProfile imported_plan_profile = trajOptPlanFromXMLFile(tesseract_common::getTempPath() + "trajopt_" - "default_" - "plan_" - "example_" - "input." - "xml"); - - // Re-write file and compare changed from default term - EXPECT_TRUE( - toXMLFile(imported_plan_profile, tesseract_common::getTempPath() + "trajopt_default_plan_example_input2.xml")); - EXPECT_TRUE(plan_profile.cartesian_cost_config.enabled == imported_plan_profile.cartesian_cost_config.enabled); - EXPECT_TRUE(plan_profile.cartesian_constraint_config.enabled == - imported_plan_profile.cartesian_constraint_config.enabled); - EXPECT_TRUE(plan_profile.joint_cost_config.enabled == imported_plan_profile.joint_cost_config.enabled); - EXPECT_TRUE(plan_profile.joint_constraint_config.enabled == imported_plan_profile.joint_constraint_config.enabled); - EXPECT_TRUE(plan_profile.cartesian_cost_config.coeff == imported_plan_profile.cartesian_cost_config.coeff); - EXPECT_TRUE(plan_profile.joint_cost_config.coeff == imported_plan_profile.joint_cost_config.coeff); - EXPECT_TRUE(plan_profile.cartesian_constraint_config.lower_tolerance == - imported_plan_profile.cartesian_constraint_config.lower_tolerance); - EXPECT_TRUE(plan_profile.cartesian_constraint_config.upper_tolerance == - imported_plan_profile.cartesian_constraint_config.upper_tolerance); - EXPECT_TRUE(plan_profile.joint_constraint_config.lower_tolerance == - imported_plan_profile.joint_constraint_config.lower_tolerance); - EXPECT_TRUE(plan_profile.joint_constraint_config.upper_tolerance == - imported_plan_profile.joint_constraint_config.upper_tolerance); -} - int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv);