sensojoint.h

Go to the documentation of this file.

 
#pragma once
#include <ethercatcpp/core.h>
 
#include <cmath>
#include <string>
#include <optional>
#include <memory>
 
namespace ethercatcpp {
class SensoJoint : public coe::cia402::CIA402Device {
public:
    struct EndStop {
        double stiffness{0.}; // Nm/rad
        double damping{0.};   // Nm x s /rad
        double position{0.};  // rad
        double offset{0.};    // rad
    };
    struct Options {
        double control_period_{0.};                              // is s
        double initial_adjusted_position{0.};                    // in rad
        double min_target_position{0.}, max_target_position{0.}; // in rad
        double zero_pos_offset{0.};                              // in rad
        double zero_torque_offset{0.};                           // in Nm
        double max_motor_torque{0.};                             // in Nm
        double max_motor_velocity{0.};                           // in rad / s
        double output_inertia{0.};                               // in kg.m^2
        double profile_velocity{0.};                             // in rad / s
        double profile_acceleration{0.};                         // in rad / s^2
        double profile_deceleration{0.};                         // in rad / s^2
        double position_window{0.};         // in positive rad
        double position_window_time{0.};    // in s
        double velocity_threshold{0.};      // in rad / s
        double velocity_threshold_time{0.}; // in  s
        double velocity_window{0.};         // in rad / s
        double velocity_window_time{0.};    // in  s
        double torque_slope{0.};            // in Nm / s
        EndStop endstop_specs;
        bool enable_haptics{false};
 
        uint8_t controlled_digital_outputs{0};
        uint8_t digital_outputs_init_value{0};
        uint8_t digital_outputs_end_value{0};
        uint8_t read_digital_inputs{0};
 
        Options& offsets(double pos_offset, double torque_offset);
        Options& use_haptics();
        Options& inertia(double output);
        Options& endstops(double stiffness, double damping);
        Options& max(double motor_torque, double motor_vel);
        Options& profile(double velocity, double accel = 0., double decel = 0.);
        Options& torque_profile(double slope);
        Options& position_reached(double window, double time);
        Options& velocity_reached(double window, double time);
        Options& standstill(double threshold = 0.001, double time = 0.001);
        Options& controlled_outputs(uint8_t outputs, uint8_t init_value = 0,
                                    uint8_t end_value = 0);
        Options& read_inputs(uint8_t inputs);
    };
    static SensoJoint::Options gen_options(double control_period,
                                           double min_target_pos,
                                           double max_target_pos);
 
    enum class control_mode_t {
        monitor,
        profile_position,
        profile_velocity,
        profile_torque,
        cyclic_position,
        cyclic_velocity,
        cyclic_torque,
        profile_output_position_vibration_damping,
        profile_output_velocity,
        cyclic_output_position_vibration_damping,
        cyclic_output_velocity,
        cyclic_output_torque
    };
 
    virtual bool profile_mode() const override;
    virtual bool cyclic_mode() const override;
    virtual bool position_mode() const override;
    virtual bool velocity_mode() const override;
    virtual bool torque_mode() const override;
    virtual bool profile_mode_queried() const override;
    virtual bool cyclic_mode_queried() const override;
    virtual bool position_mode_queried() const override;
    virtual bool velocity_mode_queried() const override;
    virtual bool torque_mode_queried() const override;
 
    static bool is_advanced_mode(control_mode_t);
    static bool is_profile_mode(control_mode_t);
    static bool is_cyclic_mode(control_mode_t);
    static bool is_position_mode(control_mode_t);
    static bool is_velocity_mode(control_mode_t);
    static bool is_torque_mode(control_mode_t);
 
    using operation_state_t = CIA402Device::state_t;
 
    enum Model { MODEL_7016, MODEL_5016, MODEL_4010, MODEL_3010 };
    SensoJoint(const Model& model);
    SensoJoint(const Model& model, const Options& options);
    SensoJoint(const SensoJoint&) = delete;
    SensoJoint(SensoJoint&&) = delete;
    SensoJoint& operator=(const SensoJoint&) = delete;
    SensoJoint& operator=(SensoJoint&&) = delete;
    virtual ~SensoJoint();
 
    void set_target_position(double target_position);
 
    void set_target_velocity(double target_velocity);
 
 
    void set_target_output_torque(double target_torque);
 
    void set_target_motor_torque(double target_torque);
 
    void set_motor_torque_feedforward(double torque_offset);
 
    void set_output_torque_feedforward(double torque_offset);
 
    void set_external_load_torque_offset(double offset);
 
    void set_velocity_feedforward(double velocity_offset);
 
    [[nodiscard]] bool set_control_mode(control_mode_t control_mode);
 
    control_mode_t control_mode() const;
 
    std::string_view control_mode_str() const;
 
    static std::string_view control_mode_to_str(control_mode_t);
 
    double position() const;
 
    double velocity() const;
 
    double motor_torque() const;
 
    double motor_current() const;
 
    double output_torque() const;
 
    double friction_torque() const;
 
    double motor_temperature() const;
 
    double drive_temperature() const;
 
    double sensor_temperature() const;
 
    // Profile Position Mode (PPM) specific config
 
    bool following_error() const;
 
    bool standstill() const;
 
    bool sto_active() const;
 
    bool sbc_active() const;
 
    bool brake_applied() const;
 
    const Options& options() const;
 
    const std::string& fault() const;
 
    uint16_t error_code() const;
 
    bool warning() const;
 
    void set_spring(double neutral_position, double stiffness, double damping);
 
    enum IOFlag {
        gpio_3_io_fpp_pin = 1 << 0, // Fast push-pull
        gpio_4_out_pin = 1 << 1,    // Slow push-pull
        gpio_5_io_sbo_pin = 1 << 2, // Slow bi directionnal
        gpio_6_io_sbo_pin = 1 << 3, // Slow bi directionnal
    };
 
    void set_digital_outputs(uint8_t outputs);
 
    uint8_t digital_inputs() const;
 
    double initial_position() const;
 
protected:
    static bool advanced_mode(int8_t);
    static bool profile_mode(int8_t);
    static bool cyclic_mode(int8_t);
    static bool position_mode(int8_t);
    static bool velocity_mode(int8_t);
    static bool torque_mode(int8_t);
 
private:
    void initialize(const std::optional<Options>& options);
    void pdo_maps_configuration();
 
    void unpack_status_buffer();
    void update_internal_state();
 
    // Used to change device state in async mode (only on init)
    void read_base_units_values();
    void find_initial_position();
    void manage_ctrl_params(const std::optional<Options>& options);
 
    //----------------------------------------------------------------------------//
    //                           INTERNAL STATE //
    //----------------------------------------------------------------------------//
    Options options_;
    Model model_;
    // Status/Command datas
    struct OpaqueStateCommands;
    std::unique_ptr<OpaqueStateCommands> sta_cmd_;
    void bind_buffers();
    static const std::map<int8_t, std::string_view> control_mode_decode_;
    struct InternalParameters;
    std::unique_ptr<InternalParameters> internal_parameters_;
    struct ConversionRatios;
    std::unique_ptr<ConversionRatios> conversion_;
 
    void manage_control_period(const std::optional<Options>& options);
    void manage_torque_slope(const std::optional<Options>& options);
    void manage_torque_offset(const std::optional<Options>& options);
    void manage_position_offset(const std::optional<Options>& options);
    void manage_max_output_torque();
    void manage_max_output_velocity();
    void manage_max_motor_torque();
    void manage_max_motor_current();
    void manage_max_motor_velocity();
    void manage_user_max_motor_torque(const std::optional<Options>& options);
    void manage_user_max_motor_velocity(const std::optional<Options>& options);
    void manage_user_position_limits(const std::optional<Options>& options);
    void manage_quickstop_config(const std::optional<Options>& options);
    void manage_profile_config(const std::optional<Options>& options);
    void manage_endstop_config(const std::optional<Options>& options);
    void manage_digital_ios(const std::optional<Options>& options);
    void manage_digital_ios_end();
 
    void manage_torque_based_quick_stop();
    void force_target_output_torque(double target_torque);
    void force_target_motor_torque(double target_torque);
 
    static int8_t control_mode_to_code(control_mode_t);
    static control_mode_t control_code_to_mode(int8_t);
 
    static bool can_swap_mode(control_mode_t current, control_mode_t next);
};
 
} // namespace ethercatcpp