reaction.hpp

Go to the documentation of this file.

#pragma once
 
#include <cmath>
#include <iostream>
#include <functional>
#include <memory>
#include <optional>
#include <franka/robot_state.h>
 
#include "franky/motion/motion.hpp"
#include "franky/motion/condition.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