From 4c28a74343e1d8470689cae9e23eb30c91bdcb4d 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 --- .../src/trajectory_generator_circ.cpp | 64 ++++++++++++------- .../src/trajectory_generator_lin.cpp | 30 +++++---- .../src/trajectory_generator_ptp.cpp | 29 +++++++-- 3 files changed, 85 insertions(+), 38 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 ae1e7b9b10..e0ef8ce126 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 @@ -97,7 +97,6 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni RCLCPP_DEBUG(getLogger(), "Extract necessary information from motion plan request."); info.group_name = req.group_name; - std::string frame_id{ robot_model_->getModelFrame() }; moveit::core::RobotState robot_state = scene->getCurrentState(); // goal given in joint space @@ -130,6 +129,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()) @@ -142,39 +142,59 @@ 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 + } } computeLinkFK(robot_state, 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 43e5235e0c..941290b90e 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 @@ -74,7 +74,6 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin RCLCPP_DEBUG(getLogger(), "Extract necessary information from motion plan request."); info.group_name = req.group_name; - std::string frame_id{ robot_model_->getModelFrame() }; moveit::core::RobotState robot_state = scene->getCurrentState(); // goal given in joint space @@ -103,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()) @@ -115,20 +116,25 @@ 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()); - } - computeLinkFK(robot_state, info.link_name, info.start_joint_position, info.start_pose); + // 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()); + // 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()); + } } + + // 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); } 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 b12f64224c..f7c45ac29a 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 @@ -233,11 +233,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()); } } }