From d2b11debc8b6ab414c078333430db4004771445b Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Mon, 23 Oct 2023 09:19:57 -0500 Subject: [PATCH] Fix contact check program to support joint and state waypoints --- tesseract_motion_planners/core/src/utils.cpp | 96 +++++++++++--------- 1 file changed, 55 insertions(+), 41 deletions(-) diff --git a/tesseract_motion_planners/core/src/utils.cpp b/tesseract_motion_planners/core/src/utils.cpp index 76fd556da6..d3d9fc98b5 100644 --- a/tesseract_motion_planners/core/src/utils.cpp +++ b/tesseract_motion_planners/core/src/utils.cpp @@ -316,8 +316,8 @@ bool contactCheckProgram(std::vector& con if (debug_logging) { // Grab the first waypoint to get the joint names - const auto& swp = mi.front().get().as().getWaypoint().as(); - traj_contacts = std::make_unique(swp.getNames(), + const auto& joint_names = getJointNames(mi.front().get().as().getWaypoint()); + traj_contacts = std::make_unique(joint_names, static_cast(program.size())); } @@ -331,8 +331,9 @@ bool contactCheckProgram(std::vector& con bool found = false; if (config.check_program_mode == tesseract_collision::CollisionCheckProgramType::START_ONLY) { - const auto& swp = mi.front().get().as().getWaypoint().as(); - tesseract_scene_graph::SceneState state = state_solver.getState(swp.getNames(), swp.getPosition()); + const auto& joint_names = getJointNames(mi.front().get().as().getWaypoint()); + const auto& joint_positions = getJointPosition(mi.front().get().as().getWaypoint()); + tesseract_scene_graph::SceneState state = state_solver.getState(joint_names, joint_positions); sub_state_results.clear(); tesseract_environment::checkTrajectoryState( sub_state_results, manager, state.link_transforms, config.contact_request); @@ -344,7 +345,7 @@ bool contactCheckProgram(std::vector& con state_results.addInterpolatedCollisionResults( sub_state_results, 0, 0, manager.getActiveCollisionObjects(), 0, false); if (debug_logging) - printContinuousDebugInfo(swp.getNames(), swp.getPosition(), swp.getPosition(), 0, mi.size() - 1); + printContinuousDebugInfo(joint_names, joint_positions, joint_positions, 0, mi.size() - 1); } contacts.push_back(state_results); return found; @@ -352,8 +353,9 @@ bool contactCheckProgram(std::vector& con if (config.check_program_mode == tesseract_collision::CollisionCheckProgramType::END_ONLY) { - const auto& swp = mi.back().get().as().getWaypoint().as(); - tesseract_scene_graph::SceneState state = state_solver.getState(swp.getNames(), swp.getPosition()); + const auto& joint_names = getJointNames(mi.back().get().as().getWaypoint()); + const auto& joint_positions = getJointPosition(mi.back().get().as().getWaypoint()); + tesseract_scene_graph::SceneState state = state_solver.getState(joint_names, joint_positions); sub_state_results.clear(); tesseract_environment::checkTrajectoryState( sub_state_results, manager, state.link_transforms, config.contact_request); @@ -365,7 +367,7 @@ bool contactCheckProgram(std::vector& con state_results.addInterpolatedCollisionResults( sub_state_results, 0, 0, manager.getActiveCollisionObjects(), 0, false); if (debug_logging) - printContinuousDebugInfo(swp.getNames(), swp.getPosition(), swp.getPosition(), 0, mi.size() - 1); + printContinuousDebugInfo(joint_names, joint_positions, joint_positions, 0, mi.size() - 1); } contacts.push_back(state_results); return found; @@ -379,24 +381,25 @@ bool contactCheckProgram(std::vector& con { state_results.clear(); - const auto& swp0 = mi.at(iStep).get().as().getWaypoint().as(); - const auto& swp1 = mi.at(iStep + 1).get().as().getWaypoint().as(); + const auto& joint_names = getJointNames(mi.at(iStep).get().as().getWaypoint()); + const auto& joint_positions0 = getJointPosition(mi.at(iStep).get().as().getWaypoint()); + const auto& joint_positions1 = getJointPosition(mi.at(iStep + 1).get().as().getWaypoint()); // TODO: Should check joint names and make sure they are in the same order - double dist = (swp1.getPosition() - swp0.getPosition()).norm(); + double dist = (joint_positions1 - joint_positions0).norm(); if (dist > config.longest_valid_segment_length) { auto cnt = static_cast(std::ceil(dist / config.longest_valid_segment_length)) + 1; - tesseract_common::TrajArray subtraj(cnt, swp0.getPosition().size()); - for (long iVar = 0; iVar < swp0.getPosition().size(); ++iVar) - subtraj.col(iVar) = Eigen::VectorXd::LinSpaced(cnt, swp0.getPosition()(iVar), swp1.getPosition()(iVar)); + tesseract_common::TrajArray subtraj(cnt, joint_positions0.size()); + for (long iVar = 0; iVar < joint_positions0.size(); ++iVar) + subtraj.col(iVar) = Eigen::VectorXd::LinSpaced(cnt, joint_positions0(iVar), joint_positions1(iVar)); tesseract_collision::ContactTrajectoryStepResults::UPtr step_contacts; if (debug_logging) { step_contacts = std::make_unique( - static_cast(iStep + 1), swp0.getPosition(), swp1.getPosition(), static_cast(subtraj.rows())); + static_cast(iStep + 1), joint_positions0, joint_positions1, static_cast(subtraj.rows())); } auto sub_segment_last_index = static_cast(subtraj.rows() - 1); @@ -426,8 +429,8 @@ bool contactCheckProgram(std::vector& con static_cast(iSubStep) + 1, subtraj.row(iSubStep), subtraj.row(iSubStep + 1)); } - tesseract_scene_graph::SceneState state0 = state_solver.getState(swp0.getNames(), subtraj.row(iSubStep)); - tesseract_scene_graph::SceneState state1 = state_solver.getState(swp0.getNames(), subtraj.row(iSubStep + 1)); + tesseract_scene_graph::SceneState state0 = state_solver.getState(joint_names, subtraj.row(iSubStep)); + tesseract_scene_graph::SceneState state1 = state_solver.getState(joint_names, subtraj.row(iSubStep + 1)); sub_state_results.clear(); tesseract_environment::checkTrajectorySegment( sub_state_results, manager, state0.link_transforms, state1.link_transforms, config.contact_request); @@ -481,10 +484,14 @@ bool contactCheckProgram(std::vector& con continue; } - const auto& swp0 = mi.at(iStep).get().as().getWaypoint().as(); - const auto& swp1 = mi.at(iStep + 1).get().as().getWaypoint().as(); - tesseract_scene_graph::SceneState state0 = state_solver.getState(swp0.getNames(), swp0.getPosition()); - tesseract_scene_graph::SceneState state1 = state_solver.getState(swp1.getNames(), swp1.getPosition()); + const auto& joint_names0 = getJointNames(mi.at(iStep).get().as().getWaypoint()); + const auto& joint_positions0 = getJointPosition(mi.at(iStep).get().as().getWaypoint()); + + const auto& joint_names1 = getJointNames(mi.at(iStep + 1).get().as().getWaypoint()); + const auto& joint_positions1 = getJointPosition(mi.at(iStep + 1).get().as().getWaypoint()); + + tesseract_scene_graph::SceneState state0 = state_solver.getState(joint_names0, joint_positions0); + tesseract_scene_graph::SceneState state1 = state_solver.getState(joint_names1, joint_positions1); tesseract_collision::ContactTrajectoryStepResults::UPtr step_contacts; tesseract_collision::ContactTrajectorySubstepResults::UPtr substep_contacts; @@ -492,9 +499,9 @@ bool contactCheckProgram(std::vector& con if (debug_logging) { step_contacts = std::make_unique( - static_cast(iStep + 1), swp0.getPosition(), swp1.getPosition(), 1); + static_cast(iStep + 1), joint_positions0, joint_positions1, 1); substep_contacts = std::make_unique( - 1, swp0.getPosition(), swp1.getPosition()); + 1, joint_positions0, joint_positions1); } tesseract_environment::checkTrajectorySegment( @@ -541,19 +548,23 @@ bool contactCheckProgram(std::vector& con { state_results.clear(); - const auto& swp0 = mi.at(iStep).get().as().getWaypoint().as(); - const auto& swp1 = mi.at(iStep + 1).get().as().getWaypoint().as(); - tesseract_scene_graph::SceneState state0 = state_solver.getState(swp0.getNames(), swp0.getPosition()); - tesseract_scene_graph::SceneState state1 = state_solver.getState(swp1.getNames(), swp1.getPosition()); + const auto& joint_names0 = getJointNames(mi.at(iStep).get().as().getWaypoint()); + const auto& joint_positions0 = getJointPosition(mi.at(iStep).get().as().getWaypoint()); + + const auto& joint_names1 = getJointNames(mi.at(iStep + 1).get().as().getWaypoint()); + const auto& joint_positions1 = getJointPosition(mi.at(iStep + 1).get().as().getWaypoint()); + + tesseract_scene_graph::SceneState state0 = state_solver.getState(joint_names0, joint_positions0); + tesseract_scene_graph::SceneState state1 = state_solver.getState(joint_names1, joint_positions1); tesseract_collision::ContactTrajectoryStepResults::UPtr step_contacts; tesseract_collision::ContactTrajectorySubstepResults::UPtr substep_contacts; if (debug_logging) { step_contacts = std::make_unique( - static_cast(iStep + 1), swp0.getPosition(), swp1.getPosition(), 1); + static_cast(iStep + 1), joint_positions0, joint_positions1, 1); substep_contacts = std::make_unique( - 1, swp0.getPosition(), swp1.getPosition()); + 1, joint_positions0, joint_positions1); } tesseract_environment::checkTrajectorySegment( @@ -612,8 +623,8 @@ bool contactCheckProgram(std::vector& con if (debug_logging) { // Grab the first waypoint to get the joint names - const auto& swp = mi.front().get().as().getWaypoint().as(); - traj_contacts = std::make_unique(swp.getNames(), + const auto& joint_names = getJointNames(mi.front().get().as().getWaypoint()); + traj_contacts = std::make_unique(joint_names, static_cast(program.size())); } @@ -627,8 +638,9 @@ bool contactCheckProgram(std::vector& con bool found = false; if (config.check_program_mode == tesseract_collision::CollisionCheckProgramType::START_ONLY) { - const auto& swp = mi.front().get().as().getWaypoint().as(); - tesseract_scene_graph::SceneState state = state_solver.getState(swp.getNames(), swp.getPosition()); + const auto& joint_names = getJointNames(mi.front().get().as().getWaypoint()); + const auto& joint_positions = getJointPosition(mi.front().get().as().getWaypoint()); + tesseract_scene_graph::SceneState state = state_solver.getState(joint_names, joint_positions); sub_state_results.clear(); tesseract_environment::checkTrajectoryState( sub_state_results, manager, state.link_transforms, config.contact_request); @@ -640,7 +652,7 @@ bool contactCheckProgram(std::vector& con state_results.addInterpolatedCollisionResults( sub_state_results, 0, 0, manager.getActiveCollisionObjects(), 0, true); if (debug_logging) - printDiscreteDebugInfo(swp.getNames(), swp.getPosition(), 0, mi.size() - 1); + printDiscreteDebugInfo(joint_names, joint_positions, 0, mi.size() - 1); } contacts.push_back(state_results); return found; @@ -648,8 +660,9 @@ bool contactCheckProgram(std::vector& con if (config.check_program_mode == tesseract_collision::CollisionCheckProgramType::END_ONLY) { - const auto& swp = mi.back().get().as().getWaypoint().as(); - tesseract_scene_graph::SceneState state = state_solver.getState(swp.getNames(), swp.getPosition()); + const auto& joint_names = getJointNames(mi.back().get().as().getWaypoint()); + const auto& joint_positions = getJointPosition(mi.back().get().as().getWaypoint()); + tesseract_scene_graph::SceneState state = state_solver.getState(joint_names, joint_positions); sub_state_results.clear(); tesseract_environment::checkTrajectoryState( sub_state_results, manager, state.link_transforms, config.contact_request); @@ -661,7 +674,7 @@ bool contactCheckProgram(std::vector& con state_results.addInterpolatedCollisionResults( sub_state_results, 0, 0, manager.getActiveCollisionObjects(), 0, true); if (debug_logging) - printDiscreteDebugInfo(swp.getNames(), swp.getPosition(), 0, mi.size() - 1); + printDiscreteDebugInfo(joint_names, joint_positions, 0, mi.size() - 1); } contacts.push_back(state_results); return found; @@ -674,15 +687,16 @@ bool contactCheckProgram(std::vector& con auto sub_segment_last_index = static_cast(mi.size() - 1); state_results.clear(); - const auto& swp = mi.front().get().as().getWaypoint().as(); - tesseract_scene_graph::SceneState state = state_solver.getState(swp.getNames(), swp.getPosition()); + const auto& joint_names = getJointNames(mi.front().get().as().getWaypoint()); + const auto& joint_positions = getJointPosition(mi.front().get().as().getWaypoint()); + tesseract_scene_graph::SceneState state = state_solver.getState(joint_names, joint_positions); tesseract_collision::ContactTrajectoryStepResults::UPtr step_contacts; tesseract_collision::ContactTrajectorySubstepResults::UPtr substep_contacts; if (debug_logging) { - step_contacts = std::make_unique(1, swp.getPosition()); - substep_contacts = std::make_unique(1, swp.getPosition()); + step_contacts = std::make_unique(1, joint_positions); + substep_contacts = std::make_unique(1, joint_positions); } sub_state_results.clear();