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;