Enhancement/ports moveit 3522

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<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(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();
   }
 }
 

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<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,

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());
     }
   }
 }