waypoint_motion.hpp

Go to the documentation of this file.

#pragma once
 
#include <optional>
#include <ruckig/ruckig.hpp>
 
#include "franky/motion/motion.hpp"
 
namespace franky {
 
struct MotionPlannerException : std::runtime_error {
  using std::runtime_error::runtime_error;
};
 
template <typename TargetType>
struct Waypoint {
  TargetType target;
 
  RelativeDynamicsFactor relative_dynamics_factor{1.0};
 
  std::optional<franka::Duration> minimum_time{std::nullopt};
 
  franka::Duration hold_target_duration{0};
 
  std::optional<franka::Duration> max_total_duration{std::nullopt};
 
  double state_estimate_weight{0.0};
};
 
template <typename ControlSignalType, typename WaypointType, typename TargetType>
class WaypointMotion : public Motion<ControlSignalType> {
  static_assert(
      std::is_base_of_v<Waypoint<TargetType>, WaypointType>, "WaypointType must inherit from Waypoint<TargetType>");
 
 public:
  explicit WaypointMotion(std::vector<WaypointType> waypoints, bool return_when_finished = true)
      : waypoints_(std::move(waypoints)), return_when_finished_(return_when_finished), prev_result_() {}
 
 protected:
  void initImpl(const RobotState &robot_state, const std::optional<ControlSignalType> &previous_command) override {
    target_reached_time_ = std::nullopt;
    waypoint_started_time_ = franka::Duration(0);
 
    initWaypointMotion(robot_state, previous_command, input_parameter_);
 
    waypoint_iterator_ = waypoints_.begin();
    if (waypoint_iterator_ != waypoints_.end()) {
      checkWaypointPrivate(*waypoint_iterator_);
      setNewWaypoint(robot_state, previous_command, *waypoint_iterator_, input_parameter_);
      setInputLimits(*waypoint_iterator_, input_parameter_);
      prev_result_ = ruckig::Result::Working;
    } else {
      prev_result_ = ruckig::Result::Finished;
    }
  }
 
  ControlSignalType nextCommandImpl(
      const RobotState &robot_state, franka::Duration time_step, franka::Duration rel_time, franka::Duration abs_time,
      const std::optional<ControlSignalType> &previous_command) override {
    const auto expected_time_step = franka::Duration(1);
    if (time_step > expected_time_step) {
      // In this case, we missed a couple of steps for some reason. Hence, extrapolate the way the robot does if it
      // does not receive data (constant acceleration model).
      // See https://frankaemika.github.io/docs/libfranka.html#under-the-hood
      extrapolateMotion(time_step - expected_time_step, input_parameter_, output_parameter_);
      output_parameter_.pass_to_input(input_parameter_);
    }
    auto max_time_reached = false;
    if (waypoint_iterator_ != waypoints_.end() && waypoint_iterator_->max_total_duration.has_value()) {
      max_time_reached = rel_time - waypoint_started_time_ >= waypoint_iterator_->max_total_duration.value();
    }
    if (prev_result_ == ruckig::Result::Finished || max_time_reached) {
      if (!target_reached_time_.has_value()) {
        target_reached_time_ = rel_time;
      }
      auto hold_target_duration = waypoint_iterator_->hold_target_duration;
      if (waypoint_iterator_ + 1 == waypoints_.end()) {
        // Allow cooldown of motion, so that the low-pass filter has time to adjust to target values
        hold_target_duration = std::max(hold_target_duration, franka::Duration(5));
      }
      if (rel_time - target_reached_time_.value() >= hold_target_duration || max_time_reached) {
        target_reached_time_ = std::nullopt;
        if (waypoint_iterator_ != waypoints_.end()) {
          ++waypoint_iterator_;
        }
      }
      if (waypoint_iterator_ == waypoints_.end()) {
        auto command = getControlSignal(robot_state, time_step, previous_command, input_parameter_);
        if (!return_when_finished_) return command;
        return franka::MotionFinished(command);
      } else {
        checkWaypointPrivate(*waypoint_iterator_);
        setNewWaypoint(robot_state, previous_command, *waypoint_iterator_, input_parameter_);
        setInputLimits(*waypoint_iterator_, input_parameter_);
        waypoint_started_time_ = rel_time;
      }
    }
    if (waypoint_iterator_ != waypoints_.end()) {
      auto blend = waypoint_iterator_->state_estimate_weight;
 
      if (blend != 0) {
        auto [pos_s, vel_s, acc_s] = getStateEstimate(robot_state);
        auto pos_t = toEigen(input_parameter_.current_position);
        auto vel_t = toEigen(input_parameter_.current_velocity);
        auto acc_t = toEigen(input_parameter_.current_acceleration);
 
        auto pos_b = (1 - blend) * pos_t + blend * pos_s;
        auto vel_b = (1 - blend) * vel_t + blend * vel_s;
        auto acc_b = (1 - blend) * acc_t + blend * acc_s;
 
        input_parameter_.current_position = toStdD<7>(pos_b);
        input_parameter_.current_velocity = toStdD<7>(vel_b);
        input_parameter_.current_acceleration = toStdD<7>(acc_b);
      }
 
      prev_result_ = trajectory_generator_.update(input_parameter_, output_parameter_);
      if (prev_result_ == ruckig::Result::Working || prev_result_ == ruckig::Result::Finished) {
        output_parameter_.pass_to_input(input_parameter_);
      } else {
        throw MotionPlannerException("Motion planner failed with error code " + std::to_string(prev_result_));
      }
    }
 
    return getControlSignal(robot_state, time_step, previous_command, input_parameter_);
  };
 
  virtual void initWaypointMotion(
      const RobotState &robot_state, const std::optional<ControlSignalType> &previous_command,
      ruckig::InputParameter<7> &input_parameter) = 0;
 
  virtual void setNewWaypoint(
      const RobotState &robot_state, const std::optional<ControlSignalType> &previous_command,
      const WaypointType &new_waypoint, ruckig::InputParameter<7> &input_parameter) = 0;
 
  virtual void extrapolateMotion(
      const franka::Duration &time_step, const ruckig::InputParameter<7> &input_parameter,
      ruckig::OutputParameter<7> &output_parameter) const = 0;
 
  virtual void checkWaypoint(const WaypointType &waypoint) const {}
 
  [[nodiscard]] virtual std::tuple<Vector7d, Vector7d, Vector7d> getStateEstimate(
      const RobotState &robot_state) const = 0;
 
  [[nodiscard]] virtual std::tuple<Vector7d, Vector7d, Vector7d> getAbsoluteInputLimits() const = 0;
 
  [[nodiscard]] virtual ControlSignalType getControlSignal(
      const RobotState &robot_state, const franka::Duration &time_step,
      const std::optional<ControlSignalType> &previous_command, const ruckig::InputParameter<7> &input_parameter) = 0;
 
  virtual void setInputLimits(const WaypointType &waypoint, ruckig::InputParameter<7> &input_parameter) const = 0;
 
 private:
  std::vector<WaypointType> waypoints_;
  bool return_when_finished_{true};
 
  ruckig::Ruckig<7> trajectory_generator_{Robot::control_rate};
  ruckig::Result prev_result_;
  ruckig::InputParameter<7> input_parameter_;
  ruckig::OutputParameter<7> output_parameter_;
 
  typename std::vector<WaypointType>::iterator waypoint_iterator_;
 
  std::optional<franka::Duration> target_reached_time_;
  franka::Duration waypoint_started_time_;
 
  void checkWaypointPrivate(const WaypointType &waypoint) const {
    if (waypoint.state_estimate_weight < 0 || waypoint.state_estimate_weight > 1)
      throw std::runtime_error(
          "state_estimate_weight must be between 0 and 1. Got " + std::to_string(waypoint.state_estimate_weight));
    checkWaypoint(waypoint);
  }
};
 
}  // namespace franky