lwr_impedance_controller::CartImpTrajectoryPoint sampleInterpolation() {
    lwr_impedance_controller::CartImpTrajectoryPoint next_point;

    double timeFromStart =
        (double) (now() - trajectory_.header.stamp).toSec();
    double segStartTime = last_point_.time_from_start.toSec();
    double segEndTime =
        trajectory_.trajectory[trajectory_index_].time_from_start.toSec();

    next_point = setpoint_;

    // interpolate position
    // x
    next_point.pose.position.x = linearlyInterpolate(timeFromStart,
        segStartTime, segEndTime, last_point_.pose.position.x,
        trajectory_.trajectory[trajectory_index_].pose.position.x);
    // y
    next_point.pose.position.y = linearlyInterpolate(timeFromStart,
        segStartTime, segEndTime, last_point_.pose.position.y,
        trajectory_.trajectory[trajectory_index_].pose.position.y);
    // z
    next_point.pose.position.z = linearlyInterpolate(timeFromStart,
        segStartTime, segEndTime, last_point_.pose.position.z,
        trajectory_.trajectory[trajectory_index_].pose.position.z);

    // interpolate orientation

    Eigen::Quaternion<double> start = Eigen::Quaternion<double>(last_point_.pose.orientation.w,
        last_point_.pose.orientation.x, last_point_.pose.orientation.y,
        last_point_.pose.orientation.z);
    Eigen::Quaternion<double> end = Eigen::Quaternion<double>(
        trajectory_.trajectory[trajectory_index_].pose.orientation.w,
        trajectory_.trajectory[trajectory_index_].pose.orientation.x,
        trajectory_.trajectory[trajectory_index_].pose.orientation.y,
        trajectory_.trajectory[trajectory_index_].pose.orientation.z);

    double t = linearlyInterpolate(timeFromStart, segStartTime, segEndTime, 0,
        1);

    Eigen::Quaternion<double> rot = start.slerp(t, end);

    next_point.pose.orientation.x = rot.x();
    next_point.pose.orientation.y = rot.y();
    next_point.pose.orientation.z = rot.z();
    next_point.pose.orientation.w = rot.w();


    /*
     // x
     next_point.pose.orientation.x = linearlyInterpolate(timeFromStart,
     segStartTime, segEndTime, last_point_.pose.orientation.x,
     trajectory_.trajectory[trajectory_index_].pose.orientation.x);
     // y
     next_point.pose.orientation.y = linearlyInterpolate(timeFromStart,
     segStartTime, segEndTime, last_point_.pose.orientation.y,
     trajectory_.trajectory[trajectory_index_].pose.orientation.y);
     // z
     next_point.pose.orientation.z = linearlyInterpolate(timeFromStart,
     segStartTime, segEndTime, last_point_.pose.orientation.z,
     trajectory_.trajectory[trajectory_index_].pose.orientation.z);
     // w
     next_point.pose.orientation.w = linearlyInterpolate(timeFromStart,
     segStartTime, segEndTime, last_point_.pose.orientation.w,
     trajectory_.trajectory[trajectory_index_].pose.orientation.w);
     */
    //interpolate stiffness
    // x
    next_point.impedance.stiffness.force.x = linearlyInterpolate(timeFromStart,
        segStartTime, segEndTime, last_point_.impedance.stiffness.force.x,
        trajectory_.trajectory[trajectory_index_].impedance.stiffness.force.x);

    next_point.impedance.stiffness.force.y = linearlyInterpolate(timeFromStart,
        segStartTime, segEndTime, last_point_.impedance.stiffness.force.y,
        trajectory_.trajectory[trajectory_index_].impedance.stiffness.force.y);

    next_point.impedance.stiffness.force.z = linearlyInterpolate(timeFromStart,
        segStartTime, segEndTime, last_point_.impedance.stiffness.force.z,
        trajectory_.trajectory[trajectory_index_].impedance.stiffness.force.z);

    next_point.impedance.stiffness.torque.x = linearlyInterpolate(timeFromStart,
        segStartTime, segEndTime, last_point_.impedance.stiffness.torque.x,
        trajectory_.trajectory[trajectory_index_].impedance.stiffness.torque.x);

    next_point.impedance.stiffness.torque.y = linearlyInterpolate(timeFromStart,
        segStartTime, segEndTime, last_point_.impedance.stiffness.torque.y,
        trajectory_.trajectory[trajectory_index_].impedance.stiffness.torque.y);

    next_point.impedance.stiffness.torque.z = linearlyInterpolate(timeFromStart,
        segStartTime, segEndTime, last_point_.impedance.stiffness.torque.z,
        trajectory_.trajectory[trajectory_index_].impedance.stiffness.torque.z);

    next_point.impedance.damping =
        trajectory_.trajectory[trajectory_index_].impedance.damping;
    next_point.wrench = trajectory_.trajectory[trajectory_index_].wrench;

    return next_point;
  }