elbow_state.hpp

Go to the documentation of this file.

#pragma once
 
#include <optional>
#include <array>
#include <iostream>
 
namespace franky {
 
enum class FlipDirection {
  kNegative = -1,
  kNeutral = 0,
  kPositive = 1
};
 
std::ostream& operator<<(std::ostream& os, const FlipDirection& flip_direction);
 
class ElbowState {
 public:
  explicit ElbowState(double joint_3_pos, std::optional<FlipDirection> joint_4_flip = std::nullopt)
      : joint_3_pos_(joint_3_pos), joint_4_flip_(joint_4_flip) {}
 
  explicit ElbowState(const std::array<double, 2>& elbow_state)
      : joint_3_pos_(elbow_state[0]),
        joint_4_flip_([&]() -> std::optional<FlipDirection> {
          if (elbow_state[1] < 0.0) {
            return FlipDirection::kNegative;
          } else if (elbow_state[1] == 0.0) {
            return FlipDirection::kNeutral;
          } else {
            return FlipDirection::kPositive;
          }
        }()) {}
 
  ElbowState(const ElbowState&) = default;
  ElbowState() = default;
 
  [[nodiscard]] inline std::array<double, 2> to_array(
      FlipDirection default_flip_direction = FlipDirection::kNegative) const {
    return {joint_3_pos_, static_cast<double>(joint_4_flip_.value_or(default_flip_direction))};
  }
 
  [[nodiscard]] inline double joint_3_pos() const { return joint_3_pos_; }
 
  [[nodiscard]] inline std::optional<FlipDirection> joint_4_flip() const { return joint_4_flip_; }
 
  friend std::ostream& operator<<(std::ostream& os, const ElbowState& elbow_state);
 
 private:
  double joint_3_pos_{0.0};
  std::optional<FlipDirection> joint_4_flip_{std::nullopt};
};
 
}  // namespace franky