From ad151fbcd180b2e6013a5cf6bd74220c1dff8487 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Thu, 26 Dec 2024 09:51:18 -0600 Subject: [PATCH] Improve descartes plan profile to include sample min, max and ik solver setting --- .../descartes/descartes_utils.h | 31 ++++++++++++----- .../impl/descartes_motion_planner.hpp | 12 ++----- .../descartes_default_plan_profile.hpp | 34 ++++++++++++++++--- .../profile/descartes_default_plan_profile.h | 12 +++++-- .../descartes/profile/descartes_profile.h | 6 ++-- .../descartes/src/descartes_utils.cpp | 27 +++++++++------ .../descartes_default_plan_profile.cpp | 3 ++ 7 files changed, 85 insertions(+), 40 deletions(-) diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_utils.h b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_utils.h index a750350ce9..347cf86f8f 100644 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_utils.h +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_utils.h @@ -38,39 +38,52 @@ namespace tesseract_planning using PoseSamplerFn = std::function; /** - * @brief Given a tool pose create samples from [-PI, PI) around the provided axis. + * @brief Given a tool pose create samples from [minimum, maximum] around the provided axis. * @param tool_pose Tool pose to be sampled - * @param resolution The resolution to sample at * @param axis The axis to sample around + * @param resolution The resolution to sample at + * @param minimum The minimum angle to start sampling at + * @param maximum The maximum angle to stop sampling at * @return A vector of tool poses */ tesseract_common::VectorIsometry3d sampleToolAxis(const Eigen::Isometry3d& tool_pose, + const Eigen::Vector3d& axis, double resolution, - const Eigen::Vector3d& axis); + double minimum, + double maximum); /** - * @brief Given a tool pose create samples from [-PI, PI) around the x axis. + * @brief Given a tool pose create samples from [minimum, maximum] around the x axis. * @param tool_pose Tool pose to be sampled * @param resolution The resolution to sample at + * @param minimum The minimum angle to start sampling at + * @param maximum The maximum angle to stop sampling at * @return A vector of tool poses */ -tesseract_common::VectorIsometry3d sampleToolXAxis(const Eigen::Isometry3d& tool_pose, double resolution); +tesseract_common::VectorIsometry3d +sampleToolXAxis(const Eigen::Isometry3d& tool_pose, double resolution, double minimum, double maximum); /** - * @brief Given a tool pose create samples from [-PI, PI) around the y axis. + * @brief Given a tool pose create samples from [minimum, maximum] around the y axis. * @param tool_pose Tool pose to be sampled * @param resolution The resolution to sample at + * @param minimum The minimum angle to start sampling at + * @param maximum The maximum angle to stop sampling at * @return A vector of tool poses */ -tesseract_common::VectorIsometry3d sampleToolYAxis(const Eigen::Isometry3d& tool_pose, double resolution); +tesseract_common::VectorIsometry3d +sampleToolYAxis(const Eigen::Isometry3d& tool_pose, double resolution, double minimum, double maximum); /** - * @brief Given a tool pose create samples from [-PI, PI) around the z axis. + * @brief Given a tool pose create samples from [minimum, maximum] around the z axis. * @param tool_pose Tool pose to be sampled * @param resolution The resolution to sample at + * @param minimum The minimum angle to start sampling at + * @param maximum The maximum angle to stop sampling at * @return A vector of tool poses */ -tesseract_common::VectorIsometry3d sampleToolZAxis(const Eigen::Isometry3d& tool_pose, double resolution); +tesseract_common::VectorIsometry3d +sampleToolZAxis(const Eigen::Isometry3d& tool_pose, double resolution, double minimum, double maximum); /** * @brief This is the default sample with if a fixed pose sampler 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 b18f31cc5b..52b2f57745 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 @@ -89,12 +89,6 @@ PlannerResponse DescartesMotionPlanner::solve(const PlannerRequest& r assert(instruction.get().isMoveInstruction()); const auto& move_instruction = instruction.get().template as(); - // If plan instruction has manipulator information then use it over the one provided by the composite. - tesseract_common::ManipulatorInfo manip_info = composite_mi.getCombined(move_instruction.getManipulatorInfo()); - - if (manip_info.empty()) - throw std::runtime_error("Descartes, manipulator info is empty!"); - // Get Plan Profile auto cur_plan_profile = getProfile>(name_, @@ -109,10 +103,10 @@ PlannerResponse DescartesMotionPlanner::solve(const PlannerRequest& r !move_instruction.getWaypoint().as().isConstrained()) continue; - waypoint_samplers.push_back(cur_plan_profile->createWaypointSampler(move_instruction, manip_info, request.env)); - state_evaluators.push_back(cur_plan_profile->createStateEvaluator(move_instruction, manip_info, request.env)); + waypoint_samplers.push_back(cur_plan_profile->createWaypointSampler(move_instruction, composite_mi, request.env)); + state_evaluators.push_back(cur_plan_profile->createStateEvaluator(move_instruction, composite_mi, request.env)); if (index != 0) - edge_evaluators.push_back(cur_plan_profile->createEdgeEvaluator(move_instruction, manip_info, request.env)); + edge_evaluators.push_back(cur_plan_profile->createEdgeEvaluator(move_instruction, composite_mi, request.env)); ++index; } diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/profile/descartes_default_plan_profile.hpp b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/profile/descartes_default_plan_profile.hpp index 2b83f4af78..77bec221b1 100644 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/profile/descartes_default_plan_profile.hpp +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/profile/descartes_default_plan_profile.hpp @@ -69,18 +69,32 @@ PoseSamplerFn DescartesDefaultPlanProfile::createPoseSampler( if (target_pose_fixed) return sampleFixed; - return [axis = target_pose_sample_axis, resolution = target_pose_sample_resolution]( - const Eigen::Isometry3d& tool_pose) { return sampleToolAxis(tool_pose, resolution, axis); }; + return [axis = target_pose_sample_axis, + resolution = target_pose_sample_resolution, + minimum = target_pose_sample_min, + maximum = target_pose_sample_max](const Eigen::Isometry3d& tool_pose) { + return sampleToolAxis(tool_pose, axis, resolution, minimum, maximum); + }; } template std::unique_ptr> DescartesDefaultPlanProfile::createWaypointSampler( const MoveInstructionPoly& move_instruction, - const tesseract_common::ManipulatorInfo& manip_info, + const tesseract_common::ManipulatorInfo& composite_manip_info, const std::shared_ptr& env) const { + // If plan instruction has manipulator information then use it over the one provided by the composite. + tesseract_common::ManipulatorInfo manip_info = + composite_manip_info.getCombined(move_instruction.getManipulatorInfo()); + if (!manipulator_ik_solver.empty()) + manip_info.manipulator_ik_solver = manipulator_ik_solver; + + if (manip_info.empty()) + throw std::runtime_error("Descartes, manipulator info is empty!"); + auto manip = DescartesPlanProfile::createKinematicGroup(manip_info, *env); + if (!move_instruction.getWaypoint().isCartesianWaypoint()) { assert(checkJointPositionFormat(manip->getJointNames(), move_instruction.getWaypoint())); @@ -121,10 +135,20 @@ DescartesDefaultPlanProfile::createWaypointSampler( template std::unique_ptr> DescartesDefaultPlanProfile::createEdgeEvaluator( const MoveInstructionPoly& move_instruction, - const tesseract_common::ManipulatorInfo& manip_info, + const tesseract_common::ManipulatorInfo& composite_manip_info, const std::shared_ptr& env) const { + // If plan instruction has manipulator information then use it over the one provided by the composite. + tesseract_common::ManipulatorInfo manip_info = + composite_manip_info.getCombined(move_instruction.getManipulatorInfo()); + if (!manipulator_ik_solver.empty()) + manip_info.manipulator_ik_solver = manipulator_ik_solver; + + if (manip_info.empty()) + throw std::runtime_error("Descartes, manipulator info is empty!"); + auto manip = DescartesPlanProfile::createKinematicGroup(manip_info, *env); + if (move_instruction.getWaypoint().isCartesianWaypoint()) { if (!enable_edge_collision) @@ -155,7 +179,7 @@ template std::unique_ptr> DescartesDefaultPlanProfile::createStateEvaluator( const MoveInstructionPoly& /*move_instruction*/, - const tesseract_common::ManipulatorInfo& /*manip_info*/, + const tesseract_common::ManipulatorInfo& /*composite_manip_info*/, const std::shared_ptr& /*env*/) const { return std::make_unique>(); diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_default_plan_profile.h b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_default_plan_profile.h index db8e2ec1db..d6204fbd33 100644 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_default_plan_profile.h +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_default_plan_profile.h @@ -49,9 +49,15 @@ class DescartesDefaultPlanProfile : public DescartesPlanProfile DescartesDefaultPlanProfile() = default; + /** @brief Target pose sampling params */ bool target_pose_fixed{ true }; Eigen::Vector3d target_pose_sample_axis{ 0, 0, 1 }; double target_pose_sample_resolution{ M_PI_2 }; + double target_pose_sample_min{ -M_PI }; + double target_pose_sample_max{ M_PI - M_PI_2 }; // Subtract resolution to avoid duplicate states at -PI and PI + + /** @brief Override the manipulator ik solver */ + std::string manipulator_ik_solver; /** * @brief Flag to indicate that collisions should not cause failures during state/edge evaluation @@ -80,17 +86,17 @@ class DescartesDefaultPlanProfile : public DescartesPlanProfile std::unique_ptr> createWaypointSampler(const MoveInstructionPoly& move_instruction, - const tesseract_common::ManipulatorInfo& manip_info, + const tesseract_common::ManipulatorInfo& composite_manip_info, const std::shared_ptr& env) const override; std::unique_ptr> createEdgeEvaluator(const MoveInstructionPoly& move_instruction, - const tesseract_common::ManipulatorInfo& manip_info, + const tesseract_common::ManipulatorInfo& composite_manip_info, const std::shared_ptr& env) const override; std::unique_ptr> createStateEvaluator(const MoveInstructionPoly& move_instruction, - const tesseract_common::ManipulatorInfo& manip_info, + const tesseract_common::ManipulatorInfo& composite_manip_info, const std::shared_ptr& env) const override; protected: diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_profile.h b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_profile.h index 81d3b12fdd..3e40330cf6 100644 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_profile.h +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_profile.h @@ -90,17 +90,17 @@ class DescartesPlanProfile : public Profile virtual std::unique_ptr> createWaypointSampler(const MoveInstructionPoly& move_instruction, - const tesseract_common::ManipulatorInfo& manip_info, + const tesseract_common::ManipulatorInfo& composite_manip_info, const std::shared_ptr& env) const = 0; virtual std::unique_ptr> createEdgeEvaluator(const MoveInstructionPoly& move_instruction, - const tesseract_common::ManipulatorInfo& manip_info, + const tesseract_common::ManipulatorInfo& composite_manip_info, const std::shared_ptr& env) const = 0; virtual std::unique_ptr> createStateEvaluator(const MoveInstructionPoly& move_instruction, - const tesseract_common::ManipulatorInfo& manip_info, + const tesseract_common::ManipulatorInfo& composite_manip_info, const std::shared_ptr& env) const = 0; protected: diff --git a/tesseract_motion_planners/descartes/src/descartes_utils.cpp b/tesseract_motion_planners/descartes/src/descartes_utils.cpp index ef6bf3e915..aac12cbc10 100644 --- a/tesseract_motion_planners/descartes/src/descartes_utils.cpp +++ b/tesseract_motion_planners/descartes/src/descartes_utils.cpp @@ -29,14 +29,16 @@ namespace tesseract_planning { tesseract_common::VectorIsometry3d sampleToolAxis(const Eigen::Isometry3d& tool_pose, + const Eigen::Vector3d& axis, double resolution, - const Eigen::Vector3d& axis) + double minimum, + double maximum) { tesseract_common::VectorIsometry3d samples; - auto cnt = static_cast(std::ceil(2.0 * M_PI / resolution)) + 1; - Eigen::VectorXd angles = Eigen::VectorXd::LinSpaced(cnt, -M_PI, M_PI); - samples.reserve(static_cast(angles.size()) - 1UL); - for (long i = 0; i < static_cast(angles.size() - 1); ++i) + auto cnt = static_cast(std::ceil((maximum - minimum) / resolution)) + 1; + Eigen::VectorXd angles = Eigen::VectorXd::LinSpaced(cnt, minimum, maximum); + samples.reserve(static_cast(angles.size())); + for (long i = 0; i < static_cast(angles.size()); ++i) { Eigen::Isometry3d p = tool_pose * Eigen::AngleAxisd(angles(i), axis); samples.push_back(p); @@ -44,19 +46,22 @@ tesseract_common::VectorIsometry3d sampleToolAxis(const Eigen::Isometry3d& tool_ return samples; } -tesseract_common::VectorIsometry3d sampleToolXAxis(const Eigen::Isometry3d& tool_pose, double resolution) +tesseract_common::VectorIsometry3d +sampleToolXAxis(const Eigen::Isometry3d& tool_pose, double resolution, double minimum, double maximum) { - return sampleToolAxis(tool_pose, resolution, Eigen::Vector3d::UnitX()); // NOLINT + return sampleToolAxis(tool_pose, Eigen::Vector3d::UnitX(), resolution, minimum, maximum); // NOLINT } -tesseract_common::VectorIsometry3d sampleToolYAxis(const Eigen::Isometry3d& tool_pose, double resolution) +tesseract_common::VectorIsometry3d +sampleToolYAxis(const Eigen::Isometry3d& tool_pose, double resolution, double minimum, double maximum) { - return sampleToolAxis(tool_pose, resolution, Eigen::Vector3d::UnitY()); // NOLINT + return sampleToolAxis(tool_pose, Eigen::Vector3d::UnitY(), resolution, minimum, maximum); // NOLINT } -tesseract_common::VectorIsometry3d sampleToolZAxis(const Eigen::Isometry3d& tool_pose, double resolution) +tesseract_common::VectorIsometry3d +sampleToolZAxis(const Eigen::Isometry3d& tool_pose, double resolution, double minimum, double maximum) { - return sampleToolAxis(tool_pose, resolution, Eigen::Vector3d::UnitZ()); // NOLINT + return sampleToolAxis(tool_pose, Eigen::Vector3d::UnitZ(), resolution, minimum, maximum); // NOLINT } tesseract_common::VectorIsometry3d sampleFixed(const Eigen::Isometry3d& tool_pose) diff --git a/tesseract_motion_planners/descartes/src/profile/descartes_default_plan_profile.cpp b/tesseract_motion_planners/descartes/src/profile/descartes_default_plan_profile.cpp index 63fbe27734..0b43a71429 100644 --- a/tesseract_motion_planners/descartes/src/profile/descartes_default_plan_profile.cpp +++ b/tesseract_motion_planners/descartes/src/profile/descartes_default_plan_profile.cpp @@ -45,6 +45,9 @@ void DescartesDefaultPlanProfile::serialize(Archive& ar, const unsign ar& BOOST_SERIALIZATION_NVP(target_pose_fixed); ar& BOOST_SERIALIZATION_NVP(target_pose_sample_axis); ar& BOOST_SERIALIZATION_NVP(target_pose_sample_resolution); + ar& BOOST_SERIALIZATION_NVP(target_pose_sample_min); + ar& BOOST_SERIALIZATION_NVP(target_pose_sample_max); + ar& BOOST_SERIALIZATION_NVP(manipulator_ik_solver); ar& BOOST_SERIALIZATION_NVP(allow_collision); ar& BOOST_SERIALIZATION_NVP(enable_collision); ar& BOOST_SERIALIZATION_NVP(vertex_collision_check_config);