feedback.hpp

Go to the documentation of this file.

#pragma once

#include "hebi.h"

#include "color.hpp"
#include "quaternion_f.hpp"
#include "message_helpers.hpp"
#include "util.hpp"
#include "vector_3_f.hpp"

namespace hebi {

class Feedback final {
public:
  enum class TemperatureState {
    Normal,
    Critical,
    ExceedMaxMotor,
    ExceedMaxBoard,
  };

  enum class MstopState {
    Triggered,
    NotTriggered,
  };

  enum class PositionLimitState {
    Below,
    AtLower,
    Inside,
    AtUpper,
    Above,
    Uninitialized,
  };

  enum class VelocityLimitState {
    Below,
    AtLower,
    Inside,
    AtUpper,
    Above,
    Uninitialized,
  };

  enum class EffortLimitState {
    Below,
    AtLower,
    Inside,
    AtUpper,
    Above,
    Uninitialized,
  };

  enum class CommandLifetimeState {
    Unlocked,
    LockedByOther,
    LockedBySender,
  };

  enum class ArQuality {
    ArQualityNotAvailable,
    ArQualityLimitedUnknown,
    ArQualityLimitedInitializing,
    ArQualityLimitedRelocalizing,
    ArQualityLimitedExcessiveMotion,
    ArQualityLimitedInsufficientFeatures,
    ArQualityNormal,
  };

  enum class MotorHallState {
    State0,
    State1,
    State2,
    State3,
    State4,
    State5,
    Error,
  };

  enum class DrivetrainState {
    Normal,
    UncalibratedCurrent,
    UncalibratedPosition,
    UncalibratedEffort,
    SafetyFault,
  };

protected:
  class FloatField final {
  public:
#ifndef DOXYGEN_OMIT_INTERNAL
    FloatField(const HebiFeedbackRef& internal, HebiFeedbackFloatField field);
#endif // DOXYGEN_OMIT_INTERNAL
    explicit operator bool() const { return has(); }
    bool has() const;
    float get() const;

    HEBI_DISABLE_COPY_MOVE(FloatField)
  private:
    const HebiFeedbackRef& internal_;
    HebiFeedbackFloatField const field_;
  };

  class HighResAngleField final {
  public:
#ifndef DOXYGEN_OMIT_INTERNAL
    HighResAngleField(const HebiFeedbackRef& internal, HebiFeedbackHighResAngleField field);
#endif // DOXYGEN_OMIT_INTERNAL
    explicit operator bool() const { return has(); }
    bool has() const;
    double get() const;
    void get(int64_t* revolutions, float* radian_offset) const;

    HEBI_DISABLE_COPY_MOVE(HighResAngleField)
  private:
    const HebiFeedbackRef& internal_;
    HebiFeedbackHighResAngleField const field_;
  };

  class NumberedFloatField final {
  public:
#ifndef DOXYGEN_OMIT_INTERNAL
    NumberedFloatField(const HebiFeedbackRef& internal, HebiFeedbackNumberedFloatField field);
#endif // DOXYGEN_OMIT_INTERNAL
    bool has(size_t fieldNumber) const;
    float get(size_t fieldNumber) const;

    HEBI_DISABLE_COPY_MOVE(NumberedFloatField)
  private:
    const HebiFeedbackRef& internal_;
    HebiFeedbackNumberedFloatField const field_;
  };

  class UInt64Field final {
  public:
#ifndef DOXYGEN_OMIT_INTERNAL
    UInt64Field(const HebiFeedbackRef& internal, HebiFeedbackUInt64Field field);
#endif // DOXYGEN_OMIT_INTERNAL
    explicit operator bool() const { return has(); }
    bool has() const;
    uint64_t get() const;

    HEBI_DISABLE_COPY_MOVE(UInt64Field)
  private:
    const HebiFeedbackRef& internal_;
    HebiFeedbackUInt64Field const field_;
  };

  class Vector3fField final {
  public:
#ifndef DOXYGEN_OMIT_INTERNAL
    Vector3fField(const HebiFeedbackRef& internal, HebiFeedbackVector3fField field);
#endif // DOXYGEN_OMIT_INTERNAL
    explicit operator bool() const { return has(); }
    bool has() const;
    Vector3f get() const;

    HEBI_DISABLE_COPY_MOVE(Vector3fField)
  private:
    const HebiFeedbackRef& internal_;
    HebiFeedbackVector3fField const field_;
  };

  class QuaternionfField final {
  public:
#ifndef DOXYGEN_OMIT_INTERNAL
    QuaternionfField(const HebiFeedbackRef& internal, HebiFeedbackQuaternionfField field);
#endif // DOXYGEN_OMIT_INTERNAL
    explicit operator bool() const { return has(); }
    bool has() const;
    Quaternionf get() const;

    HEBI_DISABLE_COPY_MOVE(QuaternionfField)
  private:
    const HebiFeedbackRef& internal_;
    HebiFeedbackQuaternionfField const field_;
  };

  template<typename T>
  class EnumField final {
  public:
#ifndef DOXYGEN_OMIT_INTERNAL
    EnumField(const HebiFeedbackRef& internal, HebiFeedbackEnumField field) : internal_(internal), field_(field) {}
#endif // DOXYGEN_OMIT_INTERNAL
    explicit operator bool() const { return has(); }
    bool has() const {
      return (enumGetter(internal_, field_, nullptr) == HebiStatusSuccess);
    }
    T get() const {
      int32_t ret{};
      enumGetter(internal_, field_, &ret);
      return static_cast<T>(ret);
    }

    HEBI_DISABLE_COPY_MOVE(EnumField)
  private:
    const HebiFeedbackRef& internal_;
    HebiFeedbackEnumField const field_;
  };

  class IoBank final {
  public:
#ifndef DOXYGEN_OMIT_INTERNAL
    IoBank(const HebiFeedbackRef& internal, HebiFeedbackIoPinBank bank);
#endif // DOXYGEN_OMIT_INTERNAL
    bool hasInt(size_t pinNumber) const;
    bool hasFloat(size_t pinNumber) const;
    int64_t getInt(size_t pinNumber) const;
    float getFloat(size_t pinNumber) const;

    HEBI_DISABLE_COPY_MOVE(IoBank)
  private:
    const HebiFeedbackRef& internal_;
    HebiFeedbackIoPinBank const bank_;
  };

  class LedField final {
  public:
#ifndef DOXYGEN_OMIT_INTERNAL
    LedField(const HebiFeedbackRef& internal, HebiFeedbackLedField field);
#endif // DOXYGEN_OMIT_INTERNAL
    explicit operator bool() const { return hasColor(); }
    bool hasColor() const;
    Color getColor() const;

    HEBI_DISABLE_COPY_MOVE(LedField)
  private:
    const HebiFeedbackRef& internal_;
    HebiFeedbackLedField const field_;
  };

  class Io final {
  public:
#ifndef DOXYGEN_OMIT_INTERNAL
    Io(const HebiFeedbackRef& internal)
      : a_(internal, HebiFeedbackIoBankA),
        b_(internal, HebiFeedbackIoBankB),
        c_(internal, HebiFeedbackIoBankC),
        d_(internal, HebiFeedbackIoBankD),
        e_(internal, HebiFeedbackIoBankE),
        f_(internal, HebiFeedbackIoBankF) {}
#endif // DOXYGEN_OMIT_INTERNAL

    // With all submessage and field getters: Note that the returned reference
    // should not be used after the lifetime of this parent.

    // Subfields ----------------

    const IoBank& a() const { return a_; }
    const IoBank& b() const { return b_; }
    const IoBank& c() const { return c_; }
    const IoBank& d() const { return d_; }
    const IoBank& e() const { return e_; }
    const IoBank& f() const { return f_; }

    HEBI_DISABLE_COPY_MOVE(Io)
  private:
    IoBank a_;
    IoBank b_;
    IoBank c_;
    IoBank d_;
    IoBank e_;
    IoBank f_;
  };

  class Actuator final {
  public:
#ifndef DOXYGEN_OMIT_INTERNAL
    Actuator(const HebiFeedbackRef& internal)
      : velocity_(internal, HebiFeedbackFloatVelocity),
        effort_(internal, HebiFeedbackFloatEffort),
        velocity_command_(internal, HebiFeedbackFloatVelocityCommand),
        effort_command_(internal, HebiFeedbackFloatEffortCommand),
        deflection_(internal, HebiFeedbackFloatDeflection),
        deflection_velocity_(internal, HebiFeedbackFloatDeflectionVelocity),
        motor_velocity_(internal, HebiFeedbackFloatMotorVelocity),
        motor_current_(internal, HebiFeedbackFloatMotorCurrent),
        motor_sensor_temperature_(internal, HebiFeedbackFloatMotorSensorTemperature),
        motor_winding_current_(internal, HebiFeedbackFloatMotorWindingCurrent),
        motor_winding_temperature_(internal, HebiFeedbackFloatMotorWindingTemperature),
        motor_housing_temperature_(internal, HebiFeedbackFloatMotorHousingTemperature),
        pwm_command_(internal, HebiFeedbackFloatPwmCommand),
        inner_effort_command_(internal, HebiFeedbackFloatInnerEffortCommand),
        position_(internal, HebiFeedbackHighResAnglePosition),
        position_command_(internal, HebiFeedbackHighResAnglePositionCommand),
        motor_position_(internal, HebiFeedbackHighResAngleMotorPosition),
        temperature_state_(internal, HebiFeedbackEnumTemperatureState),
        mstop_state_(internal, HebiFeedbackEnumMstopState),
        position_limit_state_(internal, HebiFeedbackEnumPositionLimitState),
        velocity_limit_state_(internal, HebiFeedbackEnumVelocityLimitState),
        effort_limit_state_(internal, HebiFeedbackEnumEffortLimitState),
        command_lifetime_state_(internal, HebiFeedbackEnumCommandLifetimeState),
        motor_hall_state_(internal, HebiFeedbackEnumMotorHallState),
        drivetrain_state_(internal, HebiFeedbackEnumDrivetrainState) {}
#endif // DOXYGEN_OMIT_INTERNAL

    // With all submessage and field getters: Note that the returned reference
    // should not be used after the lifetime of this parent.

    // Subfields ----------------

    const FloatField& velocity() const { return velocity_; }
    const FloatField& effort() const { return effort_; }
    const FloatField& velocityCommand() const { return velocity_command_; }
    const FloatField& effortCommand() const { return effort_command_; }
    const FloatField& deflection() const { return deflection_; }
    const FloatField& deflectionVelocity() const { return deflection_velocity_; }
    const FloatField& motorVelocity() const { return motor_velocity_; }
    const FloatField& motorCurrent() const { return motor_current_; }
    const FloatField& motorSensorTemperature() const { return motor_sensor_temperature_; }
    const FloatField& motorWindingCurrent() const { return motor_winding_current_; }
    const FloatField& motorWindingTemperature() const { return motor_winding_temperature_; }
    const FloatField& motorHousingTemperature() const { return motor_housing_temperature_; }
    const FloatField& pwmCommand() const { return pwm_command_; }
    const FloatField& innerEffortCommand() const { return inner_effort_command_; }
    const HighResAngleField& position() const { return position_; }
    const HighResAngleField& positionCommand() const { return position_command_; }
    const HighResAngleField& motorPosition() const { return motor_position_; }
    const EnumField<TemperatureState>& temperatureState() const { return temperature_state_; }
    const EnumField<MstopState>& mstopState() const { return mstop_state_; }
    const EnumField<PositionLimitState>& positionLimitState() const { return position_limit_state_; }
    const EnumField<VelocityLimitState>& velocityLimitState() const { return velocity_limit_state_; }
    const EnumField<EffortLimitState>& effortLimitState() const { return effort_limit_state_; }
    const EnumField<CommandLifetimeState>& commandLifetimeState() const { return command_lifetime_state_; }
    const EnumField<MotorHallState>& motorHallState() const { return motor_hall_state_; }
    const EnumField<DrivetrainState>& drivetrainState() const { return drivetrain_state_; }
    HEBI_DISABLE_COPY_MOVE(Actuator)
  private:
    FloatField velocity_;
    FloatField effort_;
    FloatField velocity_command_;
    FloatField effort_command_;
    FloatField deflection_;
    FloatField deflection_velocity_;
    FloatField motor_velocity_;
    FloatField motor_current_;
    FloatField motor_sensor_temperature_;
    FloatField motor_winding_current_;
    FloatField motor_winding_temperature_;
    FloatField motor_housing_temperature_;
    FloatField pwm_command_;
    FloatField inner_effort_command_;
    HighResAngleField position_;
    HighResAngleField position_command_;
    HighResAngleField motor_position_;
    EnumField<TemperatureState> temperature_state_;
    EnumField<MstopState> mstop_state_;
    EnumField<PositionLimitState> position_limit_state_;
    EnumField<VelocityLimitState> velocity_limit_state_;
    EnumField<EffortLimitState> effort_limit_state_;
    EnumField<CommandLifetimeState> command_lifetime_state_;
    EnumField<MotorHallState> motor_hall_state_;
    EnumField<DrivetrainState> drivetrain_state_;
  };

  class Mobile final {
  public:
#ifndef DOXYGEN_OMIT_INTERNAL
    Mobile(const HebiFeedbackRef& internal)
      : battery_level_(internal, HebiFeedbackFloatBatteryLevel),
        ar_position_(internal, HebiFeedbackVector3fArPosition),
        ar_orientation_(internal, HebiFeedbackQuaternionfArOrientation),
        ar_quality_(internal, HebiFeedbackEnumArQuality) {}
#endif // DOXYGEN_OMIT_INTERNAL

    // With all submessage and field getters: Note that the returned reference
    // should not be used after the lifetime of this parent.

    // Subfields ----------------

    const FloatField& batteryLevel() const { return battery_level_; }
    const Vector3fField& arPosition() const { return ar_position_; }
    const QuaternionfField& arOrientation() const { return ar_orientation_; }
    const EnumField<ArQuality>& arQuality() const { return ar_quality_; }

    HEBI_DISABLE_COPY_MOVE(Mobile)
  private:
    FloatField battery_level_;
    Vector3fField ar_position_;
    QuaternionfField ar_orientation_;
    EnumField<ArQuality> ar_quality_;
  };

  class Imu final {
  public:
#ifndef DOXYGEN_OMIT_INTERNAL
    Imu(const HebiFeedbackRef& internal)
      : accelerometer_(internal, HebiFeedbackVector3fAccelerometer),
        gyro_(internal, HebiFeedbackVector3fGyro),
        orientation_(internal, HebiFeedbackQuaternionfOrientation) {}
#endif // DOXYGEN_OMIT_INTERNAL

    // With all submessage and field getters: Note that the returned reference
    // should not be used after the lifetime of this parent.

    // Subfields ----------------

    const Vector3fField& accelerometer() const { return accelerometer_; }
    const Vector3fField& gyro() const { return gyro_; }
    const QuaternionfField& orientation() const { return orientation_; }

    HEBI_DISABLE_COPY_MOVE(Imu)
  private:
    Vector3fField accelerometer_;
    Vector3fField gyro_;
    QuaternionfField orientation_;
  };

private:
  HebiFeedbackPtr internal_;
  HebiFeedbackRef internal_ref_;

public:
#ifndef DOXYGEN_OMIT_INTERNAL

  Feedback(HebiFeedbackPtr);
#endif // DOXYGEN_OMIT_INTERNAL

  Feedback(Feedback&& other);

  // With all submessage and field getters: Note that the returned reference
  // should not be used after the lifetime of this parent.

  // Submessages -------------------------------------------------------------

  const Io& io() const { return io_; }
  const Actuator& actuator() const { return actuator_; }
  const Mobile& mobile() const { return mobile_; }
  const Imu& imu() const { return imu_; }

  // Subfields -------------------------------------------------------------

  const FloatField& boardTemperature() const { return board_temperature_; }
  const FloatField& processorTemperature() const { return processor_temperature_; }
  const FloatField& voltage() const { return voltage_; }
#ifndef DOXYGEN_OMIT_INTERNAL
  const NumberedFloatField& debug() const { return debug_; }
#endif // DOXYGEN_OMIT_INTERNAL
  const UInt64Field& sequenceNumber() const { return sequence_number_; }
  const UInt64Field& receiveTimeUs() const { return receive_time_us_; }
  const UInt64Field& transmitTimeUs() const { return transmit_time_us_; }
  const UInt64Field& hardwareReceiveTimeUs() const { return hardware_receive_time_us_; }
  const UInt64Field& hardwareTransmitTimeUs() const { return hardware_transmit_time_us_; }
  const UInt64Field& senderId() const { return sender_id_; }
  const UInt64Field& rxSequenceNumber() const { return rx_sequence_number_; }
  const LedField& led() const { return led_; }
  const Vector3fField& force() const { return force_; }
  const Vector3fField& torque() const { return torque_; }

  HEBI_DISABLE_COPY(Feedback)

  /* Disable move assigment operator. */
  Feedback& operator=(Feedback&& other) = delete;

private:
  Io io_;
  Actuator actuator_;
  Mobile mobile_;
  Imu imu_;

  FloatField board_temperature_;
  FloatField processor_temperature_;
  FloatField voltage_;
  NumberedFloatField debug_;
  UInt64Field sequence_number_;
  UInt64Field receive_time_us_;
  UInt64Field transmit_time_us_;
  UInt64Field hardware_receive_time_us_;
  UInt64Field hardware_transmit_time_us_;
  UInt64Field sender_id_;
  UInt64Field rx_sequence_number_;
  LedField led_;
  Vector3fField force_;
  Vector3fField torque_;
};

} // namespace hebi