Remove env_state from planning request

tesseract-robotics · Dec 14, 2024 · f5773e8 · f5773e8
1 parent ad73309
commit f5773e8
Show file tree

Hide file tree

Showing 21 changed files with 82 additions and 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
@@ -56,9 +56,6 @@ struct PlannerRequest
   /** @brief The environment */
   std::shared_ptr<const tesseract_environment::Environment> env;
 
-  /** @brief The start state to use for planning */
-  tesseract_scene_graph::SceneState env_state;
-
   /** @brief The profile dictionary */
   std::shared_ptr<const ProfileDictionary> profiles;
 

diff --git a/...s/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp b/...s/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp
@@ -259,8 +259,8 @@ DescartesMotionPlanner<FloatType>::createProblem(const PlannerRequest& request)
     return prob;
   }
 
-  prob->env_state = request.env_state;
   prob->env = request.env;
+  prob->env_state = request.env->getState();
 
   std::vector<std::string> 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
@@ -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<DescartesDefaultPlanProfileD>();
@@ -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<DescartesDefaultPlanProfileD>();
@@ -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<DescartesDefaultPlanProfileD>();
@@ -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
@@ -345,10 +345,10 @@ OMPLMotionPlanner::createSubProblem(const PlannerRequest& request,
   config.end_uuid = end_instruction.getUUID();
   config.problem = std::make_shared<OMPLProblem>();
   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
@@ -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<OMPLDefaultPlanProfile>();
@@ -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<OMPLDefaultPlanProfile>();
@@ -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<const Eigen::VectorXd>(start_state.data(), static_cast<long>(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<OMPLDefaultPlanProfile>();
@@ -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
@@ -515,7 +515,6 @@ std::array<Eigen::VectorXd, 2> getClosestJointSolution(const KinematicGroupInstr
 
 /** @brief Provided for backwards compatibility */
 CompositeInstruction generateInterpolatedProgram(const CompositeInstruction& instructions,
-                                                 const tesseract_scene_graph::SceneState& current_state,
                                                  const std::shared_ptr<const tesseract_environment::Environment>& env,
                                                  double state_longest_valid_segment_length = 5 * M_PI / 180,
                                                  double translation_longest_valid_segment_length = 0.15,

diff --git a/...t_motion_planners/simple/include/tesseract_motion_planners/simple/simple_motion_planner.h b/...t_motion_planners/simple/include/tesseract_motion_planners/simple/simple_motion_planner.h
@@ -35,6 +35,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_motion_planners/core/planner.h>
 #include <tesseract_command_language/fwd.h>
+#include <tesseract_scene_graph/fwd.h>
 
 namespace tesseract_planning
 {
@@ -66,9 +67,10 @@ class SimpleMotionPlanner : public MotionPlanner
   std::unique_ptr<MotionPlanner> 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
@@ -1325,7 +1325,6 @@ std::array<Eigen::VectorXd, 2> getClosestJointSolution(const KinematicGroupInstr
 }
 
 CompositeInstruction generateInterpolatedProgram(const CompositeInstruction& instructions,
-                                                 const tesseract_scene_graph::SceneState& current_state,
                                                  const std::shared_ptr<const tesseract_environment::Environment>& 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/...ract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_plan_profile.cpp b/...ract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_plan_profile.cpp
@@ -33,7 +33,7 @@
 #include <tesseract_common/kinematic_limits.h>
 
 #include <tesseract_kinematics/core/kinematic_group.h>
-
+#include <tesseract_environment/environment.h>
 #include <tesseract_command_language/poly/move_instruction_poly.h>
 
 #include <boost/serialization/base_object.hpp>
@@ -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<double>(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
@@ -30,7 +30,7 @@
 #include <tesseract_motion_planners/core/utils.h>
 
 #include <tesseract_common/manipulator_info.h>
-
+#include <tesseract_environment/environment.h>
 #include <tesseract_command_language/poly/move_instruction_poly.h>
 
 #include <boost/serialization/base_object.hpp>
@@ -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 <class Archive>

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
@@ -30,7 +30,7 @@
 #include <tesseract_motion_planners/core/utils.h>
 
 #include <tesseract_common/manipulator_info.h>
-
+#include <tesseract_environment/environment.h>
 #include <tesseract_command_language/poly/move_instruction_poly.h>
 
 #include <boost/serialization/base_object.hpp>
@@ -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 <class Archive>

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
@@ -30,7 +30,7 @@
 #include <tesseract_motion_planners/core/utils.h>
 
 #include <tesseract_common/manipulator_info.h>
-
+#include <tesseract_environment/environment.h>
 #include <tesseract_command_language/poly/move_instruction_poly.h>
 
 #include <boost/serialization/base_object.hpp>
@@ -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 <class Archive>