Skip to content

Commit 9026ac8

Browse files
rhaschkerhaschke
authored
Make TimeParamerization configurable (#339)
1 parent 8beb0f4 commit 9026ac8

File tree

8 files changed

+53
-21
lines changed

8 files changed

+53
-21
lines changed

‎core/include/moveit/task_constructor/merge.h‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -39,6 +39,7 @@
3939
#include <moveit/task_constructor/storage.h>
4040
#include <moveit/robot_model/robot_model.h>
4141
#include <moveit/robot_trajectory/robot_trajectory.h>
42+
#include <moveit/trajectory_processing/time_parameterization.h>
4243

4344
namespace moveit {
4445
namespace task_constructor {
@@ -57,6 +58,7 @@ moveit::core::JointModelGroup* merge(const std::vector<const moveit::core::Joint
5758
*/
5859
robot_trajectory::RobotTrajectoryPtr
5960
merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
60-
const moveit::core::RobotState& base_state, moveit::core::JointModelGroup*& merged_group);
61+
const moveit::core::RobotState& base_state, moveit::core::JointModelGroup*& merged_group,
62+
const trajectory_processing::TimeParameterization& time_parameterization);
6163
} // namespace task_constructor
6264
} // namespace moveit

‎core/include/moveit/task_constructor/moveit_compat.h‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -39,6 +39,7 @@
3939
#pragma once
4040

4141
#include <moveit/version.h>
42+
#include <moveit/macros/class_forward.h>
4243

4344
#define MOVEIT_VERSION_GE(major, minor, patch) \
4445
(MOVEIT_VERSION_MAJOR * 1'000'000 + MOVEIT_VERSION_MINOR * 1'000 + MOVEIT_VERSION_PATCH >= \
@@ -57,3 +58,10 @@
5758
#define MOVEIT_HAS_OBJECT_POSE MOVEIT_VERSION_GE(1, 1, 6)
5859

5960
#define MOVEIT_HAS_STATE_RIGID_PARENT_LINK MOVEIT_VERSION_GE(1, 1, 6)
61+
62+
#if !MOVEIT_VERSION_GE(1, 1, 9)
63+
// the pointers are not yet available
64+
namespace trajectory_processing {
65+
MOVEIT_CLASS_FORWARD(TimeParameterization);
66+
}
67+
#endif

‎core/src/container.cpp‎

Lines changed: 12 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -37,7 +37,9 @@
3737
#include <moveit/task_constructor/container_p.h>
3838
#include <moveit/task_constructor/introspection.h>
3939
#include <moveit/task_constructor/merge.h>
40+
#include <moveit/task_constructor/moveit_compat.h>
4041
#include <moveit/planning_scene/planning_scene.h>
42+
#include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
4143

4244
#include <ros/console.h>
4345

@@ -49,6 +51,7 @@
4951
#include <functional>
5052

5153
using namespace std::placeholders;
54+
using namespace trajectory_processing;
5255

5356
namespace moveit {
5457
namespace task_constructor {
@@ -635,7 +638,7 @@ void SerialContainerPrivate::validateConnectivity() const {
635638
ContainerBasePrivate::validateConnectivity();
636639

637640
InterfaceFlags mine = interfaceFlags();
638-
// check that input / output interface of first / last child matches this' resp. interface
641+
// check that input/output interface of first/last child matches this' resp. interface
639642
validateInterface<START_IF_MASK>(*children().front()->pimpl(), mine);
640643
validateInterface<END_IF_MASK>(*children().back()->pimpl(), mine);
641644

@@ -647,7 +650,7 @@ void SerialContainerPrivate::validateConnectivity() const {
647650
const StagePrivate* const cur_impl = **cur;
648651
InterfaceFlags required = cur_impl->interfaceFlags();
649652

650-
// get iterators to prev / next stage in sequence
653+
// get iterators to prev/next stage in sequence
651654
auto prev = cur;
652655
--prev;
653656
auto next = cur;
@@ -750,7 +753,7 @@ void ParallelContainerBasePrivate::validateInterfaces(const StagePrivate& child,
750753
void ParallelContainerBasePrivate::validateConnectivity() const {
751754
InterfaceFlags my_interface = interfaceFlags();
752755

753-
// check that input / output interfaces of all children are handled by my interface
756+
// check that input/output interfaces of all children are handled by my interface
754757
for (const auto& child : children())
755758
validateInterfaces(*child->pimpl(), my_interface);
756759

@@ -1084,7 +1087,10 @@ void MergerPrivate::resolveInterface(InterfaceFlags expected) {
10841087
}
10851088
}
10861089

1087-
Merger::Merger(const std::string& name) : Merger(new MergerPrivate(this, name)) {}
1090+
Merger::Merger(const std::string& name) : Merger(new MergerPrivate(this, name)) {
1091+
properties().declare<TimeParameterizationPtr>("time_parameterization",
1092+
std::make_shared<TimeOptimalTrajectoryGeneration>());
1093+
}
10881094

10891095
void Merger::reset() {
10901096
ParallelContainerBase::reset();
@@ -1237,7 +1243,8 @@ void MergerPrivate::merge(const ChildSolutionList& sub_solutions,
12371243
moveit::core::JointModelGroup* jmg = jmg_merged_.get();
12381244
robot_trajectory::RobotTrajectoryPtr merged;
12391245
try {
1240-
merged = task_constructor::merge(sub_trajectories, start_scene->getCurrentState(), jmg);
1246+
auto timing = me_->properties().get<TimeParameterizationPtr>("time_parameterization");
1247+
merged = task_constructor::merge(sub_trajectories, start_scene->getCurrentState(), jmg, *timing);
12411248
} catch (const std::runtime_error& e) {
12421249
SubTrajectory t;
12431250
t.markAsFailure();

‎core/src/merge.cpp‎

Lines changed: 3 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -36,7 +36,6 @@
3636
/* Authors: Luca Lach, Robert Haschke */
3737

3838
#include <moveit/task_constructor/merge.h>
39-
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
4039

4140
#include <boost/range/adaptor/transformed.hpp>
4241
#include <boost/algorithm/string/join.hpp>
@@ -106,7 +105,8 @@ moveit::core::JointModelGroup* merge(const std::vector<const moveit::core::Joint
106105

107106
robot_trajectory::RobotTrajectoryPtr
108107
merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
109-
const robot_state::RobotState& base_state, moveit::core::JointModelGroup*& merged_group) {
108+
const robot_state::RobotState& base_state, moveit::core::JointModelGroup*& merged_group,
109+
const trajectory_processing::TimeParameterization& time_parameterization) {
110110
if (sub_trajectories.size() <= 1)
111111
throw std::runtime_error("Expected multiple sub solutions");
112112

@@ -166,8 +166,7 @@ merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajecto
166166
}
167167

168168
// add timing
169-
trajectory_processing::IterativeParabolicTimeParameterization timing;
170-
timing.computeTimeStamps(*merged_traj, 1.0, 1.0);
169+
time_parameterization.computeTimeStamps(*merged_traj, 1.0, 1.0);
171170
return merged_traj;
172171
}
173172
} // namespace task_constructor

‎core/src/solvers/cartesian_path.cpp‎

Lines changed: 6 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -40,11 +40,13 @@
4040
#include <moveit/task_constructor/moveit_compat.h>
4141

4242
#include <moveit/planning_scene/planning_scene.h>
43-
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
43+
#include <moveit/trajectory_processing/time_parameterization.h>
4444
#if MOVEIT_HAS_CARTESIAN_INTERPOLATOR
4545
#include <moveit/robot_state/cartesian_interpolator.h>
4646
#endif
4747

48+
using namespace trajectory_processing;
49+
4850
namespace moveit {
4951
namespace task_constructor {
5052
namespace solvers {
@@ -107,9 +109,9 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons
107109
for (const auto& waypoint : trajectory)
108110
result->addSuffixWayPoint(waypoint, 0.0);
109111

110-
trajectory_processing::IterativeParabolicTimeParameterization timing;
111-
timing.computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
112-
props.get<double>("max_acceleration_scaling_factor"));
112+
auto timing = props.get<TimeParameterizationPtr>("time_parameterization");
113+
timing->computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
114+
props.get<double>("max_acceleration_scaling_factor"));
113115

114116
return achieved_fraction >= props.get<double>("min_fraction");
115117
}

‎core/src/solvers/joint_interpolation.cpp‎

Lines changed: 7 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -37,15 +37,18 @@
3737
*/
3838

3939
#include <moveit/task_constructor/solvers/joint_interpolation.h>
40+
#include <moveit/task_constructor/moveit_compat.h>
4041
#include <moveit/planning_scene/planning_scene.h>
41-
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
42+
#include <moveit/trajectory_processing/time_parameterization.h>
4243

4344
#include <chrono>
4445

4546
namespace moveit {
4647
namespace task_constructor {
4748
namespace solvers {
4849

50+
using namespace trajectory_processing;
51+
4952
JointInterpolationPlanner::JointInterpolationPlanner() {
5053
auto& p = properties();
5154
p.declare<double>("max_step", 0.1, "max joint step");
@@ -89,10 +92,9 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
8992
if (from->isStateColliding(to_state, jmg->getName()))
9093
return false;
9194

92-
// add timing, TODO: use a generic method to add timing via plugins
93-
trajectory_processing::IterativeParabolicTimeParameterization timing;
94-
timing.computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
95-
props.get<double>("max_acceleration_scaling_factor"));
95+
auto timing = props.get<TimeParameterizationPtr>("time_parameterization");
96+
timing->computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
97+
props.get<double>("max_acceleration_scaling_factor"));
9698

9799
return true;
98100
}

‎core/src/solvers/planner_interface.cpp‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -37,6 +37,10 @@
3737
*/
3838

3939
#include <moveit/task_constructor/solvers/planner_interface.h>
40+
#include <moveit/task_constructor/moveit_compat.h>
41+
#include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
42+
43+
using namespace trajectory_processing;
4044

4145
namespace moveit {
4246
namespace task_constructor {
@@ -46,6 +50,7 @@ PlannerInterface::PlannerInterface() {
4650
auto& p = properties();
4751
p.declare<double>("max_velocity_scaling_factor", 1.0, "scale down max velocity by this factor");
4852
p.declare<double>("max_acceleration_scaling_factor", 1.0, "scale down max acceleration by this factor");
53+
p.declare<TimeParameterizationPtr>("time_parameterization", std::make_shared<TimeOptimalTrajectoryGeneration>());
4954
}
5055
} // namespace solvers
5156
} // namespace task_constructor

‎core/src/stages/connect.cpp‎

Lines changed: 9 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -38,9 +38,13 @@
3838

3939
#include <moveit/task_constructor/stages/connect.h>
4040
#include <moveit/task_constructor/merge.h>
41+
#include <moveit/task_constructor/moveit_compat.h>
42+
#include <moveit/task_constructor/cost_terms.h>
43+
4144
#include <moveit/planning_scene/planning_scene.h>
45+
#include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
4246

43-
#include <moveit/task_constructor/cost_terms.h>
47+
using namespace trajectory_processing;
4448

4549
namespace moveit {
4650
namespace task_constructor {
@@ -54,6 +58,8 @@ Connect::Connect(const std::string& name, const GroupPlannerVector& planners) :
5458
p.declare<MergeMode>("merge_mode", WAYPOINTS, "merge mode");
5559
p.declare<moveit_msgs::Constraints>("path_constraints", moveit_msgs::Constraints(),
5660
"constraints to maintain during trajectory");
61+
properties().declare<TimeParameterizationPtr>("merge_time_parameterization",
62+
std::make_shared<TimeOptimalTrajectoryGeneration>());
5763
}
5864

5965
void Connect::reset() {
@@ -219,7 +225,8 @@ SubTrajectoryPtr Connect::merge(const std::vector<robot_trajectory::RobotTraject
219225

220226
auto jmg = merged_jmg_.get();
221227
assert(jmg);
222-
robot_trajectory::RobotTrajectoryPtr trajectory = task_constructor::merge(sub_trajectories, state, jmg);
228+
auto timing = properties().get<TimeParameterizationPtr>("merge_time_parameterization");
229+
robot_trajectory::RobotTrajectoryPtr trajectory = task_constructor::merge(sub_trajectories, state, jmg, *timing);
223230
if (!trajectory)
224231
return SubTrajectoryPtr();
225232

0 commit comments

Comments
 (0)