command.hpp

Go to the documentation of this file.

#pragma once

#include "hebi.h"

#include <string>

#include "color.hpp"
#include "gains.hpp"
#include "ip_address.hpp"
#include "message_helpers.hpp"
#include "util.hpp"
#include "vector_3_f.hpp"

namespace hebi {

class Command final {
public:
  enum class ControlStrategy {
    Off,
    DirectPWM,
    Strategy2,
    Strategy3,
    Strategy4,
    Strategy5
  };

  enum class MstopStrategy {
    Disabled,
    MotorOff,
    HoldPosition,
  };

  enum class PositionLimitStrategy {
    HoldPosition,
    DampedSpring,
    MotorOff,
    Disabled,
  };

protected:
  class FloatField final {
  public:
#ifndef DOXYGEN_OMIT_INTERNAL
    FloatField(HebiCommandRef& internal, HebiCommandFloatField field);
#endif // DOXYGEN_OMIT_INTERNAL
    explicit operator bool() const { return has(); }
    bool has() const;
    float get() const;
    void set(float value);
    void clear();

    HEBI_DISABLE_COPY_MOVE(FloatField)
  private:
    HebiCommandRef& internal_;
    HebiCommandFloatField const field_;
  };

  class HighResAngleField final {
  public:
#ifndef DOXYGEN_OMIT_INTERNAL
    HighResAngleField(HebiCommandRef& internal, HebiCommandHighResAngleField 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;
    void set(double radians);
    void set(int64_t revolutions, float radian_offset);
    void clear();

    HEBI_DISABLE_COPY_MOVE(HighResAngleField)
  private:
    HebiCommandRef& internal_;
    HebiCommandHighResAngleField const field_;
  };

  class NumberedFloatField final {
  public:
#ifndef DOXYGEN_OMIT_INTERNAL
    NumberedFloatField(HebiCommandRef& internal, HebiCommandNumberedFloatField field);
#endif // DOXYGEN_OMIT_INTERNAL
    bool has(size_t fieldNumber) const;
    float get(size_t fieldNumber) const;
    void set(size_t fieldNumber, float value);
    void clear(size_t fieldNumber);

    HEBI_DISABLE_COPY_MOVE(NumberedFloatField)
  private:
    HebiCommandRef& internal_;
    HebiCommandNumberedFloatField const field_;
  };

  class IpAddressField final {
  public:
#ifndef DOXYGEN_OMIT_INTERNAL
    IpAddressField(HebiCommandRef& internal, HebiCommandUInt64Field field);
#endif // DOXYGEN_OMIT_INTERNAL
    explicit operator bool() const { return has(); }
    bool has() const;
    IpAddress get() const;
    void set(const IpAddress& value);
    void clear();

    HEBI_DISABLE_COPY_MOVE(IpAddressField)
  private:
    HebiCommandRef& internal_;
    HebiCommandUInt64Field const field_;
  };

  class BoolField final {
  public:
#ifndef DOXYGEN_OMIT_INTERNAL
    BoolField(HebiCommandRef& internal, HebiCommandBoolField field);
#endif // DOXYGEN_OMIT_INTERNAL
    bool has() const;
    bool get() const;
    void set(bool value);
    void clear();

    HEBI_DISABLE_COPY_MOVE(BoolField)
  private:
    HebiCommandRef& internal_;
    HebiCommandBoolField const field_;
  };

  class StringField final {
  public:
#ifndef DOXYGEN_OMIT_INTERNAL
    StringField(HebiCommandPtr internal, HebiCommandStringField field);
#endif // DOXYGEN_OMIT_INTERNAL
    explicit operator bool() const { return has(); }
    bool has() const;
    std::string get() const;
    void set(const std::string& value);
    void clear();

    HEBI_DISABLE_COPY_MOVE(StringField)
  private:
    HebiCommandPtr const internal_;
    HebiCommandStringField const field_;
  };

  class FlagField final {
  public:
#ifndef DOXYGEN_OMIT_INTERNAL
    FlagField(HebiCommandRef& internal, HebiCommandFlagField field);
#endif // DOXYGEN_OMIT_INTERNAL
    explicit operator bool() const { return has(); }
    bool has() const;
    void set();
    void clear();

    HEBI_DISABLE_COPY_MOVE(FlagField)
  private:
    HebiCommandRef& internal_;
    HebiCommandFlagField const field_;
  };


  class Vector3fField final {
  public:
#ifndef DOXYGEN_OMIT_INTERNAL
    Vector3fField(HebiCommandRef& internal, HebiCommandVector3fField field);
#endif // DOXYGEN_OMIT_INTERNAL
    explicit operator bool() const { return has(); }
    bool has() const;
    Vector3f get() const;
    void set(const Vector3f& value);
    void clear();

    HEBI_DISABLE_COPY_MOVE(Vector3fField)
  private:
    HebiCommandRef& internal_;
    HebiCommandVector3fField const field_;
  };

  template<typename T>
  class EnumField final {
  public:
#ifndef DOXYGEN_OMIT_INTERNAL
    EnumField(HebiCommandRef& internal, HebiCommandEnumField 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);
    }
    void set(T _value) {
      int32_t value = static_cast<int32_t>(_value);
      hebiCommandSetEnum(internal_, field_, &value);
    }
    void clear() {
      hebiCommandSetEnum(internal_, field_, nullptr);
    }

    HEBI_DISABLE_COPY_MOVE(EnumField)
  private:
    HebiCommandRef& internal_;
    HebiCommandEnumField const field_;
  };

  class IoBank final {
  public:
#ifndef DOXYGEN_OMIT_INTERNAL
    IoBank(HebiCommandPtr internal, HebiCommandRef& internal_ref, HebiCommandIoPinBank 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;
    void setInt(size_t pinNumber, int64_t value);
    void setFloat(size_t pinNumber, float value);
    bool hasLabel(size_t pinNumber) const;
    std::string getLabel(size_t pinNumber) const;
    void setLabel(size_t pinNumber, const std::string&);
    void clear(size_t pinNumber);

    HEBI_DISABLE_COPY_MOVE(IoBank)
  private:
    HebiCommandPtr internal_;
    HebiCommandRef& internal_ref_;
    HebiCommandIoPinBank const bank_;
  };
  class LedField final {
  public:
#ifndef DOXYGEN_OMIT_INTERNAL
    LedField(HebiCommandRef& internal, HebiCommandLedField field);
#endif // DOXYGEN_OMIT_INTERNAL
    bool has() const;
    Color get() const;
    void set(const Color& color);
    void clear();

    HEBI_DISABLE_COPY_MOVE(LedField)
  private:
    HebiCommandRef& internal_;
    HebiCommandLedField const field_;
  };

  class Io final {
  public:
#ifndef DOXYGEN_OMIT_INTERNAL
    Io(HebiCommandPtr internal, HebiCommandRef& internal_ref)
      : a_(internal, internal_ref, HebiCommandIoBankA),
        b_(internal, internal_ref, HebiCommandIoBankB),
        c_(internal, internal_ref, HebiCommandIoBankC),
        d_(internal, internal_ref, HebiCommandIoBankD),
        e_(internal, internal_ref, HebiCommandIoBankE),
        f_(internal, internal_ref, HebiCommandIoBankF) {}
#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 ----------------

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

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

  using CommandGains = Gains<HebiCommandRef, FloatField, BoolField, HebiCommandFloatField, HebiCommandBoolField>;

  class Settings final {
  protected:
    class Actuator final {
    public:
#ifndef DOXYGEN_OMIT_INTERNAL
      Actuator(HebiCommandRef& internal)
        : position_gains_(internal, HebiCommandFloatPositionKp, HebiCommandBoolPositionDOnError),
          velocity_gains_(internal, HebiCommandFloatVelocityKp, HebiCommandBoolVelocityDOnError),
          effort_gains_(internal, HebiCommandFloatEffortKp, HebiCommandBoolEffortDOnError),
          spring_constant_(internal, HebiCommandFloatSpringConstant),
          reference_position_(internal, HebiCommandFloatReferencePosition),
          reference_effort_(internal, HebiCommandFloatReferenceEffort),
          velocity_limit_min_(internal, HebiCommandFloatVelocityLimitMin),
          velocity_limit_max_(internal, HebiCommandFloatVelocityLimitMax),
          effort_limit_min_(internal, HebiCommandFloatEffortLimitMin),
          effort_limit_max_(internal, HebiCommandFloatEffortLimitMax),
          offset_reference_position_(internal, HebiCommandFloatOffsetReferencePosition),
          position_limit_min_(internal, HebiCommandHighResAnglePositionLimitMin),
          position_limit_max_(internal, HebiCommandHighResAnglePositionLimitMax),
          control_strategy_(internal, HebiCommandEnumControlStrategy),
          mstop_strategy_(internal, HebiCommandEnumMstopStrategy),
          min_position_limit_strategy_(internal, HebiCommandEnumMinPositionLimitStrategy),
          max_position_limit_strategy_(internal, HebiCommandEnumMaxPositionLimitStrategy) {}
#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.

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

      CommandGains& positionGains() { return position_gains_; }
      const CommandGains& positionGains() const { return position_gains_; }
      CommandGains& velocityGains() { return velocity_gains_; }
      const CommandGains& velocityGains() const { return velocity_gains_; }
      CommandGains& effortGains() { return effort_gains_; }
      const CommandGains& effortGains() const { return effort_gains_; }

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

      FloatField& springConstant() { return spring_constant_; }
      const FloatField& springConstant() const { return spring_constant_; }
      FloatField& referencePosition() { return reference_position_; }
      const FloatField& referencePosition() const { return reference_position_; }
      FloatField& referenceEffort() { return reference_effort_; }
      const FloatField& referenceEffort() const { return reference_effort_; }
      FloatField& velocityLimitMin() { return velocity_limit_min_; }
      const FloatField& velocityLimitMin() const { return velocity_limit_min_; }
      FloatField& velocityLimitMax() { return velocity_limit_max_; }
      const FloatField& velocityLimitMax() const { return velocity_limit_max_; }
      FloatField& effortLimitMin() { return effort_limit_min_; }
      const FloatField& effortLimitMin() const { return effort_limit_min_; }
      FloatField& effortLimitMax() { return effort_limit_max_; }
      const FloatField& effortLimitMax() const { return effort_limit_max_; }
      FloatField& offsetReferencePosition() { return offset_reference_position_; }
      const FloatField& offsetReferencePosition() const { return offset_reference_position_; }
      HighResAngleField& positionLimitMin() { return position_limit_min_; }
      const HighResAngleField& positionLimitMin() const { return position_limit_min_; }
      HighResAngleField& positionLimitMax() { return position_limit_max_; }
      const HighResAngleField& positionLimitMax() const { return position_limit_max_; }
      EnumField<ControlStrategy>& controlStrategy() { return control_strategy_; }
      const EnumField<ControlStrategy>& controlStrategy() const { return control_strategy_; }
      EnumField<MstopStrategy>& mstopStrategy() { return mstop_strategy_; }
      const EnumField<MstopStrategy>& mstopStrategy() const { return mstop_strategy_; }
      EnumField<PositionLimitStrategy>& minPositionLimitStrategy() { return min_position_limit_strategy_; }
      const EnumField<PositionLimitStrategy>& minPositionLimitStrategy() const { return min_position_limit_strategy_; }
      EnumField<PositionLimitStrategy>& maxPositionLimitStrategy() { return max_position_limit_strategy_; }
      const EnumField<PositionLimitStrategy>& maxPositionLimitStrategy() const { return max_position_limit_strategy_; }
    
      HEBI_DISABLE_COPY_MOVE(Actuator)
    private:
      CommandGains position_gains_;
      CommandGains velocity_gains_;
      CommandGains effort_gains_;

      FloatField spring_constant_;
      FloatField reference_position_;
      FloatField reference_effort_;
      FloatField velocity_limit_min_;
      FloatField velocity_limit_max_;
      FloatField effort_limit_min_;
      FloatField effort_limit_max_;
      FloatField offset_reference_position_;
      HighResAngleField position_limit_min_;
      HighResAngleField position_limit_max_;
      EnumField<ControlStrategy> control_strategy_;
      EnumField<MstopStrategy> mstop_strategy_;
      EnumField<PositionLimitStrategy> min_position_limit_strategy_;
      EnumField<PositionLimitStrategy> max_position_limit_strategy_;
    };

    class Imu final {
    public:
#ifndef DOXYGEN_OMIT_INTERNAL
      Imu(HebiCommandRef& internal)
        : accel_includes_gravity_(internal, HebiCommandBoolAccelIncludesGravity) {}
#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 ----------------

      BoolField& accelIncludesGravity() { return accel_includes_gravity_; }
      const BoolField& accelIncludesGravity() const { return accel_includes_gravity_; }

      HEBI_DISABLE_COPY_MOVE(Imu)
    private:
      BoolField accel_includes_gravity_;
    };

  public:
#ifndef DOXYGEN_OMIT_INTERNAL
    Settings(HebiCommandPtr internal_ptr, HebiCommandRef& internal)
      : actuator_(internal),
        imu_(internal),
        name_(internal_ptr, HebiCommandStringName),
        family_(internal_ptr, HebiCommandStringFamily),
        user_settings_bytes_1_(internal_ptr, HebiCommandStringUserSettingsBytes1),
        user_settings_bytes_2_(internal_ptr, HebiCommandStringUserSettingsBytes2),
        user_settings_bytes_3_(internal_ptr, HebiCommandStringUserSettingsBytes3),
        user_settings_bytes_4_(internal_ptr, HebiCommandStringUserSettingsBytes4),
        user_settings_bytes_5_(internal_ptr, HebiCommandStringUserSettingsBytes5),
        user_settings_bytes_6_(internal_ptr, HebiCommandStringUserSettingsBytes6),
        user_settings_bytes_7_(internal_ptr, HebiCommandStringUserSettingsBytes7),
        user_settings_bytes_8_(internal_ptr, HebiCommandStringUserSettingsBytes8),
        user_settings_float_1_(internal, HebiCommandFloatUserSettingsFloat1),
        user_settings_float_2_(internal, HebiCommandFloatUserSettingsFloat2),
        user_settings_float_3_(internal, HebiCommandFloatUserSettingsFloat3),
        user_settings_float_4_(internal, HebiCommandFloatUserSettingsFloat4),
        user_settings_float_5_(internal, HebiCommandFloatUserSettingsFloat5),
        user_settings_float_6_(internal, HebiCommandFloatUserSettingsFloat6),
        user_settings_float_7_(internal, HebiCommandFloatUserSettingsFloat7),
        user_settings_float_8_(internal, HebiCommandFloatUserSettingsFloat8),
        ip_address_(internal, HebiCommandUInt64IpAddress),
        subnet_mask_(internal, HebiCommandUInt64SubnetMask),
        save_current_settings_(internal, HebiCommandFlagSaveCurrentSettings) {}
#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.

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

    Actuator& actuator() { return actuator_; }
    const Actuator& actuator() const { return actuator_; }
    Imu& imu() { return imu_; }
    const Imu& imu() const { return imu_; }

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

    StringField& name() { return name_; }
    const StringField& name() const { return name_; }
    StringField& family() { return family_; }
    const StringField& family() const { return family_; }
    StringField& userSettingsBytes(size_t number)
    {
      switch (number) {
        case 1: return user_settings_bytes_1_;
        case 2: return user_settings_bytes_2_;
        case 3: return user_settings_bytes_3_;
        case 4: return user_settings_bytes_4_;
        case 5: return user_settings_bytes_5_;
        case 6: return user_settings_bytes_6_;
        case 7: return user_settings_bytes_7_;
        case 8: return user_settings_bytes_8_;
      }
      throw std::out_of_range("Invalid option for bytes array user setting entry!");
    }
    const StringField& userSettingsBytes(size_t number) const
    {
      switch (number) {
        case 1: return user_settings_bytes_1_;
        case 2: return user_settings_bytes_2_;
        case 3: return user_settings_bytes_3_;
        case 4: return user_settings_bytes_4_;
        case 5: return user_settings_bytes_5_;
        case 6: return user_settings_bytes_6_;
        case 7: return user_settings_bytes_7_;
        case 8: return user_settings_bytes_8_;
      }
      throw std::out_of_range("Invalid option for bytes array user setting entry!");
    }
    FloatField& userSettingsFloat(size_t number)
    {
      switch (number) {
        case 1: return user_settings_float_1_;
        case 2: return user_settings_float_2_;
        case 3: return user_settings_float_3_;
        case 4: return user_settings_float_4_;
        case 5: return user_settings_float_5_;
        case 6: return user_settings_float_6_;
        case 7: return user_settings_float_7_;
        case 8: return user_settings_float_8_;
      }
      throw std::out_of_range("Invalid option for float user setting entry!");
    }
    const FloatField& userSettingsFloat(size_t number) const
    {
      switch (number) {
        case 1: return user_settings_float_1_;
        case 2: return user_settings_float_2_;
        case 3: return user_settings_float_3_;
        case 4: return user_settings_float_4_;
        case 5: return user_settings_float_5_;
        case 6: return user_settings_float_6_;
        case 7: return user_settings_float_7_;
        case 8: return user_settings_float_8_;
      }
      throw std::out_of_range("Invalid option for float user setting entry!");
    }
    IpAddressField& ipAddress() { return ip_address_; }
    const IpAddressField& ipAddress() const { return ip_address_; }
    IpAddressField& subnetMask() { return subnet_mask_; }
    const IpAddressField& subnetMask() const { return subnet_mask_; }
    FlagField& saveCurrentSettings() { return save_current_settings_; }
    const FlagField& saveCurrentSettings() const { return save_current_settings_; }

    HEBI_DISABLE_COPY_MOVE(Settings)

  private:
    Actuator actuator_;
    Imu imu_;

    StringField name_;
    StringField family_;
    StringField user_settings_bytes_1_;
    StringField user_settings_bytes_2_;
    StringField user_settings_bytes_3_;
    StringField user_settings_bytes_4_;
    StringField user_settings_bytes_5_;
    StringField user_settings_bytes_6_;
    StringField user_settings_bytes_7_;
    StringField user_settings_bytes_8_;
    FloatField user_settings_float_1_;
    FloatField user_settings_float_2_;
    FloatField user_settings_float_3_;
    FloatField user_settings_float_4_;
    FloatField user_settings_float_5_;
    FloatField user_settings_float_6_;
    FloatField user_settings_float_7_;
    FloatField user_settings_float_8_;
    IpAddressField ip_address_;
    IpAddressField subnet_mask_;
    FlagField save_current_settings_;
  };

  class Actuator final {
  public:
#ifndef DOXYGEN_OMIT_INTERNAL
    Actuator(HebiCommandRef& internal)
      : velocity_(internal, HebiCommandFloatVelocity),
        effort_(internal, HebiCommandFloatEffort),
        position_(internal, HebiCommandHighResAnglePosition) {}
#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 ----------------

    FloatField& velocity() { return velocity_; }
    const FloatField& velocity() const { return velocity_; }
    FloatField& effort() { return effort_; }
    const FloatField& effort() const { return effort_; }
    HighResAngleField& position() { return position_; }
    const HighResAngleField& position() const { return position_; }

    HEBI_DISABLE_COPY_MOVE(Actuator)
  private:
    FloatField velocity_;
    FloatField effort_;
    HighResAngleField position_;
  };

private:
  HebiCommandPtr internal_;
  HebiCommandRef internal_ref_;

public:
#ifndef DOXYGEN_OMIT_INTERNAL

  Command(HebiCommandPtr);
#endif // DOXYGEN_OMIT_INTERNAL

  Command(Command&& other);

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

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

  Io& io() { return io_; }
  const Io& io() const { return io_; }
  Settings& settings() { return settings_; }
  const Settings& settings() const { return settings_; }
  Actuator& actuator() { return actuator_; }
  const Actuator& actuator() const { return actuator_; }

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

#ifndef DOXYGEN_OMIT_INTERNAL
  NumberedFloatField& debug() { return debug_; }
  const NumberedFloatField& debug() const { return debug_; }
#endif // DOXYGEN_OMIT_INTERNAL
  StringField& appendLog() { return append_log_; }
  const StringField& appendLog() const { return append_log_; }
  FlagField& reset() { return reset_; }
  const FlagField& reset() const { return reset_; }
  FlagField& boot() { return boot_; }
  const FlagField& boot() const { return boot_; }
  FlagField& stopBoot() { return stop_boot_; }
  const FlagField& stopBoot() const { return stop_boot_; }
  FlagField& clearLog() { return clear_log_; }
  const FlagField& clearLog() const { return clear_log_; }
  LedField& led() { return led_; }
  const LedField& led() const { return led_; }
  Vector3fField& force() { return force_; }
  const Vector3fField& force() const { return force_; }
  Vector3fField& torque() { return torque_; }
  const Vector3fField& torque() const { return torque_; }

  HEBI_DISABLE_COPY(Command)

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

private:
  Io io_;
  Settings settings_;
  Actuator actuator_;

  NumberedFloatField debug_;
  StringField append_log_;
  FlagField reset_;
  FlagField boot_;
  FlagField stop_boot_;
  FlagField clear_log_;
  LedField led_;
  Vector3fField force_;
  Vector3fField torque_;
};

} // namespace hebi