From f5773e8b1be8c63b9ce4e053bebce3a2915aff0f Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Sat, 14 Dec 2024 13:33:31 -0600 Subject: [PATCH] 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