From 858f13564f508b6ac97dbbdf5009e5348e9a8159 Mon Sep 17 00:00:00 2001 From: Tom Noble Date: Fri, 8 Nov 2024 08:36:17 +0000 Subject: [PATCH 01/16] Makes PTP changes --- .../src/trajectory_generator_ptp.cpp | 29 ++++++++++++++++--- 1 file changed, 25 insertions(+), 4 deletions(-) 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 ddea11c25a..ae26336383 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 @@ -240,11 +240,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()) + { + ROS_WARN("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()); } } } From e7456ed078b370d544988f3ab9e9300933c3c42d Mon Sep 17 00:00:00 2001 From: Tom Noble Date: Fri, 8 Nov 2024 08:36:31 +0000 Subject: [PATCH 02/16] Makes LIN changes --- .../src/trajectory_generator_lin.cpp | 33 +++++++++++-------- 1 file changed, 19 insertions(+), 14 deletions(-) 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 07b222f915..d7f29114f0 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 @@ -116,7 +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()); + + // 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()); + } + + // Ignored return value because at this point the function should always + // return 'true'. + computeLinkFK(scene, info.link_name, info.start_joint_position, info.start_pose); + } assert(req.start_state.joint_state.name.size() == req.start_state.joint_state.position.size()); @@ -134,19 +152,6 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin info.start_joint_position[joint_name] = req.start_state.joint_state.position[index]; } - // Ignored return value because at this point the function should always - // return 'true'. - 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)) - { - std::ostringstream os; - os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose"; - throw LinInverseForGoalIncalculable(os.str()); - } } void TrajectoryGeneratorLIN::plan(const planning_scene::PlanningSceneConstPtr& scene, From 5969ff4f6406e91321309046ec038771cb4a9fff Mon Sep 17 00:00:00 2001 From: Tom Noble Date: Fri, 8 Nov 2024 08:43:14 +0000 Subject: [PATCH 03/16] Makes CIRC changes --- .../src/trajectory_generator_circ.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 c512e21578..e5d1f9ee71 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() }; // goal given in joint space if (!req.goal_constraints.front().joint_constraints.empty()) @@ -129,6 +128,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()) From c923c87fd245cd43f502914a370641fbf5727507 Mon Sep 17 00:00:00 2001 From: Tom Noble Date: Fri, 8 Nov 2024 08:49:04 +0000 Subject: [PATCH 04/16] Makes CIRC changes --- .../src/trajectory_generator_circ.cpp | 56 ++++++++++++------- 1 file changed, 36 insertions(+), 20 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 e5d1f9ee71..e8e8df14e2 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 @@ -141,7 +141,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()); @@ -161,33 +177,33 @@ 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)) + info.circ_path_point.first = req.path_constraints.name; + if (req.path_constraints.position_constraints.front().header.frame_id.empty()) + ROS_WARN("Frame id is not set in position constraints of " + "path. Use model frame as default"); + center_point_frame_id = robot_model_->getModelFrame(); + } + else { - // 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 + center_point_frame_id = req.path_constraints.position_constraints.front().header.frame_id; } - info.circ_path_point.first = req.path_constraints.name; + + 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::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; } } From 4e97f384b384e102ad28d03cfde0b51418206ba8 Mon Sep 17 00:00:00 2001 From: Tom Noble Date: Fri, 8 Nov 2024 08:50:23 +0000 Subject: [PATCH 05/16] Makes CIRC changes --- .../src/trajectory_generator_circ.cpp | 3 +++ 1 file changed, 3 insertions(+) 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 e8e8df14e2..4c051e969f 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 @@ -177,6 +177,9 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni computeLinkFK(scene, info.link_name, info.start_joint_position, info.start_pose); + // 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()) ROS_WARN("Frame id is not set in position constraints of " From 9bcce43695de09e49b4ecbc1025e3e1b9e899944 Mon Sep 17 00:00:00 2001 From: Tom Noble Date: Fri, 8 Nov 2024 08:51:53 +0000 Subject: [PATCH 06/16] Makes CIRC changes --- .../src/trajectory_generator_circ.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 4c051e969f..74d8d0a08b 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 @@ -207,7 +207,7 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni } else { - info.circ_path_point.second = circ_path_point; + info.circ_path_point.second = center_point_pose.translation(); } } From 0eee76c96f5fe7865b0effd84f5e4090187b878c Mon Sep 17 00:00:00 2001 From: Tom Noble Date: Fri, 8 Nov 2024 08:53:16 +0000 Subject: [PATCH 07/16] Makes LIN changes --- .../src/trajectory_generator_lin.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 d7f29114f0..9aa8b8cfac 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() }; // goal given in joint space if (!req.goal_constraints.front().joint_constraints.empty()) @@ -104,6 +103,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()) From 886bb8e4a3bc68d931587181a82316095fe17431 Mon Sep 17 00:00:00 2001 From: Tom Noble Date: Fri, 8 Nov 2024 08:55:50 +0000 Subject: [PATCH 08/16] Makes LIN changes --- .../src/trajectory_generator_lin.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) 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 9aa8b8cfac..9eeff87b1c 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 @@ -132,10 +132,6 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin throw LinInverseForGoalIncalculable(os.str()); } - // Ignored return value because at this point the function should always - // return 'true'. - computeLinkFK(scene, info.link_name, info.start_joint_position, info.start_pose); - } assert(req.start_state.joint_state.name.size() == req.start_state.joint_state.position.size()); @@ -153,6 +149,9 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin info.start_joint_position[joint_name] = req.start_state.joint_state.position[index]; } + // Ignored return value because at this point the function should always + // return 'true'. + computeLinkFK(scene, info.link_name, info.start_joint_position, info.start_pose); } void TrajectoryGeneratorLIN::plan(const planning_scene::PlanningSceneConstPtr& scene, From 4319b475aaf31786e7ead9bb93197268d6511e1b Mon Sep 17 00:00:00 2001 From: Tom Noble Date: Fri, 8 Nov 2024 09:17:58 +0000 Subject: [PATCH 09/16] Fixes formatting --- .../src/trajectory_generator_circ.cpp | 50 +++++++++---------- .../src/trajectory_generator_lin.cpp | 3 +- 2 files changed, 26 insertions(+), 27 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 74d8d0a08b..fe8e42eb44 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 @@ -145,11 +145,11 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni // 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)) + ik_solution)) { // LCOV_EXCL_START std::ostringstream os; @@ -182,33 +182,33 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni info.circ_path_point.first = req.path_constraints.name; if (req.path_constraints.position_constraints.front().header.frame_id.empty()) - ROS_WARN("Frame id is not set in position constraints of " + ROS_WARN("Frame id is not set in position constraints of " "path. Use model frame as default"); - center_point_frame_id = robot_model_->getModelFrame(); - } - else - { - center_point_frame_id = req.path_constraints.position_constraints.front().header.frame_id; - } + center_point_frame_id = robot_model_->getModelFrame(); +} +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); +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; +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(); - geometry_msgs::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 - { - info.circ_path_point.second = center_point_pose.translation(); - } +if (!req.goal_constraints.front().position_constraints.empty()) +{ + const moveit_msgs::msg::Constraints& goal = req.goal_constraints.front(); + geometry_msgs::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 +{ + info.circ_path_point.second = center_point_pose.translation(); +} } void TrajectoryGeneratorCIRC::plan(const planning_scene::PlanningSceneConstPtr& scene, 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 9eeff87b1c..2ccb7cf9e8 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 @@ -125,13 +125,12 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin // 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)) + ik_solution)) { std::ostringstream os; os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose"; throw LinInverseForGoalIncalculable(os.str()); } - } assert(req.start_state.joint_state.name.size() == req.start_state.joint_state.position.size()); From aabbbe1e739eddc9ba539e67cc4c559a890d993a Mon Sep 17 00:00:00 2001 From: Tom Noble Date: Fri, 8 Nov 2024 10:22:04 +0000 Subject: [PATCH 10/16] Replace ROS_WARN with RCLCPP_WARN --- .../src/trajectory_generator_circ.cpp | 2 +- .../src/trajectory_generator_ptp.cpp | 2 +- 2 files changed, 2 insertions(+), 2 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 80fa9bafc8..5ecd5c0eb3 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 @@ -168,7 +168,7 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni info.circ_path_point.first = req.path_constraints.name; if (req.path_constraints.position_constraints.front().header.frame_id.empty()) - ROS_WARN("Frame id is not set in position constraints of " + RCLCPP_WARN("Frame id is not set in position constraints of " "path. Use model frame as default"); center_point_frame_id = robot_model_->getModelFrame(); } 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 c50eeecf12..7dc48e4deb 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 @@ -239,7 +239,7 @@ void TrajectoryGeneratorPTP::extractMotionPlanInfo(const planning_scene::Plannin if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() || req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty()) { - ROS_WARN("Frame id is not set in position/orientation constraints of " + RCLCPP_WARN("Frame id is not set in position/orientation constraints of " "goal. Use model frame as default"); frame_id = robot_model_->getModelFrame(); } From a15312027ca94aa5d3bf651454405d6fb00cd44a Mon Sep 17 00:00:00 2001 From: Tom Noble Date: Fri, 8 Nov 2024 10:30:37 +0000 Subject: [PATCH 11/16] Fixes formatting --- .../src/trajectory_generator_circ.cpp | 2 +- .../src/trajectory_generator_ptp.cpp | 2 +- 2 files changed, 2 insertions(+), 2 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 5ecd5c0eb3..ef87bb2fc4 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 @@ -169,7 +169,7 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni info.circ_path_point.first = req.path_constraints.name; if (req.path_constraints.position_constraints.front().header.frame_id.empty()) RCLCPP_WARN("Frame id is not set in position constraints of " - "path. Use model frame as default"); + "path. Use model frame as default"); center_point_frame_id = robot_model_->getModelFrame(); } else 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 7dc48e4deb..fef8b142b7 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 @@ -240,7 +240,7 @@ void TrajectoryGeneratorPTP::extractMotionPlanInfo(const planning_scene::Plannin req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty()) { RCLCPP_WARN("Frame id is not set in position/orientation constraints of " - "goal. Use model frame as default"); + "goal. Use model frame as default"); frame_id = robot_model_->getModelFrame(); } else From d2402a6a2347dc9986f17c4a85bca10276aed7da Mon Sep 17 00:00:00 2001 From: Tom Noble Date: Fri, 8 Nov 2024 10:58:22 +0000 Subject: [PATCH 12/16] Fixes RCLCPP_WARN calls --- .../src/trajectory_generator_circ.cpp | 4 ++-- .../src/trajectory_generator_ptp.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 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 ef87bb2fc4..96ea58b26c 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 @@ -168,8 +168,8 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni info.circ_path_point.first = req.path_constraints.name; if (req.path_constraints.position_constraints.front().header.frame_id.empty()) - RCLCPP_WARN("Frame id is not set in position constraints of " - "path. Use model frame as default"); + 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(); } else 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 fef8b142b7..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 @@ -239,8 +239,8 @@ void TrajectoryGeneratorPTP::extractMotionPlanInfo(const planning_scene::Plannin 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("Frame id is not set in position/orientation constraints of " - "goal. Use model frame as default"); + 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 From 12188367ef87b5496fb8a35b1c69c383a5e084da Mon Sep 17 00:00:00 2001 From: Tom Noble Date: Fri, 8 Nov 2024 11:17:30 +0000 Subject: [PATCH 13/16] Fixes compiler errors --- .../src/trajectory_generator_lin.cpp | 17 +---------------- 1 file changed, 1 insertion(+), 16 deletions(-) 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 1d3e094d06..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 @@ -132,24 +132,9 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin } } - 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()) - { - auto it{ std::find(req.start_state.joint_state.name.cbegin(), req.start_state.joint_state.name.cend(), joint_name) }; - if (it == req.start_state.joint_state.name.cend()) - { - std::ostringstream os; - os << "Could not find joint \"" << joint_name << "\" of group \"" << req.group_name - << "\" in start state of request"; - throw LinJointMissingInStartState(os.str()); - } - size_t index = it - req.start_state.joint_state.name.cbegin(); - info.start_joint_position[joint_name] = req.start_state.joint_state.position[index]; - } - // Ignored return value because at this point the function should always // return 'true'. - computeLinkFK(scene, info.link_name, info.start_joint_position, info.start_pose); + computeLinkFK(robot_state, info.link_name, info.start_joint_position, info.start_pose); } void TrajectoryGeneratorLIN::plan(const planning_scene::PlanningSceneConstPtr& scene, From ee08ef9892a91760a64f4801f9e780dbeebbc94c Mon Sep 17 00:00:00 2001 From: Tom Noble Date: Fri, 8 Nov 2024 11:25:30 +0000 Subject: [PATCH 14/16] Fixes if statement --- .../src/trajectory_generator_circ.cpp | 1 + 1 file changed, 1 insertion(+) 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 96ea58b26c..48664fe277 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 @@ -168,6 +168,7 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni info.circ_path_point.first = req.path_constraints.name; if (req.path_constraints.position_constraints.front().header.frame_id.empty()) + { 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(); From 4cd5b8ff5fc314a3de2dd33985cf956546a7e68a Mon Sep 17 00:00:00 2001 From: Tom Noble Date: Fri, 8 Nov 2024 11:28:55 +0000 Subject: [PATCH 15/16] Fixes formatting --- .../src/trajectory_generator_circ.cpp | 44 +++++++++---------- 1 file changed, 22 insertions(+), 22 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 48664fe277..19c6344308 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 @@ -171,31 +171,31 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni { 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(); -} -else -{ - center_point_frame_id = req.path_constraints.position_constraints.front().header.frame_id; -} + center_point_frame_id = robot_model_->getModelFrame(); + } + 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); + 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; + 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(); - geometry_msgs::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 -{ - info.circ_path_point.second = center_point_pose.translation(); -} + if (!req.goal_constraints.front().position_constraints.empty()) + { + const moveit_msgs::msg::Constraints& goal = req.goal_constraints.front(); + geometry_msgs::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 + { + info.circ_path_point.second = center_point_pose.translation(); + } } void TrajectoryGeneratorCIRC::plan(const planning_scene::PlanningSceneConstPtr& scene, From 79d5d8c44f84cce7ec7b197596ce3347cc8199fa Mon Sep 17 00:00:00 2001 From: Tom Noble Date: Fri, 8 Nov 2024 11:48:34 +0000 Subject: [PATCH 16/16] Fixes msg type --- .../src/trajectory_generator_circ.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 19c6344308..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 @@ -187,7 +187,7 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni if (!req.goal_constraints.front().position_constraints.empty()) { const moveit_msgs::msg::Constraints& goal = req.goal_constraints.front(); - geometry_msgs::Point center_point = tf2::toMsg(Eigen::Vector3d(center_point_pose.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();