Interpolation for between two poses

edit

Here is a solution using Eigen.

The stamped poses (poseStamp_t1 and poseStamp_t2 in your notation) have a timestamp and a pose. The timestamps can be saved to variables t1 and t2, such as t1 = poseStamp_t1.header.stamp.toSec().

The poses can be converted to Eigen with the function tf::poseMsgToEigen (const geometry_msgs::Pose &m, Eigen::Affine3d &e), obtaining Eigen:Affine poses aff1 and aff2. From here, the rotation and the translation part can be interpolated separately. Here is how:

// C++ code

#include <Eigen/Geometry>

// Take poses at times t1 and t2. Interpolate the pose at time t.
Eigen::Affine3d pose_interp(double t, double t1, double t2, 
                            Eigen::Affine3d const& aff1, Eigen::Affine3d const& aff2) {
  // assume here t1 <= t <= t2
  double alpha = 0.0;
  if (t2 != t1)
    alpha = (t - t1) / (t2 - t1);

  Eigen::Quaternion<double> rot1(aff1.linear());
  Eigen::Quaternion<double> rot2(aff2.linear());

  Eigen::Vector3d trans1 = aff1.translation();
  Eigen::Vector3d trans2 = aff2.translation();

  Eigen::Affine3d result;
  result.translation() = (1.0 - alpha) * trans1 + alpha * trans2;
  result.linear()      = rot1.slerp(alpha, rot2).toRotationMatrix();

  return result;
}

I'd recommend looking at: https://github.com/ros/geometry2/blob... which uses the internal fork of the Bullet Linear Math library.

I'd recommend you implement your own version using the pure bullet API.