dgm_solo12.hpp

#ifndef DGM_SOLO_HH
#define DGM_SOLO_HH

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

namespace dg_blmc_robots
{

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

    ~DGMSolo12();

    bool is_in_safety_mode();

    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::Vector12d& zero_to_index_angle);

    blmc_robots::Solo12 solo_;
    
    blmc_robots::Vector12d ctrl_joint_torques_;

    bool was_in_safety_mode_;

    blmc_robots::Vector12d zero_to_index_angle_from_file_;
  };


} // namespace dg_blmc_robots

#endif // DGM_TEST_BENCH_8_MOTORS_HH