Skip to content

Commit 425b93c

Browse files
Levi-ArmstrongLevi-Armstrong
committed
Add ruckig trajectory smoothing
1 parent 59159ea commit 425b93c

19 files changed

+1192
-15
lines changed

‎tesseract_process_managers/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -70,6 +70,7 @@ add_library(
7070
src/task_generators/iterative_spline_parameterization_task_generator.cpp
7171
src/task_generators/motion_planner_task_generator.cpp
7272
src/task_generators/profile_switch_task_generator.cpp
73+
src/task_generators/ruckig_trajectory_smoothing_task_generator.cpp
7374
src/task_generators/seed_min_length_task_generator.cpp
7475
src/task_generators/time_optimal_parameterization_task_generator.cpp
7576
src/task_generators/upsample_trajectory_task_generator.cpp

‎tesseract_process_managers/include/tesseract_process_managers/core/default_process_planners.h‎

Lines changed: 12 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -31,18 +31,26 @@
3131
namespace tesseract_planning
3232
{
3333
/** @brief Create TrajOpt Process Pipeline */
34-
TaskflowGenerator::UPtr createTrajOptGenerator(bool check_input = true, bool post_collision_check = true);
34+
TaskflowGenerator::UPtr createTrajOptGenerator(bool check_input = true,
35+
bool post_collision_check = true,
36+
bool post_smoothing = false);
3537

3638
#ifdef TESSERACT_PROCESS_MANAGERS_HAS_TRAJOPT_IFOPT
3739
/** @brief Create TrajOpt IFOPT Process Pipeline */
38-
TaskflowGenerator::UPtr createTrajOptIfoptGenerator(bool check_input = true, bool post_collision_check = true);
40+
TaskflowGenerator::UPtr createTrajOptIfoptGenerator(bool check_input = true,
41+
bool post_collision_check = true,
42+
bool post_smoothing = false);
3943
#endif
4044

4145
/** @brief Create OMPL Process Pipeline */
42-
TaskflowGenerator::UPtr createOMPLGenerator(bool check_input = true, bool post_collision_check = true);
46+
TaskflowGenerator::UPtr createOMPLGenerator(bool check_input = true,
47+
bool post_collision_check = true,
48+
bool post_smoothing = false);
4349

4450
/** @brief Create Descartes Process Pipeline */
45-
TaskflowGenerator::UPtr createDescartesGenerator(bool check_input = true, bool post_collision_check = true);
51+
TaskflowGenerator::UPtr createDescartesGenerator(bool check_input = true,
52+
bool post_collision_check = true,
53+
bool post_smoothing = false);
4654

4755
/**
4856
* @brief Create Descartes Only Process Pipeline

‎tesseract_process_managers/include/tesseract_process_managers/core/default_task_namespaces.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -47,6 +47,7 @@ static const std::string PROFILE_SWITCH_DEFAULT_NAMESPACE = "
4747
static const std::string SEED_MIN_LENGTH_DEFAULT_NAMESPACE = "SEED_MIN_LENGTH_CHECK";
4848
static const std::string TIME_OPTIMAL_PARAMETERIZATION_DEFAULT_NAMESPACE = "TIME_OPTIMAL_PARAMETERIZATION";
4949
static const std::string UPSAMPLE_TRAJECTORY_DEFAULT_NAMESPACE = "UPSAMPLE_TRAJECTORY";
50+
static const std::string RUCKIG_TRAJECTORY_SMOOTHING_DEFAULT_NAMESPACE = "RUCKIG_TRAJECTORY_SMOOTHING";
5051

5152
// clang-format on
5253
} // namespace tesseract_planning::profile_ns

‎tesseract_process_managers/include/tesseract_process_managers/task_generators/ruckig_trajectory_smoothing_task_generator.h‎

Lines changed: 82 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,82 @@
1+
/**
2+
* @file ruckig_trajectory_smoothing_task_generator.h
3+
* @brief Leveraging Ruckig to smooth trajectory
4+
*
5+
* @author Levi Armstrong
6+
* @date July 27, 2022
7+
* @version TODO
8+
* @bug No known bugs
9+
*
10+
* @par License
11+
* Software License Agreement (Apache License)
12+
* @par
13+
* Licensed under the Apache License, Version 2.0 (the "License");
14+
* you may not use this file except in compliance with the License.
15+
* You may obtain a copy of the License at
16+
* http://www.apache.org/licenses/LICENSE-2.0
17+
* @par
18+
* Unless required by applicable law or agreed to in writing, software
19+
* distributed under the License is distributed on an "AS IS" BASIS,
20+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
21+
* See the License for the specific language governing permissions and
22+
* limitations under the License.
23+
*/
24+
#ifndef TESSERACT_PROCESS_MANAGERS_RUCKIG_TRAJECTORY_SMOOTHING_TASK_GENERATOR_H
25+
#define TESSERACT_PROCESS_MANAGERS_RUCKIG_TRAJECTORY_SMOOTHING_TASK_GENERATOR_H
26+
27+
#include <tesseract_common/macros.h>
28+
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
29+
#include <boost/serialization/access.hpp>
30+
TESSERACT_COMMON_IGNORE_WARNINGS_POP
31+
32+
#include <tesseract_process_managers/core/task_generator.h>
33+
#include <tesseract_time_parameterization/ruckig_trajectory_smoothing.h>
34+
#include <tesseract_process_managers/core/default_task_namespaces.h>
35+
36+
namespace tesseract_planning
37+
{
38+
class RuckigTrajectorySmoothingTaskGenerator : public TaskGenerator
39+
{
40+
public:
41+
using UPtr = std::unique_ptr<RuckigTrajectorySmoothingTaskGenerator>;
42+
43+
RuckigTrajectorySmoothingTaskGenerator(std::string name = profile_ns::RUCKIG_TRAJECTORY_SMOOTHING_DEFAULT_NAMESPACE);
44+
45+
~RuckigTrajectorySmoothingTaskGenerator() override = default;
46+
RuckigTrajectorySmoothingTaskGenerator(const RuckigTrajectorySmoothingTaskGenerator&) = delete;
47+
RuckigTrajectorySmoothingTaskGenerator& operator=(const RuckigTrajectorySmoothingTaskGenerator&) = delete;
48+
RuckigTrajectorySmoothingTaskGenerator(RuckigTrajectorySmoothingTaskGenerator&&) = delete;
49+
RuckigTrajectorySmoothingTaskGenerator& operator=(RuckigTrajectorySmoothingTaskGenerator&&) = delete;
50+
51+
int conditionalProcess(TaskInput input, std::size_t unique_id) const override final;
52+
53+
void process(TaskInput input, std::size_t unique_id) const override final;
54+
};
55+
56+
class RuckigTrajectorySmoothingTaskInfo : public TaskInfo
57+
{
58+
public:
59+
using Ptr = std::shared_ptr<RuckigTrajectorySmoothingTaskInfo>;
60+
using ConstPtr = std::shared_ptr<const RuckigTrajectorySmoothingTaskInfo>;
61+
62+
RuckigTrajectorySmoothingTaskInfo() = default;
63+
RuckigTrajectorySmoothingTaskInfo(std::size_t unique_id,
64+
std::string name = profile_ns::RUCKIG_TRAJECTORY_SMOOTHING_DEFAULT_NAMESPACE);
65+
66+
TaskInfo::UPtr clone() const override;
67+
68+
bool operator==(const RuckigTrajectorySmoothingTaskInfo& rhs) const;
69+
bool operator!=(const RuckigTrajectorySmoothingTaskInfo& rhs) const;
70+
71+
private:
72+
friend class boost::serialization::access;
73+
template <class Archive>
74+
void serialize(Archive& ar, const unsigned int version); // NOLINT
75+
};
76+
} // namespace tesseract_planning
77+
78+
#include <boost/serialization/export.hpp>
79+
#include <boost/serialization/tracking.hpp>
80+
BOOST_CLASS_EXPORT_KEY2(tesseract_planning::RuckigTrajectorySmoothingTaskInfo, "RuckigTrajectorySmoothingTaskInfo")
81+
82+
#endif // TESSERACT_PROCESS_MANAGERS_RUCKIG_TRAJECTORY_SMOOTHING_TASK_GENERATOR_H

‎tesseract_process_managers/include/tesseract_process_managers/task_profiles/ruckig_trajectory_smoothing_profile.h‎

Lines changed: 90 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,90 @@
1+
/**
2+
* @file ruckig_trajectory_smoothing_profile.h
3+
* @brief Leveraging Ruckig to smooth trajectory
4+
*
5+
* @author Levi Armstrong
6+
* @date July 27, 2022
7+
* @version TODO
8+
* @bug No known bugs
9+
*
10+
* @par License
11+
* Software License Agreement (Apache License)
12+
* @par
13+
* Licensed under the Apache License, Version 2.0 (the "License");
14+
* you may not use this file except in compliance with the License.
15+
* You may obtain a copy of the License at
16+
* http://www.apache.org/licenses/LICENSE-2.0
17+
* @par
18+
* Unless required by applicable law or agreed to in writing, software
19+
* distributed under the License is distributed on an "AS IS" BASIS,
20+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
21+
* See the License for the specific language governing permissions and
22+
* limitations under the License.
23+
*/
24+
#ifndef TESSERACT_PROCESS_MANAGERS_RUCKIG_TRAJECTORY_SMOOTHING_PROFILE_H
25+
#define TESSERACT_PROCESS_MANAGERS_RUCKIG_TRAJECTORY_SMOOTHING_PROFILE_H
26+
27+
#include <tesseract_common/macros.h>
28+
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
29+
#include <memory>
30+
TESSERACT_COMMON_IGNORE_WARNINGS_POP
31+
32+
namespace tesseract_planning
33+
{
34+
struct RuckigTrajectorySmoothingCompositeProfile
35+
{
36+
using Ptr = std::shared_ptr<RuckigTrajectorySmoothingCompositeProfile>;
37+
using ConstPtr = std::shared_ptr<const RuckigTrajectorySmoothingCompositeProfile>;
38+
39+
RuckigTrajectorySmoothingCompositeProfile() = default;
40+
RuckigTrajectorySmoothingCompositeProfile(double duration_extension_fraction,
41+
double max_duration_extension_factor,
42+
double max_velocity_scaling_factor,
43+
double max_acceleration_scaling_factor)
44+
: duration_extension_fraction(duration_extension_fraction)
45+
, max_duration_extension_factor(max_duration_extension_factor)
46+
, max_velocity_scaling_factor(max_velocity_scaling_factor)
47+
, max_acceleration_scaling_factor(max_acceleration_scaling_factor)
48+
{
49+
}
50+
51+
/** @brief duration_extension_fraction The amount to scale the trajectory each time */
52+
double duration_extension_fraction{ 1.1 };
53+
54+
/** @brief The max allow extension factor */
55+
double max_duration_extension_factor{ 10.0 };
56+
57+
/** @brief max_velocity_scaling_factor The max velocity scaling factor passed to the solver */
58+
double max_velocity_scaling_factor{ 1.0 };
59+
60+
/** @brief max_velocity_scaling_factor The max acceleration scaling factor passed to the solver */
61+
double max_acceleration_scaling_factor{ 1.0 };
62+
63+
/** @brief max_jerk_scaling_factor The max jerk scaling factor passed to the solver */
64+
double max_jerk_scaling_factor{ 1.0 };
65+
};
66+
67+
struct RuckigTrajectorySmoothingMoveProfile
68+
{
69+
using Ptr = std::shared_ptr<RuckigTrajectorySmoothingMoveProfile>;
70+
using ConstPtr = std::shared_ptr<const RuckigTrajectorySmoothingMoveProfile>;
71+
72+
RuckigTrajectorySmoothingMoveProfile() = default;
73+
RuckigTrajectorySmoothingMoveProfile(double max_velocity_scaling_factor, double max_acceleration_scaling_factor)
74+
: max_velocity_scaling_factor(max_velocity_scaling_factor)
75+
, max_acceleration_scaling_factor(max_acceleration_scaling_factor)
76+
{
77+
}
78+
79+
/** @brief max_velocity_scaling_factor The max velocity scaling factor passed to the solver */
80+
double max_velocity_scaling_factor{ 1.0 };
81+
82+
/** @brief max_velocity_scaling_factor The max acceleration scaling factor passed to the solver */
83+
double max_acceleration_scaling_factor{ 1.0 };
84+
85+
/** @brief max_jerk_scaling_factor The max jerk scaling factor passed to the solver */
86+
double max_jerk_scaling_factor{ 1.0 };
87+
};
88+
} // namespace tesseract_planning
89+
90+
#endif // TESSERACT_PROCESS_MANAGERS_RUCKIG_TRAJECTORY_SMOOTHING_PROFILE_H

‎tesseract_process_managers/src/core/default_process_planners.cpp‎

Lines changed: 61 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -39,6 +39,7 @@
3939
#include <tesseract_process_managers/task_generators/seed_min_length_task_generator.h>
4040
#include <tesseract_process_managers/task_generators/discrete_contact_check_task_generator.h>
4141
#include <tesseract_process_managers/task_generators/iterative_spline_parameterization_task_generator.h>
42+
#include <tesseract_process_managers/task_generators/ruckig_trajectory_smoothing_task_generator.h>
4243
#include <tesseract_process_managers/task_generators/check_input_task_generator.h>
4344

4445
#include <tesseract_motion_planners/simple/simple_motion_planner.h>
@@ -51,7 +52,7 @@
5152

5253
namespace tesseract_planning
5354
{
54-
TaskflowGenerator::UPtr createTrajOptGenerator(bool check_input, bool post_collision_check)
55+
TaskflowGenerator::UPtr createTrajOptGenerator(bool check_input, bool post_collision_check, bool post_smoothing)
5556
{
5657
auto tf = std::make_unique<GraphTaskflow>("TrajOptTaskflow");
5758

@@ -83,6 +84,11 @@ TaskflowGenerator::UPtr createTrajOptGenerator(bool check_input, bool post_colli
8384
// Setup time parameterization
8485
int time_parameterization_task = tf->addNode(std::make_unique<IterativeSplineParameterizationTaskGenerator>(), true);
8586

87+
// Setup trajectory smoothing
88+
int smoothing_task{ std::numeric_limits<int>::min() };
89+
if (post_smoothing)
90+
smoothing_task = tf->addNode(std::make_unique<RuckigTrajectorySmoothingTaskGenerator>(), true);
91+
8692
if (check_input)
8793
tf->addEdges(check_input_task, { GraphTaskflow::ERROR_NODE, has_seed_task });
8894

@@ -100,13 +106,21 @@ TaskflowGenerator::UPtr createTrajOptGenerator(bool check_input, bool post_colli
100106
tf->addEdges(motion_planner_task, { GraphTaskflow::ERROR_NODE, time_parameterization_task });
101107
}
102108

103-
tf->addEdges(time_parameterization_task, { GraphTaskflow::ERROR_NODE, GraphTaskflow::DONE_NODE });
109+
if (post_smoothing)
110+
{
111+
tf->addEdges(time_parameterization_task, { GraphTaskflow::ERROR_NODE, smoothing_task });
112+
tf->addEdges(smoothing_task, { GraphTaskflow::DONE_NODE });
113+
}
114+
else
115+
{
116+
tf->addEdges(time_parameterization_task, { GraphTaskflow::ERROR_NODE, GraphTaskflow::DONE_NODE });
117+
}
104118

105119
return tf;
106120
}
107121

108122
#ifdef TESSERACT_PROCESS_MANAGERS_HAS_TRAJOPT_IFOPT
109-
TaskflowGenerator::UPtr createTrajOptIfoptGenerator(bool check_input, bool post_collision_check)
123+
TaskflowGenerator::UPtr createTrajOptIfoptGenerator(bool check_input, bool post_collision_check, bool post_smoothing)
110124
{
111125
auto tf = std::make_unique<GraphTaskflow>("TrajOptIfoptTaskflow");
112126

@@ -138,6 +152,11 @@ TaskflowGenerator::UPtr createTrajOptIfoptGenerator(bool check_input, bool post_
138152
// Setup time parameterization
139153
int time_parameterization_task = tf->addNode(std::make_unique<IterativeSplineParameterizationTaskGenerator>(), true);
140154

155+
// Setup trajectory smoothing
156+
int smoothing_task{ std::numeric_limits<int>::min() };
157+
if (post_smoothing)
158+
smoothing_task = tf->addNode(std::make_unique<RuckigTrajectorySmoothingTaskGenerator>(), true);
159+
141160
if (check_input)
142161
tf->addEdges(check_input_task, { GraphTaskflow::ERROR_NODE, has_seed_task });
143162

@@ -155,13 +174,21 @@ TaskflowGenerator::UPtr createTrajOptIfoptGenerator(bool check_input, bool post_
155174
tf->addEdges(motion_planner_task, { GraphTaskflow::ERROR_NODE, time_parameterization_task });
156175
}
157176

158-
tf->addEdges(time_parameterization_task, { GraphTaskflow::ERROR_NODE, GraphTaskflow::DONE_NODE });
177+
if (post_smoothing)
178+
{
179+
tf->addEdges(time_parameterization_task, { GraphTaskflow::ERROR_NODE, smoothing_task });
180+
tf->addEdges(smoothing_task, { GraphTaskflow::DONE_NODE });
181+
}
182+
else
183+
{
184+
tf->addEdges(time_parameterization_task, { GraphTaskflow::ERROR_NODE, GraphTaskflow::DONE_NODE });
185+
}
159186

160187
return tf;
161188
}
162189
#endif
163190

164-
TaskflowGenerator::UPtr createOMPLGenerator(bool check_input, bool post_collision_check)
191+
TaskflowGenerator::UPtr createOMPLGenerator(bool check_input, bool post_collision_check, bool post_smoothing)
165192
{
166193
auto tf = std::make_unique<GraphTaskflow>("OMPLTaskflow");
167194

@@ -193,6 +220,11 @@ TaskflowGenerator::UPtr createOMPLGenerator(bool check_input, bool post_collisio
193220
// Setup time parameterization
194221
int time_parameterization_task = tf->addNode(std::make_unique<IterativeSplineParameterizationTaskGenerator>(), true);
195222

223+
// Setup trajectory smoothing
224+
int smoothing_task{ std::numeric_limits<int>::min() };
225+
if (post_smoothing)
226+
smoothing_task = tf->addNode(std::make_unique<RuckigTrajectorySmoothingTaskGenerator>(), true);
227+
196228
if (check_input)
197229
tf->addEdges(check_input_task, { GraphTaskflow::ERROR_NODE, has_seed_task });
198230

@@ -210,12 +242,20 @@ TaskflowGenerator::UPtr createOMPLGenerator(bool check_input, bool post_collisio
210242
tf->addEdges(motion_planner_task, { GraphTaskflow::ERROR_NODE, time_parameterization_task });
211243
}
212244

213-
tf->addEdges(time_parameterization_task, { GraphTaskflow::ERROR_NODE, GraphTaskflow::DONE_NODE });
245+
if (post_smoothing)
246+
{
247+
tf->addEdges(time_parameterization_task, { GraphTaskflow::ERROR_NODE, smoothing_task });
248+
tf->addEdges(smoothing_task, { GraphTaskflow::DONE_NODE });
249+
}
250+
else
251+
{
252+
tf->addEdges(time_parameterization_task, { GraphTaskflow::ERROR_NODE, GraphTaskflow::DONE_NODE });
253+
}
214254

215255
return tf;
216256
}
217257

218-
TaskflowGenerator::UPtr createDescartesGenerator(bool check_input, bool post_collision_check)
258+
TaskflowGenerator::UPtr createDescartesGenerator(bool check_input, bool post_collision_check, bool post_smoothing)
219259
{
220260
auto tf = std::make_unique<GraphTaskflow>("DescartesTaskflow");
221261

@@ -247,6 +287,11 @@ TaskflowGenerator::UPtr createDescartesGenerator(bool check_input, bool post_col
247287
// Setup time parameterization
248288
int time_parameterization_task = tf->addNode(std::make_unique<IterativeSplineParameterizationTaskGenerator>(), true);
249289

290+
// Setup trajectory smoothing
291+
int smoothing_task{ std::numeric_limits<int>::min() };
292+
if (post_smoothing)
293+
smoothing_task = tf->addNode(std::make_unique<RuckigTrajectorySmoothingTaskGenerator>(), true);
294+
250295
if (check_input)
251296
tf->addEdges(check_input_task, { GraphTaskflow::ERROR_NODE, has_seed_task });
252297

@@ -264,7 +309,15 @@ TaskflowGenerator::UPtr createDescartesGenerator(bool check_input, bool post_col
264309
tf->addEdges(motion_planner_task, { GraphTaskflow::ERROR_NODE, time_parameterization_task });
265310
}
266311

267-
tf->addEdges(time_parameterization_task, { GraphTaskflow::ERROR_NODE, GraphTaskflow::DONE_NODE });
312+
if (post_smoothing)
313+
{
314+
tf->addEdges(time_parameterization_task, { GraphTaskflow::ERROR_NODE, smoothing_task });
315+
tf->addEdges(smoothing_task, { GraphTaskflow::DONE_NODE });
316+
}
317+
else
318+
{
319+
tf->addEdges(time_parameterization_task, { GraphTaskflow::ERROR_NODE, GraphTaskflow::DONE_NODE });
320+
}
268321

269322
return tf;
270323
}

0 commit comments

Comments
 (0)