Add Pilz unit tests, defining goal poses w.r.t. frame_id != planning …

…frame (moveit#3525) Co-authored-by: Captain Yoshi <[email protected]>
ubi-agni · Nov 13, 2023 · 3acca66 · 3acca66
1 parent 979ef6a
commit 3acca66
Show file tree

Hide file tree

Showing 2 changed files with 74 additions and 10 deletions.
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp
@@ -100,14 +100,16 @@ bool testutils::getExpectedGoalPose(const moveit::core::RobotModelConstPtr& robo
   // ++++++++++++++++++++++++++++++++++++++
   // + Get goal from cartesian constraint +
   // ++++++++++++++++++++++++++++++++++++++
-  // TODO frame id
+  moveit::core::RobotState rstate(robot_model);
+  moveit::core::robotStateMsgToRobotState(moveit::core::Transforms(robot_model->getModelFrame()), req.start_state,
+                                          rstate);
+  rstate.update();
+
   link_name = req.goal_constraints.front().position_constraints.front().link_name;
-  geometry_msgs::Pose goal_pose_msg;
-  goal_pose_msg.position =
-      req.goal_constraints.front().position_constraints.front().constraint_region.primitive_poses.front().position;
-  goal_pose_msg.orientation = req.goal_constraints.front().orientation_constraints.front().orientation;
-  normalizeQuaternion(goal_pose_msg.orientation);
-  tf2::fromMsg(goal_pose_msg, goal_pose_expect);
+  goal_pose_expect =
+      rstate.getFrameTransform(req.goal_constraints.front().position_constraints.front().header.frame_id) *
+      getPose(req.goal_constraints.front());
+
   return true;
 }
 

diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_circ.cpp
@@ -187,15 +187,25 @@ void TrajectoryGeneratorCIRCTest::getCircCenter(const planning_interface::Motion
                                                 const planning_interface::MotionPlanResponse& res,
                                                 Eigen::Vector3d& circ_center)
 {
+  moveit::core::RobotState rstate(robot_model_);
+  moveit::core::robotStateMsgToRobotState(moveit::core::Transforms(robot_model_->getModelFrame()), req.start_state,
+                                          rstate);
+  rstate.update();
+
   if (req.path_constraints.name == "center")
   {
-    tf2::fromMsg(req.path_constraints.position_constraints.at(0).constraint_region.primitive_poses.at(0).position,
-                 circ_center);
+    Eigen::Isometry3d center_pose;
+    tf2::fromMsg(req.path_constraints.position_constraints.front().constraint_region.primitive_poses.front(),
+                 center_pose);
+
+    circ_center =
+        (rstate.getFrameTransform(req.path_constraints.position_constraints.front().header.frame_id) * center_pose)
+            .translation();
   }
   else if (req.path_constraints.name == "interim")
   {
     Eigen::Vector3d interim;
-    tf2::fromMsg(req.path_constraints.position_constraints.at(0).constraint_region.primitive_poses.at(0).position,
+    tf2::fromMsg(req.path_constraints.position_constraints.front().constraint_region.primitive_poses.at(0).position,
                  interim);
     Eigen::Vector3d start = res.trajectory_->getFirstWayPointPtr()->getFrameTransform(target_link_).translation();
     Eigen::Vector3d goal = res.trajectory_->getLastWayPointPtr()->getFrameTransform(target_link_).translation();
@@ -699,6 +709,58 @@ TEST_P(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdBothConstraints)
   checkCircResult(req, res);
 }
 
+/**
+ * @brief Set different frame_id's on both goal pose and path constraint
+ */
+TEST_P(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdPoseAndPathConstraints)
+{
+  auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") };
+
+  moveit_msgs::MotionPlanRequest req = circ.toRequest();
+
+  // set to start state
+  auto rstate = planning_scene_->getCurrentState();
+  moveit::core::robotStateMsgToRobotState(planning_scene_->getTransforms(), req.start_state, rstate);
+  rstate.update();
+
+  // compute offset between links
+  auto pose_tcp = rstate.getFrameTransform("prbt_tcp");
+  auto pose_link5 = rstate.getFrameTransform("prbt_link_5");
+
+  double offset_link5_to_tcp = (pose_link5.inverse() * pose_tcp).translation()[1];
+  double goal_offset_z = 0.3;
+
+  // Set goal constraint: apply Z axis translation only
+  auto& pc = req.goal_constraints.front().position_constraints.front();
+  pc.header.frame_id = "prbt_tcp";
+  pc.constraint_region.primitive_poses.front().position.x = 0.0;
+  pc.constraint_region.primitive_poses.front().position.y = 0.0;
+  pc.constraint_region.primitive_poses.front().position.z = goal_offset_z;
+
+  auto& oc = req.goal_constraints.front().orientation_constraints.front();
+  oc.header.frame_id = "prbt_tcp";
+  oc.orientation.x = 0;
+  oc.orientation.y = 0;
+  oc.orientation.z = 0;
+  oc.orientation.w = 1;
+
+  // Set path constraint:
+  //   `prbt_tcp` Z axis is aligned with `prbt_link_5` Y axis
+  //   `prbt_link_5` Y axis position is at the center of the start and end goal wrt. the `prbt_tcp` link
+  //   `prbt_link_5` X and Z axis positions can be of any value, will not affect the center between start and goal
+  auto& path_pc = req.path_constraints.position_constraints.front();
+  path_pc.header.frame_id = "prbt_link_5";
+  path_pc.constraint_region.primitive_poses.front().position.x = 0.0;
+  path_pc.constraint_region.primitive_poses.front().position.y = goal_offset_z / 2 + offset_link5_to_tcp;
+  path_pc.constraint_region.primitive_poses.front().position.z = -0.1;
+
+  planning_interface::MotionPlanResponse res;
+  ASSERT_TRUE(circ_->generate(planning_scene_, req, res));
+  EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS);
+
+  checkCircResult(req, res);
+}
+
 /**
  * @brief test the circ planner with interim point with joint goal
  */