Revision history [back]

I don't know if you've resolved this or not, but I just came across the question. You just need to do some TF manipulation with the appropriate frames. Here's one way to do it; I'm leaving the following in long form for clarity...sorry about the variable names, but I think they're distinguishable enough (and I think I kept 'em consistent).

Let's say you want to rotate left_hand in the map frame:

// set up, note that roll, pitch, and yaw are doubles you captured
// you may need to manipulate the values to make sense in radians
tf::TransformListener tf_;
tf::Quaternion q_6d = tf::createQuaternionFromRPY(roll, pitch, yaw);
tf::Stamped<tf::quaternion> qs_6d(q_6d, ros::Time(0), "/left_hand");

// get the transform from source (map) to target (hand) frame
tf::StampedTransform st_hand;
try {
    tf->lookupTransform("/left_hand", "/map", ros::Time(0), st_hand);
} catch (tf::TransformException &e) {
    ROS_ERROR("shift orientation frame: [%s]", e.what());
    return;
}
// apply your 6d rotation to target (hand) orientation
tf::Quaternion q_rot = qs_6d * st_hand.getRotation();
tf::Stamped<tf::quaternion> qs_rot(q_rot, ros::Time(0), "/left_hand");

// put rotated target (hand) quaternion back in source (map) frame
tf::Stamped<tf::quaternion> sq_map;
try {
    tf->transformQuaternion("/map", qs_rot, sq_map);
} catch (tf::TransformException &e) {
    ROS_ERROR("shift orientation quat: [%s]", e.what());
    return;
}
// convert to quaternion stamped message, can put that in pose...
geometry_msgs::QuaternionStamped qs_msg;
tf::quaternionStampedTFToMsg(sq_map, qs_msg);