From ea0c0b480d4308e0c681e53183c5d879362ad271 Mon Sep 17 00:00:00 2001
From: Julian Viereck <jv1439@nyu.edu>
Date: Wed, 21 Jul 2021 16:12:14 -0400
Subject: [PATCH] Use the odri yaml file to load the robot configuration

---
 src/CMakeLists.txt | 10 +++++++
 src/solo12.cpp     | 70 +++++-----------------------------------------
 2 files changed, 17 insertions(+), 63 deletions(-)

diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 1a27b67..7287a80 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -40,6 +40,16 @@ create_robots_library(solo8)
 create_robots_library(solo8ti)
 create_robots_library(solo12)
 
+# YAML parameters.
+string(
+  CONCAT odri_control_interface_yaml_path
+         "${PythonModules_robot_properties_solo_PATH}/"
+         "robot_properties_solo/robot_properties_solo/odri_control_interface/"
+         "solo12_driver.yaml")
+target_compile_definitions(
+  solo12
+  PUBLIC ODRI_CONTROL_INTERFACE_YAML_PATH="${odri_control_interface_yaml_path}")
+
 #
 # Build executables like the calibration programs.
 #
diff --git a/src/solo12.cpp b/src/solo12.cpp
index 05b32f3..6260f5e 100644
--- a/src/solo12.cpp
+++ b/src/solo12.cpp
@@ -3,6 +3,7 @@
 #include <odri_control_interface/common.hpp>
 #include "solo/common_programs_header.hpp"
 #include "real_time_tools/spinner.hpp"
+#include "odri_control_interface/utils.hpp"
 
 namespace solo
 {
@@ -83,69 +84,12 @@ void Solo12::initialize(const std::string& network_id,
     serial_reader_ =
         std::make_shared<blmc_drivers::SerialReader>(serial_port, 5);
 
-    main_board_ptr_ = std::make_shared<MasterBoardInterface>(network_id_);
-
-    VectorXi motor_numbers(12);
-    motor_numbers << 0, 3, 2, 1, 5, 4, 6, 9, 8, 7, 11, 10;
-    VectorXb motor_reversed(12);
-    motor_reversed << false, true, true, true, false, false, false, true, true,
-        true, false, false;
-
-    double lHAA = 0.9;
-    double lHFE = 1.45;
-    double lKFE = 2.80;
-    Eigen::VectorXd joint_lower_limits(12);
-    joint_lower_limits << -lHAA, -lHFE, -lKFE, -lHAA, -lHFE, -lKFE, -lHAA,
-        -lHFE, -lKFE, -lHAA, -lHFE, -lKFE;
-    Eigen::VectorXd joint_upper_limits(12);
-    joint_upper_limits << lHAA, lHFE, lKFE, lHAA, lHFE, lKFE, lHAA, lHFE, lKFE,
-        lHAA, lHFE, lKFE;
-
-    // Define the joint module.
-    joints_ = std::make_shared<odri_control_interface::JointModules>(
-        main_board_ptr_,
-        motor_numbers,
-        motor_torque_constants_(0),
-        joint_gear_ratios_(0),
-        motor_max_current_(0),
-        motor_reversed,
-        joint_lower_limits,
-        joint_upper_limits,
-        80.,
-        0.2);
-
-    // Define the IMU.
-    VectorXl rotate_vector(3);
-    rotate_vector << 1, 2, 3;
-    VectorXl orientation_vector(4);
-    orientation_vector << 1, 2, 3, 4;
-    imu_ = std::make_shared<odri_control_interface::IMU>(
-        main_board_ptr_, rotate_vector, orientation_vector);
-
-    // Define the robot.
-    robot_ = std::make_shared<odri_control_interface::Robot>(
-        main_board_ptr_, joints_, imu_);
-
-    std::vector<odri_control_interface::CalibrationMethod> directions{
-        odri_control_interface::POSITIVE,
-        odri_control_interface::POSITIVE,
-        odri_control_interface::POSITIVE,
-        odri_control_interface::NEGATIVE,
-        odri_control_interface::POSITIVE,
-        odri_control_interface::POSITIVE,
-        odri_control_interface::POSITIVE,
-        odri_control_interface::POSITIVE,
-        odri_control_interface::POSITIVE,
-        odri_control_interface::NEGATIVE,
-        odri_control_interface::POSITIVE,
-        odri_control_interface::POSITIVE};
-
-    // Use zero position offsets for now. Gets updated in the calibration
-    // method.
-    Eigen::VectorXd position_offsets(12);
-    position_offsets.fill(0.);
-    calib_ctrl_ = std::make_shared<odri_control_interface::JointCalibrator>(
-        robot_->joints, directions, position_offsets, 5., 0.05, 3.0, 0.001);
+    // Main driver interface.
+    robot_ = odri_control_interface::RobotFromYamlFile(
+        network_id_, ODRI_CONTROL_INTERFACE_YAML_PATH);
+
+    calib_ctrl_ = odri_control_interface::JointCalibratorFromYamlFile(
+        ODRI_CONTROL_INTERFACE_YAML_PATH, robot_->joints);
 
     // Initialize the robot.
     robot_->Init();