Skip to content

Commit d2b11de

Browse files
Levi-ArmstrongLevi-Armstrong
committed
Fix contact check program to support joint and state waypoints
1 parent 1c6104e commit d2b11de

File tree

1 file changed

+55
-41
lines changed

1 file changed

+55
-41
lines changed

‎tesseract_motion_planners/core/src/utils.cpp‎

Lines changed: 55 additions & 41 deletions
Original file line numberDiff line numberDiff line change
@@ -316,8 +316,8 @@ bool contactCheckProgram(std::vector<tesseract_collision::ContactResultMap>& con
316316
if (debug_logging)
317317
{
318318
// Grab the first waypoint to get the joint names
319-
const auto& swp = mi.front().get().as<MoveInstructionPoly>().getWaypoint().as<StateWaypointPoly>();
320-
traj_contacts = std::make_unique<tesseract_collision::ContactTrajectoryResults>(swp.getNames(),
319+
const auto& joint_names = getJointNames(mi.front().get().as<MoveInstructionPoly>().getWaypoint());
320+
traj_contacts = std::make_unique<tesseract_collision::ContactTrajectoryResults>(joint_names,
321321
static_cast<int>(program.size()));
322322
}
323323

@@ -331,8 +331,9 @@ bool contactCheckProgram(std::vector<tesseract_collision::ContactResultMap>& con
331331
bool found = false;
332332
if (config.check_program_mode == tesseract_collision::CollisionCheckProgramType::START_ONLY)
333333
{
334-
const auto& swp = mi.front().get().as<MoveInstructionPoly>().getWaypoint().as<StateWaypointPoly>();
335-
tesseract_scene_graph::SceneState state = state_solver.getState(swp.getNames(), swp.getPosition());
334+
const auto& joint_names = getJointNames(mi.front().get().as<MoveInstructionPoly>().getWaypoint());
335+
const auto& joint_positions = getJointPosition(mi.front().get().as<MoveInstructionPoly>().getWaypoint());
336+
tesseract_scene_graph::SceneState state = state_solver.getState(joint_names, joint_positions);
336337
sub_state_results.clear();
337338
tesseract_environment::checkTrajectoryState(
338339
sub_state_results, manager, state.link_transforms, config.contact_request);
@@ -344,16 +345,17 @@ bool contactCheckProgram(std::vector<tesseract_collision::ContactResultMap>& con
344345
state_results.addInterpolatedCollisionResults(
345346
sub_state_results, 0, 0, manager.getActiveCollisionObjects(), 0, false);
346347
if (debug_logging)
347-
printContinuousDebugInfo(swp.getNames(), swp.getPosition(), swp.getPosition(), 0, mi.size() - 1);
348+
printContinuousDebugInfo(joint_names, joint_positions, joint_positions, 0, mi.size() - 1);
348349
}
349350
contacts.push_back(state_results);
350351
return found;
351352
}
352353

353354
if (config.check_program_mode == tesseract_collision::CollisionCheckProgramType::END_ONLY)
354355
{
355-
const auto& swp = mi.back().get().as<MoveInstructionPoly>().getWaypoint().as<StateWaypointPoly>();
356-
tesseract_scene_graph::SceneState state = state_solver.getState(swp.getNames(), swp.getPosition());
356+
const auto& joint_names = getJointNames(mi.back().get().as<MoveInstructionPoly>().getWaypoint());
357+
const auto& joint_positions = getJointPosition(mi.back().get().as<MoveInstructionPoly>().getWaypoint());
358+
tesseract_scene_graph::SceneState state = state_solver.getState(joint_names, joint_positions);
357359
sub_state_results.clear();
358360
tesseract_environment::checkTrajectoryState(
359361
sub_state_results, manager, state.link_transforms, config.contact_request);
@@ -365,7 +367,7 @@ bool contactCheckProgram(std::vector<tesseract_collision::ContactResultMap>& con
365367
state_results.addInterpolatedCollisionResults(
366368
sub_state_results, 0, 0, manager.getActiveCollisionObjects(), 0, false);
367369
if (debug_logging)
368-
printContinuousDebugInfo(swp.getNames(), swp.getPosition(), swp.getPosition(), 0, mi.size() - 1);
370+
printContinuousDebugInfo(joint_names, joint_positions, joint_positions, 0, mi.size() - 1);
369371
}
370372
contacts.push_back(state_results);
371373
return found;
@@ -379,24 +381,25 @@ bool contactCheckProgram(std::vector<tesseract_collision::ContactResultMap>& con
379381
{
380382
state_results.clear();
381383

382-
const auto& swp0 = mi.at(iStep).get().as<MoveInstructionPoly>().getWaypoint().as<StateWaypointPoly>();
383-
const auto& swp1 = mi.at(iStep + 1).get().as<MoveInstructionPoly>().getWaypoint().as<StateWaypointPoly>();
384+
const auto& joint_names = getJointNames(mi.at(iStep).get().as<MoveInstructionPoly>().getWaypoint());
385+
const auto& joint_positions0 = getJointPosition(mi.at(iStep).get().as<MoveInstructionPoly>().getWaypoint());
386+
const auto& joint_positions1 = getJointPosition(mi.at(iStep + 1).get().as<MoveInstructionPoly>().getWaypoint());
384387

385388
// TODO: Should check joint names and make sure they are in the same order
386-
double dist = (swp1.getPosition() - swp0.getPosition()).norm();
389+
double dist = (joint_positions1 - joint_positions0).norm();
387390
if (dist > config.longest_valid_segment_length)
388391
{
389392
auto cnt = static_cast<long>(std::ceil(dist / config.longest_valid_segment_length)) + 1;
390-
tesseract_common::TrajArray subtraj(cnt, swp0.getPosition().size());
391-
for (long iVar = 0; iVar < swp0.getPosition().size(); ++iVar)
392-
subtraj.col(iVar) = Eigen::VectorXd::LinSpaced(cnt, swp0.getPosition()(iVar), swp1.getPosition()(iVar));
393+
tesseract_common::TrajArray subtraj(cnt, joint_positions0.size());
394+
for (long iVar = 0; iVar < joint_positions0.size(); ++iVar)
395+
subtraj.col(iVar) = Eigen::VectorXd::LinSpaced(cnt, joint_positions0(iVar), joint_positions1(iVar));
393396

394397
tesseract_collision::ContactTrajectoryStepResults::UPtr step_contacts;
395398

396399
if (debug_logging)
397400
{
398401
step_contacts = std::make_unique<tesseract_collision::ContactTrajectoryStepResults>(
399-
static_cast<int>(iStep + 1), swp0.getPosition(), swp1.getPosition(), static_cast<int>(subtraj.rows()));
402+
static_cast<int>(iStep + 1), joint_positions0, joint_positions1, static_cast<int>(subtraj.rows()));
400403
}
401404

402405
auto sub_segment_last_index = static_cast<int>(subtraj.rows() - 1);
@@ -426,8 +429,8 @@ bool contactCheckProgram(std::vector<tesseract_collision::ContactResultMap>& con
426429
static_cast<int>(iSubStep) + 1, subtraj.row(iSubStep), subtraj.row(iSubStep + 1));
427430
}
428431

429-
tesseract_scene_graph::SceneState state0 = state_solver.getState(swp0.getNames(), subtraj.row(iSubStep));
430-
tesseract_scene_graph::SceneState state1 = state_solver.getState(swp0.getNames(), subtraj.row(iSubStep + 1));
432+
tesseract_scene_graph::SceneState state0 = state_solver.getState(joint_names, subtraj.row(iSubStep));
433+
tesseract_scene_graph::SceneState state1 = state_solver.getState(joint_names, subtraj.row(iSubStep + 1));
431434
sub_state_results.clear();
432435
tesseract_environment::checkTrajectorySegment(
433436
sub_state_results, manager, state0.link_transforms, state1.link_transforms, config.contact_request);
@@ -481,20 +484,24 @@ bool contactCheckProgram(std::vector<tesseract_collision::ContactResultMap>& con
481484
continue;
482485
}
483486

484-
const auto& swp0 = mi.at(iStep).get().as<MoveInstructionPoly>().getWaypoint().as<StateWaypointPoly>();
485-
const auto& swp1 = mi.at(iStep + 1).get().as<MoveInstructionPoly>().getWaypoint().as<StateWaypointPoly>();
486-
tesseract_scene_graph::SceneState state0 = state_solver.getState(swp0.getNames(), swp0.getPosition());
487-
tesseract_scene_graph::SceneState state1 = state_solver.getState(swp1.getNames(), swp1.getPosition());
487+
const auto& joint_names0 = getJointNames(mi.at(iStep).get().as<MoveInstructionPoly>().getWaypoint());
488+
const auto& joint_positions0 = getJointPosition(mi.at(iStep).get().as<MoveInstructionPoly>().getWaypoint());
489+
490+
const auto& joint_names1 = getJointNames(mi.at(iStep + 1).get().as<MoveInstructionPoly>().getWaypoint());
491+
const auto& joint_positions1 = getJointPosition(mi.at(iStep + 1).get().as<MoveInstructionPoly>().getWaypoint());
492+
493+
tesseract_scene_graph::SceneState state0 = state_solver.getState(joint_names0, joint_positions0);
494+
tesseract_scene_graph::SceneState state1 = state_solver.getState(joint_names1, joint_positions1);
488495

489496
tesseract_collision::ContactTrajectoryStepResults::UPtr step_contacts;
490497
tesseract_collision::ContactTrajectorySubstepResults::UPtr substep_contacts;
491498

492499
if (debug_logging)
493500
{
494501
step_contacts = std::make_unique<tesseract_collision::ContactTrajectoryStepResults>(
495-
static_cast<int>(iStep + 1), swp0.getPosition(), swp1.getPosition(), 1);
502+
static_cast<int>(iStep + 1), joint_positions0, joint_positions1, 1);
496503
substep_contacts = std::make_unique<tesseract_collision::ContactTrajectorySubstepResults>(
497-
1, swp0.getPosition(), swp1.getPosition());
504+
1, joint_positions0, joint_positions1);
498505
}
499506

500507
tesseract_environment::checkTrajectorySegment(
@@ -541,19 +548,23 @@ bool contactCheckProgram(std::vector<tesseract_collision::ContactResultMap>& con
541548
{
542549
state_results.clear();
543550

544-
const auto& swp0 = mi.at(iStep).get().as<MoveInstructionPoly>().getWaypoint().as<StateWaypointPoly>();
545-
const auto& swp1 = mi.at(iStep + 1).get().as<MoveInstructionPoly>().getWaypoint().as<StateWaypointPoly>();
546-
tesseract_scene_graph::SceneState state0 = state_solver.getState(swp0.getNames(), swp0.getPosition());
547-
tesseract_scene_graph::SceneState state1 = state_solver.getState(swp1.getNames(), swp1.getPosition());
551+
const auto& joint_names0 = getJointNames(mi.at(iStep).get().as<MoveInstructionPoly>().getWaypoint());
552+
const auto& joint_positions0 = getJointPosition(mi.at(iStep).get().as<MoveInstructionPoly>().getWaypoint());
553+
554+
const auto& joint_names1 = getJointNames(mi.at(iStep + 1).get().as<MoveInstructionPoly>().getWaypoint());
555+
const auto& joint_positions1 = getJointPosition(mi.at(iStep + 1).get().as<MoveInstructionPoly>().getWaypoint());
556+
557+
tesseract_scene_graph::SceneState state0 = state_solver.getState(joint_names0, joint_positions0);
558+
tesseract_scene_graph::SceneState state1 = state_solver.getState(joint_names1, joint_positions1);
548559

549560
tesseract_collision::ContactTrajectoryStepResults::UPtr step_contacts;
550561
tesseract_collision::ContactTrajectorySubstepResults::UPtr substep_contacts;
551562
if (debug_logging)
552563
{
553564
step_contacts = std::make_unique<tesseract_collision::ContactTrajectoryStepResults>(
554-
static_cast<int>(iStep + 1), swp0.getPosition(), swp1.getPosition(), 1);
565+
static_cast<int>(iStep + 1), joint_positions0, joint_positions1, 1);
555566
substep_contacts = std::make_unique<tesseract_collision::ContactTrajectorySubstepResults>(
556-
1, swp0.getPosition(), swp1.getPosition());
567+
1, joint_positions0, joint_positions1);
557568
}
558569

559570
tesseract_environment::checkTrajectorySegment(
@@ -612,8 +623,8 @@ bool contactCheckProgram(std::vector<tesseract_collision::ContactResultMap>& con
612623
if (debug_logging)
613624
{
614625
// Grab the first waypoint to get the joint names
615-
const auto& swp = mi.front().get().as<MoveInstructionPoly>().getWaypoint().as<StateWaypointPoly>();
616-
traj_contacts = std::make_unique<tesseract_collision::ContactTrajectoryResults>(swp.getNames(),
626+
const auto& joint_names = getJointNames(mi.front().get().as<MoveInstructionPoly>().getWaypoint());
627+
traj_contacts = std::make_unique<tesseract_collision::ContactTrajectoryResults>(joint_names,
617628
static_cast<int>(program.size()));
618629
}
619630

@@ -627,8 +638,9 @@ bool contactCheckProgram(std::vector<tesseract_collision::ContactResultMap>& con
627638
bool found = false;
628639
if (config.check_program_mode == tesseract_collision::CollisionCheckProgramType::START_ONLY)
629640
{
630-
const auto& swp = mi.front().get().as<MoveInstructionPoly>().getWaypoint().as<StateWaypointPoly>();
631-
tesseract_scene_graph::SceneState state = state_solver.getState(swp.getNames(), swp.getPosition());
641+
const auto& joint_names = getJointNames(mi.front().get().as<MoveInstructionPoly>().getWaypoint());
642+
const auto& joint_positions = getJointPosition(mi.front().get().as<MoveInstructionPoly>().getWaypoint());
643+
tesseract_scene_graph::SceneState state = state_solver.getState(joint_names, joint_positions);
632644
sub_state_results.clear();
633645
tesseract_environment::checkTrajectoryState(
634646
sub_state_results, manager, state.link_transforms, config.contact_request);
@@ -640,16 +652,17 @@ bool contactCheckProgram(std::vector<tesseract_collision::ContactResultMap>& con
640652
state_results.addInterpolatedCollisionResults(
641653
sub_state_results, 0, 0, manager.getActiveCollisionObjects(), 0, true);
642654
if (debug_logging)
643-
printDiscreteDebugInfo(swp.getNames(), swp.getPosition(), 0, mi.size() - 1);
655+
printDiscreteDebugInfo(joint_names, joint_positions, 0, mi.size() - 1);
644656
}
645657
contacts.push_back(state_results);
646658
return found;
647659
}
648660

649661
if (config.check_program_mode == tesseract_collision::CollisionCheckProgramType::END_ONLY)
650662
{
651-
const auto& swp = mi.back().get().as<MoveInstructionPoly>().getWaypoint().as<StateWaypointPoly>();
652-
tesseract_scene_graph::SceneState state = state_solver.getState(swp.getNames(), swp.getPosition());
663+
const auto& joint_names = getJointNames(mi.back().get().as<MoveInstructionPoly>().getWaypoint());
664+
const auto& joint_positions = getJointPosition(mi.back().get().as<MoveInstructionPoly>().getWaypoint());
665+
tesseract_scene_graph::SceneState state = state_solver.getState(joint_names, joint_positions);
653666
sub_state_results.clear();
654667
tesseract_environment::checkTrajectoryState(
655668
sub_state_results, manager, state.link_transforms, config.contact_request);
@@ -661,7 +674,7 @@ bool contactCheckProgram(std::vector<tesseract_collision::ContactResultMap>& con
661674
state_results.addInterpolatedCollisionResults(
662675
sub_state_results, 0, 0, manager.getActiveCollisionObjects(), 0, true);
663676
if (debug_logging)
664-
printDiscreteDebugInfo(swp.getNames(), swp.getPosition(), 0, mi.size() - 1);
677+
printDiscreteDebugInfo(joint_names, joint_positions, 0, mi.size() - 1);
665678
}
666679
contacts.push_back(state_results);
667680
return found;
@@ -674,15 +687,16 @@ bool contactCheckProgram(std::vector<tesseract_collision::ContactResultMap>& con
674687

675688
auto sub_segment_last_index = static_cast<int>(mi.size() - 1);
676689
state_results.clear();
677-
const auto& swp = mi.front().get().as<MoveInstructionPoly>().getWaypoint().as<StateWaypointPoly>();
678-
tesseract_scene_graph::SceneState state = state_solver.getState(swp.getNames(), swp.getPosition());
690+
const auto& joint_names = getJointNames(mi.front().get().as<MoveInstructionPoly>().getWaypoint());
691+
const auto& joint_positions = getJointPosition(mi.front().get().as<MoveInstructionPoly>().getWaypoint());
692+
tesseract_scene_graph::SceneState state = state_solver.getState(joint_names, joint_positions);
679693

680694
tesseract_collision::ContactTrajectoryStepResults::UPtr step_contacts;
681695
tesseract_collision::ContactTrajectorySubstepResults::UPtr substep_contacts;
682696
if (debug_logging)
683697
{
684-
step_contacts = std::make_unique<tesseract_collision::ContactTrajectoryStepResults>(1, swp.getPosition());
685-
substep_contacts = std::make_unique<tesseract_collision::ContactTrajectorySubstepResults>(1, swp.getPosition());
698+
step_contacts = std::make_unique<tesseract_collision::ContactTrajectoryStepResults>(1, joint_positions);
699+
substep_contacts = std::make_unique<tesseract_collision::ContactTrajectorySubstepResults>(1, joint_positions);
686700
}
687701

688702
sub_state_results.clear();

0 commit comments

Comments
 (0)