motion_generator.hpp

Go to the documentation of this file.

#pragma once
 
#include <franka/duration.h>
#include <franka/robot_state.h>
 
#include <memory>
#include <mutex>
 
#include "franky/robot_state.hpp"
#include "franky/types.hpp"
#include "ruckig/ruckig.hpp"
 
namespace franky {
 
struct ReactionRecursionException : public std::runtime_error {
  using std::runtime_error::runtime_error;
};
 
class Robot;
 
template <typename ControlSignalType>
class Motion;
 
template <typename ControlSignalType>
class MotionGenerator {
 public:
  static constexpr size_t REACTION_RECURSION_LIMIT = 16;
 
  explicit MotionGenerator(Robot *robot, std::shared_ptr<Motion<ControlSignalType>> initial_motion);
 
  ControlSignalType operator()(const RobotState &robot_state, franka::Duration period);
 
  inline void registerUpdateCallback(
      const std::function<void(const RobotState &, franka::Duration, franka::Duration)> &callback) {
    update_callbacks_.push_back(callback);
  }
 
  void updateMotion(std::shared_ptr<Motion<ControlSignalType>> new_motion);
 
  bool has_new_motion();
 
  void resetTimeUnsafe();
 
 private:
  std::shared_ptr<Motion<ControlSignalType>> initial_motion_;
  std::shared_ptr<Motion<ControlSignalType>> current_motion_;
  std::shared_ptr<Motion<ControlSignalType>> new_motion_;
  std::vector<std::function<void(const RobotState &, franka::Duration, franka::Duration)>> update_callbacks_;
  std::mutex new_motion_mutex_;
  std::optional<ControlSignalType> previous_command_;
 
  franka::Duration abs_time_{0};
  franka::Duration rel_time_offset_{0};
  Robot *robot_;
};
 
}  // namespace franky