reaction.hpp

Go to the documentation of this file.

#pragma once
 
#include <franka/robot_state.h>
 
#include <cmath>
#include <functional>
#include <iostream>
#include <memory>
#include <optional>
 
#include "franky/motion/condition.hpp"
#include "franky/motion/motion.hpp"
 
namespace franky {
 
template <typename ControlSignalType>
class Motion;
 
template <typename ControlSignalType>
class Reaction {
  using MotionFunc =
      std::function<std::shared_ptr<Motion<ControlSignalType>>(const RobotState &, franka::Duration, franka::Duration)>;
 
 public:
  explicit Reaction(const Condition &condition, std::shared_ptr<Motion<ControlSignalType>> new_motion = nullptr);
 
  explicit Reaction(Condition condition, const MotionFunc &motion_func);
 
  std::shared_ptr<Motion<ControlSignalType>> operator()(
      const RobotState &robot_state, franka::Duration rel_time, franka::Duration abs_time);
 
  [[nodiscard]] bool condition(
      const RobotState &robot_state, franka::Duration rel_time, franka::Duration abs_time) const {
    return condition_(robot_state, rel_time, abs_time);
  }
 
  void registerCallback(const std::function<void(const RobotState &, franka::Duration, franka::Duration)> &callback);
 
 private:
  MotionFunc motion_func_;
  Condition condition_;
  std::mutex callback_mutex_;
  std::vector<std::function<void(const RobotState &, franka::Duration, franka::Duration)>> callbacks_{};
};
 
}  // namespace franky