From 539f48562d244c43e2cf629e4f1c665776813cb5 Mon Sep 17 00:00:00 2001 From: Matthew Powelson Date: Tue, 29 Dec 2020 18:37:43 -0700 Subject: [PATCH] Add Tesseract planner for TrajOpt Ifopt --- .../include/tesseract_collision/core/types.h | 1 + .../cartesian_waypoint.h | 6 +- .../tesseract_motion_planners/CMakeLists.txt | 37 +- .../trajopt/profile/trajopt_profile.h | 2 + .../trajopt/trajopt_utils.h | 7 +- .../default_problem_generator.h | 46 +++ .../trajopt_ifopt_default_composite_profile.h | 98 +++++ .../trajopt_ifopt_default_plan_profile.h | 78 ++++ .../profile/trajopt_ifopt_profile.h | 108 ++++++ .../trajopt_ifopt_motion_planner.h | 132 +++++++ .../trajopt_ifopt/trajopt_ifopt_problem.h | 62 ++++ .../trajopt_ifopt/trajopt_ifopt_utils.h | 106 ++++++ .../tesseract_motion_planners/package.xml | 2 + .../default_problem_generator.cpp | 2 +- .../src/trajopt/trajopt_utils.cpp | 13 +- .../default_problem_generator.cpp | 336 ++++++++++++++++++ ...rajopt_ifopt_default_composite_profile.cpp | 69 ++++ .../trajopt_ifopt_default_plan_profile.cpp | 102 ++++++ .../trajopt_ifopt_collision_config.cpp | 38 ++ .../trajopt_ifopt_motion_planner.cpp | 216 +++++++++++ .../src/trajopt_ifopt/trajopt_ifopt_utils.cpp | 222 ++++++++++++ 21 files changed, 1664 insertions(+), 19 deletions(-) create mode 100644 tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/problem_generators/default_problem_generator.h create mode 100644 tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h create mode 100644 tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h create mode 100644 tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h create mode 100644 tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_motion_planner.h create mode 100644 tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_problem.h create mode 100644 tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_utils.h create mode 100644 tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt_ifopt/problem_generators/default_problem_generator.cpp create mode 100644 tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.cpp create mode 100644 tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.cpp create mode 100644 tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt_ifopt/trajopt_ifopt_collision_config.cpp create mode 100644 tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt_ifopt/trajopt_ifopt_motion_planner.cpp create mode 100644 tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt_ifopt/trajopt_ifopt_utils.cpp diff --git a/tesseract/tesseract_collision/include/tesseract_collision/core/types.h b/tesseract/tesseract_collision/include/tesseract_collision/core/types.h index 0649eea4dab..e8fafec3281 100644 --- a/tesseract/tesseract_collision/include/tesseract_collision/core/types.h +++ b/tesseract/tesseract_collision/include/tesseract_collision/core/types.h @@ -384,6 +384,7 @@ struct ContactTestData */ enum class CollisionEvaluatorType { + NONE, DISCRETE, LVS_DISCRETE, CONTINUOUS, diff --git a/tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/cartesian_waypoint.h b/tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/cartesian_waypoint.h index e02c5979c5a..2202ed68661 100644 --- a/tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/cartesian_waypoint.h +++ b/tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/cartesian_waypoint.h @@ -264,9 +264,11 @@ class CartesianWaypoint /** @brief The Cartesian Waypoint */ Eigen::Isometry3d waypoint; - /** @brief Distance below waypoint that is allowed. Should be size = 6. First 3 elements are dx, dy, dz. The last 3 elements are angle axis error allowed (Eigen::AngleAxisd.axis() * Eigen::AngleAxisd.angle()) */ + /** @brief Distance below waypoint that is allowed. Should be size = 6. First 3 elements are dx, dy, dz. The last 3 + * elements are angle axis error allowed (Eigen::AngleAxisd.axis() * Eigen::AngleAxisd.angle()) */ Eigen::VectorXd lower_tolerance; - /** @brief Distance above waypoint that is allowed. Should be size = 6. First 3 elements are dx, dy, dz. The last 3 elements are angle axis error allowed (Eigen::AngleAxisd.axis() * Eigen::AngleAxisd.angle())*/ + /** @brief Distance above waypoint that is allowed. Should be size = 6. First 3 elements are dx, dy, dz. The last 3 + * elements are angle axis error allowed (Eigen::AngleAxisd.axis() * Eigen::AngleAxisd.angle())*/ Eigen::VectorXd upper_tolerance; bool isToleranced() const diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/CMakeLists.txt b/tesseract/tesseract_planning/tesseract_motion_planners/CMakeLists.txt index 2fde59fa96d..ad469ab071f 100644 --- a/tesseract/tesseract_planning/tesseract_motion_planners/CMakeLists.txt +++ b/tesseract/tesseract_planning/tesseract_motion_planners/CMakeLists.txt @@ -14,7 +14,9 @@ find_package(tesseract_environment REQUIRED) find_package(tesseract_common REQUIRED) find_package(tesseract_command_language REQUIRED) find_package(trajopt REQUIRED) +find_package(trajopt_ifopt REQUIRED) find_package(trajopt_sco REQUIRED) +find_package(trajopt_sqp REQUIRED) find_package(cmake_common_scripts REQUIRED) # serialization was required because ompl does not include find_dependency for its required dependencies find_package(Boost REQUIRED COMPONENTS serialization system filesystem program_options) @@ -163,12 +165,35 @@ target_include_directories(${PROJECT_NAME}_trajopt SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) -configure_package(NAMESPACE tesseract - TARGETS ${PROJECT_NAME}_core - ${PROJECT_NAME}_descartes - ${PROJECT_NAME}_ompl - ${PROJECT_NAME}_simple - ${PROJECT_NAME}_trajopt) +# Trajopt_ifopt Planner +add_library(${PROJECT_NAME}_trajopt_ifopt SHARED + src/trajopt_ifopt/trajopt_ifopt_collision_config.cpp + src/trajopt_ifopt/trajopt_ifopt_motion_planner.cpp + src/trajopt_ifopt/trajopt_ifopt_utils.cpp + src/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.cpp + src/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.cpp + src/trajopt_ifopt/problem_generators/default_problem_generator.cpp) +target_link_libraries(${PROJECT_NAME}_trajopt_ifopt PUBLIC ${Boost_LIBRARIES} ${PROJECT_NAME}_core trajopt::trajopt_ifopt trajopt::trajopt_sqp) +target_compile_options(${PROJECT_NAME}_trajopt_ifopt PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE}) +target_compile_options(${PROJECT_NAME}_trajopt_ifopt PUBLIC ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) +target_compile_definitions(${PROJECT_NAME}_trajopt_ifopt PUBLIC ${TESSERACT_COMPILE_DEFINITIONS}) +target_clang_tidy(${PROJECT_NAME}_trajopt_ifopt ARGUMENTS ${TESSERACT_CLANG_TIDY_ARGS} ENABLE ${TESSERACT_ENABLE_CLANG_TIDY}) +target_cxx_version(${PROJECT_NAME}_trajopt_ifopt PUBLIC VERSION ${TESSERACT_CXX_VERSION}) +target_code_coverage(${PROJECT_NAME}_trajopt_ifopt ALL EXCLUDE ${COVERAGE_EXCLUDE} ENABLE ${TESSERACT_ENABLE_TESTING}) +target_include_directories(${PROJECT_NAME}_trajopt_ifopt PUBLIC + "$" + "$") +target_include_directories(${PROJECT_NAME}_trajopt_ifopt SYSTEM PUBLIC + ${EIGEN3_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS}) + +configure_package(${PROJECT_NAME}_core + ${PROJECT_NAME}_descartes + ${PROJECT_NAME}_ompl + ${PROJECT_NAME}_simple + ${PROJECT_NAME}_trajopt + ${PROJECT_NAME}_trajopt_ifopt + ) # Mark cpp header files for installation install(DIRECTORY include/${PROJECT_NAME} diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h b/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h index 397deb3ab7d..61f1c25a967 100644 --- a/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h +++ b/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h @@ -34,6 +34,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include +#include #include #ifdef SWIG diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/trajopt_utils.h b/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/trajopt_utils.h index a125b7fdd2d..2a0c69ee389 100644 --- a/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/trajopt_utils.h +++ b/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/trajopt_utils.h @@ -66,10 +66,9 @@ trajopt::TermInfo::Ptr createJointWaypointTermInfo(const Eigen::VectorXd& j_wp, trajopt::TermInfo::Ptr createTolerancedJointWaypointTermInfo(const Eigen::VectorXd& j_wp, const Eigen::VectorXd& lower_tol, const Eigen::VectorXd& upper_tol, - int index, - const Eigen::VectorXd& coeffs, - trajopt::TermType type); - + int index, + const Eigen::VectorXd& coeffs, + trajopt::TermType type); trajopt::TermInfo::Ptr createCollisionTermInfo( int start_index, diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/problem_generators/default_problem_generator.h b/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/problem_generators/default_problem_generator.h new file mode 100644 index 00000000000..fe7fdeac65a --- /dev/null +++ b/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/problem_generators/default_problem_generator.h @@ -0,0 +1,46 @@ +/** + * @file default_problem_generator.h + * @brief Generates a trajopt problem from a planner request + * + * @author Levi Armstrong + * @date April 18, 2018 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_PROBLEM_GENERATOR_H +#define TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_PROBLEM_GENERATOR_H + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include + +namespace tesseract_planning +{ +std::shared_ptr +DefaultTrajoptProblemGenerator(const std::string& name, + const PlannerRequest& request, + const TrajOptIfoptPlanProfileMap& plan_profiles, + const TrajOptIfoptCompositeProfileMap& composite_profiles); + +} // namespace tesseract_planning +#endif diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h b/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h new file mode 100644 index 00000000000..7ded95cb346 --- /dev/null +++ b/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h @@ -0,0 +1,98 @@ +/** + * @file trajopt_default_composite_profile.h + * @brief + * + * @author Levi Armstrong + * @date June 18, 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_COMPOSITE_PROFILE_H +#define TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_COMPOSITE_PROFILE_H + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include + +namespace tesseract_planning +{ +class TrajOptIfoptDefaultCompositeProfile : public TrajOptIfoptCompositeProfile +{ +public: + TrajOptIfoptDefaultCompositeProfile() = default; + TrajOptIfoptDefaultCompositeProfile(const tinyxml2::XMLElement& xml_element); + + /** @brief Configuration info for collisions that are modeled as costs */ + trajopt::TrajOptCollisionConfig::Ptr collision_cost_config; + /** @brief Configuration info for collisions that are modeled as constraints */ + trajopt::TrajOptCollisionConfig::Ptr collision_constraint_config; + /** @brief If true, a joint velocity cost with a target of 0 will be applied for all timesteps Default: true*/ + bool smooth_velocities = true; + /** @brief This default to all ones, but allows you to weight different joints */ + Eigen::VectorXd velocity_coeff; + /** @brief If true, a joint acceleration cost with a target of 0 will be applied for all timesteps Default: false*/ + bool smooth_accelerations = true; + /** @brief This default to all ones, but allows you to weight different joints */ + Eigen::VectorXd acceleration_coeff; + /** @brief If true, a joint jerk cost with a target of 0 will be applied for all timesteps Default: false*/ + bool smooth_jerks = true; + /** @brief This default to all ones, but allows you to weight different joints */ + Eigen::VectorXd jerk_coeff; + + /** @brief Set the resolution at which state validity needs to be verified in order for a motion between two states + * to be considered valid in post checking of trajectory returned by TrajOptIfopt. + * + * The resolution is equal to longest_valid_segment_fraction * state_space.getMaximumExtent() + * + * Note: The planner takes the conservative of either longest_valid_segment_fraction or longest_valid_segment_length. + */ + double longest_valid_segment_fraction = 0.01; // 1% + + /** @brief Set the resolution at which state validity needs to be verified in order for a motion between two states + * to be considered valid. If norm(state1 - state0) > longest_valid_segment_length. + * + * Note: This gets converted to longest_valid_segment_fraction. + * longest_valid_segment_fraction = longest_valid_segment_length / state_space.getMaximumExtent() + */ + double longest_valid_segment_length = 0.5; + + /** @brief Special link collision cost distances */ + trajopt::SafetyMarginData::Ptr special_collision_cost{ nullptr }; + /** @brief Special link collision constraint distances */ + trajopt::SafetyMarginData::Ptr special_collision_constraint{ nullptr }; + + void apply(TrajOptIfoptProblem& problem, + int start_index, + int end_index, + const ManipulatorInfo& manip_info, + const std::vector& active_links, + const std::vector& fixed_indices) const override; + + tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; +}; +} // namespace tesseract_planning + +#endif // TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_COMPOSITE_PROFILE_H diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h b/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h new file mode 100644 index 00000000000..2e75ea2149a --- /dev/null +++ b/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h @@ -0,0 +1,78 @@ +/** + * @file TrajOptIfopt_default_plan_profile.h + * @brief + * + * @author Levi Armstrong + * @date June 18, 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_PLAN_PROFILE_H +#define TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_PLAN_PROFILE_H + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include +#include + +namespace tesseract_planning +{ +class TrajOptIfoptDefaultPlanProfile : public TrajOptIfoptPlanProfile +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + TrajOptIfoptDefaultPlanProfile() = default; + TrajOptIfoptDefaultPlanProfile(const tinyxml2::XMLElement& xml_element); + + Eigen::VectorXd cartesian_coeff{ Eigen::VectorXd::Constant(1, 1, 5) }; + Eigen::VectorXd joint_coeff{ Eigen::VectorXd::Constant(1, 1, 5) }; + TrajOptIfoptTermType term_type{ TrajOptIfoptTermType::CONSTRAINT }; + + void apply(TrajOptIfoptProblem& problem, + const CartesianWaypoint& cartesian_waypoint, + const Instruction& parent_instruction, + const ManipulatorInfo& manip_info, + const std::vector& active_links, + int index) const override; + + void apply(TrajOptIfoptProblem& problem, + const JointWaypoint& joint_waypoint, + const Instruction& parent_instruction, + const ManipulatorInfo& manip_info, + const std::vector& active_links, + int index) const override; + + tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + + // protected: + // void addConstraintErrorFunctions(TrajOptIfopt::ProblemConstructionInfo& pci, const std::vector& fixed_steps) + // const; + + // void addAvoidSingularity(TrajOptIfopt::ProblemConstructionInfo& pci, const std::vector& fixed_steps) const;s +}; +} // namespace tesseract_planning + +#endif // TESSERACT_MOTION_PLANNERS_TrajOptIfopt_IFOPT_DEFAULT_PLAN_PROFILE_H diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h b/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h new file mode 100644 index 00000000000..5d21624db45 --- /dev/null +++ b/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h @@ -0,0 +1,108 @@ +/** + * @file trajopt_profile.h + * @brief + * + * @author Levi Armstrong + * @date June 18, 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_PROFILE_H +#define TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_PROFILE_H + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include +#include +#include +#include +#include + +namespace tesseract_planning +{ +class TrajOptIfoptPlanProfile +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + TrajOptIfoptPlanProfile() = default; + virtual ~TrajOptIfoptPlanProfile() = default; + TrajOptIfoptPlanProfile(const TrajOptIfoptPlanProfile&) = default; + TrajOptIfoptPlanProfile& operator=(const TrajOptIfoptPlanProfile&) = default; + TrajOptIfoptPlanProfile(TrajOptIfoptPlanProfile&&) = default; + TrajOptIfoptPlanProfile& operator=(TrajOptIfoptPlanProfile&&) = default; + + virtual void apply(TrajOptIfoptProblem& problem, + const CartesianWaypoint& cartesian_waypoint, + const Instruction& parent_instruction, + const ManipulatorInfo& manip_info, + const std::vector& active_links, + int index) const = 0; + + virtual void apply(TrajOptIfoptProblem& problem, + const JointWaypoint& joint_waypoint, + const Instruction& parent_instruction, + const ManipulatorInfo& manip_info, + const std::vector& active_links, + int index) const = 0; + + virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0; +}; + +class TrajOptIfoptCompositeProfile +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + TrajOptIfoptCompositeProfile() = default; + virtual ~TrajOptIfoptCompositeProfile() = default; + TrajOptIfoptCompositeProfile(const TrajOptIfoptCompositeProfile&) = default; + TrajOptIfoptCompositeProfile& operator=(const TrajOptIfoptCompositeProfile&) = default; + TrajOptIfoptCompositeProfile(TrajOptIfoptCompositeProfile&&) = default; + TrajOptIfoptCompositeProfile& operator=(TrajOptIfoptCompositeProfile&&) = default; + + virtual void apply(TrajOptIfoptProblem& problem, + int start_index, + int end_index, + const ManipulatorInfo& manip_info, + const std::vector& active_links, + const std::vector& fixed_indices) const = 0; + + virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0; +}; + +using TrajOptIfoptCompositeProfileMap = std::unordered_map; +using TrajOptIfoptPlanProfileMap = std::unordered_map; + +} // namespace tesseract_planning +#ifdef SWIG +%template(TrajOptIfoptCompositeProfileMap) std::unordered_map>; +%template(TrajOptIfoptPlanProfileMap) std::unordered_map>; +#endif // SWIG + +#endif // TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_PROFILE_H diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_motion_planner.h b/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_motion_planner.h new file mode 100644 index 00000000000..1bbc920d2f4 --- /dev/null +++ b/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_motion_planner.h @@ -0,0 +1,132 @@ +/** + * @file trajopt_planner.h + * @brief Tesseract ROS Trajopt planner + * + * @author Levi Armstrong + * @date April 18, 2018 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2017, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_PLANNER_H +#define TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_PLANNER_H + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include + +namespace tesseract_planning +{ +class TrajOptIfoptMotionPlannerStatusCategory; + +using TrajOptIfoptProblemGeneratorFn = + std::function(const std::string&, + const PlannerRequest&, + const TrajOptIfoptPlanProfileMap&, + const TrajOptIfoptCompositeProfileMap&)>; + +class TrajOptIfoptMotionPlanner : public MotionPlanner +{ +public: + /** @brief Construct a basic planner */ + TrajOptIfoptMotionPlanner(); + + ~TrajOptIfoptMotionPlanner() override = default; + TrajOptIfoptMotionPlanner(const TrajOptIfoptMotionPlanner&) = delete; + TrajOptIfoptMotionPlanner& operator=(const TrajOptIfoptMotionPlanner&) = delete; + TrajOptIfoptMotionPlanner(TrajOptIfoptMotionPlanner&&) = delete; + TrajOptIfoptMotionPlanner& operator=(TrajOptIfoptMotionPlanner&&) = delete; + + const std::string& getName() const override; + + TrajOptIfoptProblemGeneratorFn problem_generator; + + /** + * @brief The available composite profiles + * + * Composite instruction is a way to namespace or organize your planning problem. The composite instruction has a + * profile which is used for applying multy waypoint costs and constraints like joint smoothing, collision avoidance, + * and velocity smoothing. + */ + TrajOptIfoptCompositeProfileMap composite_profiles; + + /** + * @brief The available plan profiles + * + * Plan instruction profiles are used to control waypoint specific information like fixed waypoint, toleranced + * waypoint, corner distance waypoint, etc. + */ + TrajOptIfoptPlanProfileMap plan_profiles; + + /** @brief Optimization parameters to be used (Optional) */ + // sco::BasicTrustRegionSQPParameters params; + + /** @brief Callback functions called on each iteration of the optimization (Optional) */ + std::vector callbacks; + + /** + * @brief Sets up the opimizer and solves a SQP problem read from json with no callbacks and dafault parameterss + * @param response The results of the optimization. Primary output is the optimized joint trajectory + * @param check_type The type of collision check to perform on the planned trajectory + * @param verbose Boolean indicating whether logging information about the motion planning solution should be printed + * to console + * @return true if optimization complete + */ + tesseract_common::StatusCode solve(const PlannerRequest& request, + PlannerResponse& response, + bool verbose = false) const override; + + bool checkUserInput(const PlannerRequest& request) const; + + bool terminate() override; + + void clear() override; + +protected: + /** @brief Name of planner */ + std::string name_{ "TrajOptIfopt" }; + + std::shared_ptr status_category_; /** @brief The planners status codes + */ +}; + +class TrajOptIfoptMotionPlannerStatusCategory : public tesseract_common::StatusCategory +{ +public: + TrajOptIfoptMotionPlannerStatusCategory(std::string name); + const std::string& name() const noexcept override; + std::string message(int code) const override; + + enum + { + SolutionFound = 0, + ErrorInvalidInput = -1, + FailedToFindValidSolution = -3, + }; + +private: + std::string name_; +}; + +} // namespace tesseract_planning +#endif // TESSERACT_PLANNING_TRAJOPT_IFOPT_PLANNER_H diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_problem.h b/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_problem.h new file mode 100644 index 00000000000..7be01f58113 --- /dev/null +++ b/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_problem.h @@ -0,0 +1,62 @@ +/** + * @file trajopt_ifopt_problem.h + * @brief + * + * @author Levi Armstrong + * @date June 18, 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_PROBLEM_H +#define TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_PROBLEM_H + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include + +namespace tesseract_planning +{ +enum class TrajOptIfoptTermType +{ + CONSTRAINT, + SQUARED_COST +}; + +struct TrajOptIfoptProblem +{ + // These are required for Tesseract to configure Descartes + tesseract_environment::Environment::ConstPtr environment; + tesseract_environment::EnvState::ConstPtr env_state; + + // Kinematic Objects + tesseract_kinematics::ForwardKinematics::ConstPtr manip_fwd_kin; + tesseract_kinematics::InverseKinematics::ConstPtr manip_inv_kin; + + std::shared_ptr nlp; + std::vector vars; +}; + +} // namespace tesseract_planning +#endif // TESSERACT_MOTION_PLANNERS_DESCARTES_PROBLEM_H diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_utils.h b/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_utils.h new file mode 100644 index 00000000000..8e08ab7c416 --- /dev/null +++ b/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_utils.h @@ -0,0 +1,106 @@ +/** + * @file trajopt_utils.h + * @brief + * + * @author Levi Armstrong + * @date June 18, 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_UTILS_H +#define TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_UTILS_H + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include +#include + +namespace tesseract_planning +{ +ifopt::ConstraintSet::Ptr createCartesianPositionConstraint(const CartesianWaypoint& cart_waypoint, + trajopt::JointPosition::ConstPtr var, + const trajopt::CartPosKinematicInfo::ConstPtr& kin_info, + const Eigen::VectorXd& coeffs); + +bool addCartesianPositionConstraint(std::shared_ptr nlp, + const CartesianWaypoint& cart_waypoint, + trajopt::JointPosition::ConstPtr var, + const trajopt::CartPosKinematicInfo::ConstPtr& kin_info, + const Eigen::Ref& coeff); + +bool addCartesianPositionSquaredCost(std::shared_ptr nlp, + const CartesianWaypoint& cart_waypoint, + trajopt::JointPosition::ConstPtr var, + const trajopt::CartPosKinematicInfo::ConstPtr& kin_info, + const Eigen::Ref& coeff); + +ifopt::ConstraintSet::Ptr createJointPositionConstraint(const JointWaypoint& joint_waypoint, + trajopt::JointPosition::ConstPtr var, + const Eigen::VectorXd& /*coeffs*/); + +bool addJointPositionConstraint(std::shared_ptr nlp, + const JointWaypoint& joint_waypoint, + trajopt::JointPosition::ConstPtr var, + const Eigen::Ref& coeff); + +bool addJointPositionSquaredCost(std::shared_ptr nlp, + const JointWaypoint& joint_waypoint, + trajopt::JointPosition::ConstPtr var, + const Eigen::Ref& coeff); + +std::vector +createCollisionConstraints(std::vector vars, + const tesseract_environment::Environment::ConstPtr& env, + const ManipulatorInfo& manip_info, + const trajopt::TrajOptCollisionConfig::ConstPtr& config); + +bool addCollisionConstraint(std::shared_ptr nlp, + std::vector vars, + const tesseract_environment::Environment::ConstPtr& env, + const ManipulatorInfo& manip_info, + const trajopt::TrajOptCollisionConfig::ConstPtr& config); + +bool addCollisionSquaredCost(std::shared_ptr nlp, + std::vector vars, + const tesseract_environment::Environment::ConstPtr& env, + const ManipulatorInfo& manip_info, + const trajopt::TrajOptCollisionConfig::ConstPtr& config); + +ifopt::ConstraintSet::Ptr createJointVelocityConstraint(const Eigen::Ref& target, + const std::vector& vars, + const Eigen::VectorXd& coeffs); + +bool addJointVelocityConstraint(std::shared_ptr nlp, + std::vector vars, + const Eigen::Ref& coeff); + +bool addJointVelocitySquaredCost(std::shared_ptr nlp, + std::vector vars, + const Eigen::Ref& coeff); + +} // namespace tesseract_planning + +#endif // TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_UTILS_H diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/package.xml b/tesseract/tesseract_planning/tesseract_motion_planners/package.xml index 2c0f1de58a2..7198fda6b91 100644 --- a/tesseract/tesseract_planning/tesseract_motion_planners/package.xml +++ b/tesseract/tesseract_planning/tesseract_motion_planners/package.xml @@ -21,7 +21,9 @@ libconsole-bridge-dev ompl trajopt + trajopt_ifopt trajopt_sco + trajopt_sqp gtest tesseract_support diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/problem_generators/default_problem_generator.cpp b/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/problem_generators/default_problem_generator.cpp index ddb8f9ec714..13e830efe33 100644 --- a/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/problem_generators/default_problem_generator.cpp +++ b/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/problem_generators/default_problem_generator.cpp @@ -173,7 +173,7 @@ DefaultTrajoptProblemGenerator(const std::string& name, std::string profile = getProfileString(plan_instruction->getProfile(), name, request.plan_profile_remapping); TrajOptPlanProfile::ConstPtr cur_plan_profile = getProfile(profile, plan_profiles, std::make_shared()); - if (!start_plan_profile) + if (!cur_plan_profile) throw std::runtime_error("TrajOptPlannerUniversalConfig: Invalid profile"); if (plan_instruction->isLinear()) diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/trajopt_utils.cpp b/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/trajopt_utils.cpp index dd8eda1d4d6..a1333158b9f 100644 --- a/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/trajopt_utils.cpp +++ b/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/trajopt_utils.cpp @@ -141,9 +141,9 @@ trajopt::TermInfo::Ptr createJointWaypointTermInfo(const Eigen::VectorXd& j_wp, trajopt::TermInfo::Ptr createTolerancedJointWaypointTermInfo(const Eigen::VectorXd& j_wp, const Eigen::VectorXd& lower_tol, const Eigen::VectorXd& upper_tol, - int index, - const Eigen::VectorXd& coeffs, - trajopt::TermType type) + int index, + const Eigen::VectorXd& coeffs, + trajopt::TermType type) { auto joint_info = std::make_shared(); if (coeffs.size() == 1) @@ -152,8 +152,10 @@ trajopt::TermInfo::Ptr createTolerancedJointWaypointTermInfo(const Eigen::Vector joint_info->coeffs = std::vector(coeffs.data(), coeffs.data() + coeffs.rows() * coeffs.cols()); joint_info->targets = std::vector(j_wp.data(), j_wp.data() + j_wp.rows() * j_wp.cols()); - joint_info->lower_tols = std::vector(lower_tol.data(), lower_tol.data() + lower_tol.rows() * lower_tol.cols()); - joint_info->upper_tols = std::vector(upper_tol.data(), upper_tol.data() + upper_tol.rows() * upper_tol.cols()); + joint_info->lower_tols = + std::vector(lower_tol.data(), lower_tol.data() + lower_tol.rows() * lower_tol.cols()); + joint_info->upper_tols = + std::vector(upper_tol.data(), upper_tol.data() + upper_tol.rows() * upper_tol.cols()); joint_info->first_step = index; joint_info->last_step = index; joint_info->name = "joint_waypoint_" + std::to_string(index); @@ -162,7 +164,6 @@ trajopt::TermInfo::Ptr createTolerancedJointWaypointTermInfo(const Eigen::Vector return joint_info; } - trajopt::TermInfo::Ptr createCollisionTermInfo(int start_index, int end_index, double collision_safety_margin, diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt_ifopt/problem_generators/default_problem_generator.cpp b/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt_ifopt/problem_generators/default_problem_generator.cpp new file mode 100644 index 00000000000..b41008e65d8 --- /dev/null +++ b/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt_ifopt/problem_generators/default_problem_generator.cpp @@ -0,0 +1,336 @@ +/** + * @file default_problem_generator.cpp + * @brief Generates a trajopt problem from a planner request + * + * @author Levi Armstrong + * @date April 18, 2018 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace tesseract_planning +{ +/// @todo: Restructure this into several smaller functions that are testable and easier to understand +std::shared_ptr +DefaultTrajoptIfoptProblemGenerator(const std::string& name, + const PlannerRequest& request, + const TrajOptIfoptPlanProfileMap& plan_profiles, + const TrajOptIfoptCompositeProfileMap& composite_profiles) +{ + // Create the problem + auto problem = std::make_shared(); + problem->env_state = request.env_state; + problem->nlp = std::make_shared(); + + // Assume all the plan instructions have the same manipulator as the composite + assert(!request.instructions.getManipulatorInfo().empty()); + const ManipulatorInfo& composite_mi = request.instructions.getManipulatorInfo(); + const std::string& manipulator = composite_mi.manipulator; + auto kin = request.env->getManipulatorManager()->getFwdKinematicSolver(manipulator); + auto inv_kin = request.env->getManipulatorManager()->getInvKinematicSolver(manipulator); + assert(kin); + problem->manip_fwd_kin = kin; + problem->manip_inv_kin = inv_kin; + + // Get kinematics information + tesseract_environment::Environment::ConstPtr env = request.env; + tesseract_environment::AdjacencyMap map( + env->getSceneGraph(), kin->getActiveLinkNames(), env->getCurrentState()->link_transforms); + const std::vector& active_links = map.getActiveLinkNames(); + + // Flatten input instructions + auto instructions_flat = flattenProgram(request.instructions); + auto seed_flat = flattenProgramToPattern(request.seed, request.instructions); + + // ---------------- + // Setup variables + // ---------------- + // Create the vector of variables to be optimized + Eigen::MatrixX2d joint_limits_eigen = kin->getLimits().joint_limits; + + // Create a variable for each instruction in the seed, setting to correct initial state + for (std::size_t i = 0; i < seed_flat.size(); i++) + { + assert(isMoveInstruction(seed_flat[i])); + auto var = std::make_shared( + getJointPosition(seed_flat[i].get().cast_const()->getWaypoint()), + kin->getJointNames(), + "Joint_Position_" + std::to_string(i)); + var->SetBounds(joint_limits_eigen); + problem->vars.push_back(var); + problem->nlp->AddVariableSet(var); + } + + // Store fixed steps + std::vector fixed_steps; + + // ---------------- + // Setup start waypoint + // ---------------- + int start_index = 0; // If it has a start instruction then skip first instruction in instructions_flat + int index = 0; + std::string profile; + Waypoint start_waypoint = NullWaypoint(); + Instruction placeholder_instruction = NullInstruction(); + const Instruction* start_instruction = nullptr; + if (request.instructions.hasStartInstruction()) + { + assert(isPlanInstruction(request.instructions.getStartInstruction())); + start_instruction = &(request.instructions.getStartInstruction()); + if (isPlanInstruction(*start_instruction)) + { + const auto* temp = start_instruction->cast_const(); + assert(temp->isStart()); + start_waypoint = temp->getWaypoint(); + profile = temp->getProfile(); + } + else + { + throw std::runtime_error("DefaultTrajoptIfoptProblemGenerator: Unsupported start instruction type!"); + } + ++start_index; + } + // If not start instruction is given, take the current state + else + { + Eigen::VectorXd current_jv = request.env_state->getJointValues(kin->getJointNames()); + StateWaypoint swp(kin->getJointNames(), current_jv); + + MoveInstruction temp_move(swp, MoveInstructionType::START); + placeholder_instruction = temp_move; + start_instruction = &placeholder_instruction; + start_waypoint = swp; + } + + // ---------------- + // Translate TCL for PlanInstructions + // ---------------- + // Transform plan instructions into trajopt cost and constraints + for (int i = start_index; i < static_cast(instructions_flat.size()); i++) + { + const auto& instruction = instructions_flat[static_cast(i)].get(); + if (isPlanInstruction(instruction)) + { + assert(isPlanInstruction(instruction)); + const auto* plan_instruction = instruction.cast_const(); + + // If plan instruction has manipulator information then use it over the one provided by the composite. + ManipulatorInfo manip_info = composite_mi.getCombined(plan_instruction->getManipulatorInfo()); + Eigen::Isometry3d tcp = request.env->findTCP(manip_info); + + assert(isCompositeInstruction(seed_flat[static_cast(i)].get())); + const auto* seed_composite = + seed_flat[static_cast(i)].get().cast_const(); + auto interpolate_cnt = static_cast(seed_composite->size()); + + std::string profile = getProfileString(plan_instruction->getProfile(), name, request.plan_profile_remapping); + TrajOptIfoptPlanProfile::ConstPtr cur_plan_profile = getProfile( + profile, plan_profiles, std::make_shared()); + if (!cur_plan_profile) + throw std::runtime_error("DefaultTrajoptIfoptProblemGenerator: Invalid profile"); + + if (plan_instruction->isLinear()) + { + if (isCartesianWaypoint(plan_instruction->getWaypoint())) + { + const auto* cur_wp = plan_instruction->getWaypoint().cast_const(); + + Eigen::Isometry3d prev_pose = Eigen::Isometry3d::Identity(); + if (isCartesianWaypoint(start_waypoint)) + { + prev_pose = *(start_waypoint.cast_const()); + } + else if (isJointWaypoint(start_waypoint) || isStateWaypoint(start_waypoint)) + { + const Eigen::VectorXd& position = getJointPosition(start_waypoint); + if (!kin->calcFwdKin(prev_pose, position)) + throw std::runtime_error("DefaultTrajoptIfoptProblemGenerator: failed to solve forward kinematics!"); + + prev_pose = env->getCurrentState()->link_transforms.at(kin->getBaseLinkName()) * prev_pose * tcp; + } + else + { + throw std::runtime_error("DefaultTrajoptIfoptProblemGenerator: unknown waypoint type."); + } + + tesseract_common::VectorIsometry3d poses = interpolate(prev_pose, *cur_wp, interpolate_cnt); + // Add intermediate points with path costs and constraints + for (std::size_t p = 1; p < poses.size() - 1; ++p) + { + /** @todo Write a path constraint for this*/ + cur_plan_profile->apply(*problem, poses[p], *plan_instruction, composite_mi, active_links, index); + + ++index; + } + + // Add final point with waypoint + cur_plan_profile->apply(*problem, *cur_wp, *plan_instruction, composite_mi, active_links, index); + + ++index; + } + else if (isJointWaypoint(plan_instruction->getWaypoint()) || isStateWaypoint(plan_instruction->getWaypoint())) + { + const JointWaypoint* cur_position; + std::shared_ptr temp; + if (isJointWaypoint(plan_instruction->getWaypoint())) + { + cur_position = plan_instruction->getWaypoint().cast_const(); + } + else + { + const StateWaypoint* state_waypoint = plan_instruction->getWaypoint().cast_const(); + temp = std::make_shared(state_waypoint->joint_names, state_waypoint->position); + cur_position = temp.get(); + } + + Eigen::Isometry3d cur_pose = Eigen::Isometry3d::Identity(); + if (!kin->calcFwdKin(cur_pose, *cur_position)) + throw std::runtime_error("DefaultTrajoptIfoptProblemGenerator: failed to solve forward kinematics!"); + + cur_pose = env->getCurrentState()->link_transforms.at(kin->getBaseLinkName()) * cur_pose * tcp; + + Eigen::Isometry3d prev_pose = Eigen::Isometry3d::Identity(); + if (isCartesianWaypoint(start_waypoint)) + { + prev_pose = *(start_waypoint.cast_const()); + } + else if (isJointWaypoint(start_waypoint) || isStateWaypoint(start_waypoint)) + { + const Eigen::VectorXd& position = getJointPosition(start_waypoint); + if (!kin->calcFwdKin(prev_pose, position)) + throw std::runtime_error("DefaultTrajoptIfoptProblemGenerator: failed to solve forward kinematics!"); + + prev_pose = env->getCurrentState()->link_transforms.at(kin->getBaseLinkName()) * prev_pose * tcp; + } + else + { + throw std::runtime_error("TrajOptPlannerUniversalConfig: uknown waypoint type."); + } + + tesseract_common::VectorIsometry3d poses = interpolate(prev_pose, cur_pose, interpolate_cnt); + // Add intermediate points with path costs and constraints + for (std::size_t p = 1; p < poses.size() - 1; ++p) + { + /** @todo Add path constraint for this */ + cur_plan_profile->apply(*problem, poses[p], *plan_instruction, composite_mi, active_links, index); + + ++index; + } + + // Add final point with waypoint + cur_plan_profile->apply(*problem, *cur_position, *plan_instruction, composite_mi, active_links, index); + + ++index; + } + else + { + throw std::runtime_error("DefaultTrajoptIfoptProblemGenerator: unknown waypoint type"); + } + } + else if (plan_instruction->isFreespace()) + { + if (isJointWaypoint(plan_instruction->getWaypoint()) || isStateWaypoint(plan_instruction->getWaypoint())) + { + const JointWaypoint* cur_position; + std::shared_ptr temp; + if (isJointWaypoint(plan_instruction->getWaypoint())) + { + cur_position = plan_instruction->getWaypoint().cast_const(); + } + else + { + const StateWaypoint* state_waypoint = plan_instruction->getWaypoint().cast_const(); + temp = std::make_shared(state_waypoint->joint_names, state_waypoint->position); + cur_position = temp.get(); + } + + // Increment index to account for intermediate points in seed + for (std::size_t s = 0; s < seed_composite->size() - 1; ++s) + { + ++i; + } + + /** @todo Should check that the joint names match the order of the manipulator */ + cur_plan_profile->apply(*problem, *cur_position, *plan_instruction, manip_info, active_links, i); + + // Add to fixed indices + fixed_steps.push_back(i); + } + else if (isCartesianWaypoint(plan_instruction->getWaypoint())) + { + const auto* cur_wp = plan_instruction->getWaypoint().cast_const(); + + // Increment index to account for intermediate points in seed + for (std::size_t s = 0; s < seed_composite->size() - 1; ++s) + { + ++i; + } + + /** @todo Should check that the joint names match the order of the manipulator */ + cur_plan_profile->apply(*problem, *cur_wp, *plan_instruction, manip_info, active_links, i); + + // Add to fixed indices + fixed_steps.push_back(i); + } + else + { + throw std::runtime_error("DefaultTrajoptIfoptProblemGenerator: unknown waypoint type"); + } + + ++i; + } + else + { + throw std::runtime_error("Unsupported!"); + } + + start_waypoint = plan_instruction->getWaypoint(); + } + } + + // ---------------- + // Translate TCL for CompositeInstructions + // ---------------- + profile = getProfileString(request.instructions.getProfile(), name, request.composite_profile_remapping); + TrajOptIfoptCompositeProfile::ConstPtr cur_composite_profile = getProfile( + profile, composite_profiles, std::make_shared()); + if (!cur_composite_profile) + throw std::runtime_error("DefaultTrajoptIfoptProblemGenerator: Invalid profile"); + + cur_composite_profile->apply( + *problem, 0, static_cast(problem->vars.size()), composite_mi, active_links, fixed_steps); + + // ---------------- + // Return problem + // ---------------- + + return problem; +} + +} // namespace tesseract_planning diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.cpp b/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.cpp new file mode 100644 index 00000000000..891678c7d24 --- /dev/null +++ b/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.cpp @@ -0,0 +1,69 @@ +/** + * @file trajopt_default_composite_profile.cpp + * @brief + * + * @author Levi Armstrong + * @date June 18, 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include + +#include +#include +#include +#include +#include + +namespace tesseract_planning +{ +void TrajOptIfoptDefaultCompositeProfile::apply(TrajOptIfoptProblem& problem, + int start_index, + int end_index, + const ManipulatorInfo& manip_info, + const std::vector& /*active_links*/, + const std::vector& /*fixed_indices*/) const +{ + const std::vector vars(&problem.vars[static_cast(start_index)], + &problem.vars[static_cast(end_index)]); + + if (collision_constraint_config->type != tesseract_collision::CollisionEvaluatorType::NONE) + addCollisionConstraint(problem.nlp, vars, problem.environment, manip_info, collision_constraint_config); + + if (collision_cost_config->type != tesseract_collision::CollisionEvaluatorType::NONE) + addCollisionSquaredCost(problem.nlp, vars, problem.environment, manip_info, collision_cost_config); + + if (smooth_velocities) + addJointVelocitySquaredCost(problem.nlp, vars, velocity_coeff); + + if (smooth_accelerations) + CONSOLE_BRIDGE_logWarn("Acceleration smoothing not yet supported by trajopt_ifopt. PRs welcome"); + + if (smooth_jerks) + CONSOLE_BRIDGE_logWarn("Jerk smoothing not yet supported by trajopt_ifopt. PRs welcome"); +} + +} // namespace tesseract_planning diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.cpp b/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.cpp new file mode 100644 index 00000000000..cb0d6c034c7 --- /dev/null +++ b/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.cpp @@ -0,0 +1,102 @@ +/** + * @file trajopt_default_plan_profile.cpp + * @brief + * + * @author Levi Armstrong + * @date June 18, 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include + +#include +#include +#include +#include +#include + +namespace tesseract_planning +{ +void TrajOptIfoptDefaultPlanProfile::apply(TrajOptIfoptProblem& problem, + const CartesianWaypoint& cartesian_waypoint, + const Instruction& parent_instruction, + const ManipulatorInfo& manip_info, + const std::vector& active_links, + int index) const +{ + assert(isPlanInstruction(parent_instruction)); + const auto* base_instruction = parent_instruction.cast_const(); + assert(!(manip_info.empty() && base_instruction->getManipulatorInfo().empty())); + const ManipulatorInfo& mi = + (base_instruction->getManipulatorInfo().empty()) ? manip_info : base_instruction->getManipulatorInfo(); + + const auto& env = problem.environment; + auto adjacency_map = std::make_shared( + env->getSceneGraph(), problem.manip_fwd_kin->getActiveLinkNames(), env->getCurrentState()->link_transforms); + + // TODO: Fix 3rd arg, TCP, etc + auto kin_info = std::make_shared( + problem.manip_fwd_kin, adjacency_map, Eigen::Isometry3d::Identity(), manip_info.manipulator); + + /* Check if this cartesian waypoint is dynamic + * (i.e. defined relative to a frame that will move with the kinematic chain) + */ + auto it = std::find(active_links.begin(), active_links.end(), mi.working_frame); + if (it != active_links.end()) + { + CONSOLE_BRIDGE_logWarn("Dynamic cartesian terms are not supported by trajopt_ifopt. PRs welcome"); + } + else + { + auto idx = static_cast(index); + switch (term_type) + { + case TrajOptIfoptTermType::CONSTRAINT: + addCartesianPositionConstraint(problem.nlp, cartesian_waypoint, problem.vars[idx], kin_info, cartesian_coeff); + + break; + case TrajOptIfoptTermType::SQUARED_COST: + addCartesianPositionConstraint(problem.nlp, cartesian_waypoint, problem.vars[idx], kin_info, cartesian_coeff); + break; + } + } +} + +void TrajOptIfoptDefaultPlanProfile::apply(TrajOptIfoptProblem& problem, + const JointWaypoint& joint_waypoint, + const Instruction& /*parent_instruction*/, + const ManipulatorInfo& /*manip_info*/, + const std::vector& /*active_links*/, + int index) const +{ + auto idx = static_cast(index); + switch (term_type) + { + case TrajOptIfoptTermType::CONSTRAINT: + addJointPositionConstraint(problem.nlp, joint_waypoint, problem.vars[idx], joint_coeff); + break; + case TrajOptIfoptTermType::SQUARED_COST: + addJointPositionConstraint(problem.nlp, joint_waypoint, problem.vars[idx], joint_coeff); + break; + } +} + +} // namespace tesseract_planning diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt_ifopt/trajopt_ifopt_collision_config.cpp b/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt_ifopt/trajopt_ifopt_collision_config.cpp new file mode 100644 index 00000000000..3b7b3ed8334 --- /dev/null +++ b/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt_ifopt/trajopt_ifopt_collision_config.cpp @@ -0,0 +1,38 @@ +/** + * @file trajopt_collision_config.cpp + * @brief TrajOpt collision configuration settings + * + * @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 +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include + +namespace tesseract_planning +{ +} // namespace tesseract_planning diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt_ifopt/trajopt_ifopt_motion_planner.cpp b/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt_ifopt/trajopt_ifopt_motion_planner.cpp new file mode 100644 index 00000000000..34d1b9aa667 --- /dev/null +++ b/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt_ifopt/trajopt_ifopt_motion_planner.cpp @@ -0,0 +1,216 @@ +/** + * @file trajopt_planner.cpp + * @brief Tesseract ROS Trajopt planner + * + * @author Levi Armstrong + * @date April 18, 2018 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2017, 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 +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include +#include +#include + +using namespace trajopt; + +namespace tesseract_planning +{ +TrajOptIfoptMotionPlannerStatusCategory::TrajOptIfoptMotionPlannerStatusCategory(std::string name) + : name_(std::move(name)) +{ +} +const std::string& TrajOptIfoptMotionPlannerStatusCategory::name() const noexcept { return name_; } +std::string TrajOptIfoptMotionPlannerStatusCategory::message(int code) const +{ + switch (code) + { + case SolutionFound: + { + return "Found valid solution"; + } + case ErrorInvalidInput: + { + return "Input to planner is invalid. Check that instructions and seed are compatible"; + } + case FailedToFindValidSolution: + { + return "Failed to find valid solution"; + } + default: + { + assert(false); + return ""; + } + } +} + +TrajOptIfoptMotionPlanner::TrajOptIfoptMotionPlanner() + : status_category_(std::make_shared(name_)) +{ +} + +const std::string& TrajOptIfoptMotionPlanner::getName() const { return name_; } + +bool TrajOptIfoptMotionPlanner::terminate() +{ + CONSOLE_BRIDGE_logWarn("Termination of ongoing optimization is not implemented yet"); + return false; +} + +void TrajOptIfoptMotionPlanner::clear() +{ + // params = sco::BasicTrustRegionSQPParameters(); + callbacks.clear(); +} + +tesseract_common::StatusCode TrajOptIfoptMotionPlanner::solve(const PlannerRequest& request, + PlannerResponse& response, + bool verbose) const +{ + if (!checkUserInput(request)) + { + response.status = + tesseract_common::StatusCode(TrajOptIfoptMotionPlannerStatusCategory::ErrorInvalidInput, status_category_); + return response.status; + } + std::shared_ptr problem; + if (request.data) + { + problem = std::static_pointer_cast(request.data); + } + else + { + if (!problem_generator) + { + CONSOLE_BRIDGE_logError("TrajOptIfoptPlanner does not have a problem generator specified."); + response.status = + tesseract_common::StatusCode(TrajOptIfoptMotionPlannerStatusCategory::ErrorInvalidInput, status_category_); + return response.status; + } + problem = problem_generator(name_, request, plan_profiles, composite_profiles); + response.data = problem; + } + + // Create optimizer + /** @todo Enable solver selection (e.g. IPOPT) */ + auto qp_solver = std::make_shared(); + trajopt_sqp::TrustRegionSQPSolver solver(qp_solver); + /** @todo Set these as the defaults in trajopt and allow setting */ + qp_solver->solver_.settings()->setVerbosity(verbose); + qp_solver->solver_.settings()->setWarmStart(true); + qp_solver->solver_.settings()->setPolish(true); + qp_solver->solver_.settings()->setAdaptiveRho(false); + qp_solver->solver_.settings()->setMaxIteraction(8192); + qp_solver->solver_.settings()->setAbsoluteTolerance(1e-4); + qp_solver->solver_.settings()->setRelativeTolerance(1e-6); + + // Add all callbacks + for (const trajopt_sqp::SQPCallback::Ptr& callback : callbacks) + { + solver.registerCallback(callback); + } + + // solve + solver.verbose = verbose; + + auto tStart = boost::posix_time::second_clock::local_time(); + solver.Solve(*(problem->nlp)); + CONSOLE_BRIDGE_logInform("planning time: %.3f", (boost::posix_time::second_clock::local_time() - tStart).seconds()); + + // Check success + if (solver.getStatus() != trajopt_sqp::SQPStatus::NLP_CONVERGED) + { + response.status = tesseract_common::StatusCode(TrajOptIfoptMotionPlannerStatusCategory::FailedToFindValidSolution, + status_category_); + return response.status; + } + + // Get the results - This can likely be simplified if we get rid of the traj array + Eigen::VectorXd x = problem->nlp->GetOptVariables()->GetValues(); + Eigen::Map trajectory(x.data(), + static_cast(problem->vars.size()), + static_cast(problem->vars[0]->GetValues().size())); + + // Flatten the results to make them easier to process + response.results = request.seed; + auto results_flattened = flattenProgramToPattern(response.results, request.instructions); + auto instructions_flattened = flattenProgram(request.instructions); + + // Loop over the flattened results and add them to response if the input was a plan instruction + Eigen::Index result_index = 0; + for (std::size_t idx = 0; idx < instructions_flattened.size(); idx++) + { + // If idx is zero then this should be the start instruction + assert((idx == 0) ? isPlanInstruction(instructions_flattened.at(idx).get()) : true); + assert((idx == 0) ? isMoveInstruction(results_flattened[idx].get()) : true); + if (isPlanInstruction(instructions_flattened.at(idx).get())) + { + // This instruction corresponds to a composite. Set all results in that composite to the results + const auto* plan_instruction = instructions_flattened.at(idx).get().cast_const(); + if (plan_instruction->isStart()) + { + assert(idx == 0); + assert(isMoveInstruction(results_flattened[idx].get())); + auto* move_instruction = results_flattened[idx].get().cast(); + move_instruction->getWaypoint().cast()->position = trajectory.row(result_index++); + } + else + { + auto* move_instructions = results_flattened[idx].get().cast(); + for (auto& instruction : *move_instructions) + instruction.cast()->getWaypoint().cast()->position = + trajectory.row(result_index++); + } + } + } + + response.status = + tesseract_common::StatusCode(TrajOptIfoptMotionPlannerStatusCategory::SolutionFound, status_category_); + return response.status; +} + +bool TrajOptIfoptMotionPlanner::checkUserInput(const PlannerRequest& request) const +{ + // Check that parameters are valid + if (request.env == nullptr) + { + CONSOLE_BRIDGE_logError("In TrajOptPlannerUniversalConfig: tesseract is a required parameter and has not been set"); + return false; + } + + if (request.instructions.empty()) + { + CONSOLE_BRIDGE_logError("TrajOptPlannerUniversalConfig requires at least one instruction"); + return false; + } + + return true; +} + +} // namespace tesseract_planning diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt_ifopt/trajopt_ifopt_utils.cpp b/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt_ifopt/trajopt_ifopt_utils.cpp new file mode 100644 index 00000000000..0f18d313f4b --- /dev/null +++ b/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt_ifopt/trajopt_ifopt_utils.cpp @@ -0,0 +1,222 @@ +/** + * @file trajopt_ifopt_utils.cpp + * @brief + * + * @author Levi Armstrong + * @date June 18, 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#include +#include +#include +#include +#include +#include + +#include + +namespace tesseract_planning +{ +ifopt::ConstraintSet::Ptr createCartesianPositionConstraint(const CartesianWaypoint& cart_waypoint, + trajopt::JointPosition::ConstPtr var, + const trajopt::CartPosKinematicInfo::ConstPtr& kin_info, + const Eigen::VectorXd& /*coeffs*/) +{ + auto constraint = + std::make_shared(cart_waypoint.waypoint, kin_info, var, "CartPos_" + var->GetName()); + return constraint; +} + +bool addCartesianPositionConstraint(std::shared_ptr nlp, + const CartesianWaypoint& cart_waypoint, + trajopt::JointPosition::ConstPtr var, + const trajopt::CartPosKinematicInfo::ConstPtr& kin_info, + const Eigen::Ref& coeff) +{ + auto constraint = createCartesianPositionConstraint(cart_waypoint, var, kin_info, coeff); + nlp->AddConstraintSet(constraint); + return true; +} + +bool addCartesianPositionSquaredCost(std::shared_ptr nlp, + const CartesianWaypoint& cart_waypoint, + trajopt::JointPosition::ConstPtr var, + const trajopt::CartPosKinematicInfo::ConstPtr& kin_info, + const Eigen::Ref& coeff) +{ + auto constraint = createCartesianPositionConstraint(cart_waypoint, var, kin_info, coeff); + + // Must link the variables to the constraint since that happens in AddConstraintSet + constraint->LinkWithVariables(nlp->GetOptVariables()); + auto cost = std::make_shared(constraint, coeff); + nlp->AddCostSet(cost); + return true; +} + +ifopt::ConstraintSet::Ptr createJointPositionConstraint(const JointWaypoint& joint_waypoint, + trajopt::JointPosition::ConstPtr var, + const Eigen::VectorXd& /*coeffs*/) +{ + assert(var); + std::vector vars(1, var); + + ifopt::ConstraintSet::Ptr constraint; + if (!joint_waypoint.isToleranced()) + { + constraint = + std::make_shared(joint_waypoint.waypoint, vars, "JointPos_" + var->GetName()); + } + else + { + Eigen::VectorXd lower_limit = joint_waypoint.waypoint + joint_waypoint.lower_tolerance; + Eigen::VectorXd upper_limit = joint_waypoint.waypoint + joint_waypoint.upper_tolerance; + auto bounds = trajopt::toBounds(lower_limit, upper_limit); + constraint = std::make_shared(bounds, vars, "JointPos_" + var->GetName()); + } + + return constraint; +} + +bool addJointPositionConstraint(std::shared_ptr nlp, + const JointWaypoint& joint_waypoint, + trajopt::JointPosition::ConstPtr var, + const Eigen::Ref& coeff) +{ + auto constraint = createJointPositionConstraint(joint_waypoint, var, coeff); + nlp->AddConstraintSet(constraint); + return true; +} + +bool addJointPositionSquaredCost(std::shared_ptr nlp, + const JointWaypoint& joint_waypoint, + trajopt::JointPosition::ConstPtr var, + const Eigen::Ref& coeff) +{ + auto vel_constraint = createJointPositionConstraint(joint_waypoint, var, coeff); + + // Must link the variables to the constraint since that happens in AddConstraintSet + vel_constraint->LinkWithVariables(nlp->GetOptVariables()); + auto vel_cost = std::make_shared(vel_constraint, coeff); + nlp->AddCostSet(vel_cost); + return true; +} + +std::vector +createCollisionConstraints(std::vector vars, + const tesseract_environment::Environment::ConstPtr& env, + const ManipulatorInfo& manip_info, + const trajopt::TrajOptCollisionConfig::ConstPtr& config) +{ + std::vector constraints; + if (config->type == tesseract_collision::CollisionEvaluatorType::NONE) + return constraints; + + if (config->type != tesseract_collision::CollisionEvaluatorType::DISCRETE) + CONSOLE_BRIDGE_logWarn("Only Single timestep collision is supported for trajopt_ifopt. PRs welcome"); + + // Add a collision cost for all steps + for (const auto& var : vars) + { + auto kin = env->getManipulatorManager()->getFwdKinematicSolver(manip_info.manipulator); + auto adjacency_map = std::make_shared( + env->getSceneGraph(), kin->getActiveLinkNames(), env->getCurrentState()->link_transforms); + + auto collision_evaluator = std::make_shared( + kin, env, adjacency_map, Eigen::Isometry3d::Identity(), *config); + + constraints.push_back(std::make_shared(collision_evaluator, var)); + } + + return constraints; +} + +bool addCollisionConstraint(std::shared_ptr nlp, + std::vector vars, + const tesseract_environment::Environment::ConstPtr& env, + const ManipulatorInfo& manip_info, + const trajopt::TrajOptCollisionConfig::ConstPtr& config) +{ + auto constraints = createCollisionConstraints(vars, env, manip_info, config); + for (auto& constraint : constraints) + nlp->AddConstraintSet(constraint); + return true; +} + +bool addCollisionSquaredCost(std::shared_ptr nlp, + std::vector vars, + const tesseract_environment::Environment::ConstPtr& env, + const ManipulatorInfo& manip_info, + const trajopt::TrajOptCollisionConfig::ConstPtr& config) +{ + auto constraints = createCollisionConstraints(vars, env, manip_info, config); + for (auto& constraint : constraints) + { + Eigen::VectorXd coeff; // todo + + // Must link the variables to the constraint since that happens in AddConstraintSet + constraint->LinkWithVariables(nlp->GetOptVariables()); + auto cost = std::make_shared(constraint, coeff); + nlp->AddCostSet(cost); + } + return true; +} + +ifopt::ConstraintSet::Ptr createJointVelocityConstraint(const Eigen::Ref& target, + const std::vector& vars, + const Eigen::VectorXd& /*coeffs*/) + +{ + assert(!vars.empty()); + trajopt::JointVelConstraint::Ptr vel_constraint = + std::make_shared(target, vars, "JointVelocity"); + return vel_constraint; +} + +bool addJointVelocityConstraint(std::shared_ptr nlp, + std::vector vars, + const Eigen::Ref& coeff) +{ + if (vars.empty()) + return true; + + Eigen::VectorXd vel_target = Eigen::VectorXd::Zero(static_cast(vars.front()->GetJointNames().size())); + auto vel_constraint = createJointVelocityConstraint(vel_target, vars, coeff); + nlp->AddConstraintSet(vel_constraint); + return true; +} + +bool addJointVelocitySquaredCost(std::shared_ptr nlp, + std::vector vars, + const Eigen::Ref& coeff) +{ + if (vars.empty()) + return true; + + Eigen::VectorXd vel_target = Eigen::VectorXd::Zero(static_cast(vars.front()->GetJointNames().size())); + auto vel_constraint = createJointVelocityConstraint(vel_target, vars, coeff); + + // Must link the variables to the constraint since that happens in AddConstraintSet + vel_constraint->LinkWithVariables(nlp->GetOptVariables()); + auto vel_cost = std::make_shared(vel_constraint); + nlp->AddCostSet(vel_cost); + return true; +} + +} // namespace tesseract_planning