From d198e440a219d52744109ab9aeb2f6bf5b52424e Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Wed, 29 May 2024 16:19:20 +0200 Subject: [PATCH] Rename SimplePlannerFixedSizeAssignPlanProfile to SimplePlannerFixedSizeAssignNoIKPlanProfile and add SimplePlannerFixedSizeAssignPlanProfile (with IK) --- .../simple/CMakeLists.txt | 2 + ...ner_fixed_size_assign_no_ik_plan_profile.h | 63 +++++++++ ...e_planner_fixed_size_assign_plan_profile.h | 6 +- ...r_fixed_size_assign_no_ik_plan_profile.cpp | 128 ++++++++++++++++++ ...planner_fixed_size_assign_plan_profile.cpp | 50 +++---- 5 files changed, 221 insertions(+), 28 deletions(-) create mode 100644 tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.h create mode 100644 tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.cpp diff --git a/tesseract_motion_planners/simple/CMakeLists.txt b/tesseract_motion_planners/simple/CMakeLists.txt index 70412100f1..41d30a898d 100644 --- a/tesseract_motion_planners/simple/CMakeLists.txt +++ b/tesseract_motion_planners/simple/CMakeLists.txt @@ -2,8 +2,10 @@ add_library( ${PROJECT_NAME}_simple src/interpolation.cpp src/simple_motion_planner.cpp + src/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.cpp src/profile/simple_planner_fixed_size_assign_plan_profile.cpp src/profile/simple_planner_fixed_size_plan_profile.cpp + src/profile/simple_planner_lvs_assign_no_ik_plan_profile.cpp src/profile/simple_planner_lvs_assign_plan_profile.cpp src/profile/simple_planner_lvs_no_ik_plan_profile.cpp src/profile/simple_planner_lvs_plan_profile.cpp) diff --git a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.h b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.h new file mode 100644 index 0000000000..48c02e87fa --- /dev/null +++ b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.h @@ -0,0 +1,63 @@ +/** + * @file simple_planner_fixed_size_assign_no_ik_plan_profile.h + * @brief + * + * @author Matthew Powelson + * @date July 23, 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef TESSERACT_MOTION_PLANNERS_SIMPLE_FIXED_SIZE_ASSIGN_NO_IK_PLAN_PROFILE_H +#define TESSERACT_MOTION_PLANNERS_SIMPLE_FIXED_SIZE_ASSIGN_NO_IK_PLAN_PROFILE_H + +#include + +namespace tesseract_planning +{ +class SimplePlannerFixedSizeAssignNoIKPlanProfile : public SimplePlannerPlanProfile +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + /** + * @brief SimplePlannerFixedSizeAssignPlanProfile + * @param freespace_steps The number of steps to use for freespace instruction + * @param linear_steps The number of steps to use for linear instruction + */ + SimplePlannerFixedSizeAssignNoIKPlanProfile(int freespace_steps = 10, int linear_steps = 10); + + std::vector generate(const MoveInstructionPoly& prev_instruction, + const MoveInstructionPoly& prev_seed, + const MoveInstructionPoly& base_instruction, + const InstructionPoly& next_instruction, + const PlannerRequest& request, + const tesseract_common::ManipulatorInfo& global_manip_info) const override; + + /** @brief The number of steps to use for freespace instruction */ + int freespace_steps; + + /** @brief The number of steps to use for linear instruction */ + int linear_steps; +}; + +} // namespace tesseract_planning + +#endif // TESSERACT_MOTION_PLANNERS_SIMPLE_FIXED_SIZE_ASSIGN_NO_IK_PLAN_PROFILE_H 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 e41730ff0c..be7065e529 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 @@ -2,12 +2,12 @@ * @file simple_planner_fixed_size_assign_plan_profile.h * @brief * - * @author Matthew Powelson - * @date July 23, 2020 + * @author Roelof Oomen + * @date May 29, 2024 * @version TODO * @bug No known bugs * - * @copyright Copyright (c) 2020, Southwest Research Institute + * @copyright Copyright (c) 2024, ROS Industrial Consortium * * @par License * Software License Agreement (Apache License) diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.cpp new file mode 100644 index 0000000000..eb05e45876 --- /dev/null +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.cpp @@ -0,0 +1,128 @@ +/** + * @file simple_planner_fixed_size_assign_no_ik_plan_profile.cpp + * @brief + * + * @author Matthew Powelson + * @date July 23, 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include + +#include +#include + +#include + +#include + +namespace tesseract_planning +{ +SimplePlannerFixedSizeAssignNoIKPlanProfile::SimplePlannerFixedSizeAssignNoIKPlanProfile(int freespace_steps, + int linear_steps) + : freespace_steps(freespace_steps), linear_steps(linear_steps) +{ +} + +std::vector +SimplePlannerFixedSizeAssignNoIKPlanProfile::generate(const MoveInstructionPoly& prev_instruction, + const MoveInstructionPoly& /*prev_seed*/, + const MoveInstructionPoly& base_instruction, + const InstructionPoly& /*next_instruction*/, + const PlannerRequest& request, + const tesseract_common::ManipulatorInfo& global_manip_info) const +{ + JointGroupInstructionInfo prev(prev_instruction, request, global_manip_info); + JointGroupInstructionInfo base(base_instruction, request, global_manip_info); + + Eigen::MatrixXd states; + if (!prev.has_cartesian_waypoint && !base.has_cartesian_waypoint) + { + const Eigen::VectorXd& jp = base.extractJointPosition(); + if (base.instruction.isLinear()) + states = jp.replicate(1, linear_steps + 1); + else if (base.instruction.isFreespace()) + states = jp.replicate(1, freespace_steps + 1); + else + throw std::runtime_error("stateJointJointWaypointFixedSize: Unsupported MoveInstructionType!"); + } + else if (!prev.has_cartesian_waypoint && base.has_cartesian_waypoint) + { + const Eigen::VectorXd& jp = prev.extractJointPosition(); + if (base.instruction.isLinear()) + states = jp.replicate(1, linear_steps + 1); + else if (base.instruction.isFreespace()) + states = jp.replicate(1, freespace_steps + 1); + else + throw std::runtime_error("stateJointJointWaypointFixedSize: Unsupported MoveInstructionType!"); + } + else if (prev.has_cartesian_waypoint && !base.has_cartesian_waypoint) + { + const Eigen::VectorXd& jp = base.extractJointPosition(); + if (base.instruction.isLinear()) + states = jp.replicate(1, linear_steps + 1); + else if (base.instruction.isFreespace()) + states = jp.replicate(1, freespace_steps + 1); + else + throw std::runtime_error("stateJointJointWaypointFixedSize: Unsupported MoveInstructionType!"); + } + else + { + Eigen::VectorXd seed = request.env_state.getJointValues(base.manip->getJointNames()); + tesseract_common::enforceLimits(seed, base.manip->getLimits().joint_limits); + + if (base.instruction.isLinear()) + states = seed.replicate(1, linear_steps + 1); + else if (base.instruction.isFreespace()) + states = seed.replicate(1, freespace_steps + 1); + else + throw std::runtime_error("stateJointJointWaypointFixedSize: Unsupported MoveInstructionType!"); + } + + // Linearly interpolate in cartesian space if linear move + if (base_instruction.isLinear()) + { + Eigen::Isometry3d p1_world; + if (prev.has_cartesian_waypoint) + p1_world = prev.extractCartesianPose(); + else + p1_world = prev.calcCartesianPose(prev.extractJointPosition()); + + Eigen::Isometry3d p2_world; + if (base.has_cartesian_waypoint) + p2_world = base.extractCartesianPose(); + else + p2_world = base.calcCartesianPose(base.extractJointPosition()); + + tesseract_common::VectorIsometry3d poses = interpolate(p1_world, p2_world, linear_steps); + for (auto& pose : poses) + pose = base.working_frame_transform.inverse() * pose; + + assert(poses.size() == states.cols()); + return getInterpolatedInstructions(poses, base.manip->getJointNames(), states, base.instruction); + } + + return getInterpolatedInstructions(base.manip->getJointNames(), states, base.instruction); +} + +} // namespace tesseract_planning 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 8dac069990..bcae7ec800 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 @@ -2,12 +2,12 @@ * @file simple_planner_fixed_size_assign_plan_profile.cpp * @brief * - * @author Matthew Powelson - * @date July 23, 2020 + * @author Roelof Oomen + * @date May 29, 2024 * @version TODO * @bug No known bugs * - * @copyright Copyright (c) 2020, Southwest Research Institute + * @copyright Copyright (c) 2024, ROS Industrial Consortium * * @par License * Software License Agreement (Apache License) @@ -52,18 +52,18 @@ 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 prev(prev_instruction, request, global_manip_info); + KinematicGroupInstructionInfo base(base_instruction, request, global_manip_info); Eigen::VectorXd j2; - if (!info2.has_cartesian_waypoint) + if (!base.has_cartesian_waypoint) { - j2 = info2.extractJointPosition(); + j2 = base.extractJointPosition(); } else { // Determine base_instruction joint position and replicate - const auto& base_cwp = info2.instruction.getWaypoint().as(); + const auto& base_cwp = base.instruction.getWaypoint().as(); if (base_cwp.hasSeed()) { // Use joint position of cartesian base_instruction @@ -71,34 +71,34 @@ SimplePlannerFixedSizeAssignPlanProfile::generate(const MoveInstructionPoly& pre } else { - if (info1.has_cartesian_waypoint) + if (prev.has_cartesian_waypoint) { - const auto& prev_cwp = info1.instruction.getWaypoint().as(); + const auto& prev_cwp = prev.instruction.getWaypoint().as(); if (prev_cwp.hasSeed()) { // Use joint position of cartesian prev_instruction as seed - j2 = getClosestJointSolution(info2, prev_cwp.getSeed().position); + j2 = getClosestJointSolution(base, prev_cwp.getSeed().position); } else { // Use current env_state as seed - j2 = getClosestJointSolution(info2, request.env_state.getJointValues(info2.manip->getJointNames())); + j2 = getClosestJointSolution(base, request.env_state.getJointValues(base.manip->getJointNames())); } } else { // Use prev_instruction as seed - j2 = getClosestJointSolution(info2, info1.extractJointPosition()); + j2 = getClosestJointSolution(base, prev.extractJointPosition()); } } - tesseract_common::enforcePositionLimits(j2, info2.manip->getLimits().joint_limits); + tesseract_common::enforceLimits(j2, base.manip->getLimits().joint_limits); } Eigen::MatrixXd states; // Replicate base_instruction joint position - if (info2.instruction.isLinear()) + if (base.instruction.isLinear()) states = j2.replicate(1, linear_steps + 1); - else if (info2.instruction.isFreespace()) + else if (base.instruction.isFreespace()) states = j2.replicate(1, freespace_steps + 1); else throw std::runtime_error("stateJointJointWaypointFixedSize: Unsupported MoveInstructionType!"); @@ -107,26 +107,26 @@ SimplePlannerFixedSizeAssignPlanProfile::generate(const MoveInstructionPoly& pre if (base_instruction.isLinear()) { Eigen::Isometry3d p1_world; - if (info1.has_cartesian_waypoint) - p1_world = info1.extractCartesianPose(); + if (prev.has_cartesian_waypoint) + p1_world = prev.extractCartesianPose(); else - p1_world = info1.calcCartesianPose(info1.extractJointPosition()); + p1_world = prev.calcCartesianPose(prev.extractJointPosition()); Eigen::Isometry3d p2_world; - if (info2.has_cartesian_waypoint) - p2_world = info2.extractCartesianPose(); + if (base.has_cartesian_waypoint) + p2_world = base.extractCartesianPose(); else - p2_world = info2.calcCartesianPose(info2.extractJointPosition()); + p2_world = base.calcCartesianPose(base.extractJointPosition()); tesseract_common::VectorIsometry3d poses = interpolate(p1_world, p2_world, linear_steps); for (auto& pose : poses) - pose = info2.working_frame_transform.inverse() * pose; + pose = base.working_frame_transform.inverse() * pose; assert(poses.size() == states.cols()); - return getInterpolatedInstructions(poses, info2.manip->getJointNames(), states, info2.instruction); + return getInterpolatedInstructions(poses, base.manip->getJointNames(), states, base.instruction); } - return getInterpolatedInstructions(info2.manip->getJointNames(), states, info2.instruction); + return getInterpolatedInstructions(base.manip->getJointNames(), states, base.instruction); } } // namespace tesseract_planning