Skip to content

Commit 5a6cfbf

Browse files
marrtsLevi-Armstrong
marrts
authored and
Levi-Armstrong
committed
Updated lvs tests to be more thorough
1 parent e4f46a7 commit 5a6cfbf

File tree

1 file changed

+194
-16
lines changed

1 file changed

+194
-16
lines changed

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

Lines changed: 194 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -92,6 +92,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo
9292
PlannerRequest request;
9393
request.tesseract = tesseract_ptr_;
9494
request.env_state = tesseract_ptr_->getEnvironment()->getCurrentState();
95+
9596
JointWaypoint wp1(joint_names_, Eigen::VectorXd::Zero(7));
9697
JointWaypoint wp2(joint_names_, Eigen::VectorXd::Ones(7));
9798
PlanInstruction instr(wp1, PlanInstructionType::FREESPACE, "TEST_PROFILE", manip_info_);
@@ -105,13 +106,30 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo
105106
}
106107
const auto* mi = composite.back().cast_const<MoveInstruction>();
107108
EXPECT_TRUE(wp2.isApprox(mi->getWaypoint().cast_const<StateWaypoint>()->position, 1e-5));
109+
110+
// Ensure equal to minimum number steps when all params set large
111+
int min_steps = 5;
112+
auto composite_short =
113+
LVSInterpolateStateWaypoint(wp1, wp2, instr, request, ManipulatorInfo(), 6.28, 0.5, 1.57, min_steps);
114+
EXPECT_EQ(composite_short.size(), min_steps);
115+
116+
// Ensure state_longest_valid_segment_length is used
117+
double longest_valid_segment_length = 0.05;
118+
auto composite_long = LVSInterpolateStateWaypoint(
119+
wp1, wp2, instr, request, ManipulatorInfo(), longest_valid_segment_length, 10, 6.28, min_steps);
120+
double dist = (wp1 - wp2).norm();
121+
int steps = int(dist / longest_valid_segment_length) + 1;
122+
EXPECT_TRUE(composite_long.size() > static_cast<std::size_t>(min_steps));
123+
EXPECT_EQ(composite_long.size(), steps);
108124
}
109125

110126
TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypoint_JointJoint_Linear) // NOLINT
111127
{
112128
PlannerRequest request;
113129
request.tesseract = tesseract_ptr_;
114130
request.env_state = tesseract_ptr_->getEnvironment()->getCurrentState();
131+
auto fwd_kin = tesseract_ptr_->getManipulatorManager()->getFwdKinematicSolver(manip_info_.manipulator);
132+
115133
JointWaypoint wp1(joint_names_, Eigen::VectorXd::Zero(7));
116134
JointWaypoint wp2(joint_names_, Eigen::VectorXd::Ones(7));
117135
PlanInstruction instr(wp1, PlanInstructionType::LINEAR, "TEST_PROFILE", manip_info_);
@@ -125,15 +143,45 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo
125143
}
126144
const auto* mi = composite.back().cast_const<MoveInstruction>();
127145
EXPECT_TRUE(wp2.isApprox(mi->getWaypoint().cast_const<StateWaypoint>()->position, 1e-5));
146+
147+
// Ensure equal to minimum number steps when all params set large
148+
int min_steps = 5;
149+
auto composite_short =
150+
LVSInterpolateStateWaypoint(wp1, wp2, instr, request, ManipulatorInfo(), 6.28, 10, 6.28, min_steps);
151+
EXPECT_EQ(composite_short.size(), min_steps);
152+
153+
// Ensure translation_longest_valid_segment_length is used when large motion given
154+
double translation_longest_valid_segment_length = 0.01;
155+
auto composite_trans_long = LVSInterpolateStateWaypoint(
156+
wp1, wp2, instr, request, ManipulatorInfo(), 6.28, translation_longest_valid_segment_length, 6.28, min_steps);
157+
Eigen::Isometry3d p1, p2;
158+
fwd_kin->calcFwdKin(p1, wp1);
159+
fwd_kin->calcFwdKin(p2, wp2);
160+
double trans_dist = (p2.translation() - p1.translation()).norm();
161+
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));
163+
EXPECT_EQ(composite_trans_long.size(), trans_steps);
164+
165+
// Ensure rotation_longest_valid_segment_length is used
166+
double rotation_longest_valid_segment_length = 0.01;
167+
auto composite_rot_long = LVSInterpolateStateWaypoint(
168+
wp1, wp2, instr, request, ManipulatorInfo(), 6.28, 10, rotation_longest_valid_segment_length, min_steps);
169+
double rot_dist = Eigen::Quaterniond(p1.linear()).angularDistance(Eigen::Quaterniond(p2.linear()));
170+
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));
172+
EXPECT_EQ(composite_rot_long.size(), rot_steps);
128173
}
129174

130175
TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypoint_JointCart_Freespace) // NOLINT
131176
{
132177
PlannerRequest request;
133178
request.tesseract = tesseract_ptr_;
134179
request.env_state = tesseract_ptr_->getEnvironment()->getCurrentState();
180+
auto fwd_kin = tesseract_ptr_->getManipulatorManager()->getFwdKinematicSolver(manip_info_.manipulator);
181+
135182
JointWaypoint wp1(joint_names_, Eigen::VectorXd::Zero(7));
136-
CartesianWaypoint wp2 = Eigen::Isometry3d::Identity();
183+
CartesianWaypoint wp2;
184+
fwd_kin->calcFwdKin(wp2, Eigen::VectorXd::Ones(7));
137185
PlanInstruction instr(wp1, PlanInstructionType::FREESPACE, "TEST_PROFILE", manip_info_);
138186

139187
auto composite = LVSInterpolateStateWaypoint(wp1, wp2, instr, request, ManipulatorInfo(), 3.14, 0.5, 1.57, 5);
@@ -145,19 +193,34 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo
145193
}
146194
const auto* mi = composite.back().cast_const<MoveInstruction>();
147195
const Eigen::VectorXd& last_position = mi->getWaypoint().cast_const<StateWaypoint>()->position;
148-
auto fwd_kin = tesseract_ptr_->getManipulatorManager()->getFwdKinematicSolver(manip_info_.manipulator);
149-
Eigen::Isometry3d final_pose = Eigen::Isometry3d::Identity();
196+
Eigen::Isometry3d final_pose;
150197
fwd_kin->calcFwdKin(final_pose, last_position);
151198
EXPECT_TRUE(wp2.isApprox(final_pose, 1e-3));
199+
200+
// Ensure equal to minimum number steps when all params set large
201+
int min_steps = 5;
202+
auto composite_short =
203+
LVSInterpolateStateWaypoint(wp1, wp2, instr, request, ManipulatorInfo(), 6.28, 0.5, 1.57, min_steps);
204+
EXPECT_EQ(composite_short.size(), min_steps);
205+
206+
// Ensure state_longest_valid_segment_length is used
207+
double longest_valid_segment_length = 0.01;
208+
auto composite_long = LVSInterpolateStateWaypoint(
209+
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));
152211
}
153212

154213
TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypoint_JointCart_Linear) // NOLINT
155214
{
156215
PlannerRequest request;
157216
request.tesseract = tesseract_ptr_;
158217
request.env_state = tesseract_ptr_->getEnvironment()->getCurrentState();
218+
auto fwd_kin = request.tesseract->getManipulatorManager()->getFwdKinematicSolver(manip_info_.manipulator);
219+
159220
JointWaypoint wp1(joint_names_, Eigen::VectorXd::Zero(7));
160-
CartesianWaypoint wp2 = Eigen::Isometry3d::Identity();
221+
CartesianWaypoint wp2;
222+
fwd_kin->calcFwdKin(wp2, Eigen::VectorXd::Ones(7));
223+
161224
PlanInstruction instr(wp1, PlanInstructionType::LINEAR, "TEST_PROFILE", manip_info_);
162225

163226
auto composite = LVSInterpolateStateWaypoint(wp1, wp2, instr, request, ManipulatorInfo(), 3.14, 0.5, 1.57, 5);
@@ -169,19 +232,48 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo
169232
}
170233
const auto* mi = composite.back().cast_const<MoveInstruction>();
171234
const Eigen::VectorXd& last_position = mi->getWaypoint().cast_const<StateWaypoint>()->position;
172-
auto fwd_kin = tesseract_ptr_->getManipulatorManager()->getFwdKinematicSolver(manip_info_.manipulator);
173-
Eigen::Isometry3d final_pose = Eigen::Isometry3d::Identity();
235+
Eigen::Isometry3d final_pose;
174236
fwd_kin->calcFwdKin(final_pose, last_position);
175237
EXPECT_TRUE(wp2.isApprox(final_pose, 1e-3));
238+
239+
// Ensure equal to minimum number steps when all params set large
240+
int min_steps = 5;
241+
auto composite_short =
242+
LVSInterpolateStateWaypoint(wp1, wp2, instr, request, ManipulatorInfo(), 6.28, 10, 6.28, min_steps);
243+
EXPECT_EQ(composite_short.size(), min_steps);
244+
245+
// Ensure translation_longest_valid_segment_length is used
246+
double translation_longest_valid_segment_length = 0.01;
247+
auto composite_trans_long = LVSInterpolateStateWaypoint(
248+
wp1, wp2, instr, request, ManipulatorInfo(), 6.28, translation_longest_valid_segment_length, 6.28, min_steps);
249+
Eigen::Isometry3d p1;
250+
fwd_kin->calcFwdKin(p1, wp1);
251+
double trans_dist = (wp2.translation() - p1.translation()).norm();
252+
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));
254+
EXPECT_EQ(composite_trans_long.size(), trans_steps);
255+
256+
// Ensure rotation_longest_valid_segment_length is used
257+
double rotation_longest_valid_segment_length = 0.01;
258+
auto composite_rot_long = LVSInterpolateStateWaypoint(
259+
wp1, wp2, instr, request, ManipulatorInfo(), 6.28, 10, rotation_longest_valid_segment_length, min_steps);
260+
double rot_dist = Eigen::Quaterniond(p1.linear()).angularDistance(Eigen::Quaterniond(wp2.linear()));
261+
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));
263+
EXPECT_EQ(composite_rot_long.size(), rot_steps);
176264
}
177265

178266
TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypoint_CartJoint_Freespace) // NOLINT
179267
{
180268
PlannerRequest request;
181269
request.tesseract = tesseract_ptr_;
182270
request.env_state = tesseract_ptr_->getEnvironment()->getCurrentState();
183-
CartesianWaypoint wp1 = Eigen::Isometry3d::Identity();
184-
JointWaypoint wp2(joint_names_, Eigen::VectorXd::Zero(7));
271+
272+
auto fwd_kin = tesseract_ptr_->getManipulatorManager()->getFwdKinematicSolver(manip_info_.manipulator);
273+
274+
CartesianWaypoint wp1;
275+
fwd_kin->calcFwdKin(wp1, Eigen::VectorXd::Zero(7));
276+
JointWaypoint wp2(joint_names_, Eigen::VectorXd::Ones(7));
185277
PlanInstruction instr(wp1, PlanInstructionType::FREESPACE, "TEST_PROFILE", manip_info_);
186278

187279
auto composite = LVSInterpolateStateWaypoint(wp1, wp2, instr, request, ManipulatorInfo(), 3.14, 0.5, 1.57, 5);
@@ -193,15 +285,31 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo
193285
}
194286
const auto* mi = composite.back().cast_const<MoveInstruction>();
195287
EXPECT_TRUE(wp2.isApprox(mi->getWaypoint().cast_const<StateWaypoint>()->position, 1e-5));
288+
289+
// Ensure equal to minimum number steps when all params set large
290+
int min_steps = 5;
291+
auto composite_short =
292+
LVSInterpolateStateWaypoint(wp1, wp2, instr, request, ManipulatorInfo(), 6.28, 10, 6.28, min_steps);
293+
EXPECT_EQ(composite_short.size(), min_steps);
294+
295+
// Ensure state_longest_valid_segment_length is used
296+
double longest_valid_segment_length = 0.01;
297+
auto composite_long = LVSInterpolateStateWaypoint(
298+
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));
196300
}
197301

198302
TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypoint_CartJoint_Linear) // NOLINT
199303
{
200304
PlannerRequest request;
201305
request.tesseract = tesseract_ptr_;
202306
request.env_state = tesseract_ptr_->getEnvironment()->getCurrentState();
203-
CartesianWaypoint wp1 = Eigen::Isometry3d::Identity();
204-
JointWaypoint wp2(joint_names_, Eigen::VectorXd::Zero(7));
307+
308+
auto fwd_kin = tesseract_ptr_->getManipulatorManager()->getFwdKinematicSolver(manip_info_.manipulator);
309+
310+
CartesianWaypoint wp1;
311+
fwd_kin->calcFwdKin(wp1, Eigen::VectorXd::Zero(7));
312+
JointWaypoint wp2(joint_names_, Eigen::VectorXd::Ones(7));
205313
PlanInstruction instr(wp1, PlanInstructionType::LINEAR, "TEST_PROFILE", manip_info_);
206314

207315
auto composite = LVSInterpolateStateWaypoint(wp1, wp2, instr, request, ManipulatorInfo(), 3.14, 0.5, 1.57, 5);
@@ -213,15 +321,46 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo
213321
}
214322
const auto* mi = composite.back().cast_const<MoveInstruction>();
215323
EXPECT_TRUE(wp2.isApprox(mi->getWaypoint().cast_const<StateWaypoint>()->position, 1e-5));
324+
325+
// Ensure equal to minimum number steps when all params set large
326+
int min_steps = 5;
327+
auto composite_short =
328+
LVSInterpolateStateWaypoint(wp1, wp2, instr, request, ManipulatorInfo(), 6.28, 10, 6.28, min_steps);
329+
EXPECT_EQ(composite_short.size(), min_steps);
330+
331+
// Ensure translation_longest_valid_segment_length is used
332+
double translation_longest_valid_segment_length = 0.01;
333+
auto composite_trans_long = LVSInterpolateStateWaypoint(
334+
wp1, wp2, instr, request, ManipulatorInfo(), 6.28, translation_longest_valid_segment_length, 6.28, min_steps);
335+
Eigen::Isometry3d p2;
336+
fwd_kin->calcFwdKin(p2, wp2);
337+
double trans_dist = (p2.translation() - wp1.translation()).norm();
338+
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));
340+
EXPECT_EQ(composite_trans_long.size(), trans_steps);
341+
342+
// Ensure rotation_longest_valid_segment_length is used
343+
double rotation_longest_valid_segment_length = 0.01;
344+
auto composite_rot_long = LVSInterpolateStateWaypoint(
345+
wp1, wp2, instr, request, ManipulatorInfo(), 6.28, 10, rotation_longest_valid_segment_length, min_steps);
346+
double rot_dist = Eigen::Quaterniond(wp1.linear()).angularDistance(Eigen::Quaterniond(p2.linear()));
347+
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));
349+
EXPECT_EQ(composite_rot_long.size(), rot_steps);
216350
}
217351

218352
TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypoint_CartCart_Freespace) // NOLINT
219353
{
220354
PlannerRequest request;
221355
request.tesseract = tesseract_ptr_;
222356
request.env_state = tesseract_ptr_->getEnvironment()->getCurrentState();
223-
CartesianWaypoint wp1 = Eigen::Isometry3d::Identity();
224-
CartesianWaypoint wp2 = Eigen::Isometry3d::Identity();
357+
358+
auto fwd_kin = tesseract_ptr_->getManipulatorManager()->getFwdKinematicSolver(manip_info_.manipulator);
359+
360+
CartesianWaypoint wp1;
361+
fwd_kin->calcFwdKin(wp1, Eigen::VectorXd::Zero(7));
362+
CartesianWaypoint wp2;
363+
fwd_kin->calcFwdKin(wp2, Eigen::VectorXd::Ones(7));
225364
PlanInstruction instr(wp1, PlanInstructionType::FREESPACE, "TEST_PROFILE", manip_info_);
226365

227366
auto composite = LVSInterpolateStateWaypoint(wp1, wp2, instr, request, ManipulatorInfo(), 3.14, 0.5, 1.57, 5);
@@ -233,19 +372,35 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo
233372
}
234373
const auto* mi = composite.back().cast_const<MoveInstruction>();
235374
const Eigen::VectorXd& last_position = mi->getWaypoint().cast_const<StateWaypoint>()->position;
236-
auto fwd_kin = tesseract_ptr_->getManipulatorManager()->getFwdKinematicSolver(manip_info_.manipulator);
237375
Eigen::Isometry3d final_pose = Eigen::Isometry3d::Identity();
238376
fwd_kin->calcFwdKin(final_pose, last_position);
239377
EXPECT_TRUE(wp2.isApprox(final_pose, 1e-3));
378+
379+
// Ensure equal to minimum number steps when all params set large
380+
int min_steps = 5;
381+
auto composite_short =
382+
LVSInterpolateStateWaypoint(wp1, wp2, instr, request, ManipulatorInfo(), 6.28, 0.5, 1.57, min_steps);
383+
EXPECT_EQ(composite_short.size(), min_steps);
384+
385+
// Ensure state_longest_valid_segment_length is used
386+
double longest_valid_segment_length = 0.01;
387+
auto composite_long = LVSInterpolateStateWaypoint(
388+
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));
240390
}
241391

242392
TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypoint_CartCart_Linear) // NOLINT
243393
{
244394
PlannerRequest request;
245395
request.tesseract = tesseract_ptr_;
246396
request.env_state = tesseract_ptr_->getEnvironment()->getCurrentState();
247-
CartesianWaypoint wp1 = Eigen::Isometry3d::Identity();
248-
CartesianWaypoint wp2 = Eigen::Isometry3d::Identity();
397+
398+
auto fwd_kin = tesseract_ptr_->getManipulatorManager()->getFwdKinematicSolver(manip_info_.manipulator);
399+
400+
CartesianWaypoint wp1;
401+
fwd_kin->calcFwdKin(wp1, Eigen::VectorXd::Zero(7));
402+
CartesianWaypoint wp2;
403+
fwd_kin->calcFwdKin(wp2, Eigen::VectorXd::Ones(7));
249404
PlanInstruction instr(wp1, PlanInstructionType::LINEAR, "TEST_PROFILE", manip_info_);
250405

251406
auto composite = LVSInterpolateStateWaypoint(wp1, wp2, instr, request, ManipulatorInfo(), 3.14, 0.5, 1.57, 5);
@@ -257,10 +412,33 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo
257412
}
258413
const auto* mi = composite.back().cast_const<MoveInstruction>();
259414
const Eigen::VectorXd& last_position = mi->getWaypoint().cast_const<StateWaypoint>()->position;
260-
auto fwd_kin = tesseract_ptr_->getManipulatorManager()->getFwdKinematicSolver(manip_info_.manipulator);
261415
Eigen::Isometry3d final_pose = Eigen::Isometry3d::Identity();
262416
fwd_kin->calcFwdKin(final_pose, last_position);
263417
EXPECT_TRUE(wp2.isApprox(final_pose, 1e-3));
418+
419+
// Ensure equal to minimum number steps when all params set large
420+
int min_steps = 5;
421+
auto composite_short =
422+
LVSInterpolateStateWaypoint(wp1, wp2, instr, request, ManipulatorInfo(), 6.28, 10, 6.28, min_steps);
423+
EXPECT_EQ(composite_short.size(), min_steps);
424+
425+
// Ensure translation_longest_valid_segment_length is used
426+
double translation_longest_valid_segment_length = 0.01;
427+
auto composite_trans_long = LVSInterpolateStateWaypoint(
428+
wp1, wp2, instr, request, ManipulatorInfo(), 6.28, translation_longest_valid_segment_length, 6.28, min_steps);
429+
double trans_dist = (wp2.translation() - wp1.translation()).norm();
430+
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));
432+
EXPECT_EQ(composite_trans_long.size(), trans_steps);
433+
434+
// Ensure rotation_longest_valid_segment_length is used
435+
double rotation_longest_valid_segment_length = 0.01;
436+
auto composite_rot_long = LVSInterpolateStateWaypoint(
437+
wp1, wp2, instr, request, ManipulatorInfo(), 6.28, 10, rotation_longest_valid_segment_length, min_steps);
438+
double rot_dist = Eigen::Quaterniond(wp1.linear()).angularDistance(Eigen::Quaterniond(wp2.linear()));
439+
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));
441+
EXPECT_EQ(composite_rot_long.size(), rot_steps);
264442
}
265443

266444
TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateCartStateWaypoint_JointJoint_Freespace) // NOLINT

0 commit comments

Comments
 (0)