Skip to content

Commit 6535ab9

Browse files
Levi-ArmstrongLevi-Armstrong
committed
Add seed min length process generator and unit tests
1 parent e53f563 commit 6535ab9

File tree

21 files changed

+1057
-272
lines changed

21 files changed

+1057
-272
lines changed

‎.github/workflows/bionic_build.yml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -27,7 +27,7 @@ jobs:
2727
PARALLEL_TESTS: false
2828
UPSTREAM_CMAKE_ARGS: "-DCMAKE_BUILD_TYPE=Release"
2929
TARGET_CMAKE_ARGS: "-DCMAKE_BUILD_TYPE=Debug -DTESSERACT_ENABLE_TESTING=ON"
30-
AFTER_SCRIPT: 'catkin build -w $target_ws --no-deps --verbose tesseract_collision tesseract_common tesseract_environment tesseract_geometry tesseract_kinematics tesseract_motion_planners tesseract_scene_graph tesseract_urdf --make-args test'
30+
AFTER_SCRIPT: 'catkin build -w $target_ws --no-deps --verbose tesseract_collision tesseract_common tesseract_environment tesseract_geometry tesseract_kinematics tesseract_motion_planners tesseract_process_managers tesseract_time_parameterization tesseract_scene_graph tesseract_urdf --make-args test'
3131
steps:
3232
- uses: actions/checkout@v1
3333

‎.github/workflows/xenial_build.yml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -27,7 +27,7 @@ jobs:
2727
PARALLEL_TESTS: false
2828
UPSTREAM_CMAKE_ARGS: "-DCMAKE_BUILD_TYPE=Release"
2929
TARGET_CMAKE_ARGS: "-DCMAKE_BUILD_TYPE=Debug -DTESSERACT_ENABLE_TESTING=ON"
30-
AFTER_SCRIPT: 'catkin build -w $target_ws --no-deps --verbose tesseract_collision tesseract_common tesseract_environment tesseract_geometry tesseract_kinematics tesseract_motion_planners tesseract_scene_graph tesseract_urdf --make-args test'
30+
AFTER_SCRIPT: 'catkin build -w $target_ws --no-deps --verbose tesseract_collision tesseract_common tesseract_environment tesseract_geometry tesseract_kinematics tesseract_motion_planners tesseract_time_parameterization tesseract_scene_graph tesseract_urdf --make-args test'
3131
steps:
3232
- uses: actions/checkout@v1
3333

‎tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/simple/profile/simple_planner_default_lvs_plan_profile.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -44,7 +44,7 @@ class SimplePlannerDefaultLVSPlanProfile : public SimplePlannerPlanProfile
4444
using ConstPtr = std::shared_ptr<const SimplePlannerDefaultLVSPlanProfile>;
4545

4646
SimplePlannerDefaultLVSPlanProfile(double state_longest_valid_segment_length = 5 * M_PI / 180,
47-
double translation_longest_valid_segment_length = 0.15,
47+
double translation_longest_valid_segment_length = 0.1,
4848
double rotation_longest_valid_segment_length = 5 * M_PI / 180,
4949
int min_steps = 1);
5050

‎tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/trajopt_utils.cpp

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -166,6 +166,9 @@ trajopt::TermInfo::Ptr createCollisionTermInfo(int start_index,
166166
trajopt::TermInfo::Ptr
167167
createSmoothVelocityTermInfo(int start_index, int end_index, int n_joints, double coeff, trajopt::TermType type)
168168
{
169+
if ((end_index - start_index) < 2)
170+
throw std::runtime_error("TrajOpt JointVelTermInfo requires at least two states!");
171+
169172
std::shared_ptr<trajopt::JointVelTermInfo> jv = std::make_shared<trajopt::JointVelTermInfo>();
170173
jv->coeffs = std::vector<double>(static_cast<std::size_t>(n_joints), coeff);
171174
jv->targets = std::vector<double>(static_cast<std::size_t>(n_joints), 0.0);
@@ -181,6 +184,9 @@ trajopt::TermInfo::Ptr createSmoothVelocityTermInfo(int start_index,
181184
const Eigen::Ref<const Eigen::VectorXd>& coeff,
182185
trajopt::TermType type)
183186
{
187+
if ((end_index - start_index) < 2)
188+
throw std::runtime_error("TrajOpt JointVelTermInfo requires at least two states!");
189+
184190
std::shared_ptr<trajopt::JointVelTermInfo> jv = std::make_shared<trajopt::JointVelTermInfo>();
185191
jv->coeffs = std::vector<double>(coeff.data(), coeff.data() + coeff.size());
186192
jv->targets = std::vector<double>(static_cast<std::size_t>(coeff.size()), 0.0);
@@ -194,6 +200,9 @@ trajopt::TermInfo::Ptr createSmoothVelocityTermInfo(int start_index,
194200
trajopt::TermInfo::Ptr
195201
createSmoothAccelerationTermInfo(int start_index, int end_index, int n_joints, double coeff, trajopt::TermType type)
196202
{
203+
if ((end_index - start_index) < 3)
204+
throw std::runtime_error("TrajOpt JointAccTermInfo requires at least three states!");
205+
197206
std::shared_ptr<trajopt::JointAccTermInfo> ja = std::make_shared<trajopt::JointAccTermInfo>();
198207
ja->coeffs = std::vector<double>(static_cast<std::size_t>(n_joints), coeff);
199208
ja->targets = std::vector<double>(static_cast<std::size_t>(n_joints), 0.0);
@@ -209,6 +218,9 @@ trajopt::TermInfo::Ptr createSmoothAccelerationTermInfo(int start_index,
209218
const Eigen::Ref<const Eigen::VectorXd>& coeff,
210219
trajopt::TermType type)
211220
{
221+
if ((end_index - start_index) < 3)
222+
throw std::runtime_error("TrajOpt JointAccTermInfo requires at least three states!");
223+
212224
std::shared_ptr<trajopt::JointAccTermInfo> ja = std::make_shared<trajopt::JointAccTermInfo>();
213225
ja->coeffs = std::vector<double>(coeff.data(), coeff.data() + coeff.size());
214226
ja->targets = std::vector<double>(static_cast<std::size_t>(coeff.size()), 0.0);
@@ -222,6 +234,9 @@ trajopt::TermInfo::Ptr createSmoothAccelerationTermInfo(int start_index,
222234
trajopt::TermInfo::Ptr
223235
createSmoothJerkTermInfo(int start_index, int end_index, int n_joints, double coeff, trajopt::TermType type)
224236
{
237+
if ((end_index - start_index) < 5)
238+
throw std::runtime_error("TrajOpt JointJerkTermInfo requires at least five states!");
239+
225240
std::shared_ptr<trajopt::JointJerkTermInfo> jj = std::make_shared<trajopt::JointJerkTermInfo>();
226241
jj->coeffs = std::vector<double>(static_cast<std::size_t>(n_joints), coeff);
227242
jj->targets = std::vector<double>(static_cast<std::size_t>(n_joints), 0.0);
@@ -237,6 +252,9 @@ trajopt::TermInfo::Ptr createSmoothJerkTermInfo(int start_index,
237252
const Eigen::Ref<const Eigen::VectorXd>& coeff,
238253
trajopt::TermType type)
239254
{
255+
if ((end_index - start_index) < 5)
256+
throw std::runtime_error("TrajOpt JointJerkTermInfo requires at least five states!");
257+
240258
std::shared_ptr<trajopt::JointJerkTermInfo> jj = std::make_shared<trajopt::JointJerkTermInfo>();
241259
jj->coeffs = std::vector<double>(coeff.data(), coeff.data() + coeff.size());
242260
jj->targets = std::vector<double>(static_cast<std::size_t>(coeff.size()), 0.0);

‎tesseract/tesseract_planning/tesseract_process_managers/CMakeLists.txt

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -47,6 +47,7 @@ add_library(${PROJECT_NAME}
4747
src/process_generators/iterative_spline_parameterization_process_generator.cpp
4848
src/process_generators/motion_planner_process_generator.cpp
4949
src/process_generators/profile_switch_process_generator.cpp
50+
src/process_generators/seed_min_length_process_generator.cpp
5051
src/process_managers/raster_process_manager.cpp
5152
src/process_managers/raster_global_process_manager.cpp
5253
src/process_managers/raster_only_process_manager.cpp

‎tesseract/tesseract_planning/tesseract_process_managers/examples/freespace_example_program.h

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -9,8 +9,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
99

1010
namespace tesseract_planning
1111
{
12-
inline CompositeInstruction freespaceExampleProgramIIWA(std::string composite_profile = DEFAULT_PROFILE_KEY,
13-
std::string freespace_profile = DEFAULT_PROFILE_KEY)
12+
inline CompositeInstruction freespaceExampleProgramIIWA(const std::string& composite_profile = DEFAULT_PROFILE_KEY,
13+
const std::string& freespace_profile = DEFAULT_PROFILE_KEY)
1414
{
1515
CompositeInstruction program(composite_profile, CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator"));
1616

@@ -30,8 +30,8 @@ inline CompositeInstruction freespaceExampleProgramIIWA(std::string composite_pr
3030
return program;
3131
}
3232

33-
inline CompositeInstruction freespaceExampleProgramABB(std::string composite_profile = DEFAULT_PROFILE_KEY,
34-
std::string freespace_profile = DEFAULT_PROFILE_KEY)
33+
inline CompositeInstruction freespaceExampleProgramABB(const std::string& composite_profile = DEFAULT_PROFILE_KEY,
34+
const std::string& freespace_profile = DEFAULT_PROFILE_KEY)
3535
{
3636
CompositeInstruction program(composite_profile, CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator"));
3737

‎tesseract/tesseract_planning/tesseract_process_managers/examples/raster_dt_example_program.h

Lines changed: 27 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -33,9 +33,10 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
3333
namespace tesseract_planning
3434
{
3535
/** @brief Create a example raster program with dual transitions */
36-
inline CompositeInstruction rasterDTExampleProgram()
36+
inline CompositeInstruction rasterDTExampleProgram(const std::string& freespace_profile = DEFAULT_PROFILE_KEY,
37+
const std::string& process_profile = "PROCESS")
3738
{
38-
CompositeInstruction program("raster_dt_program", CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator"));
39+
CompositeInstruction program(DEFAULT_PROFILE_KEY, CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator"));
3940

4041
// Start Joint Position for the program
4142
std::vector<std::string> joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" };
@@ -47,9 +48,9 @@ inline CompositeInstruction rasterDTExampleProgram()
4748
Eigen::Quaterniond(0, 0, -1.0, 0));
4849

4950
// Define from start composite instruction
50-
PlanInstruction plan_f0(wp1, PlanInstructionType::FREESPACE, "freespace_profile");
51+
PlanInstruction plan_f0(wp1, PlanInstructionType::FREESPACE, freespace_profile);
5152
plan_f0.setDescription("from_start_plan");
52-
CompositeInstruction from_start;
53+
CompositeInstruction from_start(freespace_profile);
5354
from_start.setDescription("from_start");
5455
from_start.push_back(plan_f0);
5556
program.push_back(from_start);
@@ -73,25 +74,25 @@ inline CompositeInstruction rasterDTExampleProgram()
7374
Waypoint wp7 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(x, 0.3, 0.8) *
7475
Eigen::Quaterniond(0, 0, -1.0, 0));
7576

76-
CompositeInstruction raster_segment;
77+
CompositeInstruction raster_segment(freespace_profile);
7778
raster_segment.setDescription("Raster #" + std::to_string(i + 1));
7879
if (i == 0 || i == 2)
7980
{
80-
raster_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, "RASTER"));
81-
raster_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, "RASTER"));
82-
raster_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, "RASTER"));
83-
raster_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, "RASTER"));
84-
raster_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, "RASTER"));
85-
raster_segment.push_back(PlanInstruction(wp7, PlanInstructionType::LINEAR, "RASTER"));
81+
raster_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile));
82+
raster_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile));
83+
raster_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile));
84+
raster_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile));
85+
raster_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile));
86+
raster_segment.push_back(PlanInstruction(wp7, PlanInstructionType::LINEAR, process_profile));
8687
}
8788
else
8889
{
89-
raster_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, "RASTER"));
90-
raster_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, "RASTER"));
91-
raster_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, "RASTER"));
92-
raster_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, "RASTER"));
93-
raster_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, "RASTER"));
94-
raster_segment.push_back(PlanInstruction(wp1, PlanInstructionType::LINEAR, "RASTER"));
90+
raster_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile));
91+
raster_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile));
92+
raster_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile));
93+
raster_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile));
94+
raster_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile));
95+
raster_segment.push_back(PlanInstruction(wp1, PlanInstructionType::LINEAR, process_profile));
9596
}
9697
program.push_back(raster_segment);
9798

@@ -106,17 +107,17 @@ inline CompositeInstruction rasterDTExampleProgram()
106107
CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8 + (i * 0.1), -0.3, 0.8) *
107108
Eigen::Quaterniond(0, 0, -1.0, 0));
108109

109-
PlanInstruction plan_f1(wp7, PlanInstructionType::FREESPACE, "freespace_profile");
110+
PlanInstruction plan_f1(wp7, PlanInstructionType::FREESPACE, freespace_profile);
110111
plan_f1.setDescription("transition_from_end_plan");
111112

112-
PlanInstruction plan_f1_dt(wp7_dt, PlanInstructionType::FREESPACE, "freespace_profile");
113+
PlanInstruction plan_f1_dt(wp7_dt, PlanInstructionType::FREESPACE, freespace_profile);
113114
plan_f1_dt.setDescription("transition_to_start_plan");
114115

115-
CompositeInstruction transition_from_end;
116+
CompositeInstruction transition_from_end(freespace_profile);
116117
transition_from_end.setDescription("transition_from_end");
117118
transition_from_end.push_back(plan_f1);
118119

119-
CompositeInstruction transition_to_start;
120+
CompositeInstruction transition_to_start(freespace_profile);
120121
transition_to_start.setDescription("transition_to_start");
121122
transition_to_start.push_back(plan_f1_dt);
122123

@@ -135,17 +136,17 @@ inline CompositeInstruction rasterDTExampleProgram()
135136
CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8 + (i * 0.1), 0.3, 0.8) *
136137
Eigen::Quaterniond(0, 0, -1.0, 0));
137138

138-
PlanInstruction plan_f1(wp1, PlanInstructionType::FREESPACE, "freespace_profile");
139+
PlanInstruction plan_f1(wp1, PlanInstructionType::FREESPACE, freespace_profile);
139140
plan_f1.setDescription("transition_from_end_plan");
140141

141-
PlanInstruction plan_f1_dt(wp1_dt, PlanInstructionType::FREESPACE, "freespace_profile");
142+
PlanInstruction plan_f1_dt(wp1_dt, PlanInstructionType::FREESPACE, freespace_profile);
142143
plan_f1_dt.setDescription("transition_to_start_plan");
143144

144-
CompositeInstruction transition_from_end;
145+
CompositeInstruction transition_from_end(freespace_profile);
145146
transition_from_end.setDescription("transition_from_end");
146147
transition_from_end.push_back(plan_f1);
147148

148-
CompositeInstruction transition_to_start;
149+
CompositeInstruction transition_to_start(freespace_profile);
149150
transition_to_start.setDescription("transition_to_start");
150151
transition_to_start.push_back(plan_f1_dt);
151152

@@ -156,7 +157,7 @@ inline CompositeInstruction rasterDTExampleProgram()
156157
}
157158
}
158159

159-
PlanInstruction plan_f2(swp1, PlanInstructionType::FREESPACE, "freespace_profile");
160+
PlanInstruction plan_f2(swp1, PlanInstructionType::FREESPACE, freespace_profile);
160161
plan_f2.setDescription("to_end_plan");
161162
CompositeInstruction to_end;
162163
to_end.setDescription("to_end");

0 commit comments

Comments
 (0)