How can I get the current end effector pose without moving using end effector pose?

[rlgraf]

Hello,

I’m wondering if there is a function which retrieves and stores the 4 orientation and 3 position quaternian end effecter pose values where the robot currently is. I would like to create a program which has the following layout:

code which moves the robot to a joint group position

Then, at the new joint group position, do the following:
geometry_msgs::msg::Pose target_pose1;
line of code which pulls and stores the end effecter position and orientation into target_pose1 (what I need help with)

std::vector<geometry_msgs::msg::Pose> waypoints;
target_pose1.position.x += 0.1;
waypoints.push_back(target_pose1);
target_pose1.position.y += 0.1;
waypoints.push_back(target_pose1);
moveit_msgs::msg::RobotTrajectory trajectory;
…

In other words, I would like to start using waypoints with the current end effector pose WITHOUT having to first manually set end effector poses. I want to move using joint group positions, then get values for the current end effector pose, and then move again using waypoints where I can simply add or subtract from the current end effector pose. Can anyone help with this?

Thank you!

[girishkumar.kannan]

Hi @rlgraf ,

You can get the arm’s current pose and the end effector’s current pose using the getCurrentPose() function available from the MoveGroupInterface object.

Refer below to know how to use it:

// initialize move_group node
rclcpp::Node::SharedPtr move_group_node =
  rclcpp::Node::make_shared("move_group_interface_tutorial", node_options);

// initialize move_group_interface for end effector
moveit::planning_interface::MoveGroupInterface move_group_endeffector(
  move_group_node, PLANNING_GROUP_ENDEFFECTOR);

// get the current pose of end effector
current_pose_endeffector = move_group_endeffector.getCurrentPose().pose;

// print the pose information
RCLCPP_INFO(
  LOGGER,
  "\n Current Pose End Effector: "
  "Px:%+0.4f Py:%+0.4f Pz:%+0.4f "
  "Qx:%+0.4f Qy:%+0.4f Qz:%+0.4f Qw:%+0.4f",
  current_pose_endeffector.position.x,
  current_pose_endeffector.position.y,
  current_pose_endeffector.position.z,
  current_pose_endeffector.orientation.x,
  current_pose_endeffector.orientation.y,
  current_pose_endeffector.orientation.z,
  current_pose_endeffector.orientation.w);

Also, I think you are implementing Cartesian waypoints incorrectly.

@rlgraf

std::vector<geometry_msgs::msg::Pose> waypoints;
target_pose1.position.x += 0.1;
waypoints.push_back(target_pose1);
target_pose1.position.y += 0.1;
waypoints.push_back(target_pose1);
moveit_msgs::msg::RobotTrajectory trajectory;

Instead of doing += 0.1 and push_back() twice, you can do +=0.2 and push_back() once and set step to 0.1 (with jump set to 0.0) in Cartesian planning variables.

@rlgraf

In other words, I would like to start using waypoints with the current end effector pose WITHOUT having to first manually set end effector poses.

In that case you need set get the current state of the move group and set the current state as tehe starting state.

@rlgraf

I want to move using joint group positions, then get values for the current end effector pose, and then move again using waypoints where I can simply add or subtract from the current end effector pose. Can anyone help with this?

This whole task is actually explained in the ROS2 Manipulation Basics course. Have you completed the course fully? Are you currently working on the final project?
I suggest that you give the MoveIt2 documentation(s) a look!

Regards,
Girish

[rlgraf]

Girish,

Thank you so much for your help! This seems to have worked,

Russell

[girishkumar.kannan]

Hi @rlgraf ,

I am glad to know that you found my solution useful!

Please mark the post as solution so this issue can be closed.

Regards,
Girish