From 425b93c970c6545cfd6f4871ac361f27b3d0b871 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Wed, 27 Jul 2022 18:25:50 -0500 Subject: [PATCH] Add ruckig trajectory smoothing --- tesseract_process_managers/CMakeLists.txt | 1 + .../core/default_process_planners.h | 16 +- .../core/default_task_namespaces.h | 1 + ...ckig_trajectory_smoothing_task_generator.h | 82 ++++ .../ruckig_trajectory_smoothing_profile.h | 90 ++++ .../src/core/default_process_planners.cpp | 69 +++- ...ig_trajectory_smoothing_task_generator.cpp | 189 +++++++++ .../CMakeLists.txt | 9 +- ...ract_time_parameterization-config.cmake.in | 1 + .../instructions_trajectory.h | 3 + .../ruckig_trajectory_smoothing.h | 127 ++++++ .../tesseract_common_trajectory.h | 3 + .../trajectory_container.h | 3 + tesseract_time_parameterization/package.xml | 1 + .../src/instructions_trajectory.cpp | 37 ++ .../src/ruckig_trajectory_smoothing.cpp | 387 ++++++++++++++++++ .../src/tesseract_common_trajectory.cpp | 16 + .../test/CMakeLists.txt | 18 + .../ruckig_trajectory_smoothing_tests.cpp | 154 +++++++ 19 files changed, 1192 insertions(+), 15 deletions(-) create mode 100644 tesseract_process_managers/include/tesseract_process_managers/task_generators/ruckig_trajectory_smoothing_task_generator.h create mode 100644 tesseract_process_managers/include/tesseract_process_managers/task_profiles/ruckig_trajectory_smoothing_profile.h create mode 100644 tesseract_process_managers/src/task_generators/ruckig_trajectory_smoothing_task_generator.cpp create mode 100644 tesseract_time_parameterization/include/tesseract_time_parameterization/ruckig_trajectory_smoothing.h create mode 100644 tesseract_time_parameterization/src/ruckig_trajectory_smoothing.cpp create mode 100644 tesseract_time_parameterization/test/ruckig_trajectory_smoothing_tests.cpp diff --git a/tesseract_process_managers/CMakeLists.txt b/tesseract_process_managers/CMakeLists.txt index dd997109d8e..5494a71b8dd 100644 --- a/tesseract_process_managers/CMakeLists.txt +++ b/tesseract_process_managers/CMakeLists.txt @@ -70,6 +70,7 @@ add_library( src/task_generators/iterative_spline_parameterization_task_generator.cpp src/task_generators/motion_planner_task_generator.cpp src/task_generators/profile_switch_task_generator.cpp + src/task_generators/ruckig_trajectory_smoothing_task_generator.cpp src/task_generators/seed_min_length_task_generator.cpp src/task_generators/time_optimal_parameterization_task_generator.cpp src/task_generators/upsample_trajectory_task_generator.cpp diff --git a/tesseract_process_managers/include/tesseract_process_managers/core/default_process_planners.h b/tesseract_process_managers/include/tesseract_process_managers/core/default_process_planners.h index d0bb3857360..7e5039a3019 100644 --- a/tesseract_process_managers/include/tesseract_process_managers/core/default_process_planners.h +++ b/tesseract_process_managers/include/tesseract_process_managers/core/default_process_planners.h @@ -31,18 +31,26 @@ namespace tesseract_planning { /** @brief Create TrajOpt Process Pipeline */ -TaskflowGenerator::UPtr createTrajOptGenerator(bool check_input = true, bool post_collision_check = true); +TaskflowGenerator::UPtr createTrajOptGenerator(bool check_input = true, + bool post_collision_check = true, + bool post_smoothing = false); #ifdef TESSERACT_PROCESS_MANAGERS_HAS_TRAJOPT_IFOPT /** @brief Create TrajOpt IFOPT Process Pipeline */ -TaskflowGenerator::UPtr createTrajOptIfoptGenerator(bool check_input = true, bool post_collision_check = true); +TaskflowGenerator::UPtr createTrajOptIfoptGenerator(bool check_input = true, + bool post_collision_check = true, + bool post_smoothing = false); #endif /** @brief Create OMPL Process Pipeline */ -TaskflowGenerator::UPtr createOMPLGenerator(bool check_input = true, bool post_collision_check = true); +TaskflowGenerator::UPtr createOMPLGenerator(bool check_input = true, + bool post_collision_check = true, + bool post_smoothing = false); /** @brief Create Descartes Process Pipeline */ -TaskflowGenerator::UPtr createDescartesGenerator(bool check_input = true, bool post_collision_check = true); +TaskflowGenerator::UPtr createDescartesGenerator(bool check_input = true, + bool post_collision_check = true, + bool post_smoothing = false); /** * @brief Create Descartes Only Process Pipeline diff --git a/tesseract_process_managers/include/tesseract_process_managers/core/default_task_namespaces.h b/tesseract_process_managers/include/tesseract_process_managers/core/default_task_namespaces.h index 92c933521c4..b8b1f99aa0a 100644 --- a/tesseract_process_managers/include/tesseract_process_managers/core/default_task_namespaces.h +++ b/tesseract_process_managers/include/tesseract_process_managers/core/default_task_namespaces.h @@ -47,6 +47,7 @@ static const std::string PROFILE_SWITCH_DEFAULT_NAMESPACE = " static const std::string SEED_MIN_LENGTH_DEFAULT_NAMESPACE = "SEED_MIN_LENGTH_CHECK"; static const std::string TIME_OPTIMAL_PARAMETERIZATION_DEFAULT_NAMESPACE = "TIME_OPTIMAL_PARAMETERIZATION"; static const std::string UPSAMPLE_TRAJECTORY_DEFAULT_NAMESPACE = "UPSAMPLE_TRAJECTORY"; +static const std::string RUCKIG_TRAJECTORY_SMOOTHING_DEFAULT_NAMESPACE = "RUCKIG_TRAJECTORY_SMOOTHING"; // clang-format on } // namespace tesseract_planning::profile_ns diff --git a/tesseract_process_managers/include/tesseract_process_managers/task_generators/ruckig_trajectory_smoothing_task_generator.h b/tesseract_process_managers/include/tesseract_process_managers/task_generators/ruckig_trajectory_smoothing_task_generator.h new file mode 100644 index 00000000000..9fdb14de15f --- /dev/null +++ b/tesseract_process_managers/include/tesseract_process_managers/task_generators/ruckig_trajectory_smoothing_task_generator.h @@ -0,0 +1,82 @@ +/** + * @file ruckig_trajectory_smoothing_task_generator.h + * @brief Leveraging Ruckig to smooth trajectory + * + * @author Levi Armstrong + * @date July 27, 2022 + * @version TODO + * @bug No known bugs + * + * @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_PROCESS_MANAGERS_RUCKIG_TRAJECTORY_SMOOTHING_TASK_GENERATOR_H +#define TESSERACT_PROCESS_MANAGERS_RUCKIG_TRAJECTORY_SMOOTHING_TASK_GENERATOR_H + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include +#include + +namespace tesseract_planning +{ +class RuckigTrajectorySmoothingTaskGenerator : public TaskGenerator +{ +public: + using UPtr = std::unique_ptr; + + RuckigTrajectorySmoothingTaskGenerator(std::string name = profile_ns::RUCKIG_TRAJECTORY_SMOOTHING_DEFAULT_NAMESPACE); + + ~RuckigTrajectorySmoothingTaskGenerator() override = default; + RuckigTrajectorySmoothingTaskGenerator(const RuckigTrajectorySmoothingTaskGenerator&) = delete; + RuckigTrajectorySmoothingTaskGenerator& operator=(const RuckigTrajectorySmoothingTaskGenerator&) = delete; + RuckigTrajectorySmoothingTaskGenerator(RuckigTrajectorySmoothingTaskGenerator&&) = delete; + RuckigTrajectorySmoothingTaskGenerator& operator=(RuckigTrajectorySmoothingTaskGenerator&&) = delete; + + int conditionalProcess(TaskInput input, std::size_t unique_id) const override final; + + void process(TaskInput input, std::size_t unique_id) const override final; +}; + +class RuckigTrajectorySmoothingTaskInfo : public TaskInfo +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + RuckigTrajectorySmoothingTaskInfo() = default; + RuckigTrajectorySmoothingTaskInfo(std::size_t unique_id, + std::string name = profile_ns::RUCKIG_TRAJECTORY_SMOOTHING_DEFAULT_NAMESPACE); + + TaskInfo::UPtr clone() const override; + + bool operator==(const RuckigTrajectorySmoothingTaskInfo& rhs) const; + bool operator!=(const RuckigTrajectorySmoothingTaskInfo& rhs) const; + +private: + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int version); // NOLINT +}; +} // namespace tesseract_planning + +#include +#include +BOOST_CLASS_EXPORT_KEY2(tesseract_planning::RuckigTrajectorySmoothingTaskInfo, "RuckigTrajectorySmoothingTaskInfo") + +#endif // TESSERACT_PROCESS_MANAGERS_RUCKIG_TRAJECTORY_SMOOTHING_TASK_GENERATOR_H diff --git a/tesseract_process_managers/include/tesseract_process_managers/task_profiles/ruckig_trajectory_smoothing_profile.h b/tesseract_process_managers/include/tesseract_process_managers/task_profiles/ruckig_trajectory_smoothing_profile.h new file mode 100644 index 00000000000..55321225ebc --- /dev/null +++ b/tesseract_process_managers/include/tesseract_process_managers/task_profiles/ruckig_trajectory_smoothing_profile.h @@ -0,0 +1,90 @@ +/** + * @file ruckig_trajectory_smoothing_profile.h + * @brief Leveraging Ruckig to smooth trajectory + * + * @author Levi Armstrong + * @date July 27, 2022 + * @version TODO + * @bug No known bugs + * + * @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_PROCESS_MANAGERS_RUCKIG_TRAJECTORY_SMOOTHING_PROFILE_H +#define TESSERACT_PROCESS_MANAGERS_RUCKIG_TRAJECTORY_SMOOTHING_PROFILE_H + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +namespace tesseract_planning +{ +struct RuckigTrajectorySmoothingCompositeProfile +{ + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + RuckigTrajectorySmoothingCompositeProfile() = default; + RuckigTrajectorySmoothingCompositeProfile(double duration_extension_fraction, + double max_duration_extension_factor, + double max_velocity_scaling_factor, + double max_acceleration_scaling_factor) + : duration_extension_fraction(duration_extension_fraction) + , max_duration_extension_factor(max_duration_extension_factor) + , max_velocity_scaling_factor(max_velocity_scaling_factor) + , max_acceleration_scaling_factor(max_acceleration_scaling_factor) + { + } + + /** @brief duration_extension_fraction The amount to scale the trajectory each time */ + double duration_extension_fraction{ 1.1 }; + + /** @brief The max allow extension factor */ + double max_duration_extension_factor{ 10.0 }; + + /** @brief max_velocity_scaling_factor The max velocity scaling factor passed to the solver */ + double max_velocity_scaling_factor{ 1.0 }; + + /** @brief max_velocity_scaling_factor The max acceleration scaling factor passed to the solver */ + double max_acceleration_scaling_factor{ 1.0 }; + + /** @brief max_jerk_scaling_factor The max jerk scaling factor passed to the solver */ + double max_jerk_scaling_factor{ 1.0 }; +}; + +struct RuckigTrajectorySmoothingMoveProfile +{ + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + RuckigTrajectorySmoothingMoveProfile() = default; + RuckigTrajectorySmoothingMoveProfile(double max_velocity_scaling_factor, double max_acceleration_scaling_factor) + : max_velocity_scaling_factor(max_velocity_scaling_factor) + , max_acceleration_scaling_factor(max_acceleration_scaling_factor) + { + } + + /** @brief max_velocity_scaling_factor The max velocity scaling factor passed to the solver */ + double max_velocity_scaling_factor{ 1.0 }; + + /** @brief max_velocity_scaling_factor The max acceleration scaling factor passed to the solver */ + double max_acceleration_scaling_factor{ 1.0 }; + + /** @brief max_jerk_scaling_factor The max jerk scaling factor passed to the solver */ + double max_jerk_scaling_factor{ 1.0 }; +}; +} // namespace tesseract_planning + +#endif // TESSERACT_PROCESS_MANAGERS_RUCKIG_TRAJECTORY_SMOOTHING_PROFILE_H diff --git a/tesseract_process_managers/src/core/default_process_planners.cpp b/tesseract_process_managers/src/core/default_process_planners.cpp index 1fc59219aed..6220602ae1e 100644 --- a/tesseract_process_managers/src/core/default_process_planners.cpp +++ b/tesseract_process_managers/src/core/default_process_planners.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include #include @@ -51,7 +52,7 @@ namespace tesseract_planning { -TaskflowGenerator::UPtr createTrajOptGenerator(bool check_input, bool post_collision_check) +TaskflowGenerator::UPtr createTrajOptGenerator(bool check_input, bool post_collision_check, bool post_smoothing) { auto tf = std::make_unique("TrajOptTaskflow"); @@ -83,6 +84,11 @@ TaskflowGenerator::UPtr createTrajOptGenerator(bool check_input, bool post_colli // Setup time parameterization int time_parameterization_task = tf->addNode(std::make_unique(), true); + // Setup trajectory smoothing + int smoothing_task{ std::numeric_limits::min() }; + if (post_smoothing) + smoothing_task = tf->addNode(std::make_unique(), true); + if (check_input) tf->addEdges(check_input_task, { GraphTaskflow::ERROR_NODE, has_seed_task }); @@ -100,13 +106,21 @@ TaskflowGenerator::UPtr createTrajOptGenerator(bool check_input, bool post_colli tf->addEdges(motion_planner_task, { GraphTaskflow::ERROR_NODE, time_parameterization_task }); } - tf->addEdges(time_parameterization_task, { GraphTaskflow::ERROR_NODE, GraphTaskflow::DONE_NODE }); + if (post_smoothing) + { + tf->addEdges(time_parameterization_task, { GraphTaskflow::ERROR_NODE, smoothing_task }); + tf->addEdges(smoothing_task, { GraphTaskflow::DONE_NODE }); + } + else + { + tf->addEdges(time_parameterization_task, { GraphTaskflow::ERROR_NODE, GraphTaskflow::DONE_NODE }); + } return tf; } #ifdef TESSERACT_PROCESS_MANAGERS_HAS_TRAJOPT_IFOPT -TaskflowGenerator::UPtr createTrajOptIfoptGenerator(bool check_input, bool post_collision_check) +TaskflowGenerator::UPtr createTrajOptIfoptGenerator(bool check_input, bool post_collision_check, bool post_smoothing) { auto tf = std::make_unique("TrajOptIfoptTaskflow"); @@ -138,6 +152,11 @@ TaskflowGenerator::UPtr createTrajOptIfoptGenerator(bool check_input, bool post_ // Setup time parameterization int time_parameterization_task = tf->addNode(std::make_unique(), true); + // Setup trajectory smoothing + int smoothing_task{ std::numeric_limits::min() }; + if (post_smoothing) + smoothing_task = tf->addNode(std::make_unique(), true); + if (check_input) tf->addEdges(check_input_task, { GraphTaskflow::ERROR_NODE, has_seed_task }); @@ -155,13 +174,21 @@ TaskflowGenerator::UPtr createTrajOptIfoptGenerator(bool check_input, bool post_ tf->addEdges(motion_planner_task, { GraphTaskflow::ERROR_NODE, time_parameterization_task }); } - tf->addEdges(time_parameterization_task, { GraphTaskflow::ERROR_NODE, GraphTaskflow::DONE_NODE }); + if (post_smoothing) + { + tf->addEdges(time_parameterization_task, { GraphTaskflow::ERROR_NODE, smoothing_task }); + tf->addEdges(smoothing_task, { GraphTaskflow::DONE_NODE }); + } + else + { + tf->addEdges(time_parameterization_task, { GraphTaskflow::ERROR_NODE, GraphTaskflow::DONE_NODE }); + } return tf; } #endif -TaskflowGenerator::UPtr createOMPLGenerator(bool check_input, bool post_collision_check) +TaskflowGenerator::UPtr createOMPLGenerator(bool check_input, bool post_collision_check, bool post_smoothing) { auto tf = std::make_unique("OMPLTaskflow"); @@ -193,6 +220,11 @@ TaskflowGenerator::UPtr createOMPLGenerator(bool check_input, bool post_collisio // Setup time parameterization int time_parameterization_task = tf->addNode(std::make_unique(), true); + // Setup trajectory smoothing + int smoothing_task{ std::numeric_limits::min() }; + if (post_smoothing) + smoothing_task = tf->addNode(std::make_unique(), true); + if (check_input) tf->addEdges(check_input_task, { GraphTaskflow::ERROR_NODE, has_seed_task }); @@ -210,12 +242,20 @@ TaskflowGenerator::UPtr createOMPLGenerator(bool check_input, bool post_collisio tf->addEdges(motion_planner_task, { GraphTaskflow::ERROR_NODE, time_parameterization_task }); } - tf->addEdges(time_parameterization_task, { GraphTaskflow::ERROR_NODE, GraphTaskflow::DONE_NODE }); + if (post_smoothing) + { + tf->addEdges(time_parameterization_task, { GraphTaskflow::ERROR_NODE, smoothing_task }); + tf->addEdges(smoothing_task, { GraphTaskflow::DONE_NODE }); + } + else + { + tf->addEdges(time_parameterization_task, { GraphTaskflow::ERROR_NODE, GraphTaskflow::DONE_NODE }); + } return tf; } -TaskflowGenerator::UPtr createDescartesGenerator(bool check_input, bool post_collision_check) +TaskflowGenerator::UPtr createDescartesGenerator(bool check_input, bool post_collision_check, bool post_smoothing) { auto tf = std::make_unique("DescartesTaskflow"); @@ -247,6 +287,11 @@ TaskflowGenerator::UPtr createDescartesGenerator(bool check_input, bool post_col // Setup time parameterization int time_parameterization_task = tf->addNode(std::make_unique(), true); + // Setup trajectory smoothing + int smoothing_task{ std::numeric_limits::min() }; + if (post_smoothing) + smoothing_task = tf->addNode(std::make_unique(), true); + if (check_input) tf->addEdges(check_input_task, { GraphTaskflow::ERROR_NODE, has_seed_task }); @@ -264,7 +309,15 @@ TaskflowGenerator::UPtr createDescartesGenerator(bool check_input, bool post_col tf->addEdges(motion_planner_task, { GraphTaskflow::ERROR_NODE, time_parameterization_task }); } - tf->addEdges(time_parameterization_task, { GraphTaskflow::ERROR_NODE, GraphTaskflow::DONE_NODE }); + if (post_smoothing) + { + tf->addEdges(time_parameterization_task, { GraphTaskflow::ERROR_NODE, smoothing_task }); + tf->addEdges(smoothing_task, { GraphTaskflow::DONE_NODE }); + } + else + { + tf->addEdges(time_parameterization_task, { GraphTaskflow::ERROR_NODE, GraphTaskflow::DONE_NODE }); + } return tf; } diff --git a/tesseract_process_managers/src/task_generators/ruckig_trajectory_smoothing_task_generator.cpp b/tesseract_process_managers/src/task_generators/ruckig_trajectory_smoothing_task_generator.cpp new file mode 100644 index 00000000000..162e233a6ca --- /dev/null +++ b/tesseract_process_managers/src/task_generators/ruckig_trajectory_smoothing_task_generator.cpp @@ -0,0 +1,189 @@ +/** + * @file ruckig_trajectory_smoothing_task_generator.h + * @brief Leveraging Ruckig to smooth trajectory + * + * @author Levi Armstrong + * @date July 27, 2022 + * @version TODO + * @bug No known bugs + * + * @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 +#include + +namespace tesseract_planning +{ +RuckigTrajectorySmoothingTaskGenerator::RuckigTrajectorySmoothingTaskGenerator(std::string name) + : TaskGenerator(std::move(name)) +{ +} + +int RuckigTrajectorySmoothingTaskGenerator::conditionalProcess(TaskInput input, std::size_t unique_id) const +{ + if (input.isAborted()) + return 0; + + auto info = std::make_unique(unique_id, name_); + info->return_value = 0; + tesseract_common::Timer timer; + timer.start(); + saveInputs(*info, input); + + // -------------------- + // Check that inputs are valid + // -------------------- + InstructionPoly* input_results = input.getResults(); + if (!input_results->isCompositeInstruction()) + { + CONSOLE_BRIDGE_logError("Input results to iterative spline parameterization must be a composite instruction"); + saveOutputs(*info, input); + info->elapsed_time = timer.elapsedSeconds(); + input.addTaskInfo(std::move(info)); + return 0; + } + + auto& ci = input_results->as(); + CompositeInstruction original_ci{ ci }; + const tesseract_common::ManipulatorInfo& manip_info = ci.getManipulatorInfo(); + auto joint_group = input.env->getJointGroup(manip_info.manipulator); + auto limits = joint_group->getLimits(); + + // Get Composite Profile + std::string profile = ci.getProfile(); + profile = getProfileString(name_, profile, input.composite_profile_remapping); + auto cur_composite_profile = getProfile( + name_, profile, *input.profiles, std::make_shared()); + cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.profile_overrides); + + RuckigTrajectorySmoothing solver(cur_composite_profile->duration_extension_fraction, + cur_composite_profile->max_duration_extension_factor); + + // Create data structures for checking for plan profile overrides + auto flattened = ci.flatten(moveFilter); + if (flattened.empty()) + { + CONSOLE_BRIDGE_logWarn("Ruckig trajectory smoothing found no MoveInstructions to process"); + info->return_value = 1; + saveOutputs(*info, input); + info->elapsed_time = timer.elapsedSeconds(); + input.addTaskInfo(std::move(info)); + return 1; + } + + Eigen::VectorXd velocity_scaling_factors = Eigen::VectorXd::Ones(static_cast(flattened.size())) * + cur_composite_profile->max_velocity_scaling_factor; + Eigen::VectorXd acceleration_scaling_factors = Eigen::VectorXd::Ones(static_cast(flattened.size())) * + cur_composite_profile->max_acceleration_scaling_factor; + Eigen::VectorXd jerk_scaling_factors = Eigen::VectorXd::Ones(static_cast(flattened.size())) * + cur_composite_profile->max_jerk_scaling_factor; + + // Loop over all MoveInstructions + for (Eigen::Index idx = 0; idx < static_cast(flattened.size()); idx++) + { + const auto& mi = flattened[static_cast(idx)].get().as(); + std::string plan_profile = mi.getProfile(); + + // Check for remapping of the plan profile + plan_profile = getProfileString(name_, profile, input.plan_profile_remapping); + auto cur_move_profile = getProfile( + name_, plan_profile, *input.profiles, std::make_shared()); + // cur_move_profile = applyProfileOverrides(name_, profile, cur_move_profile, mi.profile_overrides); + + // If there is a move profile associated with it, override the parameters + if (cur_move_profile) + { + velocity_scaling_factors[idx] = cur_move_profile->max_velocity_scaling_factor; + acceleration_scaling_factors[idx] = cur_move_profile->max_acceleration_scaling_factor; + jerk_scaling_factors[idx] = cur_move_profile->max_jerk_scaling_factor; + } + } + + // Solve using parameters + TrajectoryContainer::Ptr trajectory = std::make_shared(ci); + if (!solver.compute(*trajectory, + limits.velocity_limits, + limits.acceleration_limits, + Eigen::VectorXd::Constant(limits.velocity_limits.rows(), 1000), + velocity_scaling_factors, + acceleration_scaling_factors, + jerk_scaling_factors)) + { + CONSOLE_BRIDGE_logInform("Failed to perform ruckig trajectory smoothing for process input: %s!", + input_results->getDescription().c_str()); + saveOutputs(*info, input); + info->elapsed_time = timer.elapsedSeconds(); + input.addTaskInfo(std::move(info)); + // reset back to original + ci = original_ci; + return 0; + } + + CONSOLE_BRIDGE_logDebug("Ruckig trajectory smoothing succeeded"); + info->return_value = 1; + saveOutputs(*info, input); + info->elapsed_time = timer.elapsedSeconds(); + input.addTaskInfo(std::move(info)); + return 1; +} + +void RuckigTrajectorySmoothingTaskGenerator::process(TaskInput input, std::size_t unique_id) const +{ + conditionalProcess(input, unique_id); +} + +RuckigTrajectorySmoothingTaskInfo::RuckigTrajectorySmoothingTaskInfo(std::size_t unique_id, std::string name) + : TaskInfo(unique_id, std::move(name)) +{ +} + +TaskInfo::UPtr RuckigTrajectorySmoothingTaskInfo::clone() const +{ + return std::make_unique(*this); +} + +bool RuckigTrajectorySmoothingTaskInfo::operator==(const RuckigTrajectorySmoothingTaskInfo& rhs) const +{ + bool equal = true; + equal &= TaskInfo::operator==(rhs); + return equal; +} +bool RuckigTrajectorySmoothingTaskInfo::operator!=(const RuckigTrajectorySmoothingTaskInfo& rhs) const +{ + return !operator==(rhs); +} + +template +void RuckigTrajectorySmoothingTaskInfo::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TaskInfo); +} +} // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::RuckigTrajectorySmoothingTaskInfo) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::RuckigTrajectorySmoothingTaskInfo) diff --git a/tesseract_time_parameterization/CMakeLists.txt b/tesseract_time_parameterization/CMakeLists.txt index a1f05c84901..3f14e567d57 100644 --- a/tesseract_time_parameterization/CMakeLists.txt +++ b/tesseract_time_parameterization/CMakeLists.txt @@ -13,6 +13,7 @@ find_package(console_bridge REQUIRED) find_package(tesseract_common REQUIRED) find_package(tesseract_command_language REQUIRED) find_package(Eigen3 REQUIRED) +find_package(ruckig REQUIRED) if(NOT TARGET console_bridge::console_bridge) add_library(console_bridge::console_bridge INTERFACE IMPORTED) @@ -41,15 +42,17 @@ tesseract_variables() add_library( ${PROJECT_NAME} src/instructions_trajectory.cpp - src/iterative_spline_parameterization - src/time_optimal_trajectory_generation + src/iterative_spline_parameterization.cpp + src/ruckig_trajectory_smoothing.cpp + src/time_optimal_trajectory_generation.cpp src/tesseract_common_trajectory.cpp) target_link_libraries( ${PROJECT_NAME} PUBLIC tesseract::tesseract_common tesseract::tesseract_command_language console_bridge::console_bridge - Eigen3::Eigen) + Eigen3::Eigen + ruckig::ruckig) target_compile_options(${PROJECT_NAME} PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE}) target_compile_options(${PROJECT_NAME} PUBLIC ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) target_compile_definitions(${PROJECT_NAME} PUBLIC ${TESSERACT_COMPILE_DEFINITIONS}) diff --git a/tesseract_time_parameterization/cmake/tesseract_time_parameterization-config.cmake.in b/tesseract_time_parameterization/cmake/tesseract_time_parameterization-config.cmake.in index daf9765f0e3..55b6089bb34 100644 --- a/tesseract_time_parameterization/cmake/tesseract_time_parameterization-config.cmake.in +++ b/tesseract_time_parameterization/cmake/tesseract_time_parameterization-config.cmake.in @@ -8,6 +8,7 @@ include(CMakeFindDependencyMacro) find_dependency(tesseract_common) find_dependency(tesseract_command_language) find_dependency(Eigen3) +find_dependency(ruckig) find_dependency(console_bridge) if(NOT TARGET console_bridge::console_bridge) diff --git a/tesseract_time_parameterization/include/tesseract_time_parameterization/instructions_trajectory.h b/tesseract_time_parameterization/include/tesseract_time_parameterization/instructions_trajectory.h index 6c8886cdc93..cd2a8c61ed3 100644 --- a/tesseract_time_parameterization/include/tesseract_time_parameterization/instructions_trajectory.h +++ b/tesseract_time_parameterization/include/tesseract_time_parameterization/instructions_trajectory.h @@ -44,8 +44,11 @@ class InstructionsTrajectory : public TrajectoryContainer InstructionsTrajectory(CompositeInstruction& program); const Eigen::VectorXd& getPosition(Eigen::Index i) const final; + Eigen::VectorXd& getPosition(Eigen::Index i) final; const Eigen::VectorXd& getVelocity(Eigen::Index i) const final; + Eigen::VectorXd& getVelocity(Eigen::Index i) final; const Eigen::VectorXd& getAcceleration(Eigen::Index i) const final; + Eigen::VectorXd& getAcceleration(Eigen::Index i) final; double getTimeFromStart(Eigen::Index i) const final; void setData(Eigen::Index i, const Eigen::VectorXd& velocity, const Eigen::VectorXd& acceleration, double time) final; diff --git a/tesseract_time_parameterization/include/tesseract_time_parameterization/ruckig_trajectory_smoothing.h b/tesseract_time_parameterization/include/tesseract_time_parameterization/ruckig_trajectory_smoothing.h new file mode 100644 index 00000000000..d9f3e34d2d7 --- /dev/null +++ b/tesseract_time_parameterization/include/tesseract_time_parameterization/ruckig_trajectory_smoothing.h @@ -0,0 +1,127 @@ +/** + * @file ruckig_trajectory_smoothing.h + * @brief Leveraging Ruckig to smooth trajectory + * + * @author Levi Armstrong + * @date July 27, 2022 + * @version TODO + * @bug No known bugs + * + * @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_TIME_PARAMETERIZATION_RUCKIG_TRAJECTORY_SMOOTHING_H +#define TESSERACT_TIME_PARAMETERIZATION_RUCKIG_TRAJECTORY_SMOOTHING_H + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include + +namespace tesseract_planning +{ +class RuckigTrajectorySmoothing +{ +public: + RuckigTrajectorySmoothing(double duration_extension_fraction = 1.1, double max_duration_extension_factor = 10); + virtual ~RuckigTrajectorySmoothing() = default; + RuckigTrajectorySmoothing(const RuckigTrajectorySmoothing&) = default; + RuckigTrajectorySmoothing& operator=(const RuckigTrajectorySmoothing&) = default; + RuckigTrajectorySmoothing(RuckigTrajectorySmoothing&&) = default; + RuckigTrajectorySmoothing& operator=(RuckigTrajectorySmoothing&&) = default; + + /** @brief Set the duration extension fraction */ + void setDurationExtensionFraction(double duration_extension_fraction); + + /** @brief Set the max duration extension factor */ + void setMaxDurationExtensionFactor(double max_duration_extension_factor); + + /** + * @brief Compute the time stamps for a flattened vector of move instruction + * @param trajectory Flattended vector of move instruction + * @param max_velocities The max velocities for each joint + * @param max_accelerations The max acceleration for each joint + * @param max_velocity_scaling_factor The max velocity scaling factor + * @param max_acceleration_scaling_factor The max acceleration scaling factor + * @return True if successful, otherwise false + */ + bool compute(TrajectoryContainer& trajectory, + const double& max_velocity, + const double& max_acceleration, + const double& max_jerk, + double max_velocity_scaling_factor = 1.0, + double max_acceleration_scaling_factor = 1.0, + double max_jerk_scaling_factor = 1.0) const; + + /** + * @brief Compute the time stamps for a flattened vector of move instruction + * @param trajectory Flattended vector of move instruction + * @param max_velocities The max velocities for each joint + * @param max_accelerations The max acceleration for each joint + * @param max_velocity_scaling_factor The max velocity scaling factor + * @param max_acceleration_scaling_factor The max acceleration scaling factor + * @return True if successful, otherwise false + */ + bool compute(TrajectoryContainer& trajectory, + const std::vector& max_velocity, + const std::vector& max_acceleration, + const std::vector& max_jerk, + double max_velocity_scaling_factor = 1.0, + double max_acceleration_scaling_factor = 1.0, + double max_jerk_scaling_factor = 1.0) const; + + /** + * @brief Compute the time stamps for a flattened vector of move instruction + * @param trajectory Flattended vector of move instruction + * @param max_velocities The max velocities for each joint + * @param max_accelerations The max acceleration for each joint + * @param max_velocity_scaling_factor The max velocity scaling factor + * @param max_acceleration_scaling_factor The max acceleration scaling factor + * @return True if successful, otherwise false + */ + bool compute(TrajectoryContainer& trajectory, + const Eigen::Ref& max_velocity, + const Eigen::Ref& max_acceleration, + const Eigen::Ref& max_jerk, + double max_velocity_scaling_factor = 1.0, + double max_acceleration_scaling_factor = 1.0, + double max_jerk_scaling_factor = 1.0) const; + + /** + * @brief Compute the time stamps for a flattened vector of move instruction + * @param trajectory Flattended vector of move instruction + * @param max_velocities The max velocities for each joint + * @param max_accelerations The max acceleration for each joint + * @param max_velocity_scaling_factor The max velocity scaling factor. Size should be trajectory.size() + * @param max_acceleration_scaling_factor The max acceleration scaling factor. Size should be trajectory.size() + * @return True if successful, otherwise false + */ + bool compute(TrajectoryContainer& trajectory, + const Eigen::Ref& max_velocity, + const Eigen::Ref& max_acceleration, + const Eigen::Ref& max_jerk, + const Eigen::Ref& max_velocity_scaling_factors, + const Eigen::Ref& max_acceleration_scaling_factors, + const Eigen::Ref& max_jerk_scaling_factors) const; + +protected: + double duration_extension_fraction_; + double max_duration_extension_factor_; +}; +} // namespace tesseract_planning + +#endif // TESSERACT_TIME_PARAMETERIZATION_RUCKIG_TRAJECTORY_SMOOTHING_H diff --git a/tesseract_time_parameterization/include/tesseract_time_parameterization/tesseract_common_trajectory.h b/tesseract_time_parameterization/include/tesseract_time_parameterization/tesseract_common_trajectory.h index bcf18a5f29a..20b632f17dc 100644 --- a/tesseract_time_parameterization/include/tesseract_time_parameterization/tesseract_common_trajectory.h +++ b/tesseract_time_parameterization/include/tesseract_time_parameterization/tesseract_common_trajectory.h @@ -42,8 +42,11 @@ class TesseractCommonTrajectory : public TrajectoryContainer TesseractCommonTrajectory(tesseract_common::JointTrajectory& trajectory); const Eigen::VectorXd& getPosition(Eigen::Index i) const override final; + Eigen::VectorXd& getPosition(Eigen::Index i) override final; const Eigen::VectorXd& getVelocity(Eigen::Index i) const override final; + Eigen::VectorXd& getVelocity(Eigen::Index i) override final; const Eigen::VectorXd& getAcceleration(Eigen::Index i) const override final; + Eigen::VectorXd& getAcceleration(Eigen::Index i) override final; double getTimeFromStart(Eigen::Index i) const final; void setData(Eigen::Index i, diff --git a/tesseract_time_parameterization/include/tesseract_time_parameterization/trajectory_container.h b/tesseract_time_parameterization/include/tesseract_time_parameterization/trajectory_container.h index d4bfae5669c..24f14130526 100644 --- a/tesseract_time_parameterization/include/tesseract_time_parameterization/trajectory_container.h +++ b/tesseract_time_parameterization/include/tesseract_time_parameterization/trajectory_container.h @@ -54,6 +54,7 @@ class TrajectoryContainer * @return The position data */ virtual const Eigen::VectorXd& getPosition(Eigen::Index i) const = 0; + virtual Eigen::VectorXd& getPosition(Eigen::Index i) = 0; /** * @brief Get the velocity data at a given index @@ -61,6 +62,7 @@ class TrajectoryContainer * @return The velocity data */ virtual const Eigen::VectorXd& getVelocity(Eigen::Index i) const = 0; + virtual Eigen::VectorXd& getVelocity(Eigen::Index i) = 0; /** * @brief Get the acceleration data at a given index @@ -68,6 +70,7 @@ class TrajectoryContainer * @return The acceleration data */ virtual const Eigen::VectorXd& getAcceleration(Eigen::Index i) const = 0; + virtual Eigen::VectorXd& getAcceleration(Eigen::Index i) = 0; /** * @brief Get the time from start at a given index diff --git a/tesseract_time_parameterization/package.xml b/tesseract_time_parameterization/package.xml index 14b6b835e54..2d8b87e6a14 100644 --- a/tesseract_time_parameterization/package.xml +++ b/tesseract_time_parameterization/package.xml @@ -13,6 +13,7 @@ libconsole-bridge-dev tesseract_common tesseract_command_language + ruckig ros_industrial_cmake_boilerplate eigen diff --git a/tesseract_time_parameterization/src/instructions_trajectory.cpp b/tesseract_time_parameterization/src/instructions_trajectory.cpp index 61daa3bb682..b2cc657c343 100644 --- a/tesseract_time_parameterization/src/instructions_trajectory.cpp +++ b/tesseract_time_parameterization/src/instructions_trajectory.cpp @@ -73,6 +73,19 @@ const Eigen::VectorXd& InstructionsTrajectory::getPosition(Eigen::Index i) const .as() .getPosition(); } + +Eigen::VectorXd& InstructionsTrajectory::getPosition(Eigen::Index i) +{ + assert(trajectory_[static_cast(i)].get().isMoveInstruction()); + assert(trajectory_[static_cast(i)].get().as().getWaypoint().isStateWaypoint()); + return trajectory_[static_cast(i)] + .get() + .as() + .getWaypoint() + .as() + .getPosition(); +} + const Eigen::VectorXd& InstructionsTrajectory::getVelocity(Eigen::Index i) const { assert(trajectory_[static_cast(i)].get().isMoveInstruction()); @@ -85,6 +98,18 @@ const Eigen::VectorXd& InstructionsTrajectory::getVelocity(Eigen::Index i) const .getVelocity(); } +Eigen::VectorXd& InstructionsTrajectory::getVelocity(Eigen::Index i) +{ + assert(trajectory_[static_cast(i)].get().isMoveInstruction()); + assert(trajectory_[static_cast(i)].get().as().getWaypoint().isStateWaypoint()); + return trajectory_[static_cast(i)] + .get() + .as() + .getWaypoint() + .as() + .getVelocity(); +} + const Eigen::VectorXd& InstructionsTrajectory::getAcceleration(Eigen::Index i) const { assert(trajectory_[static_cast(i)].get().isMoveInstruction()); @@ -97,6 +122,18 @@ const Eigen::VectorXd& InstructionsTrajectory::getAcceleration(Eigen::Index i) c .getAcceleration(); } +Eigen::VectorXd& InstructionsTrajectory::getAcceleration(Eigen::Index i) +{ + assert(trajectory_[static_cast(i)].get().isMoveInstruction()); + assert(trajectory_[static_cast(i)].get().as().getWaypoint().isStateWaypoint()); + return trajectory_[static_cast(i)] + .get() + .as() + .getWaypoint() + .as() + .getAcceleration(); +} + double InstructionsTrajectory::getTimeFromStart(Eigen::Index i) const { assert(trajectory_[static_cast(i)].get().isMoveInstruction()); diff --git a/tesseract_time_parameterization/src/ruckig_trajectory_smoothing.cpp b/tesseract_time_parameterization/src/ruckig_trajectory_smoothing.cpp new file mode 100644 index 00000000000..34d55dc7f84 --- /dev/null +++ b/tesseract_time_parameterization/src/ruckig_trajectory_smoothing.cpp @@ -0,0 +1,387 @@ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2021, PickNik Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ + +/* Author: Jack Center, Wyatt Rees, Andy Zelenak, Stephanie Eng, Levi Armstrong */ + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include + +#include + +#include +#include +#include + +namespace tesseract_planning +{ +RuckigTrajectorySmoothing::RuckigTrajectorySmoothing(double duration_extension_fraction, + double max_duration_extension_factor) + : duration_extension_fraction_(duration_extension_fraction) + , max_duration_extension_factor_(max_duration_extension_factor) +{ +} + +void RuckigTrajectorySmoothing::setDurationExtensionFraction(double duration_extension_fraction) +{ + duration_extension_fraction_ = duration_extension_fraction; +} + +void RuckigTrajectorySmoothing::setMaxDurationExtensionFactor(double max_duration_extension_factor) +{ + max_duration_extension_factor_ = max_duration_extension_factor; +} + +bool RuckigTrajectorySmoothing::compute(TrajectoryContainer& trajectory, + const double& max_velocity, + const double& max_acceleration, + const double& max_jerk, + double max_velocity_scaling_factor, + double max_acceleration_scaling_factor, + double max_jerk_scaling_factor) const +{ + // NOLINTNEXTLINE(clang-analyzer-core.UndefinedBinaryOperatorResult) + return compute(trajectory, + std::vector(static_cast(trajectory.dof()), max_velocity), + std::vector(static_cast(trajectory.dof()), max_acceleration), + std::vector(static_cast(trajectory.dof()), max_jerk), + max_velocity_scaling_factor, + max_acceleration_scaling_factor, + max_jerk_scaling_factor); +} + +bool RuckigTrajectorySmoothing::compute(TrajectoryContainer& trajectory, + const std::vector& max_velocity, + const std::vector& max_acceleration, + const std::vector& max_jerk, + double max_velocity_scaling_factor, + double max_acceleration_scaling_factor, + double max_jerk_scaling_factor) const +{ + // NOLINTNEXTLINE(clang-analyzer-core.UndefinedBinaryOperatorResult) + return compute(trajectory, + Eigen::Map(max_velocity.data(), static_cast(max_velocity.size())), + Eigen::Map(max_acceleration.data(), static_cast(max_acceleration.size())), + Eigen::Map(max_jerk.data(), static_cast(max_jerk.size())), + max_velocity_scaling_factor, + max_acceleration_scaling_factor, + max_jerk_scaling_factor); +} + +bool RuckigTrajectorySmoothing::compute(TrajectoryContainer& trajectory, + const Eigen::Ref& max_velocity, + const Eigen::Ref& max_acceleration, + const Eigen::Ref& max_jerk, + double max_velocity_scaling_factor, + double max_acceleration_scaling_factor, + double max_jerk_scaling_factor) const +{ + Eigen::VectorXd max_velocity_scaling_factors = + Eigen::VectorXd::Ones(static_cast(trajectory.size())) * max_velocity_scaling_factor; + Eigen::VectorXd max_acceleration_scaling_factors = + Eigen::VectorXd::Ones(static_cast(trajectory.size())) * max_acceleration_scaling_factor; + Eigen::VectorXd max_jerk_scaling_factors = + Eigen::VectorXd::Ones(static_cast(trajectory.size())) * max_jerk_scaling_factor; + // NOLINTNEXTLINE(clang-analyzer-core.UndefinedBinaryOperatorResult) + return compute(trajectory, + max_velocity, + max_acceleration, + max_jerk, + max_velocity_scaling_factors, + max_acceleration_scaling_factors, + max_jerk_scaling_factors); +} + +#ifdef WITH_ONLINE_CLIENT +bool RuckigTrajectorySmoothing::compute(TrajectoryContainer& trajectory, + const Eigen::Ref& max_velocity, + const Eigen::Ref& max_acceleration, + const Eigen::Ref& max_jerk, + const Eigen::Ref& max_velocity_scaling_factors, + const Eigen::Ref& max_acceleration_scaling_factors, + const Eigen::Ref& max_jerk_scaling_factors) const +{ + if (trajectory.size() < 2) + return true; + + if (max_velocity.size() != trajectory.dof() || max_acceleration.size() != trajectory.dof()) + return false; + + // Create input parameters + const std::size_t end_index = trajectory.size() - 1; + ruckig::InputParameter input; + + input.max_velocity = std::vector(max_velocity.data(), max_velocity.data() + max_velocity.rows()); + input.max_acceleration = + std::vector(max_acceleration.data(), max_acceleration.data() + max_acceleration.rows()); + // input.max_jerk = {4.0, 3.0, 2.0}; + + { // Set start position + const Eigen::VectorXd& position = trajectory.getPosition(static_cast(0)); + const Eigen::VectorXd& velocity = trajectory.getVelocity(static_cast(0)); + const Eigen::VectorXd& accleration = trajectory.getAcceleration(static_cast(0)); + + input.current_position = std::vector(position.data(), position.data() + position.rows()); + + if (velocity.rows() != 0) + input.current_velocity = std::vector(velocity.data(), velocity.data() + velocity.rows()); + + if (accleration.rows() != 0) + input.current_acceleration = std::vector(accleration.data(), accleration.data() + accleration.rows()); + } + + { // Set end position + const Eigen::VectorXd& position = trajectory.getPosition(static_cast(end_index)); + const Eigen::VectorXd& velocity = trajectory.getVelocity(static_cast(end_index)); + const Eigen::VectorXd& accleration = trajectory.getAcceleration(static_cast(end_index)); + + input.target_position = std::vector(position.data(), position.data() + position.rows()); + + if (velocity.rows() != 0) + input.target_velocity = std::vector(velocity.data(), velocity.data() + velocity.rows()); + + if (accleration.rows() != 0) + input.target_acceleration = std::vector(accleration.data(), accleration.data() + accleration.rows()); + } + + // Add intermediate positions + for (std::size_t j = 1; j < trajectory.size() - 1; j++) + input.intermediate_positions.push_back(std::vector(position.data(), position.data() + position.rows())); + + // We don't need to pass the control rate (cycle time) when using only offline features + ruckig::Ruckig otg; + ruckig::Trajectory trajectory; + + // Calculate the trajectory in an offline manner (outside of the control loop) + ruckig::Result result = otg.calculate(input, trajectory); + if (result != ruckig::Result::Finished) + return false; +} +#else + +void getNextRuckigInput(ruckig::InputParameter& ruckig_input, + TrajectoryContainer& trajectory, + Eigen::Index current_index, + Eigen::Index next_index, + const Eigen::Ref& max_velocity, + const Eigen::Ref& max_acceleration) +{ + // Set current state + const auto& current_position = trajectory.getPosition(current_index); + Eigen::VectorXd& current_velocity = trajectory.getVelocity(current_index); + Eigen::VectorXd& current_accleration = trajectory.getAcceleration(current_index); + + // clamp due to small numerical errors + current_velocity = current_velocity.array().min(max_velocity.array()).max((-1.0 * max_velocity).array()); + current_accleration = + current_accleration.array().min(max_acceleration.array()).max((-1.0 * max_acceleration).array()); + + const Eigen::VectorXd& next_position = trajectory.getPosition(next_index); + Eigen::VectorXd& next_velocity = trajectory.getVelocity(next_index); + Eigen::VectorXd& next_accleration = trajectory.getAcceleration(next_index); + + // clamp due to small numerical errors + next_velocity = next_velocity.array().min(max_velocity.array()).max((-1.0 * max_velocity).array()); + next_accleration = next_accleration.array().min(max_acceleration.array()).max((-1.0 * max_acceleration).array()); + + // Update input + ruckig_input.current_position = + std::vector(current_position.data(), current_position.data() + current_position.rows()); + ruckig_input.current_velocity = + std::vector(current_velocity.data(), current_velocity.data() + current_velocity.rows()); + ruckig_input.current_acceleration = + std::vector(current_accleration.data(), current_accleration.data() + current_accleration.rows()); + + ruckig_input.target_position = std::vector(next_position.data(), next_position.data() + next_position.rows()); + ruckig_input.target_velocity = std::vector(next_velocity.data(), next_velocity.data() + next_velocity.rows()); + ruckig_input.target_acceleration = + std::vector(next_accleration.data(), next_accleration.data() + next_accleration.rows()); +} + +void initializeRuckigState(ruckig::InputParameter& ruckig_input, + ruckig::OutputParameter& ruckig_output, + TrajectoryContainer& trajectory, + const Eigen::Ref& max_velocity, + const Eigen::Ref& max_acceleration) +{ + // Set current state + const Eigen::VectorXd& current_position = trajectory.getPosition(0); + Eigen::VectorXd& current_velocity = trajectory.getVelocity(0); + Eigen::VectorXd& current_accleration = trajectory.getAcceleration(0); + + // clamp due to small numerical errors + current_velocity = current_velocity.array().min(max_velocity.array()).max((-1.0 * max_velocity).array()); + current_accleration = + current_accleration.array().min(max_acceleration.array()).max((-1.0 * max_acceleration).array()); + + // Intialize Ruckig state + ruckig_input.current_position = + std::vector(current_position.data(), current_position.data() + current_position.rows()); + ruckig_input.current_velocity = + std::vector(current_velocity.data(), current_velocity.data() + current_velocity.rows()); + ruckig_input.current_acceleration = + std::vector(current_accleration.data(), current_accleration.data() + current_accleration.rows()); + + ruckig_output.new_position = ruckig_input.current_position; + ruckig_output.new_velocity = ruckig_input.current_velocity; + ruckig_output.new_acceleration = ruckig_input.current_acceleration; +} + +bool RuckigTrajectorySmoothing::compute(TrajectoryContainer& trajectory, + const Eigen::Ref& max_velocity, + const Eigen::Ref& max_acceleration, + const Eigen::Ref& max_jerk, + const Eigen::Ref& /*max_velocity_scaling_factors*/, + const Eigen::Ref& /*max_acceleration_scaling_factors*/, + const Eigen::Ref& /*max_jerk_scaling_factors*/) const +{ + if (trajectory.size() < 2) + return true; + + if (max_velocity.size() != trajectory.dof() || max_acceleration.size() != trajectory.dof()) + return false; + + // Create input parameters + const auto dof = static_cast(trajectory.dof()); + const auto num_waypoints = static_cast(trajectory.size()); + ruckig::InputParameter ruckig_input{ dof }; + ruckig::OutputParameter ruckig_output{ dof }; + + const Eigen::VectorXd max_scaled_velocity = + max_velocity; // (max_velocity.array() * max_velocity_scaling_factors.array()).transpose(); + ruckig_input.max_velocity = + std::vector(max_scaled_velocity.data(), max_scaled_velocity.data() + max_scaled_velocity.rows()); + + const Eigen::VectorXd max_scaled_acceleration = + max_acceleration; // max_acceleration.array() * max_acceleration_scaling_factors.array(); + ruckig_input.max_acceleration = std::vector(max_scaled_acceleration.data(), + max_scaled_acceleration.data() + max_scaled_acceleration.rows()); + + if (!(max_jerk.array() < 0).all()) + { + const Eigen::VectorXd max_scaled_jerk = max_jerk; // max_jerk.array() * max_jerk_scaling_factors.array(); + ruckig_input.max_jerk = + std::vector(max_scaled_jerk.data(), max_scaled_jerk.data() + max_scaled_jerk.rows()); + } + + // Get origina data + std::vector original_velocities; + std::vector original_accelerations; + Eigen::VectorXd original_duration_from_previous; + + original_velocities.reserve(num_waypoints); + original_accelerations.reserve(num_waypoints); + original_duration_from_previous.resize(trajectory.size()); + + original_velocities.push_back(trajectory.getVelocity(0)); + original_accelerations.push_back(trajectory.getAcceleration(0)); + original_duration_from_previous[0] = trajectory.getTimeFromStart(0); + double previous_time = original_duration_from_previous[0]; + for (Eigen::Index i = 1; i < trajectory.size(); ++i) + { + original_velocities.push_back(trajectory.getVelocity(i)); + original_accelerations.push_back(trajectory.getAcceleration(i)); + const double current_time = trajectory.getTimeFromStart(i); + original_duration_from_previous(i) = current_time - previous_time; + previous_time = current_time; + } + + // Initialize Ruckig + double timestep = original_duration_from_previous.sum() / static_cast(num_waypoints - 1); + auto ruckig_ptr = std::make_unique >(dof, timestep); + initializeRuckigState(ruckig_input, ruckig_output, trajectory, max_scaled_velocity, max_scaled_acceleration); + + // Smooth trajectory + ruckig::Result ruckig_result{}; + double duration_extension_factor = 1; + bool smoothing_complete = false; + while ((duration_extension_factor < max_duration_extension_factor_) && !smoothing_complete) + { + for (Eigen::Index waypoint_idx = 0; waypoint_idx < num_waypoints - 1; ++waypoint_idx) + { + // Get Next Input + getNextRuckigInput( + ruckig_input, trajectory, waypoint_idx, waypoint_idx + 1, max_scaled_velocity, max_scaled_acceleration); + + // Run Ruckig + ruckig_result = ruckig_ptr->update(ruckig_input, ruckig_output); + + if ((waypoint_idx == num_waypoints - 2) && ruckig_result == ruckig::Result::Finished) + { + smoothing_complete = true; + break; + } + + // Extend the trajectory duration if Ruckig could not reach the waypoint successfully + if (ruckig_result != ruckig::Result::Finished) + { + duration_extension_factor *= duration_extension_fraction_; + Eigen::VectorXd new_duration_from_previous = original_duration_from_previous; + + double time_from_start = original_duration_from_previous(0); + for (Eigen::Index time_stretch_idx = 1; time_stretch_idx < num_waypoints; ++time_stretch_idx) + { + double duration_from_previous = duration_extension_factor * original_duration_from_previous(time_stretch_idx); + new_duration_from_previous(time_stretch_idx) = duration_from_previous; + time_from_start += duration_from_previous; + + // re-calculate waypoint velocity and acceleration + timestep = new_duration_from_previous.sum() / static_cast(new_duration_from_previous.rows() - 1); + Eigen::VectorXd new_velocity = + (1 / duration_extension_factor) * original_velocities[static_cast(time_stretch_idx)]; + Eigen::VectorXd new_acceleration = (new_velocity - trajectory.getVelocity(time_stretch_idx - 1)) / timestep; + trajectory.setData(time_stretch_idx, new_velocity, new_acceleration, time_from_start); + } + ruckig_ptr = std::make_unique >(dof, timestep); + initializeRuckigState(ruckig_input, ruckig_output, trajectory, max_scaled_velocity, max_scaled_acceleration); + + // Begin the while() loop again + break; + } + } + } + + if (ruckig_result != ruckig::Result::Finished) + { + CONSOLE_BRIDGE_logError("Ruckig trajectory smoothing failed. Ruckig error: %s", ruckig_result); + return false; + } + + return true; +} +#endif +} // namespace tesseract_planning diff --git a/tesseract_time_parameterization/src/tesseract_common_trajectory.cpp b/tesseract_time_parameterization/src/tesseract_common_trajectory.cpp index 8fb7a3dea35..488a1a1a616 100644 --- a/tesseract_time_parameterization/src/tesseract_common_trajectory.cpp +++ b/tesseract_time_parameterization/src/tesseract_common_trajectory.cpp @@ -42,16 +42,32 @@ const Eigen::VectorXd& TesseractCommonTrajectory::getPosition(Eigen::Index i) co // TODO add assert that i(i)).position; } + +Eigen::VectorXd& TesseractCommonTrajectory::getPosition(Eigen::Index i) +{ + return trajectory_.at(static_cast(i)).position; +} + const Eigen::VectorXd& TesseractCommonTrajectory::getVelocity(Eigen::Index i) const { return trajectory_.at(static_cast(i)).velocity; } +Eigen::VectorXd& TesseractCommonTrajectory::getVelocity(Eigen::Index i) +{ + return trajectory_.at(static_cast(i)).velocity; +} + const Eigen::VectorXd& TesseractCommonTrajectory::getAcceleration(Eigen::Index i) const { return trajectory_.at(static_cast(i)).acceleration; } +Eigen::VectorXd& TesseractCommonTrajectory::getAcceleration(Eigen::Index i) +{ + return trajectory_.at(static_cast(i)).acceleration; +} + double TesseractCommonTrajectory::getTimeFromStart(Eigen::Index i) const { return trajectory_.at(static_cast(i)).time; diff --git a/tesseract_time_parameterization/test/CMakeLists.txt b/tesseract_time_parameterization/test/CMakeLists.txt index 31bf9502703..bf899ab6fa4 100644 --- a/tesseract_time_parameterization/test/CMakeLists.txt +++ b/tesseract_time_parameterization/test/CMakeLists.txt @@ -66,3 +66,21 @@ target_code_coverage( add_gtest_discover_tests(${PROJECT_NAME}_time_optimal_trajectory_generation_tests) add_dependencies(${PROJECT_NAME}_time_optimal_trajectory_generation_tests ${PROJECT_NAME}) add_dependencies(run_tests ${PROJECT_NAME}_time_optimal_trajectory_generation_tests) + +# Ruckig Timeparameterization Tests +add_executable(${PROJECT_NAME}_ruckig_trajectory_smoothing_tests ruckig_trajectory_smoothing_tests.cpp) +target_link_libraries(${PROJECT_NAME}_ruckig_trajectory_smoothing_tests PRIVATE GTest::GTest GTest::Main + ${PROJECT_NAME}) +target_compile_options(${PROJECT_NAME}_ruckig_trajectory_smoothing_tests PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE}) +target_compile_options(${PROJECT_NAME}_ruckig_trajectory_smoothing_tests PUBLIC ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) +target_clang_tidy(${PROJECT_NAME}_ruckig_trajectory_smoothing_tests ENABLE ${TESSERACT_ENABLE_CLANG_TIDY}) +target_cxx_version(${PROJECT_NAME}_ruckig_trajectory_smoothing_tests PRIVATE VERSION ${TESSERACT_CXX_VERSION}) +target_code_coverage( + ${PROJECT_NAME}_ruckig_trajectory_smoothing_tests + PRIVATE + ALL + EXCLUDE ${COVERAGE_EXCLUDE} + ENABLE ${TESSERACT_ENABLE_CODE_COVERAGE}) +add_gtest_discover_tests(${PROJECT_NAME}_ruckig_trajectory_smoothing_tests) +add_dependencies(${PROJECT_NAME}_ruckig_trajectory_smoothing_tests ${PROJECT_NAME}) +add_dependencies(run_tests ${PROJECT_NAME}_ruckig_trajectory_smoothing_tests) diff --git a/tesseract_time_parameterization/test/ruckig_trajectory_smoothing_tests.cpp b/tesseract_time_parameterization/test/ruckig_trajectory_smoothing_tests.cpp new file mode 100644 index 00000000000..1f09789baf1 --- /dev/null +++ b/tesseract_time_parameterization/test/ruckig_trajectory_smoothing_tests.cpp @@ -0,0 +1,154 @@ +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include +#include +#include +#include + +#include +#include +#include + +using namespace tesseract_planning; + +// Initialize one-joint, 3 points exactly the same. +CompositeInstruction createRepeatedPointTrajectory() +{ + const int num = 3; + std::vector joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" }; + + CompositeInstruction program; + for (int i = 0; i < num; i++) + { + StateWaypointPoly swp{ StateWaypoint(joint_names, Eigen::VectorXd::Zero(6)) }; + swp.getPosition()[0] = 1; + if (i == 0) + program.setStartInstruction(MoveInstruction(swp, MoveInstructionType::START)); + else + program.appendMoveInstruction(MoveInstruction(swp, MoveInstructionType::FREESPACE)); + } + + return program; +} + +// Initialize one-joint, straight-line trajectory +CompositeInstruction createStraightTrajectory() +{ + const int num = 10; + const double max = 2.0; + + std::vector joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" }; + + CompositeInstruction program; + for (int i = 0; i < num; i++) + { + StateWaypointPoly swp{ StateWaypoint(joint_names, Eigen::VectorXd::Zero(6)) }; + swp.getPosition()[0] = i * max / num; + if (i == 0) + program.setStartInstruction(MoveInstruction(swp, MoveInstructionType::START)); + else + program.appendMoveInstruction(MoveInstruction(swp, MoveInstructionType::FREESPACE)); + } + + // leave final velocity/acceleration unset + StateWaypointPoly swp{ StateWaypoint(joint_names, Eigen::VectorXd::Zero(6)) }; + swp.getPosition()[0] = max; + program.appendMoveInstruction(MoveInstruction(swp, MoveInstructionType::FREESPACE)); + + return program; +} + +TEST(RuckigTrajectorySmoothingTest, Example) // NOLINT +{ + // Create input parameters + ruckig::InputParameter<3> input; + input.current_position = { 0.0, 0.0, 0.5 }; + input.current_velocity = { 0.0, -2.2, -0.5 }; + input.current_acceleration = { 0.0, 2.5, -0.5 }; + + input.target_position = { 5.0, -2.0, -3.5 }; + input.target_velocity = { 0.0, -0.5, -2.0 }; + input.target_acceleration = { 0.0, 0.0, 0.5 }; + + input.max_velocity = { 3.0, 1.0, 3.0 }; + input.max_acceleration = { 3.0, 2.0, 1.0 }; + input.max_jerk = { 4.0, 3.0, 2.0 }; + + // Set different constraints for negative direction + input.min_velocity = { -2.0, -0.5, -3.0 }; + input.min_acceleration = { -2.0, -2.0, -2.0 }; + + // We don't need to pass the control rate (cycle time) when using only offline features + ruckig::Ruckig<3> otg; + ruckig::Trajectory<3> trajectory; + + // Calculate the trajectory in an offline manner (outside of the control loop) + ruckig::Result result = otg.calculate(input, trajectory); + if (result == ruckig::Result::ErrorInvalidInput) + { + std::cout << "Invalid input!" << std::endl; + return; + } + + // Get duration of the trajectory + std::cout << "Trajectory duration: " << trajectory.get_duration() << " [s]." << std::endl; + + double new_time{ 1.0 }; + + // Then, we can calculate the kinematic state at a given time + std::array new_position{}, new_velocity{}, new_acceleration{}; + trajectory.at_time(new_time, new_position, new_velocity, new_acceleration); + + std::cout << "Position at time " << new_time << " [s]: " << new_position[0] << ", " << new_position[1] << ", " + << new_position[2] << std::endl; + + // Get some info about the position extrema of the trajectory + std::array position_extrema = trajectory.get_position_extrema(); + std::cout << "Position extremas for DoF 4 are " << position_extrema[2].min << " (min) to " << position_extrema[2].max + << " (max)" << std::endl; +} + +TEST(RuckigTrajectorySmoothingTest, RuckigTrajectorySmoothingSolve) // NOLINT +{ + IterativeSplineParameterization time_parameterization(false); + CompositeInstruction program = createStraightTrajectory(); + std::vector max_velocity = { 2.088, 2.082, 3.27, 3.6, 3.3, 3.078 }; + std::vector max_acceleration = { 1, 1, 1, 1, 1, 1 }; + std::vector max_jerk = { 1000, 1000, 1000, 1000, 1000, 1000 }; + TrajectoryContainer::Ptr trajectory = std::make_shared(program); + EXPECT_TRUE(time_parameterization.compute(*trajectory, max_velocity, max_acceleration)); + ASSERT_LT(program.back().as().getWaypoint().as().getTime(), 5.0); + + RuckigTrajectorySmoothing traj_smoothing; + EXPECT_TRUE(traj_smoothing.compute(*trajectory, max_velocity, max_acceleration, max_jerk)); + ASSERT_LT(program.back().as().getWaypoint().as().getTime(), 8.0); +} + +TEST(RuckigTrajectorySmoothingTest, RuckigTrajectorySmoothingRepeatedPointSolve) // NOLINT +{ + IterativeSplineParameterization time_parameterization(true); + CompositeInstruction program = createRepeatedPointTrajectory(); + std::vector max_velocity = { 2.088, 2.082, 3.27, 3.6, 3.3, 3.078 }; + std::vector max_acceleration = { 1, 1, 1, 1, 1, 1 }; + std::vector max_jerk = { 1000, 1000, 1000, 1000, 1000, 1000 }; + TrajectoryContainer::Ptr trajectory = std::make_shared(program); + EXPECT_TRUE(time_parameterization.compute(*trajectory, max_velocity, max_acceleration)); + ASSERT_LT(program.back().as().getWaypoint().as().getTime(), 0.001); + + RuckigTrajectorySmoothing traj_smoothing; + EXPECT_TRUE(traj_smoothing.compute(*trajectory, max_velocity, max_acceleration, max_jerk)); + ASSERT_LT(program.back().as().getWaypoint().as().getTime(), 0.001); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + + console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG); + return RUN_ALL_TESTS(); +}