Ask Your Question
1

Interpolation for between two poses

asked 2018-08-05 19:08:29 -0500

Fabs89 gravatar image

Hey all,

I am looking for a function which interpolate between two poseStamped poses for a given time. I know there has to be something withing the tf/tf2 API but I could not find it. Something along the line of

poseStamped = poseInterpolation (poseStamp_t1,poseStamp_t2, t) where t_1 < t < t_2

Would be glad for some pointers.

Cheers

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
1

answered 2020-04-19 20:37:31 -0500

oleg.alexandrov gravatar image

updated 2020-04-19 20:38:36 -0500

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;
}
edit flag offensive delete link more
1

answered 2018-08-05 23:09:54 -0500

tfoote gravatar image

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.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2018-08-05 19:08:29 -0500

Seen: 1,668 times

Last updated: Apr 19 '20