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 cd4e6b1de5..d3a145ca8f 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
@@ -95,7 +95,6 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni
RCLCPP_DEBUG(LOGGER, "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
@@ -128,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())
@@ -140,39 +140,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<std::string, double> 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<std::string, double> 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(LOGGER, "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 bef1f5132f..0caf4b6327 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
@@ -72,7 +72,6 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin
RCLCPP_DEBUG(LOGGER, "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
@@ -101,6 +100,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())
@@ -113,20 +114,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<std::string, double> 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<std::string, double> 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 c1a0e62b8a..fe6b096c5d 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
@@ -227,11 +227,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(LOGGER, "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());
}
}
}