trajectory_interpolator.h

Go to the documentation of this file.

 
/* Based on MoveIt code authored by: Ioan Sucan, Adam Leeper */
 
#ifndef TESSERACT_VISUALIZATION_TRAJECTORY_INTERPOLATOR_H
#define TESSERACT_VISUALIZATION_TRAJECTORY_INTERPOLATOR_H
 
#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <vector>
#include <memory>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
#include <tesseract_common/joint_state.h>
 
namespace tesseract_visualization
{
class TrajectoryInterpolator
{
public:
  using UPtr = std::unique_ptr<TrajectoryInterpolator>;
 
  TrajectoryInterpolator(tesseract_common::JointTrajectory trajectory);
  virtual ~TrajectoryInterpolator() = default;
  TrajectoryInterpolator(const TrajectoryInterpolator&) = delete;
  TrajectoryInterpolator& operator=(const TrajectoryInterpolator&) = delete;
  TrajectoryInterpolator(TrajectoryInterpolator&&) = delete;
  TrajectoryInterpolator& operator=(TrajectoryInterpolator&&) = delete;
 
  tesseract_common::JointState getState(double request_duration) const;
 
  double getStateDuration(long index) const;
 
  long getStateCount() const;
 
  bool empty() const;
 
private:
  tesseract_common::JointTrajectory trajectory_;
  std::vector<double> duration_from_previous_;
 
  void findStateIndices(const double& duration, long& before, long& after, double& blend) const;
 
  static tesseract_common::JointState interpolate(const tesseract_common::JointState& start,
                                                  const tesseract_common::JointState& end,
                                                  double f);
};
}  // namespace tesseract_visualization
#endif  // TESSERACT_VISUALIZATION_TRAJECTORY_INTERPOLATOR_H