Move_group get/setPoseReferenceFrame() broken / ignored / misdocumented

[normanhendrich]

Hello,

I am trying to teach our service robot some new tricks. I have a valid URDF and SRDF, and move_group works on the real robot and in simulation/Gazebo. So far, so good.

Our robot also has a tray fixed to the base platform, and I would like to execute pick&place operations on the tray, meaning that the motions are relative to /base_link or /tray_link of the robot.

Unfortunately, get/setPoseReferenceFrame() in move_group are no help in this. The methods are kind of broken or misleading, according to viewpoint. From my experimentation so far:

1. move_group.setPoseTarget() followed by move_group.plan() does respect the user-specified reference frame previously set via setPoseReferenceFrame().
2. move_group.getCurrentPose() IGNORES the reference frame, and insists on /map as the reference frame. This is bad. I also dislike the extra error and delay inherent to go via /map when I am doing things on the robot itself.
3. computeCartesianPath() seems to ignore the reference frame also.
4. I know that I can ask tf for the transformations and get the poses I need, but that still incurs the /map -> odom/amcl -> base_link errors due to localization.
5. I realize that collision detection and avoidance may need access to the /map frame to check against environment obstacles. But it still would be more convenient to specify and analyse motions in the base_link (or tray_link) frames in some cases.

So, is there any "official" way to get move_group to do things relative to a user-specified frame?
Maybe I can somehow get access to the RobotModel and switch between /map and /base_link there?

...
std::string tcp_frame = "doro/jaco_link_6";
groupPtr->setPoseReferenceFrame( "doro/base_link" );
geometry_msgs::PoseStamped curr = groupPtr->getCurrentPose ( tcp_frame );
ROS_INFO( "moveTo: initial pose is (%6.2f %6.2f %6.2f) (%6.2f %6.2f %6.2f %6.2f) frame '%s'",
curr.pose.position.x, curr.pose.position.y, curr.pose.position.z,
curr.pose.orientation.x, curr.pose.orientation.y, curr.pose.orientation.z, curr.pose.orientation.w,
curr.header.frame_id.c_str() );
...

prints:
moveTo: reached pose is ( -0.36 0.16 0.74) ( 0.27 0.96 -0.01 0.01) frame '/map'

move_group.cpp:
geometry_msgs::PoseStamped moveit::planning_interface::MoveGroup::getCurrentPose(const std::string &end_effector_link)
{
const std::string &eef = end_effector_link.empty() ? getEndEffectorLink() : end_effector_link;
Eigen::Affine3d pose;
pose.setIdentity();
if (eef.empty())
ROS_ERROR("No end-effector specified");
else
{
robot_state::RobotStatePtr current_state;
if (impl_->getCurrentState(current_state))
{
const robot_model::LinkModel *lm = current_state->getLinkModel(eef);
if (lm)
pose = current_state->getGlobalLinkTransform(lm);
}
}
geometry_msgs::PoseStamped pose_msg;
pose_msg.header.stamp = ros::Time::now();
pose_msg.header.frame_id = impl_->getRobotModel()->getModelFrame();
tf::poseEigenToMsg(pose, pose_msg.pose);
return pose_msg;
}