The PR resolves a small bug in the DMP execution. The problem was that the initial position and the goal were both taken with respect to the base link frame, but during the execution, the current pose of the end effector was taken with respect to the odometry frame. As a result, the arm motion would drift over time.
To resolve this, both the initial pose and the goal pose are now transformed to the odometry frame before execution starts, thereby ensuring that all poses are in the same frame.
Tests to verify the changes
The changes have been verified on our HSR and seem to resolve the issue.
Short description
The PR resolves a small bug in the DMP execution. The problem was that the initial position and the goal were both taken with respect to the base link frame, but during the execution, the current pose of the end effector was taken with respect to the odometry frame. As a result, the arm motion would drift over time.
To resolve this, both the initial pose and the goal pose are now transformed to the odometry frame before execution starts, thereby ensuring that all poses are in the same frame.
Tests to verify the changes
The changes have been verified on our HSR and seem to resolve the issue.