dgm_solo8ti.hpp

#pragma once

#include <dynamic_graph_manager/dynamic_graph_manager.hh>
#include <blmc_robots/solo8ti.hpp>
#include "dg_blmc_robots/JointCalibration.h"
#include "yaml_cpp_catkin/yaml_cpp_fwd.hpp"

namespace dg_blmc_robots
{

  class DGMSolo8TI : public dynamic_graph::DynamicGraphManager
  {
  public:
    DGMSolo8TI();

    ~DGMSolo8TI();

    void initialize_hardware_communication_process();

    void get_sensors_to_map(dynamic_graph::VectorDGMap& map);

    void set_motor_controls_from_map(const dynamic_graph::VectorDGMap& map);

    bool calibrate_joint_position_callback(
        dg_blmc_robots::JointCalibration::Request& req,
        dg_blmc_robots::JointCalibration::Response& res);

  private:
    void calibrate_joint_position(const blmc_robots::Vector8d& zero_to_index_angle);

    blmc_robots::Solo8TI solo_;

    blmc_robots::Vector8d ctrl_joint_torques_;

    bool was_in_safety_mode_;

    blmc_robots::Vector8d zero_to_index_angle_from_file_;
  };


} // namespace dg_blmc_robots