/** Transforms a stamped pose to the given target frame, and returns the result in stampedOut. */
public void transformPose(
String targetFrameID, Stamped<Matrix4d> stampedIn, Stamped<Matrix4d> stampedOut) {
StampedTransform transform =
lookupTransform(targetFrameID, stampedIn.frameID, stampedIn.timeStamp);
transform.transformPose(stampedIn.getData(), stampedOut.getData());
stampedOut.frameID = targetFrameID;
stampedOut.timeStamp = stampedIn.timeStamp;
}
/**
* Transforms a stamped point to the given target frame and time, based on a given fixed frame,
* and returns the result in stampedOut.
*/
public void transformPoint(
String targetFrameID,
Time targetTime,
Stamped<Point3d> stampedIn,
String fixedFrameID,
Stamped<Point3d> stampedOut) {
StampedTransform transform =
lookupTransform(
targetFrameID, targetTime, stampedIn.frameID, stampedIn.timeStamp, fixedFrameID);
transform.transformPoint(stampedIn.getData(), stampedOut.getData());
stampedOut.frameID = targetFrameID;
stampedOut.timeStamp = stampedIn.timeStamp;
}
