twist_transform.cpp

// The parameter `angular_velocity_about_ee_frame` is used to select between the old behaviour and the proper behaviour.

const TwistCommand Servo::toPlanningFrame(const TwistCommand& command)
{

  Eigen::Vector<double, 6> transformed_twist = command.velocities;

  if (command.frame_id != servo_params_.planning_frame)
  {
    Eigen::Vector<double, 6> reordered_twist;
    // (linear, angular) -> (angular, linear)
    reordered_twist.head<3>() = command.velocities.tail<3>();
    reordered_twist.tail<3>() = command.velocities.head<3>();    

    const auto command_to_planning_frame =
        tf2::transformToEigen(
          transform_buffer_.lookupTransform(servo_params_.planning_frame, command.frame_id, rclcpp::Time(0)));
    
    if(servo_params_.angular_velocity_about_ee_frame)
    {
      reordered_twist.head<3>() = command_to_planning_frame.rotation() * reordered_twist.head<3>();
      reordered_twist.tail<3>() = command_to_planning_frame.rotation() * reordered_twist.tail<3>();
    }
    else
    {
      Eigen::MatrixXd adjoint(6, 6);
      const Eigen::Matrix3d& rotation = command_to_planning_frame.rotation();
      const Eigen::Vector3d& translation = command_to_planning_frame.translation();

      Eigen::Matrix3d skew_translation;
      skew_translation.row(0) << 0, -translation(2), translation(1);
      skew_translation.row(1) << translation(2), 0, -translation(0);
      skew_translation.row(2) << -translation(1), translation(0), 0;

      adjoint.topLeftCorner(3, 3) = rotation;
      adjoint.topRightCorner(3, 3).setZero();
      adjoint.bottomLeftCorner(3, 3) = skew_translation * rotation;
      adjoint.bottomRightCorner(3, 3) = rotation;

      reordered_twist = adjoint * reordered_twist;
    }

    // (angular, linear) -> (linear, angular)
    transformed_twist.head<3>() = reordered_twist.tail<3>();
    transformed_twist.tail<3>() = reordered_twist.head<3>();
  }

  return TwistCommand{ servo_params_.planning_frame, transformed_twist };
}