motion.hpp

Go to the documentation of this file.

#pragma once
 
#include <atomic>
#include <list>
#include <mutex>
 
#include "franky/robot.hpp"
#include "franky/robot_state.hpp"
#include "reaction.hpp"
 
namespace franky {
 
class Robot;
 
template <typename ControlSignalType>
class Reaction;
 
template <typename ControlSignalType>
class Motion {
 public:
  virtual ~Motion() = default;
  using CallbackType = std::function<void(
      const RobotState &, franka::Duration, franka::Duration, franka::Duration, const ControlSignalType &)>;
 
  void addReaction(std::shared_ptr<Reaction<ControlSignalType>> reaction);
 
  void addReactionFront(std::shared_ptr<Reaction<ControlSignalType>> reaction);
 
  std::vector<std::shared_ptr<Reaction<ControlSignalType>>> reactions();
 
  void registerCallback(CallbackType callback);
 
  void init(Robot *robot, const RobotState &robot_state, const std::optional<ControlSignalType> &previous_command);
 
  [[nodiscard]] bool has_started() const { return started_.load(); }
 
  ControlSignalType nextCommand(
      const RobotState &robot_state, franka::Duration time_step, franka::Duration rel_time, franka::Duration abs_time,
      const std::optional<ControlSignalType> &previous_command);
 
  std::shared_ptr<Motion<ControlSignalType>> checkAndCallReactions(
      const RobotState &robot_state, franka::Duration rel_time, franka::Duration abs_time);
 
 protected:
  explicit Motion();
 
  virtual void initImpl(const RobotState &robot_state, const std::optional<ControlSignalType> &previous_command) {}
 
  virtual ControlSignalType nextCommandImpl(
      const RobotState &robot_state, franka::Duration time_step, franka::Duration rel_time, franka::Duration abs_time,
      const std::optional<ControlSignalType> &previous_command) = 0;
 
  [[nodiscard]] Robot *robot() const { return robot_; }
 
 private:
  std::mutex reaction_mutex_;
  std::list<std::shared_ptr<Reaction<ControlSignalType>>> reactions_;
  std::mutex callback_mutex_;
  std::vector<CallbackType> callbacks_;
  Robot *robot_;
  std::atomic<bool> started_{false};
};
 
}  // namespace franky