From f5773e8b1be8c63b9ce4e053bebce3a2915aff0f Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Sat, 14 Dec 2024 13:33:31 -0600 Subject: [PATCH 1/2] Remove env_state from planning request --- .../tesseract_motion_planners/core/types.h | 3 - .../impl/descartes_motion_planner.hpp | 2 +- .../test/descartes_planner_tests.cpp | 32 +--------- .../ompl/src/ompl_motion_planner.cpp | 4 +- .../ompl/test/ompl_planner_tests.cpp | 16 ++--- .../simple/interpolation.h | 1 - .../simple/simple_motion_planner.h | 6 +- .../simple/src/interpolation.cpp | 2 - ...planner_fixed_size_assign_plan_profile.cpp | 8 +-- ...simple_planner_fixed_size_plan_profile.cpp | 8 +-- .../simple_planner_lvs_no_ik_plan_profile.cpp | 8 +-- .../simple_planner_lvs_plan_profile.cpp | 8 +-- .../simple/src/simple_motion_planner.cpp | 25 ++++---- ...ple_planner_fixed_size_assign_position.cpp | 14 ++--- ...imple_planner_fixed_size_interpolation.cpp | 16 ++--- .../test/simple_planner_lvs_interpolation.cpp | 27 ++++----- .../trajopt/src/trajopt_motion_planner.cpp | 2 +- .../trajopt/test/trajopt_planner_tests.cpp | 59 +++---------------- .../src/trajopt_ifopt_motion_planner.cpp | 4 +- .../planning/nodes/motion_planner_task.hpp | 1 - .../planning/src/nodes/min_length_task.cpp | 1 - 21 files changed, 82 insertions(+), 165 deletions(-) diff --git a/tesseract_motion_planners/core/include/tesseract_motion_planners/core/types.h b/tesseract_motion_planners/core/include/tesseract_motion_planners/core/types.h index 2d8d816ea2..696e76552c 100644 --- a/tesseract_motion_planners/core/include/tesseract_motion_planners/core/types.h +++ b/tesseract_motion_planners/core/include/tesseract_motion_planners/core/types.h @@ -56,9 +56,6 @@ struct PlannerRequest /** @brief The environment */ std::shared_ptr env; - /** @brief The start state to use for planning */ - tesseract_scene_graph::SceneState env_state; - /** @brief The profile dictionary */ std::shared_ptr profiles; diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp index 105fc082f5..ade84d6c14 100644 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp @@ -259,8 +259,8 @@ DescartesMotionPlanner::createProblem(const PlannerRequest& request) return prob; } - prob->env_state = request.env_state; prob->env = request.env; + prob->env_state = request.env->getState(); std::vector joint_names = prob->manip->getJointNames(); diff --git a/tesseract_motion_planners/descartes/test/descartes_planner_tests.cpp b/tesseract_motion_planners/descartes/test/descartes_planner_tests.cpp index df74d6dccd..9debf0705e 100644 --- a/tesseract_motion_planners/descartes/test/descartes_planner_tests.cpp +++ b/tesseract_motion_planners/descartes/test/descartes_planner_tests.cpp @@ -109,13 +109,6 @@ TEST(TesseractPlanningDescartesSerializeUnit, SerializeDescartesDefaultPlanToXml TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerFixedPoses) // NOLINT { - // Create the planner and the responses that will store the results - PlannerResponse planning_response; - - tesseract_kinematics::KinematicGroup::Ptr kin_group = - env_->getKinematicGroup(manip.manipulator, manip.manipulator_ik_solver); - auto cur_state = env_->getState(); - // Specify a start waypoint CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, -.20, 0.8) * Eigen::Quaterniond(0, 0, -1.0, 0)) }; @@ -137,8 +130,7 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerFixedPoses) // NOLINT program.appendMoveInstruction(plan_f1); // Create a seed - CompositeInstruction interpolated_program = - generateInterpolatedProgram(program, cur_state, env_, 3.14, 1.0, 3.14, 10); + CompositeInstruction interpolated_program = generateInterpolatedProgram(program, env_, 3.14, 1.0, 3.14, 10); // Create Profiles auto plan_profile = std::make_shared(); @@ -155,7 +147,6 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerFixedPoses) // NOLINT PlannerRequest request; request.instructions = interpolated_program; request.env = env_; - request.env_state = cur_state; request.profiles = profiles; PlannerResponse single_planner_response = single_descartes_planner.solve(request); @@ -221,13 +212,6 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerFixedPoses) // NOLINT TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerAxialSymetric) // NOLINT { - // Create the planner and the responses that will store the results - PlannerResponse planning_response; - - tesseract_kinematics::KinematicGroup::Ptr kin_group = - env_->getKinematicGroup(manip.manipulator, manip.manipulator_ik_solver); - auto cur_state = env_->getState(); - // Specify a start waypoint CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, -.20, 0.8) * Eigen::Quaterniond(0, 0, -1.0, 0)) }; @@ -249,8 +233,7 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerAxialSymetric) // NOLINT program.appendMoveInstruction(plan_f1); // Create a seed - CompositeInstruction interpolated_program = - generateInterpolatedProgram(program, cur_state, env_, 3.14, 1.0, 3.14, 10); + CompositeInstruction interpolated_program = generateInterpolatedProgram(program, env_, 3.14, 1.0, 3.14, 10); // Create Profiles auto plan_profile = std::make_shared(); @@ -271,7 +254,6 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerAxialSymetric) // NOLINT PlannerRequest request; request.instructions = interpolated_program; request.env = env_; - request.env_state = cur_state; request.profiles = profiles; auto problem = single_descartes_planner.createProblem(request); @@ -324,13 +306,6 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerAxialSymetric) // NOLINT TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerCollisionEdgeEvaluator) // NOLINT { - // Create the planner and the responses that will store the results - PlannerResponse planning_response; - - tesseract_kinematics::KinematicGroup::Ptr kin_group = - env_->getKinematicGroup(manip.manipulator, manip.manipulator_ik_solver); - auto cur_state = env_->getState(); - // Specify a start waypoint CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, -.10, 0.8) * Eigen::Quaterniond(0, 0, -1.0, 0)) }; @@ -352,7 +327,7 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerCollisionEdgeEvaluator) program.appendMoveInstruction(plan_f1); // Create a seed - CompositeInstruction interpolated_program = generateInterpolatedProgram(program, cur_state, env_, 3.14, 1.0, 3.14, 2); + CompositeInstruction interpolated_program = generateInterpolatedProgram(program, env_, 3.14, 1.0, 3.14, 2); // Create Profiles auto plan_profile = std::make_shared(); @@ -370,7 +345,6 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerCollisionEdgeEvaluator) PlannerRequest request; request.instructions = interpolated_program; request.env = env_; - request.env_state = cur_state; request.profiles = profiles; // Create Planner diff --git a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp index 2532d3fdd4..c35aaa89b1 100644 --- a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp +++ b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp @@ -345,10 +345,10 @@ OMPLMotionPlanner::createSubProblem(const PlannerRequest& request, config.end_uuid = end_instruction.getUUID(); config.problem = std::make_shared(); config.problem->env = request.env; - config.problem->env_state = request.env_state; + config.problem->env_state = request.env->getState(); config.problem->manip = manip; config.problem->contact_checker = request.env->getDiscreteContactManager(); - config.problem->contact_checker->setCollisionObjectsTransform(request.env_state.link_transforms); + config.problem->contact_checker->setCollisionObjectsTransform(config.problem->env_state.link_transforms); config.problem->contact_checker->setActiveCollisionObjects(active_link_names); cur_plan_profile->setup(*config.problem); diff --git a/tesseract_motion_planners/ompl/test/ompl_planner_tests.cpp b/tesseract_motion_planners/ompl/test/ompl_planner_tests.cpp index 45052b5b2b..1fa5b17981 100644 --- a/tesseract_motion_planners/ompl/test/ompl_planner_tests.cpp +++ b/tesseract_motion_planners/ompl/test/ompl_planner_tests.cpp @@ -198,7 +198,6 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespacePlannerUnit) // NOLINT // Step 3: Create ompl planner config and populate it auto joint_group = env->getJointGroup(manip.manipulator); - auto cur_state = env->getState(); // Specify a start waypoint JointWaypointPoly wp1{ JointWaypoint( @@ -225,7 +224,7 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespacePlannerUnit) // NOLINT program.appendMoveInstruction(plan_f2); // Create a seed - CompositeInstruction interpolated_program = generateInterpolatedProgram(program, cur_state, env, 3.14, 1.0, 3.14, 10); + CompositeInstruction interpolated_program = generateInterpolatedProgram(program, env, 3.14, 1.0, 3.14, 10); // Create Profiles auto plan_profile = std::make_shared(); @@ -248,7 +247,6 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespacePlannerUnit) // NOLINT PlannerRequest request; request.instructions = interpolated_program; request.env = env; - request.env_state = cur_state; request.profiles = profiles; // Create Planner and solve @@ -295,7 +293,7 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespacePlannerUnit) // NOLINT program.appendMoveInstruction(plan_f1); // Create a new seed - interpolated_program = generateInterpolatedProgram(program, cur_state, env, 3.14, 1.0, 3.14, 10); + interpolated_program = generateInterpolatedProgram(program, env, 3.14, 1.0, 3.14, 10); // Update Configuration request.instructions = interpolated_program; @@ -328,7 +326,7 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespacePlannerUnit) // NOLINT program.appendMoveInstruction(plan_f1); // Create a new seed - interpolated_program = generateInterpolatedProgram(program, cur_state, env, 3.14, 1.0, 3.14, 10); + interpolated_program = generateInterpolatedProgram(program, env, 3.14, 1.0, 3.14, 10); // Update Configuration request.instructions = interpolated_program; @@ -364,7 +362,6 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianGoalPlannerUnit) // NOLINT // Step 3: Create ompl planner config and populate it auto kin_group = env->getKinematicGroup(manip.manipulator); - auto cur_state = env->getState(); // Specify a start waypoint JointWaypointPoly wp1{ JointWaypoint( @@ -389,7 +386,7 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianGoalPlannerUnit) // NOLINT program.appendMoveInstruction(plan_f1); // Create a seed - CompositeInstruction interpolated_program = generateInterpolatedProgram(program, cur_state, env, 3.14, 1.0, 3.14, 10); + CompositeInstruction interpolated_program = generateInterpolatedProgram(program, env, 3.14, 1.0, 3.14, 10); // Create Profiles auto plan_profile = std::make_shared(); @@ -412,7 +409,6 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianGoalPlannerUnit) // NOLINT PlannerRequest request; request.instructions = interpolated_program; request.env = env; - request.env_state = cur_state; request.profiles = profiles; // Create Planner and solve @@ -459,7 +455,6 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianStartPlannerUnit) // NOLINT // Step 3: Create ompl planner config and populate it auto kin_group = env->getKinematicGroup(manip.manipulator); - auto cur_state = env->getState(); // Specify a start waypoint auto start_jv = Eigen::Map(start_state.data(), static_cast(start_state.size())); @@ -484,7 +479,7 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianStartPlannerUnit) // NOLINT program.appendMoveInstruction(plan_f1); // Create a seed - CompositeInstruction interpolated_program = generateInterpolatedProgram(program, cur_state, env, 3.14, 1.0, 3.14, 10); + CompositeInstruction interpolated_program = generateInterpolatedProgram(program, env, 3.14, 1.0, 3.14, 10); // Create Profiles auto plan_profile = std::make_shared(); @@ -507,7 +502,6 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianStartPlannerUnit) // NOLINT PlannerRequest request; request.instructions = interpolated_program; request.env = env; - request.env_state = cur_state; request.profiles = profiles; // Create Planner and solve diff --git a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/interpolation.h b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/interpolation.h index 706d1d9a63..9d7e02cfb8 100644 --- a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/interpolation.h +++ b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/interpolation.h @@ -515,7 +515,6 @@ std::array getClosestJointSolution(const KinematicGroupInstr /** @brief Provided for backwards compatibility */ CompositeInstruction generateInterpolatedProgram(const CompositeInstruction& instructions, - const tesseract_scene_graph::SceneState& current_state, const std::shared_ptr& env, double state_longest_valid_segment_length = 5 * M_PI / 180, double translation_longest_valid_segment_length = 0.15, diff --git a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/simple_motion_planner.h b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/simple_motion_planner.h index 063023398b..2cfa0bc0e4 100644 --- a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/simple_motion_planner.h +++ b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/simple_motion_planner.h @@ -35,6 +35,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include namespace tesseract_planning { @@ -66,9 +67,10 @@ class SimpleMotionPlanner : public MotionPlanner std::unique_ptr clone() const override; protected: - CompositeInstruction processCompositeInstruction(const CompositeInstruction& instructions, - MoveInstructionPoly& prev_instruction, + CompositeInstruction processCompositeInstruction(MoveInstructionPoly& prev_instruction, MoveInstructionPoly& prev_seed, + const CompositeInstruction& instructions, + const tesseract_scene_graph::SceneState& start_state, const PlannerRequest& request) const; }; diff --git a/tesseract_motion_planners/simple/src/interpolation.cpp b/tesseract_motion_planners/simple/src/interpolation.cpp index 850db0d7ca..5f8e66cbf4 100644 --- a/tesseract_motion_planners/simple/src/interpolation.cpp +++ b/tesseract_motion_planners/simple/src/interpolation.cpp @@ -1325,7 +1325,6 @@ std::array getClosestJointSolution(const KinematicGroupInstr } CompositeInstruction generateInterpolatedProgram(const CompositeInstruction& instructions, - const tesseract_scene_graph::SceneState& current_state, const std::shared_ptr& env, double state_longest_valid_segment_length, double translation_longest_valid_segment_length, @@ -1335,7 +1334,6 @@ CompositeInstruction generateInterpolatedProgram(const CompositeInstruction& ins // Fill out request and response PlannerRequest request; request.instructions = instructions; - request.env_state = current_state; request.env = env; // Set up planner diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_plan_profile.cpp index 9bf5c2e475..66523dc6c9 100644 --- a/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_plan_profile.cpp +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_plan_profile.cpp @@ -33,7 +33,7 @@ #include #include - +#include #include #include @@ -54,8 +54,8 @@ SimplePlannerFixedSizeAssignPlanProfile::generate(const MoveInstructionPoly& pre const PlannerRequest& request, const tesseract_common::ManipulatorInfo& global_manip_info) const { - KinematicGroupInstructionInfo info1(prev_instruction, request, global_manip_info); - KinematicGroupInstructionInfo info2(base_instruction, request, global_manip_info); + KinematicGroupInstructionInfo info1(prev_instruction, *request.env, global_manip_info); + KinematicGroupInstructionInfo info2(base_instruction, *request.env, global_manip_info); Eigen::MatrixXd states; if (!info1.has_cartesian_waypoint && !info2.has_cartesian_waypoint) @@ -90,7 +90,7 @@ SimplePlannerFixedSizeAssignPlanProfile::generate(const MoveInstructionPoly& pre } else { - Eigen::VectorXd seed = request.env_state.getJointValues(info2.manip->getJointNames()); + Eigen::VectorXd seed = request.env->getCurrentJointValues(info2.manip->getJointNames()); tesseract_common::enforceLimits(seed, info2.manip->getLimits().joint_limits); if (info2.instruction.isLinear()) diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_plan_profile.cpp index b6efdedb9b..564d41642e 100644 --- a/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_plan_profile.cpp +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_plan_profile.cpp @@ -30,7 +30,7 @@ #include #include - +#include #include #include @@ -51,8 +51,8 @@ SimplePlannerFixedSizePlanProfile::generate(const MoveInstructionPoly& prev_inst const PlannerRequest& request, const tesseract_common::ManipulatorInfo& global_manip_info) const { - KinematicGroupInstructionInfo info1(prev_instruction, request, global_manip_info); - KinematicGroupInstructionInfo info2(base_instruction, request, global_manip_info); + KinematicGroupInstructionInfo info1(prev_instruction, *request.env, global_manip_info); + KinematicGroupInstructionInfo info2(base_instruction, *request.env, global_manip_info); if (!info1.has_cartesian_waypoint && !info2.has_cartesian_waypoint) return interpolateJointJointWaypoint(info1, info2, linear_steps, freespace_steps); @@ -63,7 +63,7 @@ SimplePlannerFixedSizePlanProfile::generate(const MoveInstructionPoly& prev_inst if (info1.has_cartesian_waypoint && !info2.has_cartesian_waypoint) return interpolateCartJointWaypoint(info1, info2, linear_steps, freespace_steps); - return interpolateCartCartWaypoint(info1, info2, linear_steps, freespace_steps, request.env_state); + return interpolateCartCartWaypoint(info1, info2, linear_steps, freespace_steps, request.env->getState()); } template diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_no_ik_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_no_ik_plan_profile.cpp index 5efa23ecb4..ae925a3aef 100644 --- a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_no_ik_plan_profile.cpp +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_no_ik_plan_profile.cpp @@ -30,7 +30,7 @@ #include #include - +#include #include #include @@ -59,8 +59,8 @@ SimplePlannerLVSNoIKPlanProfile::generate(const MoveInstructionPoly& prev_instru const PlannerRequest& request, const tesseract_common::ManipulatorInfo& global_manip_info) const { - JointGroupInstructionInfo info1(prev_instruction, request, global_manip_info); - JointGroupInstructionInfo info2(base_instruction, request, global_manip_info); + JointGroupInstructionInfo info1(prev_instruction, *request.env, global_manip_info); + JointGroupInstructionInfo info2(base_instruction, *request.env, global_manip_info); if (!info1.has_cartesian_waypoint && !info2.has_cartesian_waypoint) return interpolateJointJointWaypoint(info1, @@ -96,7 +96,7 @@ SimplePlannerLVSNoIKPlanProfile::generate(const MoveInstructionPoly& prev_instru rotation_longest_valid_segment_length, min_steps, max_steps, - request.env_state); + request.env->getState()); } template diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_plan_profile.cpp index fc54a00112..b17f2d72dc 100644 --- a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_plan_profile.cpp +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_plan_profile.cpp @@ -30,7 +30,7 @@ #include #include - +#include #include #include @@ -59,8 +59,8 @@ SimplePlannerLVSPlanProfile::generate(const MoveInstructionPoly& prev_instructio const PlannerRequest& request, const tesseract_common::ManipulatorInfo& global_manip_info) const { - KinematicGroupInstructionInfo info1(prev_instruction, request, global_manip_info); - KinematicGroupInstructionInfo info2(base_instruction, request, global_manip_info); + KinematicGroupInstructionInfo info1(prev_instruction, *request.env, global_manip_info); + KinematicGroupInstructionInfo info2(base_instruction, *request.env, global_manip_info); if (!info1.has_cartesian_waypoint && !info2.has_cartesian_waypoint) return interpolateJointJointWaypoint(info1, @@ -96,7 +96,7 @@ SimplePlannerLVSPlanProfile::generate(const MoveInstructionPoly& prev_instructio rotation_longest_valid_segment_length, min_steps, max_steps, - request.env_state); + request.env->getState()); } template diff --git a/tesseract_motion_planners/simple/src/simple_motion_planner.cpp b/tesseract_motion_planners/simple/src/simple_motion_planner.cpp index 81c3ff9934..05ce62916e 100644 --- a/tesseract_motion_planners/simple/src/simple_motion_planner.cpp +++ b/tesseract_motion_planners/simple/src/simple_motion_planner.cpp @@ -85,11 +85,13 @@ PlannerResponse SimpleMotionPlanner::solve(const PlannerRequest& request) const // Assume all the plan instructions have the same manipulator as the composite const std::string manipulator = request.instructions.getManipulatorInfo().manipulator; - const std::string manipulator_ik_solver = request.instructions.getManipulatorInfo().manipulator_ik_solver; // Initialize tesseract_kinematics::JointGroup::UPtr manip = request.env->getJointGroup(manipulator); + // Start State + tesseract_scene_graph::SceneState start_state = request.env->getState(); + // Create seed CompositeInstruction seed; @@ -101,8 +103,8 @@ PlannerResponse SimpleMotionPlanner::solve(const PlannerRequest& request) const { MoveInstructionPoly start_instruction_copy = null_instruction; MoveInstructionPoly start_instruction_seed_copy = null_instruction; - seed = - processCompositeInstruction(request.instructions, start_instruction_copy, start_instruction_seed_copy, request); + seed = processCompositeInstruction( + start_instruction_copy, start_instruction_seed_copy, request.instructions, start_state, request); } catch (std::exception& e) { @@ -148,10 +150,12 @@ PlannerResponse SimpleMotionPlanner::solve(const PlannerRequest& request) const return response; } -CompositeInstruction SimpleMotionPlanner::processCompositeInstruction(const CompositeInstruction& instructions, - MoveInstructionPoly& prev_instruction, - MoveInstructionPoly& prev_seed, - const PlannerRequest& request) const +CompositeInstruction +SimpleMotionPlanner::processCompositeInstruction(MoveInstructionPoly& prev_instruction, + MoveInstructionPoly& prev_seed, + const CompositeInstruction& instructions, + const tesseract_scene_graph::SceneState& start_state, + const PlannerRequest& request) const { CompositeInstruction seed(instructions); seed.clear(); @@ -162,8 +166,8 @@ CompositeInstruction SimpleMotionPlanner::processCompositeInstruction(const Comp if (instruction.isCompositeInstruction()) { - seed.push_back( - processCompositeInstruction(instruction.as(), prev_instruction, prev_seed, request)); + seed.push_back(processCompositeInstruction( + prev_instruction, prev_seed, instruction.as(), start_state, request)); } else if (instruction.isMoveInstruction()) { @@ -171,7 +175,6 @@ CompositeInstruction SimpleMotionPlanner::processCompositeInstruction(const Comp if (prev_instruction.isNull()) { const std::string manipulator = request.instructions.getManipulatorInfo().manipulator; - const std::string manipulator_ik_solver = request.instructions.getManipulatorInfo().manipulator_ik_solver; tesseract_kinematics::JointGroup::UPtr manip = request.env->getJointGroup(manipulator); prev_instruction = base_instruction; @@ -186,7 +189,7 @@ CompositeInstruction SimpleMotionPlanner::processCompositeInstruction(const Comp { // Run IK to find solution closest to start KinematicGroupInstructionInfo info(prev_instruction, request, request.instructions.getManipulatorInfo()); - auto start_seed = getClosestJointSolution(info, request.env_state.getJointValues(manip->getJointNames())); + auto start_seed = getClosestJointSolution(info, start_state.getJointValues(manip->getJointNames())); start_waypoint.as().setSeed( tesseract_common::JointState(manip->getJointNames(), start_seed)); } diff --git a/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_position.cpp b/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_position.cpp index 85f4d6301d..4f0f74ff25 100644 --- a/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_position.cpp +++ b/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_position.cpp @@ -72,11 +72,11 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, JointCartesian { PlannerRequest request; request.env = env_; - request.env_state = env_->getState(); + JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; - instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); + instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, env_->getCurrentJointValues(joint_names_))); CartesianWaypointPoly wp2{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; MoveInstruction instr2(wp2, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); @@ -114,11 +114,11 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, CartesianJoint { PlannerRequest request; request.env = env_; - request.env_state = env_->getState(); + CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; - instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); + instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, env_->getCurrentJointValues(joint_names_))); JointWaypointPoly wp2{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; MoveInstruction instr2(wp2, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); @@ -155,11 +155,11 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, CartesianCarte { PlannerRequest request; request.env = env_; - request.env_state = env_->getState(); + CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; - instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); + instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, env_->getCurrentJointValues(joint_names_))); CartesianWaypointPoly wp2{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; MoveInstruction instr2(wp2, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); @@ -170,7 +170,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, CartesianCarte std::vector move_instructions = profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); auto fwd_kin = env_->getJointGroup(manip_info_.manipulator); - Eigen::VectorXd position = request.env_state.getJointValues(fwd_kin->getJointNames()); + Eigen::VectorXd position = env_->getCurrentJointValues(fwd_kin->getJointNames()); EXPECT_EQ(move_instructions.size(), 10); for (std::size_t i = 0; i < move_instructions.size() - 1; ++i) { diff --git a/tesseract_motion_planners/simple/test/simple_planner_fixed_size_interpolation.cpp b/tesseract_motion_planners/simple/test/simple_planner_fixed_size_interpolation.cpp index 2ff99cffb9..fe8d2218d0 100644 --- a/tesseract_motion_planners/simple/test/simple_planner_fixed_size_interpolation.cpp +++ b/tesseract_motion_planners/simple/test/simple_planner_fixed_size_interpolation.cpp @@ -72,11 +72,11 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, JointJoint_Join { PlannerRequest request; request.env = env_; - request.env_state = env_->getState(); + JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; - instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); + instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, env_->getCurrentJointValues(joint_names_))); JointWaypointPoly wp2{ JointWaypoint(joint_names_, Eigen::VectorXd::Ones(7)) }; MoveInstruction instr2(wp2, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); @@ -114,11 +114,11 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, JointCart_Joint { PlannerRequest request; request.env = env_; - request.env_state = env_->getState(); + JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; - instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); + instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, env_->getCurrentJointValues(joint_names_))); CartesianWaypointPoly wp2{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; wp2.getTransform().translation() = Eigen::Vector3d(0.25, 0, 1); @@ -160,12 +160,12 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, CartJoint_Joint { PlannerRequest request; request.env = env_; - request.env_state = env_->getState(); + CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; wp1.getTransform().translation() = Eigen::Vector3d(0.25, 0, 1); MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; - instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); + instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, env_->getCurrentJointValues(joint_names_))); JointWaypointPoly wp2{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; MoveInstruction instr2(wp2, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); @@ -203,12 +203,12 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, CartCart_JointI { PlannerRequest request; request.env = env_; - request.env_state = env_->getState(); + CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; wp1.getTransform().translation() = Eigen::Vector3d(0.25, -0.1, 1); MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; - instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); + instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, env_->getCurrentJointValues(joint_names_))); CartesianWaypointPoly wp2{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; wp2.getTransform().translation() = Eigen::Vector3d(0.25, 0.1, 1); diff --git a/tesseract_motion_planners/simple/test/simple_planner_lvs_interpolation.cpp b/tesseract_motion_planners/simple/test/simple_planner_lvs_interpolation.cpp index 49b834f42f..584a466f58 100644 --- a/tesseract_motion_planners/simple/test/simple_planner_lvs_interpolation.cpp +++ b/tesseract_motion_planners/simple/test/simple_planner_lvs_interpolation.cpp @@ -72,12 +72,11 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo { PlannerRequest request; request.env = env_; - request.env_state = env_->getState(); JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; - instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); + instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, env_->getCurrentJointValues(joint_names_))); JointWaypointPoly wp2{ JointWaypoint(joint_names_, Eigen::VectorXd::Ones(7)) }; MoveInstruction instr2(wp2, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); @@ -129,13 +128,13 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo { PlannerRequest request; request.env = env_; - request.env_state = env_->getState(); + auto joint_group = env_->getJointGroup(manip_info_.manipulator); JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; MoveInstruction instr1(wp1, MoveInstructionType::LINEAR, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; - instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); + instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, env_->getCurrentJointValues(joint_names_))); JointWaypointPoly wp2{ JointWaypoint(joint_names_, Eigen::VectorXd::Ones(7)) }; MoveInstruction instr2(wp2, MoveInstructionType::LINEAR, "TEST_PROFILE", manip_info_); @@ -197,14 +196,14 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo { PlannerRequest request; request.env = env_; - request.env_state = env_->getState(); + auto joint_group = env_->getJointGroup(manip_info_.manipulator); JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; - instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); + instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, env_->getCurrentJointValues(joint_names_))); CartesianWaypointPoly wp2{ CartesianWaypoint( joint_group->calcFwdKin(Eigen::VectorXd::Ones(7)).at(manip_info_.tcp_frame)) }; @@ -255,13 +254,13 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo { PlannerRequest request; request.env = env_; - request.env_state = env_->getState(); + auto joint_group = request.env->getJointGroup(manip_info_.manipulator); JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; MoveInstruction instr1(wp1, MoveInstructionType::LINEAR, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; - instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); + instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, env_->getCurrentJointValues(joint_names_))); CartesianWaypointPoly wp2{ CartesianWaypoint( joint_group->calcFwdKin(Eigen::VectorXd::Ones(7)).at(manip_info_.tcp_frame)) }; @@ -324,7 +323,6 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo { PlannerRequest request; request.env = env_; - request.env_state = env_->getState(); auto joint_group = env_->getJointGroup(manip_info_.manipulator); @@ -332,7 +330,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo joint_group->calcFwdKin(Eigen::VectorXd::Zero(7)).at(manip_info_.tcp_frame)) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; - instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); + instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, env_->getCurrentJointValues(joint_names_))); JointWaypointPoly wp2{ JointWaypoint(joint_names_, Eigen::VectorXd::Ones(7)) }; MoveInstruction instr2(wp2, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); @@ -381,7 +379,6 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo { PlannerRequest request; request.env = env_; - request.env_state = env_->getState(); auto joint_group = env_->getJointGroup(manip_info_.manipulator); @@ -389,7 +386,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo joint_group->calcFwdKin(Eigen::VectorXd::Zero(7)).at(manip_info_.tcp_frame)) }; MoveInstruction instr1(wp1, MoveInstructionType::LINEAR, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; - instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); + instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, env_->getCurrentJointValues(joint_names_))); JointWaypointPoly wp2{ JointWaypoint(joint_names_, Eigen::VectorXd::Ones(7)) }; MoveInstruction instr2(wp2, MoveInstructionType::LINEAR, "TEST_PROFILE", manip_info_); @@ -450,7 +447,6 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo { PlannerRequest request; request.env = env_; - request.env_state = env_->getState(); auto joint_group = env_->getJointGroup(manip_info_.manipulator); @@ -458,7 +454,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo joint_group->calcFwdKin(Eigen::VectorXd::Zero(7)).at(manip_info_.tcp_frame)) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; - instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); + instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, env_->getCurrentJointValues(joint_names_))); CartesianWaypointPoly wp2{ CartesianWaypoint( joint_group->calcFwdKin(Eigen::VectorXd::Ones(7)).at(manip_info_.tcp_frame)) }; @@ -509,7 +505,6 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo { PlannerRequest request; request.env = env_; - request.env_state = env_->getState(); auto joint_group = env_->getJointGroup(manip_info_.manipulator); @@ -517,7 +512,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo joint_group->calcFwdKin(Eigen::VectorXd::Zero(7)).at(manip_info_.tcp_frame)) }; MoveInstruction instr1(wp1, MoveInstructionType::LINEAR, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; - instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); + instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, env_->getCurrentJointValues(joint_names_))); CartesianWaypointPoly wp2{ CartesianWaypoint( joint_group->calcFwdKin(Eigen::VectorXd::Ones(7)).at(manip_info_.tcp_frame)) }; diff --git a/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp index 022c3de54d..4959dac7b7 100644 --- a/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp +++ b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp @@ -282,7 +282,7 @@ TrajOptMotionPlanner::createProblem(const PlannerRequest& request) const } else { - seed_states.push_back(request.env_state.getJointValues(joint_names)); + seed_states.push_back(request.env->getCurrentJointValues(joint_names)); } /** @todo If fixed cartesian and not term_type cost add as fixed */ diff --git a/tesseract_motion_planners/trajopt/test/trajopt_planner_tests.cpp b/tesseract_motion_planners/trajopt/test/trajopt_planner_tests.cpp index 869b3341d7..75bd1867e3 100644 --- a/tesseract_motion_planners/trajopt/test/trajopt_planner_tests.cpp +++ b/tesseract_motion_planners/trajopt/test/trajopt_planner_tests.cpp @@ -120,7 +120,6 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptPlannerBooleanFlagsJointJoint) // N { auto joint_group = env_->getJointGroup(manip.manipulator); std::vector joint_names = joint_group->getJointNames(); - auto cur_state = env_->getState(); // Specify a JointWaypoint as the start JointWaypointPoly wp1{ JointWaypoint(joint_names, Eigen::VectorXd::Zero(7)) }; @@ -143,8 +142,7 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptPlannerBooleanFlagsJointJoint) // N program.appendMoveInstruction(plan_f1); // Create a seed - CompositeInstruction interpolated_program = - generateInterpolatedProgram(program, cur_state, env_, 3.14, 1.0, 3.14, 10); + CompositeInstruction interpolated_program = generateInterpolatedProgram(program, env_, 3.14, 1.0, 3.14, 10); // Create Profiles auto plan_profile = std::make_shared(); @@ -164,7 +162,6 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptPlannerBooleanFlagsJointJoint) // N PlannerRequest request; request.instructions = interpolated_program; request.env = env_; - request.env_state = cur_state; request.profiles = profiles; // Loop over all combinations of these 4. 0001, 0010, 0011, ... , 1111 @@ -197,7 +194,6 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceJointJoint) // NOLINT { auto joint_group = env_->getJointGroup(manip.manipulator); std::vector joint_names = joint_group->getJointNames(); - auto cur_state = env_->getState(); // Specify a JointWaypoint as the start JointWaypointPoly wp1{ JointWaypoint(joint_names, Eigen::VectorXd::Zero(7)) }; @@ -220,8 +216,7 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceJointJoint) // NOLINT program.appendMoveInstruction(plan_f1); // Create a seed - CompositeInstruction interpolated_program = - generateInterpolatedProgram(program, cur_state, env_, 3.14, 1.0, 3.14, 10); + CompositeInstruction interpolated_program = generateInterpolatedProgram(program, env_, 3.14, 1.0, 3.14, 10); // Create Profiles auto plan_profile = std::make_shared(); @@ -241,7 +236,6 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceJointJoint) // NOLINT PlannerRequest request; request.instructions = interpolated_program; request.env = env_; - request.env_state = cur_state; request.profiles = profiles; { @@ -286,7 +280,6 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceJointCart) // NOLINT { auto joint_group = env_->getJointGroup(manip.manipulator); std::vector joint_names = joint_group->getJointNames(); - auto cur_state = env_->getState(); // Specify a JointWaypoint as the start JointWaypointPoly wp1{ JointWaypoint(joint_names, Eigen::VectorXd::Zero(7)) }; @@ -309,8 +302,7 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceJointCart) // NOLINT program.appendMoveInstruction(plan_f1); // Create a seed - CompositeInstruction interpolated_program = - generateInterpolatedProgram(program, cur_state, env_, 3.14, 1.0, 3.14, 10); + CompositeInstruction interpolated_program = generateInterpolatedProgram(program, env_, 3.14, 1.0, 3.14, 10); // Create Profiles auto plan_profile = std::make_shared(); @@ -330,7 +322,6 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceJointCart) // NOLINT PlannerRequest request; request.instructions = interpolated_program; request.env = env_; - request.env_state = cur_state; request.profiles = profiles; { @@ -373,12 +364,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceJointCart) // NOLINT // This test tests freespace motion b/n 1 cartesian waypoint and 1 joint waypoint TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartJoint) // NOLINT { - // Create the planner and the responses that will store the results - PlannerResponse planning_response; - auto joint_group = env_->getJointGroup(manip.manipulator); std::vector joint_names = joint_group->getJointNames(); - auto cur_state = env_->getState(); // Specify a JointWaypoint as the start CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(-.20, .4, 0.2) * @@ -402,8 +389,7 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartJoint) // NOLINT program.appendMoveInstruction(plan_f1); // Create a seed - CompositeInstruction interpolated_program = - generateInterpolatedProgram(program, cur_state, env_, 3.14, 1.0, 3.14, 10); + CompositeInstruction interpolated_program = generateInterpolatedProgram(program, env_, 3.14, 1.0, 3.14, 10); // Create Profiles auto plan_profile = std::make_shared(); @@ -423,7 +409,6 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartJoint) // NOLINT PlannerRequest request; request.instructions = interpolated_program; request.env = env_; - request.env_state = cur_state; request.profiles = profiles; { @@ -466,12 +451,6 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartJoint) // NOLINT // This test tests freespace motion b/n 2 cartesian waypoints TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartCart) // NOLINT { - // Create the planner and the responses that will store the results - PlannerResponse planning_response; - - auto joint_group = env_->getJointGroup(manip.manipulator); - auto cur_state = env_->getState(); - // Specify a CartesianWaypoint as the start CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(-.20, .4, 0.2) * Eigen::Quaterniond(0, 0, 1.0, 0)) }; @@ -495,8 +474,7 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartCart) // NOLINT program.appendMoveInstruction(plan_f1); // Create a seed - CompositeInstruction interpolated_program = - generateInterpolatedProgram(program, cur_state, env_, 3.14, 1.0, 3.14, 10); + CompositeInstruction interpolated_program = generateInterpolatedProgram(program, env_, 3.14, 1.0, 3.14, 10); // Create Profiles auto plan_profile = std::make_shared(); @@ -516,7 +494,6 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartCart) // NOLINT PlannerRequest request; request.instructions = interpolated_program; request.env = env_; - request.env_state = cur_state; request.profiles = profiles; { @@ -559,12 +536,6 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartCart) // NOLINT // are / added correctly TEST_F(TesseractPlanningTrajoptUnit, TrajoptPlannerBooleanFlagsCartCart) // NOLINT { - // Create the planner and the responses that will store the results - PlannerResponse planning_response; - - auto joint_group = env_->getJointGroup(manip.manipulator); - auto cur_state = env_->getState(); - // Specify a JointWaypoint as the start CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(-.20, .4, 0.8) * Eigen::Quaterniond(0, 0, 1.0, 0)) }; @@ -588,8 +559,7 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptPlannerBooleanFlagsCartCart) // NOL program.appendMoveInstruction(plan_f1); // Create a seed - CompositeInstruction interpolated_program = - generateInterpolatedProgram(program, cur_state, env_, 3.14, 1.0, 3.14, 10); + CompositeInstruction interpolated_program = generateInterpolatedProgram(program, env_, 3.14, 1.0, 3.14, 10); // Create Profiles auto plan_profile = std::make_shared(); @@ -609,7 +579,6 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptPlannerBooleanFlagsCartCart) // NOL PlannerRequest request; request.instructions = interpolated_program; request.env = env_; - request.env_state = cur_state; request.profiles = profiles; std::shared_ptr pci; @@ -655,12 +624,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptPlannerBooleanFlagsCartCart) // NOL // This test checks that the terms are being added correctly for joint cnts TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayJointConstraint) // NOLINT { - // Create the planner and the responses that will store the results - PlannerResponse planning_response; - auto joint_group = env_->getJointGroup(manip.manipulator); std::vector joint_names = joint_group->getJointNames(); - auto cur_state = env_->getState(); // Create a program CompositeInstruction program("TEST_PROFILE"); @@ -677,8 +642,7 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayJointConstraint) // NOLINT } // Create a seed - CompositeInstruction interpolated_program = - generateInterpolatedProgram(program, cur_state, env_, 3.14, 1.0, 3.14, 10); + CompositeInstruction interpolated_program = generateInterpolatedProgram(program, env_, 3.14, 1.0, 3.14, 10); // Create Profiles auto plan_profile = std::make_shared(); @@ -702,7 +666,6 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayJointConstraint) // NOLINT PlannerRequest request; request.instructions = interpolated_program; request.env = env_; - request.env_state = cur_state; request.profiles = profiles; std::shared_ptr pci = test_planner.createProblem(request); @@ -721,12 +684,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayJointConstraint) // NOLINT // This test checks that the terms are being added correctly for joint costs TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayJointCost) // NOLINT { - // Create the planner and the responses that will store the results - PlannerResponse planning_response; - auto joint_group = env_->getJointGroup(manip.manipulator); const std::vector& joint_names = joint_group->getJointNames(); - auto cur_state = env_->getState(); // Create a program CompositeInstruction program("TEST_PROFILE"); @@ -743,8 +702,7 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayJointCost) // NOLINT } // Create a seed - CompositeInstruction interpolated_program = - generateInterpolatedProgram(program, cur_state, env_, 3.14, 1.0, 3.14, 10); + CompositeInstruction interpolated_program = generateInterpolatedProgram(program, env_, 3.14, 1.0, 3.14, 10); // Create Profiles auto plan_profile = std::make_shared(); @@ -769,7 +727,6 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayJointCost) // NOLINT PlannerRequest request; request.instructions = interpolated_program; request.env = env_; - request.env_state = cur_state; request.profiles = profiles; std::shared_ptr pci = test_planner.createProblem(request); diff --git a/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp b/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp index afc5658b3c..2d2c3d3b23 100644 --- a/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp +++ b/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp @@ -178,7 +178,7 @@ std::shared_ptr TrajOptIfoptMotionPlanner::createProblem(co // Create the problem auto problem = std::make_shared(); problem->environment = request.env; - problem->env_state = request.env_state; + problem->env_state = request.env->getState(); problem->nlp = std::make_shared(); // Assume all the plan instructions have the same manipulator as the composite @@ -286,7 +286,7 @@ std::shared_ptr TrajOptIfoptMotionPlanner::createProblem(co } else { - seed_state = request.env_state.getJointValues(joint_names); + seed_state = request.env->getCurrentJointValues(joint_names); } // Add variable set to problem diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task.hpp b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task.hpp index b4e30e6091..296f0fa649 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task.hpp +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task.hpp @@ -181,7 +181,6 @@ class MotionPlannerTask : public TaskComposerTask // Fill out request // -------------------- PlannerRequest request; - request.env_state = env->getState(); request.env = env; request.instructions = instructions; request.profiles = profiles; diff --git a/tesseract_task_composer/planning/src/nodes/min_length_task.cpp b/tesseract_task_composer/planning/src/nodes/min_length_task.cpp index 352e6c86bd..1c090e2d1a 100644 --- a/tesseract_task_composer/planning/src/nodes/min_length_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/min_length_task.cpp @@ -137,7 +137,6 @@ std::unique_ptr MinLengthTask::runImpl(TaskComposerContext // Fill out request and response PlannerRequest request; request.instructions = ci; - request.env_state = env->getState(); request.env = env; // Set up planner From dfaeddbfd463fa13197da3de17b9e3075e00cb6e Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Sat, 14 Dec 2024 13:48:50 -0600 Subject: [PATCH 2/2] Remove PlanningRequest form simple profile interface --- .../simple/interpolation.h | 8 -- ...e_planner_fixed_size_assign_plan_profile.h | 2 +- .../simple_planner_fixed_size_plan_profile.h | 2 +- .../simple_planner_lvs_no_ik_plan_profile.h | 2 +- .../profile/simple_planner_lvs_plan_profile.h | 2 +- .../simple/profile/simple_planner_profile.h | 3 +- .../simple/src/interpolation.cpp | 14 ---- ...planner_fixed_size_assign_plan_profile.cpp | 8 +- ...simple_planner_fixed_size_plan_profile.cpp | 8 +- .../simple_planner_lvs_no_ik_plan_profile.cpp | 8 +- .../simple_planner_lvs_plan_profile.cpp | 8 +- .../simple/src/simple_motion_planner.cpp | 5 +- ...ple_planner_fixed_size_assign_position.cpp | 15 +--- ...imple_planner_fixed_size_interpolation.cpp | 20 +---- .../test/simple_planner_lvs_interpolation.cpp | 82 +++++++------------ 15 files changed, 61 insertions(+), 126 deletions(-) diff --git a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/interpolation.h b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/interpolation.h index 9d7e02cfb8..205b68c229 100644 --- a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/interpolation.h +++ b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/interpolation.h @@ -46,10 +46,6 @@ namespace tesseract_planning /** @brief The Joint Group Instruction Information struct */ struct JointGroupInstructionInfo { - JointGroupInstructionInfo(const MoveInstructionPoly& plan_instruction, - const PlannerRequest& request, - const tesseract_common::ManipulatorInfo& manip_info); - JointGroupInstructionInfo(const MoveInstructionPoly& plan_instruction, const tesseract_environment::Environment& env, const tesseract_common::ManipulatorInfo& manip_info); @@ -101,10 +97,6 @@ struct JointGroupInstructionInfo /** @brief The Kinematic Group Instruction Information struct */ struct KinematicGroupInstructionInfo { - KinematicGroupInstructionInfo(const MoveInstructionPoly& plan_instruction, - const PlannerRequest& request, - const tesseract_common::ManipulatorInfo& manip_info); - KinematicGroupInstructionInfo(const MoveInstructionPoly& plan_instruction, const tesseract_environment::Environment& env, const tesseract_common::ManipulatorInfo& manip_info); diff --git a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_plan_profile.h b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_plan_profile.h index dbef4fa041..a652400a49 100644 --- a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_plan_profile.h +++ b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_plan_profile.h @@ -48,7 +48,7 @@ class SimplePlannerFixedSizeAssignPlanProfile : public SimplePlannerPlanProfile const MoveInstructionPoly& prev_seed, const MoveInstructionPoly& base_instruction, const InstructionPoly& next_instruction, - const PlannerRequest& request, + const std::shared_ptr& env, const tesseract_common::ManipulatorInfo& global_manip_info) const override; /** @brief The number of steps to use for freespace instruction */ diff --git a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_plan_profile.h b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_plan_profile.h index 341ef430db..85cb50a59d 100644 --- a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_plan_profile.h +++ b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_plan_profile.h @@ -47,7 +47,7 @@ class SimplePlannerFixedSizePlanProfile : public SimplePlannerPlanProfile const MoveInstructionPoly& prev_seed, const MoveInstructionPoly& base_instruction, const InstructionPoly& next_instruction, - const PlannerRequest& request, + const std::shared_ptr& env, const tesseract_common::ManipulatorInfo& global_manip_info) const override; /** @brief The number of steps to use for freespace instruction */ diff --git a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_no_ik_plan_profile.h b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_no_ik_plan_profile.h index 412d550bff..0cdf7d4396 100644 --- a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_no_ik_plan_profile.h +++ b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_no_ik_plan_profile.h @@ -61,7 +61,7 @@ class SimplePlannerLVSNoIKPlanProfile : public SimplePlannerPlanProfile const MoveInstructionPoly& prev_seed, const MoveInstructionPoly& base_instruction, const InstructionPoly& next_instruction, - const PlannerRequest& request, + const std::shared_ptr& env, const tesseract_common::ManipulatorInfo& global_manip_info) const override; /** @brief The maximum joint distance, the norm of changes to all joint positions between successive steps. */ diff --git a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_plan_profile.h b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_plan_profile.h index 8fa90c9619..92a52006b4 100644 --- a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_plan_profile.h +++ b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_plan_profile.h @@ -60,7 +60,7 @@ class SimplePlannerLVSPlanProfile : public SimplePlannerPlanProfile const MoveInstructionPoly& prev_seed, const MoveInstructionPoly& base_instruction, const InstructionPoly& next_instruction, - const PlannerRequest& request, + const std::shared_ptr& env, const tesseract_common::ManipulatorInfo& global_manip_info) const override; /** @brief The maximum joint distance, the norm of changes to all joint positions between successive steps. */ diff --git a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_profile.h b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_profile.h index 05dd83a33e..07d969a42c 100644 --- a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_profile.h +++ b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_profile.h @@ -34,6 +34,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include #include #include @@ -73,7 +74,7 @@ class SimplePlannerPlanProfile : public Profile const MoveInstructionPoly& prev_seed, const MoveInstructionPoly& base_instruction, const InstructionPoly& next_instruction, - const PlannerRequest& request, + const std::shared_ptr& env, const tesseract_common::ManipulatorInfo& global_manip_info) const = 0; protected: diff --git a/tesseract_motion_planners/simple/src/interpolation.cpp b/tesseract_motion_planners/simple/src/interpolation.cpp index 5f8e66cbf4..7766a23d7f 100644 --- a/tesseract_motion_planners/simple/src/interpolation.cpp +++ b/tesseract_motion_planners/simple/src/interpolation.cpp @@ -55,13 +55,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_planning { -JointGroupInstructionInfo::JointGroupInstructionInfo(const MoveInstructionPoly& plan_instruction, - const PlannerRequest& request, - const tesseract_common::ManipulatorInfo& manip_info) - : JointGroupInstructionInfo(plan_instruction, *request.env, manip_info) -{ -} - JointGroupInstructionInfo::JointGroupInstructionInfo(const MoveInstructionPoly& plan_instruction, const tesseract_environment::Environment& env, const tesseract_common::ManipulatorInfo& manip_info) @@ -129,13 +122,6 @@ const Eigen::VectorXd& JointGroupInstructionInfo::extractJointPosition() const return getJointPosition(instruction.getWaypoint()); } -KinematicGroupInstructionInfo::KinematicGroupInstructionInfo(const MoveInstructionPoly& plan_instruction, - const PlannerRequest& request, - const tesseract_common::ManipulatorInfo& manip_info) - : KinematicGroupInstructionInfo(plan_instruction, *request.env, manip_info) -{ -} - KinematicGroupInstructionInfo::KinematicGroupInstructionInfo(const MoveInstructionPoly& plan_instruction, const tesseract_environment::Environment& env, const tesseract_common::ManipulatorInfo& manip_info) diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_plan_profile.cpp index 66523dc6c9..6fcd5097e7 100644 --- a/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_plan_profile.cpp +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_plan_profile.cpp @@ -51,11 +51,11 @@ SimplePlannerFixedSizeAssignPlanProfile::generate(const MoveInstructionPoly& pre const MoveInstructionPoly& /*prev_seed*/, const MoveInstructionPoly& base_instruction, const InstructionPoly& /*next_instruction*/, - const PlannerRequest& request, + const std::shared_ptr& env, const tesseract_common::ManipulatorInfo& global_manip_info) const { - KinematicGroupInstructionInfo info1(prev_instruction, *request.env, global_manip_info); - KinematicGroupInstructionInfo info2(base_instruction, *request.env, global_manip_info); + KinematicGroupInstructionInfo info1(prev_instruction, *env, global_manip_info); + KinematicGroupInstructionInfo info2(base_instruction, *env, global_manip_info); Eigen::MatrixXd states; if (!info1.has_cartesian_waypoint && !info2.has_cartesian_waypoint) @@ -90,7 +90,7 @@ SimplePlannerFixedSizeAssignPlanProfile::generate(const MoveInstructionPoly& pre } else { - Eigen::VectorXd seed = request.env->getCurrentJointValues(info2.manip->getJointNames()); + Eigen::VectorXd seed = env->getCurrentJointValues(info2.manip->getJointNames()); tesseract_common::enforceLimits(seed, info2.manip->getLimits().joint_limits); if (info2.instruction.isLinear()) diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_plan_profile.cpp index 564d41642e..47285a69f7 100644 --- a/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_plan_profile.cpp +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_plan_profile.cpp @@ -48,11 +48,11 @@ SimplePlannerFixedSizePlanProfile::generate(const MoveInstructionPoly& prev_inst const MoveInstructionPoly& /*prev_seed*/, const MoveInstructionPoly& base_instruction, const InstructionPoly& /*next_instruction*/, - const PlannerRequest& request, + const std::shared_ptr& env, const tesseract_common::ManipulatorInfo& global_manip_info) const { - KinematicGroupInstructionInfo info1(prev_instruction, *request.env, global_manip_info); - KinematicGroupInstructionInfo info2(base_instruction, *request.env, global_manip_info); + KinematicGroupInstructionInfo info1(prev_instruction, *env, global_manip_info); + KinematicGroupInstructionInfo info2(base_instruction, *env, global_manip_info); if (!info1.has_cartesian_waypoint && !info2.has_cartesian_waypoint) return interpolateJointJointWaypoint(info1, info2, linear_steps, freespace_steps); @@ -63,7 +63,7 @@ SimplePlannerFixedSizePlanProfile::generate(const MoveInstructionPoly& prev_inst if (info1.has_cartesian_waypoint && !info2.has_cartesian_waypoint) return interpolateCartJointWaypoint(info1, info2, linear_steps, freespace_steps); - return interpolateCartCartWaypoint(info1, info2, linear_steps, freespace_steps, request.env->getState()); + return interpolateCartCartWaypoint(info1, info2, linear_steps, freespace_steps, env->getState()); } template diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_no_ik_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_no_ik_plan_profile.cpp index ae925a3aef..d11487843b 100644 --- a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_no_ik_plan_profile.cpp +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_no_ik_plan_profile.cpp @@ -56,11 +56,11 @@ SimplePlannerLVSNoIKPlanProfile::generate(const MoveInstructionPoly& prev_instru const MoveInstructionPoly& /*prev_seed*/, const MoveInstructionPoly& base_instruction, const InstructionPoly& /*next_instruction*/, - const PlannerRequest& request, + const std::shared_ptr& env, const tesseract_common::ManipulatorInfo& global_manip_info) const { - JointGroupInstructionInfo info1(prev_instruction, *request.env, global_manip_info); - JointGroupInstructionInfo info2(base_instruction, *request.env, global_manip_info); + JointGroupInstructionInfo info1(prev_instruction, *env, global_manip_info); + JointGroupInstructionInfo info2(base_instruction, *env, global_manip_info); if (!info1.has_cartesian_waypoint && !info2.has_cartesian_waypoint) return interpolateJointJointWaypoint(info1, @@ -96,7 +96,7 @@ SimplePlannerLVSNoIKPlanProfile::generate(const MoveInstructionPoly& prev_instru rotation_longest_valid_segment_length, min_steps, max_steps, - request.env->getState()); + env->getState()); } template diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_plan_profile.cpp index b17f2d72dc..f74318b935 100644 --- a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_plan_profile.cpp +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_plan_profile.cpp @@ -56,11 +56,11 @@ SimplePlannerLVSPlanProfile::generate(const MoveInstructionPoly& prev_instructio const MoveInstructionPoly& /*prev_seed*/, const MoveInstructionPoly& base_instruction, const InstructionPoly& /*next_instruction*/, - const PlannerRequest& request, + const std::shared_ptr& env, const tesseract_common::ManipulatorInfo& global_manip_info) const { - KinematicGroupInstructionInfo info1(prev_instruction, *request.env, global_manip_info); - KinematicGroupInstructionInfo info2(base_instruction, *request.env, global_manip_info); + KinematicGroupInstructionInfo info1(prev_instruction, *env, global_manip_info); + KinematicGroupInstructionInfo info2(base_instruction, *env, global_manip_info); if (!info1.has_cartesian_waypoint && !info2.has_cartesian_waypoint) return interpolateJointJointWaypoint(info1, @@ -96,7 +96,7 @@ SimplePlannerLVSPlanProfile::generate(const MoveInstructionPoly& prev_instructio rotation_longest_valid_segment_length, min_steps, max_steps, - request.env->getState()); + env->getState()); } template diff --git a/tesseract_motion_planners/simple/src/simple_motion_planner.cpp b/tesseract_motion_planners/simple/src/simple_motion_planner.cpp index 05ce62916e..bdb8356a8e 100644 --- a/tesseract_motion_planners/simple/src/simple_motion_planner.cpp +++ b/tesseract_motion_planners/simple/src/simple_motion_planner.cpp @@ -188,7 +188,8 @@ SimpleMotionPlanner::processCompositeInstruction(MoveInstructionPoly& prev_instr if (!start_waypoint.as().hasSeed()) { // Run IK to find solution closest to start - KinematicGroupInstructionInfo info(prev_instruction, request, request.instructions.getManipulatorInfo()); + KinematicGroupInstructionInfo info( + prev_instruction, *request.env, request.instructions.getManipulatorInfo()); auto start_seed = getClosestJointSolution(info, start_state.getJointValues(manip->getJointNames())); start_waypoint.as().setSeed( tesseract_common::JointState(manip->getJointNames(), start_seed)); @@ -239,7 +240,7 @@ SimpleMotionPlanner::processCompositeInstruction(MoveInstructionPoly& prev_instr prev_seed, base_instruction, next_instruction, - request, + request.env, request.instructions.getManipulatorInfo()); // The data for the last instruction should be unchanged with exception to seed or tolerance joint state diff --git a/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_position.cpp b/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_position.cpp index 4f0f74ff25..5373142863 100644 --- a/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_position.cpp +++ b/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_position.cpp @@ -70,9 +70,6 @@ class TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit : public ::testi TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, JointCartesian_AssignJointPosition) // NOLINT { - PlannerRequest request; - request.env = env_; - JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; @@ -85,7 +82,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, JointCartesian SimplePlannerFixedSizeAssignPlanProfile profile(10, 10); std::vector move_instructions = - profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); EXPECT_EQ(move_instructions.size(), 10); for (std::size_t i = 0; i < move_instructions.size() - 1; ++i) { @@ -112,9 +109,6 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, JointCartesian TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, CartesianJoint_AssignJointPosition) // NOLINT { - PlannerRequest request; - request.env = env_; - CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; @@ -127,7 +121,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, CartesianJoint SimplePlannerFixedSizeAssignPlanProfile profile(10, 10); std::vector move_instructions = - profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); EXPECT_EQ(move_instructions.size(), 10); for (std::size_t i = 0; i < move_instructions.size() - 1; ++i) { @@ -153,9 +147,6 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, CartesianJoint TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, CartesianCartesian_AssignJointPosition) // NOLINT { - PlannerRequest request; - request.env = env_; - CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; @@ -168,7 +159,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, CartesianCarte SimplePlannerFixedSizeAssignPlanProfile profile(10, 10); std::vector move_instructions = - profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); auto fwd_kin = env_->getJointGroup(manip_info_.manipulator); Eigen::VectorXd position = env_->getCurrentJointValues(fwd_kin->getJointNames()); EXPECT_EQ(move_instructions.size(), 10); diff --git a/tesseract_motion_planners/simple/test/simple_planner_fixed_size_interpolation.cpp b/tesseract_motion_planners/simple/test/simple_planner_fixed_size_interpolation.cpp index fe8d2218d0..4e8fd89402 100644 --- a/tesseract_motion_planners/simple/test/simple_planner_fixed_size_interpolation.cpp +++ b/tesseract_motion_planners/simple/test/simple_planner_fixed_size_interpolation.cpp @@ -70,9 +70,6 @@ class TesseractPlanningSimplePlannerFixedSizeInterpolationUnit : public ::testin TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, JointJoint_JointInterpolation) // NOLINT { - PlannerRequest request; - request.env = env_; - JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; @@ -85,7 +82,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, JointJoint_Join SimplePlannerFixedSizePlanProfile profile(10, 10); std::vector move_instructions = - profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); EXPECT_EQ(move_instructions.size(), 10); for (std::size_t i = 0; i < move_instructions.size() - 1; ++i) { @@ -112,9 +109,6 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, JointJoint_Join TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, JointCart_JointInterpolation) // NOLINT { - PlannerRequest request; - request.env = env_; - JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; @@ -128,7 +122,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, JointCart_Joint SimplePlannerFixedSizePlanProfile profile(10, 10); std::vector move_instructions = - profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); EXPECT_EQ(move_instructions.size(), 10); for (std::size_t i = 0; i < move_instructions.size() - 1; ++i) { @@ -158,9 +152,6 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, JointCart_Joint TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, CartJoint_JointInterpolation) // NOLINT { - PlannerRequest request; - request.env = env_; - CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; wp1.getTransform().translation() = Eigen::Vector3d(0.25, 0, 1); MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); @@ -174,7 +165,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, CartJoint_Joint SimplePlannerFixedSizePlanProfile profile(10, 10); std::vector move_instructions = - profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); EXPECT_EQ(move_instructions.size(), 10); for (std::size_t i = 0; i < move_instructions.size() - 1; ++i) { @@ -201,9 +192,6 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, CartJoint_Joint TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, CartCart_JointInterpolation) // NOLINT { - PlannerRequest request; - request.env = env_; - CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; wp1.getTransform().translation() = Eigen::Vector3d(0.25, -0.1, 1); MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); @@ -218,7 +206,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, CartCart_JointI SimplePlannerFixedSizePlanProfile profile(10, 10); std::vector move_instructions = - profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); EXPECT_EQ(move_instructions.size(), 10); for (std::size_t i = 0; i < move_instructions.size() - 1; ++i) { diff --git a/tesseract_motion_planners/simple/test/simple_planner_lvs_interpolation.cpp b/tesseract_motion_planners/simple/test/simple_planner_lvs_interpolation.cpp index 584a466f58..f7cc4431d9 100644 --- a/tesseract_motion_planners/simple/test/simple_planner_lvs_interpolation.cpp +++ b/tesseract_motion_planners/simple/test/simple_planner_lvs_interpolation.cpp @@ -70,9 +70,6 @@ class TesseractPlanningSimplePlannerLVSInterpolationUnit : public ::testing::Tes TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypoint_JointJoint_Freespace) // NOLINT { - PlannerRequest request; - request.env = env_; - JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; @@ -85,7 +82,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo SimplePlannerLVSPlanProfile profile(3.14, 0.5, 1.57, 5); auto move_instructions = - profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); for (std::size_t i = 0; i < move_instructions.size() - 1; ++i) { const MoveInstructionPoly& mi = move_instructions.at(i); @@ -111,13 +108,13 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo // Ensure equal to minimum number steps when all params set large int min_steps = 5; SimplePlannerLVSPlanProfile cs_profile(6.28, 0.5, 1.57, min_steps); - auto cs = cs_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto cs = cs_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); EXPECT_EQ(cs.size(), min_steps); // Ensure state_longest_valid_segment_length is used double longest_valid_segment_length = 0.05; SimplePlannerLVSPlanProfile cl_profile(longest_valid_segment_length, 10, 6.28, min_steps); - auto cl = cl_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto cl = cl_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); double dist = (wp1.getPosition() - wp2.getPosition()).norm(); int steps = int(dist / longest_valid_segment_length) + 1; EXPECT_TRUE(static_cast(cl.size()) > min_steps); @@ -126,9 +123,6 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypoint_JointJoint_Linear) // NOLINT { - PlannerRequest request; - request.env = env_; - auto joint_group = env_->getJointGroup(manip_info_.manipulator); JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; @@ -143,7 +137,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo SimplePlannerLVSPlanProfile profile(3.14, 0.5, 1.57, 5); std::vector move_instructions = - profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); for (std::size_t i = 0; i < move_instructions.size() - 1; ++i) { const MoveInstructionPoly& mi = move_instructions.at(i); @@ -168,13 +162,13 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo // Ensure equal to minimum number steps when all params set large int min_steps = 5; SimplePlannerLVSPlanProfile cs_profile(6.28, 10, 6.28, min_steps); - auto cs = cs_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto cs = cs_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); EXPECT_EQ(cs.size(), min_steps); // Ensure translation_longest_valid_segment_length is used when large motion given double translation_longest_valid_segment_length = 0.01; SimplePlannerLVSPlanProfile ctl_profile(6.28, translation_longest_valid_segment_length, 6.28, min_steps); - auto ctl = ctl_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto ctl = ctl_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); Eigen::Isometry3d p1 = joint_group->calcFwdKin(wp1.getPosition()).at(manip_info_.tcp_frame); Eigen::Isometry3d p2 = joint_group->calcFwdKin(wp2.getPosition()).at(manip_info_.tcp_frame); double trans_dist = (p2.translation() - p1.translation()).norm(); @@ -185,7 +179,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo // Ensure rotation_longest_valid_segment_length is used double rotation_longest_valid_segment_length = 0.01; SimplePlannerLVSPlanProfile crl_profile(6.28, 10, rotation_longest_valid_segment_length, min_steps); - auto crl = crl_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto crl = crl_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); double rot_dist = Eigen::Quaterniond(p1.linear()).angularDistance(Eigen::Quaterniond(p2.linear())); int rot_steps = int(rot_dist / rotation_longest_valid_segment_length) + 1; EXPECT_TRUE(static_cast(crl.size()) > min_steps); @@ -194,9 +188,6 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypoint_JointCart_Freespace) // NOLINT { - PlannerRequest request; - request.env = env_; - auto joint_group = env_->getJointGroup(manip_info_.manipulator); JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; @@ -213,7 +204,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo SimplePlannerLVSPlanProfile profile(3.14, 0.5, 1.57, 5); std::vector move_instructions = - profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); for (std::size_t i = 0; i < move_instructions.size() - 1; ++i) { const MoveInstructionPoly& mi = move_instructions.at(i); @@ -240,22 +231,19 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo // Ensure equal to minimum number steps when all params set large int min_steps = 5; SimplePlannerLVSPlanProfile cs_profile(6.28, 0.5, 1.57, min_steps); - auto cs = cs_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto cs = cs_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); EXPECT_EQ(cs.size(), min_steps); // Ensure state_longest_valid_segment_length is used double longest_valid_segment_length = 0.01; SimplePlannerLVSPlanProfile cl_profile(longest_valid_segment_length, 10, 6.28, min_steps); - auto cl = cl_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto cl = cl_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); EXPECT_TRUE(static_cast(cl.size()) > min_steps); } TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypoint_JointCart_Linear) // NOLINT { - PlannerRequest request; - request.env = env_; - - auto joint_group = request.env->getJointGroup(manip_info_.manipulator); + auto joint_group = env_->getJointGroup(manip_info_.manipulator); JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; MoveInstruction instr1(wp1, MoveInstructionType::LINEAR, "TEST_PROFILE", manip_info_); @@ -270,7 +258,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo SimplePlannerLVSPlanProfile profile(3.14, 0.5, 1.57, 5); std::vector move_instructions = - profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); for (std::size_t i = 0; i < move_instructions.size() - 1; ++i) { const MoveInstructionPoly& mi = move_instructions.at(i); @@ -296,13 +284,13 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo // Ensure equal to minimum number steps when all params set large int min_steps = 5; SimplePlannerLVSPlanProfile cs_profile(6.28, 10, 6.28, min_steps); - auto cs = cs_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto cs = cs_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); EXPECT_EQ(cs.size(), min_steps); // Ensure translation_longest_valid_segment_length is used double translation_longest_valid_segment_length = 0.01; SimplePlannerLVSPlanProfile ctl_profile(6.28, translation_longest_valid_segment_length, 6.28, min_steps); - auto ctl = ctl_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto ctl = ctl_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); Eigen::Isometry3d p1 = joint_group->calcFwdKin(wp1.getPosition()).at(manip_info_.tcp_frame); double trans_dist = (wp2.getTransform().translation() - p1.translation()).norm(); int trans_steps = int(trans_dist / translation_longest_valid_segment_length) + 1; @@ -312,7 +300,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo // Ensure rotation_longest_valid_segment_length is used double rotation_longest_valid_segment_length = 0.01; SimplePlannerLVSPlanProfile crl_profile(6.28, 10, rotation_longest_valid_segment_length, min_steps); - auto crl = crl_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto crl = crl_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); double rot_dist = Eigen::Quaterniond(p1.linear()).angularDistance(Eigen::Quaterniond(wp2.getTransform().linear())); int rot_steps = int(rot_dist / rotation_longest_valid_segment_length) + 1; EXPECT_TRUE(static_cast(crl.size()) > min_steps); @@ -321,9 +309,6 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypoint_CartJoint_Freespace) // NOLINT { - PlannerRequest request; - request.env = env_; - auto joint_group = env_->getJointGroup(manip_info_.manipulator); CartesianWaypointPoly wp1{ CartesianWaypoint( @@ -339,7 +324,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo SimplePlannerLVSPlanProfile profile(3.14, 0.5, 1.57, 5); std::vector move_instructions = - profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); for (std::size_t i = 0; i < move_instructions.size() - 1; ++i) { const MoveInstructionPoly& mi = move_instructions.at(i); @@ -365,21 +350,18 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo // Ensure equal to minimum number steps when all params set large int min_steps = 5; SimplePlannerLVSPlanProfile cs_profile(6.28, 10, 6.28, min_steps); - auto cs = cs_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto cs = cs_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); EXPECT_EQ(cs.size(), min_steps); // Ensure state_longest_valid_segment_length is used double longest_valid_segment_length = 0.01; SimplePlannerLVSPlanProfile cl_profile(longest_valid_segment_length, 10, 6.28, min_steps); - auto cl = cl_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto cl = cl_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); EXPECT_TRUE(static_cast(cl.size()) > min_steps); } TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypoint_CartJoint_Linear) // NOLINT { - PlannerRequest request; - request.env = env_; - auto joint_group = env_->getJointGroup(manip_info_.manipulator); CartesianWaypointPoly wp1{ CartesianWaypoint( @@ -395,7 +377,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo SimplePlannerLVSPlanProfile profile(3.14, 0.5, 1.57, 5); std::vector move_instructions = - profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); for (std::size_t i = 0; i < move_instructions.size() - 1; ++i) { const MoveInstructionPoly& mi = move_instructions.at(i); @@ -420,13 +402,13 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo // Ensure equal to minimum number steps when all params set large int min_steps = 5; SimplePlannerLVSPlanProfile cs_profile(6.28, 10, 6.28, min_steps); - auto cs = cs_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto cs = cs_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); EXPECT_EQ(cs.size(), min_steps); // Ensure translation_longest_valid_segment_length is used double translation_longest_valid_segment_length = 0.01; SimplePlannerLVSPlanProfile ctl_profile(6.28, translation_longest_valid_segment_length, 6.28, min_steps); - auto ctl = ctl_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto ctl = ctl_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); Eigen::Isometry3d p2 = joint_group->calcFwdKin(wp2.getPosition()).at(manip_info_.tcp_frame); double trans_dist = (p2.translation() - wp1.getTransform().translation()).norm(); int trans_steps = int(trans_dist / translation_longest_valid_segment_length) + 1; @@ -436,7 +418,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo // Ensure rotation_longest_valid_segment_length is used double rotation_longest_valid_segment_length = 0.01; SimplePlannerLVSPlanProfile crl_profile(6.28, 10, rotation_longest_valid_segment_length, min_steps); - auto crl = crl_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto crl = crl_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); double rot_dist = Eigen::Quaterniond(wp1.getTransform().linear()).angularDistance(Eigen::Quaterniond(p2.linear())); int rot_steps = int(rot_dist / rotation_longest_valid_segment_length) + 1; EXPECT_TRUE(static_cast(crl.size()) > min_steps); @@ -445,9 +427,6 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypoint_CartCart_Freespace) // NOLINT { - PlannerRequest request; - request.env = env_; - auto joint_group = env_->getJointGroup(manip_info_.manipulator); CartesianWaypointPoly wp1{ CartesianWaypoint( @@ -464,7 +443,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo SimplePlannerLVSPlanProfile profile(3.14, 0.5, 1.57, 5); std::vector move_instructions = - profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); for (std::size_t i = 0; i < move_instructions.size() - 1; ++i) { const MoveInstructionPoly& mi = move_instructions.at(i); @@ -491,21 +470,18 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo // Ensure equal to minimum number steps when all params set large int min_steps = 5; SimplePlannerLVSPlanProfile cs_profile(6.28, 0.5, 1.57, min_steps); - auto cs = cs_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto cs = cs_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); EXPECT_EQ(cs.size(), min_steps); // Ensure state_longest_valid_segment_length is used double longest_valid_segment_length = 0.01; SimplePlannerLVSPlanProfile cl_profile(longest_valid_segment_length, 10, 6.28, min_steps); - auto cl = cl_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto cl = cl_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); EXPECT_TRUE(static_cast(cl.size()) > min_steps); } TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypoint_CartCart_Linear) // NOLINT { - PlannerRequest request; - request.env = env_; - auto joint_group = env_->getJointGroup(manip_info_.manipulator); CartesianWaypointPoly wp1{ CartesianWaypoint( @@ -522,7 +498,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo SimplePlannerLVSPlanProfile profile(3.14, 0.5, 1.57, 5); std::vector move_instructions = - profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); for (std::size_t i = 0; i < move_instructions.size() - 1; ++i) { const MoveInstructionPoly& mi = move_instructions.at(i); @@ -548,13 +524,13 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo // Ensure equal to minimum number steps when all params set large int min_steps = 5; SimplePlannerLVSPlanProfile cs_profile(6.28, 10, 6.28, min_steps); - auto cs = cs_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto cs = cs_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); EXPECT_EQ(cs.size(), min_steps); // Ensure translation_longest_valid_segment_length is used double translation_longest_valid_segment_length = 0.01; SimplePlannerLVSPlanProfile ctl_profile(6.28, translation_longest_valid_segment_length, 6.28, min_steps); - auto ctl = ctl_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto ctl = ctl_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); double trans_dist = (wp2.getTransform().translation() - wp1.getTransform().translation()).norm(); int trans_steps = int(trans_dist / translation_longest_valid_segment_length) + 1; EXPECT_TRUE(static_cast(ctl.size()) > min_steps); @@ -563,7 +539,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo // Ensure rotation_longest_valid_segment_length is used double rotation_longest_valid_segment_length = 0.01; SimplePlannerLVSPlanProfile crl_profile(6.28, 10, rotation_longest_valid_segment_length, min_steps); - auto crl = crl_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto crl = crl_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); double rot_dist = Eigen::Quaterniond(wp1.getTransform().linear()).angularDistance(Eigen::Quaterniond(wp2.getTransform().linear())); int rot_steps = int(rot_dist / rotation_longest_valid_segment_length) + 1;