Skip to content

Commit e53f563

Browse files
marrtsLevi-Armstrong
marrts
authored and
Levi-Armstrong
committed
Update unit tests and fix lvs_interpolation
1 parent 0b07ef3 commit e53f563

File tree

6 files changed

+156
-38
lines changed

6 files changed

+156
-38
lines changed

‎tesseract/tesseract_planning/tesseract_motion_planners/src/simple/step_generators/lvs_interpolation.cpp

Lines changed: 8 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -53,15 +53,14 @@ CompositeInstruction LVSInterpolateStateWaypoint(const JointWaypoint& start,
5353
assert(static_cast<long>(end.joint_names.size()) == end.size());
5454

5555
assert(!(manip_info.empty() && base_instruction.getManipulatorInfo().empty()));
56+
ManipulatorInfo mi = manip_info.getCombined(base_instruction.getManipulatorInfo());
5657

5758
CompositeInstruction composite;
5859

5960
if (base_instruction.isLinear())
6061
{
6162
// Find number of states based on cartesian
6263
// Then find values
63-
const ManipulatorInfo& mi =
64-
(base_instruction.getManipulatorInfo().empty()) ? manip_info : base_instruction.getManipulatorInfo();
6564

6665
// Initialize
6766
auto fwd_kin = request.tesseract->getManipulatorManager()->getFwdKinematicSolver(mi.manipulator);
@@ -137,13 +136,11 @@ CompositeInstruction LVSInterpolateStateWaypoint(const JointWaypoint& start,
137136
int min_steps)
138137
{
139138
assert(!(manip_info.empty() && base_instruction.getManipulatorInfo().empty()));
139+
ManipulatorInfo mi = manip_info.getCombined(base_instruction.getManipulatorInfo());
140140

141141
// Joint waypoints should have joint names
142142
assert(static_cast<long>(start.joint_names.size()) == start.size());
143143

144-
const ManipulatorInfo& mi =
145-
(base_instruction.getManipulatorInfo().empty()) ? manip_info : base_instruction.getManipulatorInfo();
146-
147144
// Initialize
148145
auto inv_kin = request.tesseract->getManipulatorManager()->getInvKinematicSolver(mi.manipulator);
149146
auto world_to_base = request.env_state->link_transforms.at(inv_kin->getBaseLinkName());
@@ -251,13 +248,11 @@ CompositeInstruction LVSInterpolateStateWaypoint(const CartesianWaypoint& start,
251248
int min_steps)
252249
{
253250
assert(!(manip_info.empty() && base_instruction.getManipulatorInfo().empty()));
251+
ManipulatorInfo mi = manip_info.getCombined(base_instruction.getManipulatorInfo());
254252

255253
// Joint waypoints should have joint names
256254
assert(static_cast<long>(end.joint_names.size()) == end.size());
257255

258-
const ManipulatorInfo& mi =
259-
(base_instruction.getManipulatorInfo().empty()) ? manip_info : base_instruction.getManipulatorInfo();
260-
261256
// Initialize
262257
auto inv_kin = request.tesseract->getManipulatorManager()->getInvKinematicSolver(mi.manipulator);
263258
auto world_to_base = request.env_state->link_transforms.at(inv_kin->getBaseLinkName());
@@ -365,9 +360,7 @@ CompositeInstruction LVSInterpolateStateWaypoint(const CartesianWaypoint& start,
365360
int min_steps)
366361
{
367362
assert(!(manip_info.empty() && base_instruction.getManipulatorInfo().empty()));
368-
369-
const ManipulatorInfo& mi =
370-
(base_instruction.getManipulatorInfo().empty()) ? manip_info : base_instruction.getManipulatorInfo();
363+
ManipulatorInfo mi = manip_info.getCombined(base_instruction.getManipulatorInfo());
371364

372365
// Initialize
373366
auto inv_kin = request.tesseract->getManipulatorManager()->getInvKinematicSolver(mi.manipulator);
@@ -410,7 +403,7 @@ CompositeInstruction LVSInterpolateStateWaypoint(const CartesianWaypoint& start,
410403
/// @todo: May be nice to add contact checking to find best solution, but may not be neccessary because this is
411404
/// used to generate the seed.
412405
auto j2_solution = j2.middleRows(j * dof, dof);
413-
double d = (j2 - j1).norm();
406+
double d = (j2_solution - j1_solution).norm();
414407
if (d < dist)
415408
{
416409
j1_final = j1_solution;
@@ -486,8 +479,7 @@ CompositeInstruction LVSInterpolateCartStateWaypoint(const JointWaypoint& start,
486479
throw std::runtime_error("Not implemented, PR's are welcome!");
487480

488481
assert(!(manip_info.empty() && base_instruction.getManipulatorInfo().empty()));
489-
const ManipulatorInfo& mi =
490-
(base_instruction.getManipulatorInfo().empty()) ? manip_info : base_instruction.getManipulatorInfo();
482+
ManipulatorInfo mi = manip_info.getCombined(base_instruction.getManipulatorInfo());
491483

492484
// Initialize
493485
auto fwd_kin = request.tesseract->getManipulatorManager()->getFwdKinematicSolver(mi.manipulator);
@@ -546,8 +538,7 @@ CompositeInstruction LVSInterpolateCartStateWaypoint(const JointWaypoint& start,
546538
throw std::runtime_error("Not implemented, PR's are welcome!");
547539

548540
assert(!(manip_info.empty() && base_instruction.getManipulatorInfo().empty()));
549-
const ManipulatorInfo& mi =
550-
(base_instruction.getManipulatorInfo().empty()) ? manip_info : base_instruction.getManipulatorInfo();
541+
ManipulatorInfo mi = manip_info.getCombined(base_instruction.getManipulatorInfo());
551542

552543
// Initialize
553544
auto fwd_kin = request.tesseract->getManipulatorManager()->getFwdKinematicSolver(mi.manipulator);
@@ -603,8 +594,7 @@ CompositeInstruction LVSInterpolateCartStateWaypoint(const CartesianWaypoint& st
603594
throw std::runtime_error("Not implemented, PR's are welcome!");
604595

605596
assert(!(manip_info.empty() && base_instruction.getManipulatorInfo().empty()));
606-
const ManipulatorInfo& mi =
607-
(base_instruction.getManipulatorInfo().empty()) ? manip_info : base_instruction.getManipulatorInfo();
597+
ManipulatorInfo mi = manip_info.getCombined(base_instruction.getManipulatorInfo());
608598

609599
// Initialize
610600
auto fwd_kin = request.tesseract->getManipulatorManager()->getFwdKinematicSolver(mi.manipulator);

‎tesseract/tesseract_planning/tesseract_motion_planners/test/simple_planner_lvs_interpolation.cpp

Lines changed: 12 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -119,7 +119,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo
119119
wp1, wp2, instr, request, ManipulatorInfo(), longest_valid_segment_length, 10, 6.28, min_steps);
120120
double dist = (wp1 - wp2).norm();
121121
int steps = int(dist / longest_valid_segment_length) + 1;
122-
EXPECT_TRUE(composite_long.size() > static_cast<std::size_t>(min_steps));
122+
EXPECT_TRUE(static_cast<int>(composite_long.size()) > min_steps);
123123
EXPECT_EQ(composite_long.size(), steps);
124124
}
125125

@@ -159,7 +159,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo
159159
fwd_kin->calcFwdKin(p2, wp2);
160160
double trans_dist = (p2.translation() - p1.translation()).norm();
161161
int trans_steps = int(trans_dist / translation_longest_valid_segment_length) + 1;
162-
EXPECT_TRUE(composite_trans_long.size() > static_cast<std::size_t>(min_steps));
162+
EXPECT_TRUE(static_cast<int>(composite_trans_long.size()) > min_steps);
163163
EXPECT_EQ(composite_trans_long.size(), trans_steps);
164164

165165
// Ensure rotation_longest_valid_segment_length is used
@@ -168,7 +168,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo
168168
wp1, wp2, instr, request, ManipulatorInfo(), 6.28, 10, rotation_longest_valid_segment_length, min_steps);
169169
double rot_dist = Eigen::Quaterniond(p1.linear()).angularDistance(Eigen::Quaterniond(p2.linear()));
170170
int rot_steps = int(rot_dist / rotation_longest_valid_segment_length) + 1;
171-
EXPECT_TRUE(composite_rot_long.size() > static_cast<std::size_t>(min_steps));
171+
EXPECT_TRUE(static_cast<int>(composite_rot_long.size()) > min_steps);
172172
EXPECT_EQ(composite_rot_long.size(), rot_steps);
173173
}
174174

@@ -207,7 +207,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo
207207
double longest_valid_segment_length = 0.01;
208208
auto composite_long = LVSInterpolateStateWaypoint(
209209
wp1, wp2, instr, request, ManipulatorInfo(), longest_valid_segment_length, 10, 6.28, min_steps);
210-
EXPECT_TRUE(composite_long.size() > static_cast<std::size_t>(min_steps));
210+
EXPECT_TRUE(static_cast<int>(composite_long.size()) > min_steps);
211211
}
212212

213213
TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypoint_JointCart_Linear) // NOLINT
@@ -250,7 +250,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo
250250
fwd_kin->calcFwdKin(p1, wp1);
251251
double trans_dist = (wp2.translation() - p1.translation()).norm();
252252
int trans_steps = int(trans_dist / translation_longest_valid_segment_length) + 1;
253-
EXPECT_TRUE(composite_trans_long.size() > static_cast<std::size_t>(min_steps));
253+
EXPECT_TRUE(static_cast<int>(composite_trans_long.size()) > min_steps);
254254
EXPECT_EQ(composite_trans_long.size(), trans_steps);
255255

256256
// Ensure rotation_longest_valid_segment_length is used
@@ -259,7 +259,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo
259259
wp1, wp2, instr, request, ManipulatorInfo(), 6.28, 10, rotation_longest_valid_segment_length, min_steps);
260260
double rot_dist = Eigen::Quaterniond(p1.linear()).angularDistance(Eigen::Quaterniond(wp2.linear()));
261261
int rot_steps = int(rot_dist / rotation_longest_valid_segment_length) + 1;
262-
EXPECT_TRUE(composite_rot_long.size() > static_cast<std::size_t>(min_steps));
262+
EXPECT_TRUE(static_cast<int>(composite_rot_long.size()) > min_steps);
263263
EXPECT_EQ(composite_rot_long.size(), rot_steps);
264264
}
265265

@@ -296,7 +296,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo
296296
double longest_valid_segment_length = 0.01;
297297
auto composite_long = LVSInterpolateStateWaypoint(
298298
wp1, wp2, instr, request, ManipulatorInfo(), longest_valid_segment_length, 10, 6.28, min_steps);
299-
EXPECT_TRUE(composite_long.size() > static_cast<std::size_t>(min_steps));
299+
EXPECT_TRUE(static_cast<int>(composite_long.size()) > min_steps);
300300
}
301301

302302
TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypoint_CartJoint_Linear) // NOLINT
@@ -336,7 +336,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo
336336
fwd_kin->calcFwdKin(p2, wp2);
337337
double trans_dist = (p2.translation() - wp1.translation()).norm();
338338
int trans_steps = int(trans_dist / translation_longest_valid_segment_length) + 1;
339-
EXPECT_TRUE(composite_trans_long.size() > static_cast<std::size_t>(min_steps));
339+
EXPECT_TRUE(static_cast<int>(composite_trans_long.size()) > min_steps);
340340
EXPECT_EQ(composite_trans_long.size(), trans_steps);
341341

342342
// Ensure rotation_longest_valid_segment_length is used
@@ -345,7 +345,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo
345345
wp1, wp2, instr, request, ManipulatorInfo(), 6.28, 10, rotation_longest_valid_segment_length, min_steps);
346346
double rot_dist = Eigen::Quaterniond(wp1.linear()).angularDistance(Eigen::Quaterniond(p2.linear()));
347347
int rot_steps = int(rot_dist / rotation_longest_valid_segment_length) + 1;
348-
EXPECT_TRUE(composite_rot_long.size() > static_cast<std::size_t>(min_steps));
348+
EXPECT_TRUE(static_cast<int>(composite_rot_long.size()) > min_steps);
349349
EXPECT_EQ(composite_rot_long.size(), rot_steps);
350350
}
351351

@@ -386,7 +386,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo
386386
double longest_valid_segment_length = 0.01;
387387
auto composite_long = LVSInterpolateStateWaypoint(
388388
wp1, wp2, instr, request, ManipulatorInfo(), longest_valid_segment_length, 10, 6.28, min_steps);
389-
EXPECT_TRUE(composite_long.size() > static_cast<std::size_t>(min_steps));
389+
EXPECT_TRUE(static_cast<int>(composite_long.size()) > min_steps);
390390
}
391391

392392
TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypoint_CartCart_Linear) // NOLINT
@@ -428,7 +428,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo
428428
wp1, wp2, instr, request, ManipulatorInfo(), 6.28, translation_longest_valid_segment_length, 6.28, min_steps);
429429
double trans_dist = (wp2.translation() - wp1.translation()).norm();
430430
int trans_steps = int(trans_dist / translation_longest_valid_segment_length) + 1;
431-
EXPECT_TRUE(composite_trans_long.size() > static_cast<std::size_t>(min_steps));
431+
EXPECT_TRUE(static_cast<int>(composite_trans_long.size()) > min_steps);
432432
EXPECT_EQ(composite_trans_long.size(), trans_steps);
433433

434434
// Ensure rotation_longest_valid_segment_length is used
@@ -437,7 +437,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo
437437
wp1, wp2, instr, request, ManipulatorInfo(), 6.28, 10, rotation_longest_valid_segment_length, min_steps);
438438
double rot_dist = Eigen::Quaterniond(wp1.linear()).angularDistance(Eigen::Quaterniond(wp2.linear()));
439439
int rot_steps = int(rot_dist / rotation_longest_valid_segment_length) + 1;
440-
EXPECT_TRUE(composite_rot_long.size() > static_cast<std::size_t>(min_steps));
440+
EXPECT_TRUE(static_cast<int>(composite_rot_long.size()) > min_steps);
441441
EXPECT_EQ(composite_rot_long.size(), rot_steps);
442442
}
443443

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

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

1010
namespace tesseract_planning
1111
{
12-
inline CompositeInstruction freespaceExampleProgram()
12+
inline CompositeInstruction freespaceExampleProgramIIWA(std::string composite_profile = DEFAULT_PROFILE_KEY,
13+
std::string freespace_profile = DEFAULT_PROFILE_KEY)
1314
{
14-
CompositeInstruction program(
15-
"freespace_composite", CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator"));
15+
CompositeInstruction program(composite_profile, CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator"));
1616

1717
// Start Joint Position for the program
1818
std::vector<std::string> joint_names = { "joint_a1", "joint_a2", "joint_a3", "joint_a4",
@@ -23,7 +23,27 @@ inline CompositeInstruction freespaceExampleProgram()
2323

2424
// Define target pose
2525
Waypoint wp2 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.2, 0.2, 1.0));
26-
PlanInstruction plan_f0(wp2, PlanInstructionType::FREESPACE, "DEFAULT");
26+
PlanInstruction plan_f0(wp2, PlanInstructionType::FREESPACE, freespace_profile);
27+
plan_f0.setDescription("freespace_motion");
28+
program.push_back(plan_f0);
29+
30+
return program;
31+
}
32+
33+
inline CompositeInstruction freespaceExampleProgramABB(std::string composite_profile = DEFAULT_PROFILE_KEY,
34+
std::string freespace_profile = DEFAULT_PROFILE_KEY)
35+
{
36+
CompositeInstruction program(composite_profile, CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator"));
37+
38+
// Start Joint Position for the program
39+
std::vector<std::string> joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" };
40+
Waypoint wp1 = StateWaypoint(joint_names, Eigen::VectorXd::Zero(6));
41+
PlanInstruction start_instruction(wp1, PlanInstructionType::START);
42+
program.setStartInstruction(start_instruction);
43+
44+
// Define target pose
45+
Waypoint wp2 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.2, 0.2, 1.0));
46+
PlanInstruction plan_f0(wp2, PlanInstructionType::FREESPACE, freespace_profile);
2747
plan_f0.setDescription("freespace_motion");
2848
program.push_back(plan_f0);
2949

‎tesseract/tesseract_planning/tesseract_process_managers/examples/freespace_manager_example.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -69,7 +69,7 @@ int main()
6969
// --------------------
7070
// Define the program
7171
// --------------------
72-
CompositeInstruction program = freespaceExampleProgram();
72+
CompositeInstruction program = freespaceExampleProgramIIWA();
7373
const Instruction program_instruction{ program };
7474
Instruction seed = generateSkeletonSeed(program);
7575

‎tesseract/tesseract_planning/tesseract_process_managers/test/CMakeLists.txt

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -41,4 +41,5 @@ target_clang_tidy(${PROJECT_NAME}_unit ARGUMENTS ${TESSERACT_CLANG_TIDY_ARGS} EN
4141
target_cxx_version(${PROJECT_NAME}_unit PRIVATE VERSION ${TESSERACT_CXX_VERSION})
4242
target_code_coverage(${PROJECT_NAME}_unit ALL EXCLUDE ${COVERAGE_EXCLUDE} ENABLE ${TESSERACT_ENABLE_TESTING})
4343
add_gtest_discover_tests(${PROJECT_NAME}_unit)
44+
add_dependencies(${PROJECT_NAME}_unit ${PROJECT_NAME})
4445
add_dependencies(run_tests ${PROJECT_NAME}_unit)

0 commit comments

Comments
 (0)