From 7d15fd1341de5edae680432d654aec89ec190617 Mon Sep 17 00:00:00 2001 From: Tom Noble Date: Fri, 8 Nov 2024 18:11:36 +0000 Subject: [PATCH] Enhancement/ports moveit 3522 (#3070) * Makes PTP changes * Makes LIN changes * Makes CIRC changes * Makes CIRC changes * Makes CIRC changes * Makes CIRC changes * Makes LIN changes * Makes LIN changes * Fixes formatting * Replace ROS_WARN with RCLCPP_WARN * Fixes formatting * Fixes RCLCPP_WARN calls * Fixes compiler errors * Fixes if statement * Fixes formatting * Fixes msg type (cherry picked from commit 4c28a74343e1d8470689cae9e23eb30c91bdcb4d) # Conflicts: # moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp # moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp --- .../src/trajectory_generator_circ.cpp | 67 +++++++++++++------ .../src/trajectory_generator_lin.cpp | 27 +++++++- .../src/trajectory_generator_ptp.cpp | 29 ++++++-- 3 files changed, 97 insertions(+), 26 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp index 512497380f..667a36b078 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp @@ -91,7 +91,11 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni RCLCPP_DEBUG(LOGGER, "Extract necessary information from motion plan request."); info.group_name = req.group_name; +<<<<<<< HEAD std::string frame_id{ robot_model_->getModelFrame() }; +======= + moveit::core::RobotState robot_state = scene->getCurrentState(); +>>>>>>> 4c28a7434 (Enhancement/ports moveit 3522 (#3070)) // goal given in joint space if (!req.goal_constraints.front().joint_constraints.empty()) @@ -123,6 +127,7 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni // goal given in Cartesian space else { + std::string frame_id; info.link_name = req.goal_constraints.front().position_constraints.front().link_name; if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() || req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty()) @@ -135,7 +140,23 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni { frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id; } - info.goal_pose = getConstraintPose(req.goal_constraints.front()); + + // goal pose with optional offset wrt. the planning frame + info.goal_pose = scene->getFrameTransform(frame_id) * getConstraintPose(req.goal_constraints.front()); + frame_id = robot_model_->getModelFrame(); + + // check goal pose ik before Cartesian motion plan starts + std::map ik_solution; + if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position, + ik_solution)) + { + // LCOV_EXCL_START + std::ostringstream os; + os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose"; + throw CircInverseForGoalIncalculable(os.str()); + // LCOV_EXCL_STOP // not able to trigger here since lots of checks before + // are in place + } } assert(req.start_state.joint_state.name.size() == req.start_state.joint_state.position.size()); @@ -155,34 +176,38 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni computeLinkFK(scene, info.link_name, info.start_joint_position, info.start_pose); - // check goal pose ik before Cartesian motion plan starts - std::map ik_solution; - if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position, - ik_solution)) + // center point with wrt. the planning frame + std::string center_point_frame_id; + + info.circ_path_point.first = req.path_constraints.name; + if (req.path_constraints.position_constraints.front().header.frame_id.empty()) { - // LCOV_EXCL_START - std::ostringstream os; - os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose"; - throw CircInverseForGoalIncalculable(os.str()); - // LCOV_EXCL_STOP // not able to trigger here since lots of checks before - // are in place + RCLCPP_WARN(getLogger(), "Frame id is not set in position constraints of " + "path. Use model frame as default"); + center_point_frame_id = robot_model_->getModelFrame(); } - info.circ_path_point.first = req.path_constraints.name; + else + { + center_point_frame_id = req.path_constraints.position_constraints.front().header.frame_id; + } + + Eigen::Isometry3d center_point_pose; + tf2::fromMsg(req.path_constraints.position_constraints.front().constraint_region.primitive_poses.front(), + center_point_pose); + + center_point_pose = scene->getFrameTransform(center_point_frame_id) * center_point_pose; + if (!req.goal_constraints.front().position_constraints.empty()) { const moveit_msgs::msg::Constraints& goal = req.goal_constraints.front(); - info.circ_path_point.second = - getConstraintPose( - req.path_constraints.position_constraints.front().constraint_region.primitive_poses.front().position, - goal.orientation_constraints.front().orientation, goal.position_constraints.front().target_point_offset) - .translation(); + geometry_msgs::msg::Point center_point = tf2::toMsg(Eigen::Vector3d(center_point_pose.translation())); + info.circ_path_point.second = getConstraintPose(center_point, goal.orientation_constraints.front().orientation, + goal.position_constraints.front().target_point_offset) + .translation(); } else { - Eigen::Vector3d circ_path_point; - tf2::fromMsg(req.path_constraints.position_constraints.front().constraint_region.primitive_poses.front().position, - circ_path_point); - info.circ_path_point.second = circ_path_point; + info.circ_path_point.second = center_point_pose.translation(); } } diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp index 8dbed6dfa2..840f03bcc7 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp @@ -68,7 +68,11 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin RCLCPP_DEBUG(LOGGER, "Extract necessary information from motion plan request."); info.group_name = req.group_name; +<<<<<<< HEAD std::string frame_id{ robot_model_->getModelFrame() }; +======= + moveit::core::RobotState robot_state = scene->getCurrentState(); +>>>>>>> 4c28a7434 (Enhancement/ports moveit 3522 (#3070)) // goal given in joint space if (!req.goal_constraints.front().joint_constraints.empty()) @@ -98,6 +102,8 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin // goal given in Cartesian space else { + std::string frame_id; + info.link_name = req.goal_constraints.front().position_constraints.front().link_name; if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() || req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty()) @@ -110,9 +116,23 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin { frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id; } - info.goal_pose = getConstraintPose(req.goal_constraints.front()); + + // goal pose with optional offset wrt. the planning frame + info.goal_pose = scene->getFrameTransform(frame_id) * getConstraintPose(req.goal_constraints.front()); + frame_id = robot_model_->getModelFrame(); + + // check goal pose ik before Cartesian motion plan starts + std::map ik_solution; + if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position, + ik_solution)) + { + std::ostringstream os; + os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose"; + throw LinInverseForGoalIncalculable(os.str()); + } } +<<<<<<< HEAD assert(req.start_state.joint_state.name.size() == req.start_state.joint_state.position.size()); for (const auto& joint_name : robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames()) { @@ -141,6 +161,11 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose"; throw LinInverseForGoalIncalculable(os.str()); } +======= + // Ignored return value because at this point the function should always + // return 'true'. + computeLinkFK(robot_state, info.link_name, info.start_joint_position, info.start_pose); +>>>>>>> 4c28a7434 (Enhancement/ports moveit 3522 (#3070)) } void TrajectoryGeneratorLIN::plan(const planning_scene::PlanningSceneConstPtr& scene, diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp index bb665bca53..154535cb57 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp @@ -234,11 +234,32 @@ void TrajectoryGeneratorPTP::extractMotionPlanInfo(const planning_scene::Plannin // solve the ik else { - Eigen::Isometry3d goal_pose = getConstraintPose(req.goal_constraints.front()); - if (!computePoseIK(scene, req.group_name, req.goal_constraints.at(0).position_constraints.at(0).link_name, - goal_pose, robot_model_->getModelFrame(), info.start_joint_position, info.goal_joint_position)) + std::string frame_id; + + info.link_name = req.goal_constraints.front().position_constraints.front().link_name; + if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() || + req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty()) + { + RCLCPP_WARN(getLogger(), "Frame id is not set in position/orientation constraints of " + "goal. Use model frame as default"); + frame_id = robot_model_->getModelFrame(); + } + else + { + frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id; + } + + // goal pose with optional offset wrt. the planning frame + info.goal_pose = scene->getFrameTransform(frame_id) * getConstraintPose(req.goal_constraints.front()); + frame_id = robot_model_->getModelFrame(); + + // check goal pose ik before Cartesian motion plan start + if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position, + info.goal_joint_position)) { - throw PtpNoIkSolutionForGoalPose("No IK solution for goal pose"); + std::ostringstream os; + os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose"; + throw PtpNoIkSolutionForGoalPose(os.str()); } } }