├── .gitignore ├── CMakeLists.txt ├── demos ├── CMakeLists.txt ├── bolt_demo_calibration.cpp ├── bolt_demo_pd.cpp └── bolt_demo_sensor_reading.cpp ├── include └── bolt │ ├── bolt.hpp │ ├── bolt_humanoid.hpp │ ├── dgm_bolt.hpp │ ├── dgm_bolt_humanoid.hpp │ └── utils.hpp ├── license.txt ├── package.xml ├── python └── bolt │ ├── __init__.py │ ├── dg_bolt_base_bullet.py │ └── dg_bolt_bullet.py ├── readme.md ├── setup.py ├── src ├── CMakeLists.txt ├── bolt.cpp ├── bolt_humanoid.cpp ├── dgm_bolt.cpp ├── dgm_bolt_humanoid.cpp └── programs │ ├── bolt_dg.cpp │ ├── bolt_hw.cpp │ ├── dg_main_bolt.cpp │ ├── dg_main_bolt_humanoid.cpp │ ├── hardware_calibration.cpp │ └── hardware_humanoid_calibration.cpp ├── srcpy ├── CMakeLists.txt └── py_bolt.cpp └── tests ├── CMakeLists.txt └── main.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | *.user 2 | *.pyc 3 | *build* 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2019, New York University and Max Planck Gesellschaft. License 3 | # BSD-3 clause 4 | # 5 | 6 | # 7 | # set up the project 8 | # 9 | cmake_minimum_required(VERSION 3.10.2) 10 | 11 | project(bolt VERSION 1.0.0) 12 | 13 | # specify the C++ 17 standard. 14 | set(CMAKE_CXX_STANDARD 17) 15 | set(CMAKE_CXX_STANDARD_REQUIRED True) 16 | 17 | # external dependencies. 18 | find_package(mpi_cmake_modules REQUIRED) 19 | find_package(pybind11 REQUIRED) 20 | 21 | # local dependencies. 22 | find_package(ament_cmake REQUIRED) 23 | find_package(odri_control_interface REQUIRED) 24 | find_package(slider_box REQUIRED) 25 | find_package(real_time_tools REQUIRED) 26 | find_package(yaml_utils REQUIRED) 27 | find_package(PythonModules REQUIRED COMPONENTS robot_properties_bolt) 28 | 29 | # optional dependencies. 30 | find_package(dynamic_graph_manager QUIET) 31 | 32 | # Export dependencies. 33 | ament_export_dependencies( 34 | odri_control_interface 35 | real_time_tools 36 | yaml_utils 37 | slider_box 38 | ) 39 | if(${dynamic_graph_manager_FOUND}) 40 | ament_export_dependencies(dynamic_graph_manager) 41 | endif() 42 | 43 | # prepare the final export 44 | ament_export_interfaces(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) 45 | 46 | # 47 | # manage the creation of the libraries and executables 48 | # 49 | add_subdirectory(src) 50 | 51 | # 52 | # Manage the unit tests. 53 | # 54 | if(BUILD_TESTING) 55 | add_subdirectory(tests) 56 | endif() 57 | 58 | # 59 | # Manage the demos. 60 | # 61 | add_subdirectory(demos) 62 | 63 | # 64 | # Python bindings. 65 | # 66 | add_subdirectory(srcpy) 67 | 68 | # 69 | # Install the package 70 | # 71 | 72 | # Install includes. 73 | install(DIRECTORY include/ DESTINATION include) 74 | 75 | # Install python files. 76 | get_python_install_dir(python_install_dir) 77 | install( 78 | DIRECTORY python/${PROJECT_NAME} 79 | DESTINATION "${python_install_dir}" 80 | PATTERN "*.pyc" EXCLUDE 81 | PATTERN "__pycache__" EXCLUDE) 82 | 83 | # 84 | # Building documentation. 85 | # 86 | add_documentation() 87 | 88 | # 89 | # Export this package as a cmake package. 90 | # 91 | ament_package() 92 | -------------------------------------------------------------------------------- /demos/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # Declare a simple macro to build the demos. 3 | # 4 | 5 | set(all_demo_targets) 6 | 7 | string( 8 | CONCAT odri_control_interface_yaml_path 9 | "${PythonModules_robot_properties_bolt_PATH}/" 10 | "robot_properties_bolt/robot_properties_bolt/odri_control_interface/" 11 | "bolt_driver.yaml") 12 | 13 | macro(create_demo demo_name) 14 | # Add the executable of the demo. 15 | add_executable(${demo_name} ${demo_name}.cpp) 16 | 17 | target_include_directories( 18 | ${demo_name} PUBLIC $ 19 | $) 20 | 21 | # Add the dependencies. 22 | target_link_libraries(${demo_name} ${PROJECT_NAME}) 23 | 24 | # Check if the file exists. if(package_name AND resource_path) 25 | target_compile_definitions( 26 | ${demo_name} 27 | PUBLIC 28 | ODRI_CONTROL_INTERFACE_YAML_PATH="${odri_control_interface_yaml_path}") 29 | 30 | # Install demo 31 | install( 32 | TARGETS ${demo_name} 33 | LIBRARY DESTINATION lib 34 | ARCHIVE DESTINATION lib 35 | RUNTIME DESTINATION bin 36 | INCLUDES 37 | DESTINATION include) 38 | 39 | # Install a symlink of the executable in lib/bolt 40 | string( 41 | CONCAT symlink_command 42 | "execute_process(" 43 | " COMMAND ${CMAKE_COMMAND} -E make_directory " 44 | " ${CMAKE_INSTALL_PREFIX}/bin/)\n" 45 | "execute_process(" 46 | " COMMAND ${CMAKE_COMMAND} -E create_symlink " 47 | " ${CMAKE_INSTALL_PREFIX}/bin/${demo_name}" 48 | " ${CMAKE_INSTALL_PREFIX}/lib/${PROJECT_NAME}/${demo_name})" 49 | ) 50 | install(CODE ${symlink_command}) 51 | 52 | 53 | endmacro(create_demo demo_name) 54 | 55 | # 56 | # Build demos. 57 | # 58 | 59 | create_demo(bolt_demo_pd) 60 | 61 | create_demo(bolt_demo_calibration) 62 | 63 | create_demo(bolt_demo_sensor_reading) 64 | -------------------------------------------------------------------------------- /demos/bolt_demo_calibration.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file demo_solo12_calibration.cpp 3 | * @author Maximilien Naveau (maximilien.naveau@gmail.com) 4 | * @brief Small demo to test the calibration on the real robot. 5 | * @version 0.1 6 | * @date 2019-11-08 7 | * 8 | * @copyright Copyright (c) 2019 9 | * 10 | */ 11 | 12 | #include "bolt/bolt.hpp" 13 | #include "bolt/utils.hpp" 14 | 15 | using namespace bolt; 16 | 17 | static THREAD_FUNCTION_RETURN_TYPE control_loop(void* robot_void_ptr) 18 | { 19 | Bolt& robot = *(static_cast(robot_void_ptr)); 20 | 21 | Eigen::Vector6d zero_torques = Eigen::Vector6d::Zero(); 22 | 23 | robot.wait_until_ready(); 24 | 25 | // ask for calibration 26 | robot.request_calibration(); 27 | 28 | // The calibration commend is computed in the send_target_joint_torque. 29 | real_time_tools::Spinner spinner; 30 | spinner.set_period(0.001); 31 | rt_printf("Running calibration...\n"); 32 | while (!CTRL_C_DETECTED && robot.is_calibrating()) 33 | { 34 | robot.acquire_sensors(); 35 | robot.send_target_joint_torque(zero_torques); 36 | spinner.spin(); 37 | } 38 | rt_printf("Running calibration... Done.\n"); 39 | 40 | rt_printf("Go idle indefinitely, ctrl+c to quit.\n"); 41 | robot.send_target_joint_torque(zero_torques); 42 | int count = 0; 43 | spinner.set_period(0.001); 44 | while (!CTRL_C_DETECTED) 45 | { 46 | robot.acquire_sensors(); 47 | // re-send 0 torque command 48 | robot.send_target_joint_torque(zero_torques); 49 | spinner.spin(); 50 | ++count; 51 | } 52 | 53 | CTRL_C_DETECTED = true; 54 | return THREAD_FUNCTION_RETURN_VALUE; 55 | } // end control_loop 56 | 57 | int main(int argc, char** argv) 58 | { 59 | if (argc != 2) 60 | { 61 | throw std::runtime_error( 62 | "Wrong number of argument: `sudo ./hardware_calibration " 63 | "network_id`."); 64 | } 65 | 66 | enable_ctrl_c(); 67 | 68 | real_time_tools::RealTimeThread thread; 69 | 70 | Bolt robot; 71 | robot.initialize(argv[1]); 72 | 73 | rt_printf("Controller is set up.\n"); 74 | 75 | rt_printf("Press enter to launch the calibration.\n"); 76 | char str[256]; 77 | std::cin.get(str, 256); // get c-string 78 | 79 | thread.create_realtime_thread(&control_loop, &robot); 80 | 81 | // Wait until the application is killed. 82 | thread.join(); 83 | 84 | rt_printf("Exit cleanly.\n"); 85 | 86 | return 0; 87 | } 88 | -------------------------------------------------------------------------------- /demos/bolt_demo_pd.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file demo_bolt.cpp 3 | * \brief Implements basic PD controller reading slider values. 4 | * \author Julian Viereck 5 | * \date 21 November 2019 6 | * 7 | * This file uses the Solo12 class in a small demo. 8 | */ 9 | 10 | #include "bolt/bolt.hpp" 11 | #include "bolt/utils.hpp" 12 | 13 | using namespace bolt; 14 | 15 | static THREAD_FUNCTION_RETURN_TYPE control_loop(void* args) 16 | { 17 | Bolt& robot = *static_cast(args); 18 | 19 | // Using conversion from PD gains from example.cpp 20 | double kp = 3.0 * 9 * 0.025; 21 | double kd = 0.1 * 9 * 0.025; 22 | double t = 0.0; 23 | double dt = 0.001; 24 | double freq = 0.3; 25 | double amplitude = M_PI / 8.0; 26 | Eigen::Vector6d desired_joint_position = Eigen::Vector6d::Zero(); 27 | Eigen::Vector6d desired_torque = Eigen::Vector6d::Zero(); 28 | 29 | Eigen::Vector6d init_pose; 30 | Eigen::Matrix motor_enabled; 31 | 32 | robot.acquire_sensors(); 33 | Eigen::Vector6d initial_joint_positions = robot.get_joint_positions(); 34 | 35 | rt_printf("control loop started \n"); 36 | 37 | robot.wait_until_ready(); 38 | 39 | size_t count = 0; 40 | while (!CTRL_C_DETECTED) 41 | { 42 | // acquire the sensors 43 | robot.acquire_sensors(); 44 | 45 | // acquire the motor enabled signal. 46 | motor_enabled = robot.get_motor_enabled(); 47 | 48 | // Desired pose and vel 49 | desired_joint_position = 50 | initial_joint_positions + 51 | Eigen::Vector6d::Ones() * amplitude * sin(2 * M_PI * freq * t); 52 | t += dt; 53 | 54 | // we implement here a small pd control at the current level 55 | desired_torque = 56 | kp * (desired_joint_position - robot.get_joint_positions()) - 57 | kd * robot.get_joint_velocities(); 58 | 59 | // print ----------------------------------------------------------- 60 | if ((count % 1000) == 0) 61 | { 62 | rt_printf("\33[H\33[2J"); // clear screen 63 | print_vector("des joint_tau : ", desired_torque); 64 | print_vector("des joint_pos : ", desired_joint_position); 65 | rt_printf("\n"); 66 | print_vector("act joint_pos : ", robot.get_joint_positions()); 67 | print_vector("act joint_vel : ", robot.get_joint_velocities()); 68 | print_vector("act slider pos : ", robot.get_slider_positions()); 69 | rt_printf("act e-stop : %s\n", 70 | robot.get_active_estop() ? "true" : "false"); 71 | 72 | fflush(stdout); 73 | } 74 | ++count; 75 | 76 | // Send the current to the motor 77 | robot.send_target_joint_torque(desired_torque); 78 | 79 | real_time_tools::Timer::sleep_sec(0.001); 80 | } // endwhile 81 | return THREAD_FUNCTION_RETURN_VALUE; 82 | } // end control_loop 83 | 84 | int main(int argc, char** argv) 85 | { 86 | if (argc != 2) 87 | { 88 | throw std::runtime_error( 89 | "Wrong number of argument: `./demo_bolt network_id`."); 90 | } 91 | 92 | real_time_tools::RealTimeThread thread; 93 | enable_ctrl_c(); 94 | 95 | Bolt robot; 96 | robot.initialize(std::string(argv[1])); 97 | 98 | rt_printf("controller is set up \n"); 99 | thread.create_realtime_thread(&control_loop, &robot); 100 | 101 | rt_printf("control loop started \n"); 102 | while (!CTRL_C_DETECTED) 103 | { 104 | real_time_tools::Timer::sleep_sec(0.001); 105 | } 106 | 107 | thread.join(); 108 | 109 | return 0; 110 | } 111 | -------------------------------------------------------------------------------- /demos/bolt_demo_sensor_reading.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file demo_bolt.cpp 3 | * \brief Implements basic PD controller reading slider values. 4 | * \author Julian Viereck 5 | * \date 21 November 2019 6 | * 7 | * This file uses the Solo12 class in a small demo. 8 | */ 9 | 10 | #include "bolt/bolt.hpp" 11 | #include "bolt/utils.hpp" 12 | 13 | using namespace bolt; 14 | 15 | static THREAD_FUNCTION_RETURN_TYPE control_loop(void* args) 16 | { 17 | Bolt& robot = *static_cast(args); 18 | 19 | // 0 torques 20 | Eigen::Vector6d desired_torque = Eigen::Vector6d::Zero(); 21 | rt_printf("Sensor reading loop started \n"); 22 | 23 | robot.wait_until_ready(); 24 | 25 | size_t count = 0; 26 | while (!CTRL_C_DETECTED) 27 | { 28 | // acquire the sensors 29 | robot.acquire_sensors(); 30 | 31 | // print ----------------------------------------------------------- 32 | if ((count % 1000) == 0) 33 | { 34 | rt_printf("\33[H\33[2J"); // clear screen 35 | rt_printf("Sensory data:"); 36 | rt_printf("\n"); 37 | print_vector("des joint_tau ", 38 | desired_torque); 39 | print_vector("act joint_pos ", 40 | robot.get_joint_positions()); 41 | print_vector("act joint_vel ", 42 | robot.get_joint_velocities()); 43 | print_vector("act joint torq ", 44 | robot.get_joint_target_torques()); 45 | print_vector("act joint target torq ", 46 | robot.get_joint_torques()); 47 | print_vector_bool("act status motor ready ", 48 | robot.get_motor_ready()); 49 | print_vector_bool("act status motor enabled ", 50 | robot.get_motor_enabled()); 51 | print_vector_bool("act status motor board enabled ", 52 | robot.get_motor_board_enabled()); 53 | print_vector_int("act status motor board errors ", 54 | robot.get_motor_board_errors()); 55 | print_vector("act slider pos ", 56 | robot.get_slider_positions()); 57 | print_vector("act imu quat ", 58 | robot.get_base_attitude()); 59 | print_vector("act imu acc ", 60 | robot.get_base_accelerometer()); 61 | print_vector("act imu gyroscope ", 62 | robot.get_base_gyroscope()); 63 | print_vector("act imu lin acc ", 64 | robot.get_base_linear_acceleration()); 65 | rt_printf("act e-stop : %s\n", 66 | robot.get_active_estop() ? "true" : "false"); 67 | rt_printf("has error : %s\n", 68 | robot.has_error() ? "true" : "false"); 69 | rt_printf("\n"); 70 | fflush(stdout); 71 | } 72 | ++count; 73 | 74 | // Send the current to the motor 75 | // desired_torque.setZero(); 76 | robot.send_target_joint_torque(desired_torque); 77 | 78 | real_time_tools::Timer::sleep_sec(0.001); 79 | } // endwhile 80 | return THREAD_FUNCTION_RETURN_VALUE; 81 | } // end control_loop 82 | 83 | int main(int argc, char** argv) 84 | { 85 | if (argc != 2) 86 | { 87 | throw std::runtime_error( 88 | "Wrong number of argument: `./demo_bolt network_id`."); 89 | } 90 | 91 | real_time_tools::RealTimeThread thread; 92 | enable_ctrl_c(); 93 | 94 | Bolt robot; 95 | robot.initialize(std::string(argv[1])); 96 | 97 | rt_printf("sensor reader is set up \n"); 98 | thread.create_realtime_thread(&control_loop, &robot); 99 | while (!CTRL_C_DETECTED) 100 | { 101 | real_time_tools::Timer::sleep_sec(0.001); 102 | } 103 | 104 | thread.join(); 105 | 106 | return 0; 107 | } 108 | -------------------------------------------------------------------------------- /include/bolt/bolt.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @license BSD 3-clause 4 | * @copyright Copyright (c) 2021, New York University and Max Planck 5 | * Gesellschaft 6 | * 7 | * @brief Bolt biped low level drivers. 8 | */ 9 | 10 | #pragma once 11 | 12 | #include 13 | 14 | #include 15 | 16 | #include "slider_box/serial_reader.hpp" 17 | #include "odri_control_interface/calibration.hpp" 18 | #include "odri_control_interface/robot.hpp" 19 | 20 | namespace Eigen 21 | { 22 | /** @brief Eigen shortcut for vector of size 6. */ 23 | typedef Matrix Vector6d; 24 | } // namespace Eigen 25 | 26 | namespace bolt 27 | { 28 | #define BOLT_NB_MOTOR_BOARD 3 29 | #define BOLT_NB_MOTOR 6 30 | #define BOLT_NB_SLIDER 4 31 | 32 | /** @brief Control state of the robot. */ 33 | enum BoltControlState 34 | { 35 | initial, 36 | ready, 37 | calibrate 38 | }; 39 | 40 | /** 41 | * @brief Driver for the Bolt biped robot. 42 | */ 43 | class Bolt 44 | { 45 | public: 46 | /** 47 | * @brief Bolt is the constructor of the class. 48 | */ 49 | Bolt(); 50 | 51 | /** 52 | * @brief initialize the robot by setting aligning the motors and calibrate 53 | * the sensors to 0 54 | */ 55 | void initialize(const std::string& network_id); 56 | 57 | /** 58 | * @brief send_target_torques sends the target currents to the motors 59 | */ 60 | void send_target_joint_torque( 61 | const Eigen::Ref target_joint_torque); 62 | 63 | /** 64 | * @brief acquire_sensors acquire all available sensors, WARNING !!!! 65 | * this method has to be called prior to any getter to have up to date data. 66 | */ 67 | void acquire_sensors(); 68 | 69 | /** 70 | * @brief Wait until the hardware is ready to be controlled. 71 | */ 72 | void wait_until_ready(); 73 | 74 | /** 75 | * @brief Fill attitude quaternion. 76 | */ 77 | void fill_base_attitude_quaternion(); 78 | 79 | /** 80 | * @brief Request calibration of the joints by moving to the next joint 81 | * index position. The control is made inside the send_target_joint_torque. 82 | * 83 | * @param home_offset_rad This is the angle between the index and the zero 84 | * pose. 85 | * @return true 86 | * @return false 87 | */ 88 | void request_calibration( 89 | const Eigen::Ref home_offset_rad); 90 | 91 | /** 92 | * @brief Request calibration of the joints by moving to the next joint 93 | * index position. The control is made inside the send_target_joint_torque. 94 | * Use the yaml information in order to get the offset from the nearest 95 | * motor index. 96 | * 97 | * @return true 98 | * @return false 99 | */ 100 | void request_calibration(); 101 | 102 | /** 103 | * Sensor Data 104 | */ 105 | 106 | /** 107 | * @brief get_joint_positions 108 | * @return the joint angle of each module 109 | * WARNING !!!! 110 | * The method "()" has to be called 111 | * prior to any getter to have up to date data. 112 | */ 113 | const Eigen::Ref get_joint_positions() 114 | { 115 | return joint_positions_; 116 | } 117 | 118 | /** 119 | * @brief get_joint_velocities 120 | * @return the joint velocities 121 | * WARNING !!!! 122 | * The method "()" has to be called 123 | * prior to any getter to have up to date data. 124 | */ 125 | const Eigen::Ref get_joint_velocities() 126 | { 127 | return joint_velocities_; 128 | } 129 | 130 | /** 131 | * @brief get_joint_torques 132 | * @return the joint torques 133 | * WARNING !!!! 134 | * The method "()" has to be called 135 | * prior to any getter to have up to date data. 136 | */ 137 | const Eigen::Ref get_joint_torques() 138 | { 139 | return joint_torques_; 140 | } 141 | 142 | /** 143 | * @brief get_joint_torques 144 | * @return the target joint torques 145 | * WARNING !!!! 146 | * The method "()" has to be called 147 | * prior to any getter to have up to date data. 148 | 149 | */ 150 | const Eigen::Ref get_joint_target_torques() 151 | { 152 | return joint_target_torques_; 153 | } 154 | 155 | /** 156 | * @brief get_base_accelerometer 157 | * @return the base_accelerometer 158 | * WARNING !!!! 159 | * The method "()" has to be called 160 | * prior to any getter to have up to date data. 161 | */ 162 | const Eigen::Ref get_base_accelerometer() 163 | { 164 | return base_accelerometer_; 165 | } 166 | 167 | /** 168 | * @brief get_base_accelerometer 169 | * @return the base_accelerometer 170 | * WARNING !!!! 171 | * The method "()" has to be called 172 | * prior to any getter to have up to date data. 173 | */ 174 | const Eigen::Ref get_base_gyroscope() 175 | { 176 | return base_gyroscope_; 177 | } 178 | 179 | /** 180 | * @brief get_base_accelerometer 181 | * @return the base_accelerometer 182 | * WARNING !!!! 183 | * The method "()" has to be called 184 | * prior to any getter to have up to date data. 185 | */ 186 | const Eigen::Ref get_base_attitude() 187 | { 188 | return base_attitude_; 189 | } 190 | 191 | /** 192 | * @brief get_base_accelerometer 193 | * @return the base_accelerometer 194 | * WARNING !!!! 195 | * The method "()" has to be called 196 | * prior to any getter to have up to date data. 197 | */ 198 | const Eigen::Ref get_base_linear_acceleration() 199 | { 200 | return base_linear_acceleration_; 201 | } 202 | 203 | /** 204 | * Hardware Status 205 | */ 206 | 207 | /** 208 | * @brief get_motor_enabled 209 | * @return This gives the status (enabled/disabled) of each motors using the 210 | * joint ordering convention. 211 | */ 212 | const Eigen::Ref > 213 | get_motor_enabled() 214 | { 215 | return motor_enabled_; 216 | } 217 | 218 | /** 219 | * @brief get_motor_ready 220 | * @return This gives the status (enabled/disabled) of each motors using the 221 | * joint ordering convention. 222 | */ 223 | const Eigen::Ref > 224 | get_motor_ready() 225 | { 226 | return motor_ready_; 227 | } 228 | 229 | /** 230 | * @brief get_motor_board_enabled 231 | * @return This gives the status (enabled/disabled of the onboard control 232 | * cards). 233 | */ 234 | const Eigen::Ref > 235 | get_motor_board_enabled() 236 | { 237 | return motor_board_enabled_; 238 | } 239 | 240 | /** 241 | * @brief get_motor_board_errors 242 | * @return This gives the status (enabled/disabled of the onboard control 243 | * cards). 244 | */ 245 | const Eigen::Ref > 246 | get_motor_board_errors() 247 | { 248 | return motor_board_errors_; 249 | } 250 | 251 | /** 252 | * @brief has_error 253 | * @return Returns true if the robot hardware has an error, false otherwise. 254 | */ 255 | bool has_error() const 256 | { 257 | return robot_->HasError(); 258 | } 259 | 260 | /* 261 | * Additional data 262 | */ 263 | 264 | /** 265 | * @brief Get the slider positions in [0, 1] 266 | * 267 | * @return Eigen::Ref > 268 | */ 269 | const Eigen::Ref > 270 | get_slider_positions() 271 | { 272 | return slider_positions_; 273 | } 274 | 275 | /** 276 | * @brief Get the active estop value 277 | * 278 | * @return true if the button is pressed. 279 | * @return false otherwize. 280 | */ 281 | bool get_active_estop() const 282 | { 283 | return active_estop_; 284 | } 285 | 286 | /** 287 | * @brief is_calibrating() 288 | * @return Returns true if the calibration procedure is running right now. 289 | */ 290 | bool is_calibrating() 291 | { 292 | return (control_state_ == BoltControlState::calibrate) || calibrate_request_; 293 | } 294 | 295 | private: 296 | /* 297 | * Hardware status 298 | */ 299 | 300 | /** @brief Motor status (enabled/disabled). */ 301 | Eigen::Matrix motor_enabled_; 302 | 303 | /** @brief Motor readiness to receive commands (ready/not ready). */ 304 | Eigen::Matrix motor_ready_; 305 | 306 | /** @brief Motor Board status (enabled/disabled). */ 307 | Eigen::Matrix motor_board_enabled_; 308 | 309 | /** @brief Motor Board Error code (int). 310 | */ 311 | Eigen::Matrix motor_board_errors_; 312 | 313 | /* 314 | * Joint data 315 | */ 316 | 317 | /** @brief Joint positions. */ 318 | Eigen::Vector6d joint_positions_; 319 | 320 | /** @brief Joint velocities. */ 321 | Eigen::Vector6d joint_velocities_; 322 | 323 | /** @brief Joint torques. */ 324 | Eigen::Vector6d joint_torques_; 325 | 326 | /** @brief Target joint torques, reference from the controller. */ 327 | Eigen::Vector6d joint_target_torques_; 328 | 329 | /* 330 | * Additional data 331 | */ 332 | 333 | /** @brief Name of the network lan: Left column in ifconfig output. */ 334 | std::string network_id_; 335 | 336 | /** @brief Slider position in [0, 1]. */ 337 | Eigen::Matrix slider_positions_; 338 | 339 | /** @brief E-stop from the slider box. */ 340 | bool active_estop_; 341 | 342 | /** @brief base accelerometer. */ 343 | Eigen::Vector3d base_accelerometer_; 344 | 345 | /** @brief base accelerometer. */ 346 | Eigen::Vector3d base_gyroscope_; 347 | 348 | /** @brief base accelerometer. */ 349 | Eigen::Vector4d base_attitude_; 350 | 351 | /** @brief base accelerometer. */ 352 | Eigen::Vector3d base_linear_acceleration_; 353 | 354 | /** @brief Integers from the serial port. 355 | * - 4 sliders in [0, 1024] 356 | * - emergency stop 357 | */ 358 | std::vector slider_box_data_; 359 | 360 | /* 361 | * Controllers 362 | */ 363 | 364 | /** @brief Controller to run the calibration procedure */ 365 | std::shared_ptr calib_ctrl_; 366 | 367 | /* 368 | * Finite state machine of the controbolller. 369 | */ 370 | 371 | /** @brief Control state Initial, Ready, Calibration */ 372 | BoltControlState control_state_; 373 | 374 | /** @brief Check if the user called for the joint calibration. */ 375 | bool calibrate_request_; 376 | 377 | /** @brief Number of time we acquire the sensor readings, this is used 378 | * to prevent spamming prints */ 379 | long int nb_time_we_acquired_sensors_; 380 | 381 | /* 382 | * Drivers communication objects 383 | */ 384 | 385 | /** 386 | * @brief Robot complete interface. 387 | * Wrapper around the MasterBoardInterface. 388 | * 389 | * PC <------------------> main board <-------> Motor Board 390 | * Ethernet/Wifi SPI 391 | */ 392 | std::shared_ptr robot_; 393 | 394 | /** 395 | * @brief Reader for serial port to read arduino slider values. 396 | */ 397 | std::shared_ptr serial_reader_; 398 | }; 399 | 400 | } // namespace bolt 401 | -------------------------------------------------------------------------------- /include/bolt/bolt_humanoid.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @license BSD 3-clause 4 | * @copyright Copyright (c) 2021, New York University and Max Planck 5 | * Gesellschaft 6 | * 7 | * @brief Bolt humanoid low level drivers. 8 | */ 9 | 10 | #pragma once 11 | 12 | #include 13 | 14 | #include 15 | 16 | #include "odri_control_interface/calibration.hpp" 17 | #include "odri_control_interface/robot.hpp" 18 | #include "slider_box/serial_reader.hpp" 19 | 20 | namespace Eigen 21 | { 22 | /** @brief Eigen shortcut for vector of size 9. */ 23 | typedef Matrix Vector9d; 24 | } // namespace Eigen 25 | 26 | namespace bolt 27 | { 28 | #define BOLT_HUMANOID_NB_MOTOR_BOARD 5 29 | #define BOLT_HUMANOID_NB_MOTOR 9 30 | #define BOLT_HUMANOID_NB_SLIDER 4 31 | 32 | /** @brief Control state of the robot. */ 33 | enum BoltControlState 34 | { 35 | initial, 36 | ready, 37 | calibrate 38 | }; 39 | 40 | /** 41 | * @brief Driver for the Bolt biped robot. 42 | */ 43 | class BoltHumanoid 44 | { 45 | public: 46 | //! @brief Set slider box port to this value to disable it. 47 | inline static const std::string SLIDER_BOX_DISABLED = "none"; 48 | 49 | /** 50 | * @brief Bolt is the constructor of the class. 51 | */ 52 | BoltHumanoid(); 53 | 54 | /** 55 | * @brief initialize the robot by setting aligning the motors and calibrate 56 | * the sensors to 0 57 | * 58 | * @param network_id Name of the network interface for connection to the 59 | * robot. 60 | * @param slider_box_port Name of the serial port to which the slider box is 61 | * connected. Set to "" or "none" if no slider box is used. Set to 62 | * "auto" to auto-detect the port. 63 | */ 64 | void initialize(const std::string& network_id, 65 | const std::string& slider_box_port = SLIDER_BOX_DISABLED); 66 | 67 | /** 68 | * @brief Sets the maximum motor currents. 69 | */ 70 | void set_max_current(const double& max_current); 71 | 72 | /** 73 | * @brief send_target_torques sends the target currents to the motors 74 | */ 75 | void send_target_joint_torque( 76 | const Eigen::Ref target_joint_torque); 77 | 78 | /** 79 | * @brief Sets the desired joint position of the P controller running on 80 | * the card. 81 | */ 82 | void send_target_joint_position( 83 | const Eigen::Ref target_joint_position); 84 | 85 | /** 86 | * @brief Sets the desired joint velocity of the D controller running on 87 | * the card. 88 | */ 89 | void send_target_joint_velocity( 90 | const Eigen::Ref target_joint_velocity); 91 | 92 | /** 93 | * @brief Sets the desired joint position gain P for the P controller 94 | * running on the card. 95 | */ 96 | void send_target_joint_position_gains( 97 | const Eigen::Ref target_joint_position_gains); 98 | 99 | /** 100 | * @brief Sets the desired joint velocity gain D for the D controller 101 | * running on the card. 102 | */ 103 | void send_target_joint_velocity_gains( 104 | const Eigen::Ref target_joint_velocity_gains); 105 | 106 | /** 107 | * @brief acquire_sensors acquire all available sensors, WARNING !!!! 108 | * this method has to be called prior to any getter to have up to date data. 109 | */ 110 | void acquire_sensors(); 111 | 112 | /** 113 | * @brief Wait until the hardware is ready to be controlled. 114 | */ 115 | void wait_until_ready(); 116 | 117 | /** 118 | * @brief Check if the robot is ready. 119 | */ 120 | bool is_ready(); 121 | 122 | /** 123 | * @brief Fill attitude quaternion. 124 | */ 125 | void fill_base_attitude_quaternion(); 126 | 127 | /** 128 | * @brief Request calibration of the joints by moving to the next joint 129 | * index position. The control is made inside the send_target_joint_torque. 130 | * 131 | * @param home_offset_rad This is the angle between the index and the zero 132 | * pose. 133 | * @return true 134 | * @return false 135 | */ 136 | void request_calibration( 137 | const Eigen::Ref home_offset_rad); 138 | 139 | /** 140 | * @brief Request calibration of the joints by moving to the next joint 141 | * index position. The control is made inside the send_target_joint_torque. 142 | * Use the yaml information in order to get the offset from the nearest 143 | * motor index. 144 | * 145 | * @return true 146 | * @return false 147 | */ 148 | void request_calibration(); 149 | 150 | /** 151 | * Sensor Data 152 | */ 153 | 154 | /** 155 | * @brief get_joint_positions 156 | * @return the joint angle of each module 157 | * WARNING !!!! 158 | * The method "()" has to be called 159 | * prior to any getter to have up to date data. 160 | */ 161 | const Eigen::Ref get_joint_positions() 162 | { 163 | return joint_positions_; 164 | } 165 | 166 | /** 167 | * @brief get_joint_velocities 168 | * @return the joint velocities 169 | * WARNING !!!! 170 | * The method "()" has to be called 171 | * prior to any getter to have up to date data. 172 | */ 173 | const Eigen::Ref get_joint_velocities() 174 | { 175 | return joint_velocities_; 176 | } 177 | 178 | /** 179 | * @brief get_joint_torques 180 | * @return the joint torques 181 | * WARNING !!!! 182 | * The method "()" has to be called 183 | * prior to any getter to have up to date data. 184 | */ 185 | const Eigen::Ref get_joint_torques() 186 | { 187 | return joint_torques_; 188 | } 189 | 190 | /** 191 | * @brief get_joint_torques 192 | * @return the target joint torques 193 | * WARNING !!!! 194 | * The method "()" has to be called 195 | * prior to any getter to have up to date data. 196 | 197 | */ 198 | const Eigen::Ref get_joint_target_torques() 199 | { 200 | return joint_target_torques_; 201 | } 202 | 203 | /** 204 | * @brief get_base_accelerometer 205 | * @return the base_accelerometer 206 | * WARNING !!!! 207 | * The method "()" has to be called 208 | * prior to any getter to have up to date data. 209 | */ 210 | const Eigen::Ref get_base_accelerometer() 211 | { 212 | return base_accelerometer_; 213 | } 214 | 215 | /** 216 | * @brief get_base_accelerometer 217 | * @return the base_accelerometer 218 | * WARNING !!!! 219 | * The method "()" has to be called 220 | * prior to any getter to have up to date data. 221 | */ 222 | const Eigen::Ref get_base_gyroscope() 223 | { 224 | return base_gyroscope_; 225 | } 226 | 227 | /** 228 | * @brief get_base_accelerometer 229 | * @return the base_accelerometer 230 | * WARNING !!!! 231 | * The method "()" has to be called 232 | * prior to any getter to have up to date data. 233 | */ 234 | const Eigen::Ref get_base_attitude() 235 | { 236 | return base_attitude_; 237 | } 238 | 239 | /** 240 | * @brief get_base_accelerometer 241 | * @return the base_accelerometer 242 | * WARNING !!!! 243 | * The method "()" has to be called 244 | * prior to any getter to have up to date data. 245 | */ 246 | const Eigen::Ref get_base_linear_acceleration() 247 | { 248 | return base_linear_acceleration_; 249 | } 250 | 251 | /** 252 | * Hardware Status 253 | */ 254 | 255 | /** 256 | * @brief get_motor_enabled 257 | * @return This gives the status (enabled/disabled) of each motors using the 258 | * joint ordering convention. 259 | */ 260 | const Eigen::Ref > 261 | get_motor_enabled() 262 | { 263 | return motor_enabled_; 264 | } 265 | 266 | /** 267 | * @brief get_motor_ready 268 | * @return This gives the status (enabled/disabled) of each motors using the 269 | * joint ordering convention. 270 | */ 271 | const Eigen::Ref > 272 | get_motor_ready() 273 | { 274 | return motor_ready_; 275 | } 276 | 277 | /** 278 | * @brief get_motor_board_enabled 279 | * @return This gives the status (enabled/disabled of the onboard control 280 | * cards). 281 | */ 282 | const Eigen::Ref< 283 | const Eigen::Matrix > 284 | get_motor_board_enabled() 285 | { 286 | return motor_board_enabled_; 287 | } 288 | 289 | /** 290 | * @brief get_motor_board_errors 291 | * @return This gives the status (enabled/disabled of the onboard control 292 | * cards). 293 | */ 294 | const Eigen::Ref > 295 | get_motor_board_errors() 296 | { 297 | return motor_board_errors_; 298 | } 299 | 300 | /** 301 | * @brief has_error 302 | * @return Returns true if the robot hardware has an error, false otherwise. 303 | */ 304 | bool has_error() const 305 | { 306 | return robot_->HasError(); 307 | } 308 | 309 | /* 310 | * Additional data 311 | */ 312 | 313 | /** 314 | * @brief Get the slider positions in [0, 1] 315 | * 316 | * @return Eigen::Ref > 317 | */ 318 | const Eigen::Ref > 319 | get_slider_positions() 320 | { 321 | return slider_positions_; 322 | } 323 | 324 | /** 325 | * @brief Get the active estop value 326 | * 327 | * @return true if the button is pressed. 328 | * @return false otherwize. 329 | */ 330 | bool get_active_estop() const 331 | { 332 | return active_estop_; 333 | } 334 | 335 | /** 336 | * @brief is_calibrating() 337 | * @return Returns true if the calibration procedure is running right now. 338 | */ 339 | bool is_calibrating() 340 | { 341 | return (control_state_ == BoltControlState::calibrate) || 342 | calibrate_request_; 343 | } 344 | 345 | private: 346 | /* 347 | * Hardware status 348 | */ 349 | 350 | /** @brief Motor status (enabled/disabled). */ 351 | Eigen::Matrix motor_enabled_; 352 | 353 | /** @brief Motor readiness to receive commands (ready/not ready). */ 354 | Eigen::Matrix motor_ready_; 355 | 356 | /** @brief Motor Board status (enabled/disabled). */ 357 | Eigen::Matrix motor_board_enabled_; 358 | 359 | /** @brief Motor Board Error code (int). 360 | */ 361 | Eigen::Matrix motor_board_errors_; 362 | 363 | /* 364 | * Joint data 365 | */ 366 | 367 | /** @brief Joint positions. */ 368 | Eigen::Vector9d joint_positions_; 369 | 370 | /** @brief Joint velocities. */ 371 | Eigen::Vector9d joint_velocities_; 372 | 373 | /** @brief Joint torques. */ 374 | Eigen::Vector9d joint_torques_; 375 | 376 | /** @brief Target joint torques, reference from the controller. */ 377 | Eigen::Vector9d joint_target_torques_; 378 | 379 | /* 380 | * Additional data 381 | */ 382 | 383 | /** @brief Name of the network lan: Left column in ifconfig output. */ 384 | std::string network_id_; 385 | 386 | /** @brief Slider position in [0, 1]. */ 387 | Eigen::Matrix slider_positions_; 388 | 389 | /** @brief E-stop from the slider box. */ 390 | bool active_estop_; 391 | 392 | /** @brief base accelerometer. */ 393 | Eigen::Vector3d base_accelerometer_; 394 | 395 | /** @brief base accelerometer. */ 396 | Eigen::Vector3d base_gyroscope_; 397 | 398 | /** @brief base accelerometer. */ 399 | Eigen::Vector4d base_attitude_; 400 | 401 | /** @brief base accelerometer. */ 402 | Eigen::Vector3d base_linear_acceleration_; 403 | 404 | /** @brief Integers from the serial port. 405 | * - 4 sliders in [0, 1024] 406 | * - emergency stop 407 | */ 408 | std::vector slider_box_data_; 409 | 410 | /* 411 | * Controllers 412 | */ 413 | 414 | /** @brief Controller to run the calibration procedure */ 415 | std::shared_ptr calib_ctrl_; 416 | 417 | /* 418 | * Finite state machine of the controbolller. 419 | */ 420 | 421 | /** @brief Control state Initial, Ready, Calibration */ 422 | BoltControlState control_state_; 423 | 424 | /** @brief Check if the user called for the joint calibration. */ 425 | bool calibrate_request_; 426 | 427 | /** @brief Number of time we acquire the sensor readings, this is used 428 | * to prevent spamming prints */ 429 | long int nb_time_we_acquired_sensors_; 430 | 431 | /* 432 | * Drivers communication objects 433 | */ 434 | 435 | /** 436 | * @brief Robot complete interface. 437 | * Wrapper around the MasterBoardInterface. 438 | * 439 | * PC <------------------> main board <-------> Motor Board 440 | * Ethernet/Wifi SPI 441 | */ 442 | std::shared_ptr robot_; 443 | 444 | /** 445 | * @brief Reader for serial port to read arduino slider values. 446 | */ 447 | std::shared_ptr serial_reader_; 448 | }; 449 | 450 | } // namespace bolt 451 | -------------------------------------------------------------------------------- /include/bolt/dgm_bolt.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file dgm_bolt.hpp 3 | * @author Manuel Wuthrich 4 | * @author Maximilien Naveau 5 | * @author Julian Viereck 6 | * @author Johannes Pfleging 7 | * @license License BSD-3-Clause 8 | * @copyright Copyright (c) 2019, New York University and Max Planck 9 | * Gesellshaft. 10 | */ 11 | 12 | #ifndef DGM_SOLO_HH 13 | #define DGM_SOLO_HH 14 | 15 | #include "bolt/bolt.hpp" 16 | #include "dynamic_graph_manager/dynamic_graph_manager.hpp" 17 | #include "mim_msgs/srv/joint_calibration.hpp" 18 | #include "yaml_utils/yaml_cpp_fwd.hpp" 19 | 20 | namespace bolt 21 | { 22 | class DGMBolt : public dynamic_graph_manager::DynamicGraphManager 23 | { 24 | public: 25 | /** 26 | * @brief DemoSingleMotor is the constructor. 27 | */ 28 | DGMBolt(); 29 | 30 | /** 31 | * @brief ~DemoSingleMotor is the destructor. 32 | */ 33 | ~DGMBolt(); 34 | 35 | /** 36 | * @brief is_in_safety_mode check if the dynamic graph is still alive and 37 | * sending commands at a descent frequency. Inheriting this method is not 38 | * mandatory but recommanded. 39 | * This function make also sure that the joint velocity do not exceed 40 | * a certain value 41 | * @return true if there is a problem. 42 | */ 43 | bool is_in_safety_mode(); 44 | 45 | /** 46 | * @brief initialize_hardware_communication_process is the function that 47 | * initialize the hardware. 48 | */ 49 | void initialize_hardware_communication_process(); 50 | 51 | /** 52 | * @brief get_sensors_to_map acquieres the sensors data and feed it to the 53 | * input/output map 54 | * @param[in][out] map is the sensors data filled by this function. 55 | */ 56 | void get_sensors_to_map(dynamic_graph_manager::VectorDGMap& map); 57 | 58 | /** 59 | * @brief set_motor_controls_from_map reads the input map that contains the 60 | * controls and send these controls to the hardware. 61 | * @param map 62 | */ 63 | void set_motor_controls_from_map( 64 | const dynamic_graph_manager::VectorDGMap& map); 65 | 66 | /** 67 | * @brief Ros callback for the callibration procedure. Warning the robot 68 | * will move to the next the joint index and back to "0" upon this call. 69 | * Be sure that no controller are running in parallel. 70 | * 71 | * @param req nothing 72 | * @param res True if everything went well. 73 | * @return true if everything went well. 74 | * @return false if something went wrong. 75 | */ 76 | void calibrate_joint_position_callback( 77 | mim_msgs::srv::JointCalibration::Request::SharedPtr req, 78 | mim_msgs::srv::JointCalibration::Response::SharedPtr res); 79 | 80 | private: 81 | /** 82 | * @brief Calibrate the robot joint position 83 | * 84 | * @param zero_to_index_angle is the angle between the theoretical zero and 85 | * the next positive angle. 86 | */ 87 | void calibrate_joint_position(); 88 | 89 | /** 90 | * Entries for the real hardware. 91 | */ 92 | 93 | /** 94 | * @brief test_bench_ the real test bench hardware drivers. 95 | */ 96 | Bolt bolt_; 97 | 98 | /** 99 | * @brief ctrl_joint_torques_ the joint torques to be sent. Used in this 100 | * class to perform a local copy of the control. This is need in order 101 | * to send this copy to the Bolt class 102 | */ 103 | Eigen::Vector6d ctrl_joint_torques_; 104 | 105 | /** 106 | * @brief Check if we entered once in the safety mode and stay there if so 107 | */ 108 | bool was_in_safety_mode_; 109 | 110 | /** 111 | * @brief These are the calibration value extracted from the paramters. 112 | * They represent the distance between the theorical zero joint angle and 113 | * the next jont index. 114 | */ 115 | Eigen::Vector6d zero_to_index_angle_from_file_; 116 | }; 117 | 118 | } // namespace bolt 119 | 120 | #endif // DGM_TEST_BENCH_8_MOTORS_HH 121 | -------------------------------------------------------------------------------- /include/bolt/dgm_bolt_humanoid.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file dgm_bolt_humanoid.hpp 3 | * @license License BSD-3-Clause 4 | * @copyright Copyright (c) 2022, New York University and Max Planck 5 | * Gesellshaft. 6 | */ 7 | 8 | #pragma once 9 | 10 | #include "bolt/bolt_humanoid.hpp" 11 | #include "dynamic_graph_manager/dynamic_graph_manager.hpp" 12 | #include "mim_msgs/srv/joint_calibration.hpp" 13 | #include "yaml_utils/yaml_cpp_fwd.hpp" 14 | 15 | namespace bolt 16 | { 17 | class DGMBoltHumanoid : public dynamic_graph_manager::DynamicGraphManager 18 | { 19 | public: 20 | /** 21 | * @brief DemoSingleMotor is the constructor. 22 | */ 23 | DGMBoltHumanoid(); 24 | 25 | /** 26 | * @brief ~DemoSingleMotor is the destructor. 27 | */ 28 | ~DGMBoltHumanoid(); 29 | 30 | /** 31 | * @brief is_in_safety_mode check if the dynamic graph is still alive and 32 | * sending commands at a descent frequency. Inheriting this method is not 33 | * mandatory but recommanded. 34 | * This function make also sure that the joint velocity do not exceed 35 | * a certain value 36 | * @return true if there is a problem. 37 | */ 38 | bool is_in_safety_mode(); 39 | 40 | /** 41 | * @brief initialize_hardware_communication_process is the function that 42 | * initialize the hardware. 43 | */ 44 | void initialize_hardware_communication_process(); 45 | 46 | /** 47 | * @brief get_sensors_to_map acquieres the sensors data and feed it to the 48 | * input/output map 49 | * @param[in][out] map is the sensors data filled by this function. 50 | */ 51 | void get_sensors_to_map(dynamic_graph_manager::VectorDGMap& map); 52 | 53 | /** 54 | * @brief set_motor_controls_from_map reads the input map that contains the 55 | * controls and send these controls to the hardware. 56 | * @param map 57 | */ 58 | void set_motor_controls_from_map( 59 | const dynamic_graph_manager::VectorDGMap& map); 60 | 61 | /** 62 | * @brief Ros callback for the callibration procedure. Warning the robot 63 | * will move to the next the joint index and back to "0" upon this call. 64 | * Be sure that no controller are running in parallel. 65 | * 66 | * @param req nothing 67 | * @param res True if everything went well. 68 | * @return true if everything went well. 69 | * @return false if something went wrong. 70 | */ 71 | void calibrate_joint_position_callback( 72 | mim_msgs::srv::JointCalibration::Request::SharedPtr req, 73 | mim_msgs::srv::JointCalibration::Response::SharedPtr res); 74 | 75 | private: 76 | /** 77 | * @brief Calibrate the robot joint position 78 | * 79 | * @param zero_to_index_angle is the angle between the theoretical zero and 80 | * the next positive angle. 81 | */ 82 | void calibrate_joint_position(); 83 | 84 | /** 85 | * Entries for the real hardware. 86 | */ 87 | 88 | /** 89 | * @brief test_bench_ the real test bench hardware drivers. 90 | */ 91 | BoltHumanoid bolt_; 92 | 93 | /** 94 | * @brief ctrl_joint_torques_ the joint torques to be sent. Used in this 95 | * class to perform a local copy of the control. This is need in order 96 | * to send this copy to the Bolt class 97 | */ 98 | Eigen::Vector9d ctrl_joint_torques_; 99 | 100 | /** 101 | * @brief Check if we entered once in the safety mode and stay there if so 102 | */ 103 | bool was_in_safety_mode_; 104 | 105 | /** 106 | * @brief These are the calibration value extracted from the paramters. 107 | * They represent the distance between the theorical zero joint angle and 108 | * the next jont index. 109 | */ 110 | Eigen::Vector9d zero_to_index_angle_from_file_; 111 | }; 112 | 113 | } // namespace bolt 114 | -------------------------------------------------------------------------------- /include/bolt/utils.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file common_demo_header.hpp 3 | * @author Maximilien Naveau (maximilien.naveau@gmail.com) 4 | * @brief Contains some default tools for creating demos 5 | * @version 0.1 6 | * @date 2019-11-07 7 | * 8 | * @copyright Copyright (c) 2019 9 | * 10 | */ 11 | 12 | #include 13 | #include 14 | 15 | // manage the ctrl+c signal 16 | #include 17 | // thread safe flag for application shutdown management 18 | #include 19 | 20 | #include "real_time_tools/iostream.hpp" 21 | #include "real_time_tools/spinner.hpp" 22 | #include "real_time_tools/thread.hpp" 23 | #include "real_time_tools/timer.hpp" 24 | #include "yaml_utils/yaml_cpp_fwd.hpp" 25 | 26 | namespace bolt 27 | { 28 | /** 29 | * @brief This small structure is used for reading the calibration parameters 30 | * for the calibrations demos. 31 | * 32 | * @tparam ROBOT_TYPE 33 | */ 34 | template 35 | struct ThreadCalibrationData 36 | { 37 | std::shared_ptr robot; 38 | }; 39 | 40 | /** 41 | * @brief This boolean is here to kill cleanly the application upon ctrl+c 42 | */ 43 | std::atomic_bool CTRL_C_DETECTED(false); 44 | 45 | /** 46 | * @brief This function is the callback upon a ctrl+c call from the 47 | * terminal. 48 | * 49 | * @param s is the id of the signal 50 | */ 51 | void my_handler(int) 52 | { 53 | CTRL_C_DETECTED = true; 54 | } 55 | 56 | /** 57 | * @brief Enable to kill the demos cleanly with a ctrl+c 58 | */ 59 | void enable_ctrl_c() 60 | { 61 | // make sure we catch the ctrl+c signal to kill the application 62 | // properly. 63 | struct sigaction sigIntHandler; 64 | sigIntHandler.sa_handler = my_handler; 65 | sigemptyset(&sigIntHandler.sa_mask); 66 | sigIntHandler.sa_flags = 0; 67 | sigaction(SIGINT, &sigIntHandler, NULL); 68 | CTRL_C_DETECTED = false; 69 | } 70 | 71 | /** 72 | * @brief Usefull tool for the demos and programs in order to print data in 73 | * real time. 74 | * 75 | * @param v_name is a string defining the data to print. 76 | * @param v the vector to print. 77 | */ 78 | void print_vector(std::string v_name, const Eigen::Ref v) 79 | { 80 | v_name += ": ["; 81 | rt_printf("%s", v_name.c_str()); 82 | for (int i = 0; i < v.size(); ++i) 83 | { 84 | rt_printf("%0.3f, ", v(i)); 85 | } 86 | rt_printf("]\n"); 87 | } 88 | 89 | /** 90 | * @brief Usefull tool for the demos and programs in order to print data in 91 | * real time. 92 | * 93 | * @param v_name is a string defining the data to print. 94 | * @param v the vector to print. 95 | */ 96 | void print_vector_bool( 97 | std::string v_name, 98 | const Eigen::Ref > v) 99 | { 100 | v_name += ": ["; 101 | rt_printf("%s", v_name.c_str()); 102 | for (int i = 0; i < v.size(); ++i) 103 | { 104 | rt_printf("%s, ", v(i) ? "True" : "False"); 105 | } 106 | rt_printf("]\n"); 107 | } 108 | 109 | /** 110 | * @brief Usefull tool for the demos and programs in order to print data in 111 | * real time. 112 | * 113 | * @param v_name is a string defining the data to print. 114 | * @param v the vector to print. 115 | */ 116 | void print_vector_int( 117 | std::string v_name, 118 | const Eigen::Ref > v) 119 | { 120 | v_name += ": ["; 121 | rt_printf("%s", v_name.c_str()); 122 | for (int i = 0; i < v.size(); ++i) 123 | { 124 | rt_printf("%d, ", v(i)); 125 | } 126 | rt_printf("]\n"); 127 | } 128 | } // namespace bolt 129 | -------------------------------------------------------------------------------- /license.txt: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2019, New York University and Max Planck Gesellschaft. 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | bolt 5 | 1.0.0 6 | Drivers and interfaces to the Bolt robot. 7 | Maximilien Naveau 8 | BSD-3-clause 9 | 10 | ament_cmake 11 | 12 | mpi_cmake_modules 13 | pybind11 14 | odri_control_interface 15 | slider_box 16 | real_time_tools 17 | yaml_utils 18 | PythonModules 19 | robot_properties_bolt 20 | dynamic_graph_manager 21 | 22 | 23 | ament_cmake 24 | 25 | -------------------------------------------------------------------------------- /python/bolt/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-dynamic-robot-initiative/bolt/5bb0e92f9362eb301045a89df185b7e6ddb6f538/python/bolt/__init__.py -------------------------------------------------------------------------------- /python/bolt/dg_bolt_base_bullet.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | License: BSD 3-Clause License 4 | Copyright (C) 2018-2021, New York University , Max Planck Gesellschaft 5 | Copyright note valid unless otherwise stated in individual files. 6 | All rights reserved. 7 | """ 8 | 9 | from pathlib import Path 10 | import numpy as np 11 | import pybullet 12 | from bullet_utils.env import BulletEnvWithGround 13 | from robot_properties_bolt.bolt_wrapper import BoltRobot 14 | from dynamic_graph_manager.dynamic_graph.device import Device 15 | from dynamic_graph_manager.robot import Robot 16 | from dynamic_graph.sot.core.vector_constant import VectorConstant 17 | 18 | 19 | class DgBoltBaseRobot(Robot): 20 | """ 21 | Base implementation for bolt robot. 22 | """ 23 | 24 | def __init__( 25 | self, 26 | bolt_config, 27 | use_fixed_base=False, 28 | init_sliders_pose=4 * [0.5], 29 | ): 30 | # Copy the arguments internally 31 | self._bolt_config = bolt_config 32 | self._use_fixed_base = use_fixed_base 33 | self._init_sliders_pose = init_sliders_pose 34 | 35 | # Create an instance of the simulator. 36 | self._bullet_env = BulletEnvWithGround() 37 | 38 | # Load the robot 39 | robotStartPos = [0.0, 0, 0.26487417] 40 | robotStartOrientation = pybullet.getQuaternionFromEuler( 41 | [0.0, 0.0, 0.0] 42 | ) 43 | 44 | # Load the robot 45 | self._simulated_robot = BoltRobot( 46 | robotStartPos, 47 | robotStartOrientation, 48 | self._init_sliders_pose, 49 | self._use_fixed_base, 50 | ) 51 | 52 | self._bullet_env.add_robot(self._simulated_robot) 53 | 54 | self.q0 = np.zeros(self._simulated_robot.pin_robot.nq) 55 | 56 | # Initialize the device. 57 | self.device = Device(self._bolt_config.robot_name) 58 | assert Path(self._bolt_config.dgm_yaml_path).exists() 59 | self.device.initialize(self._bolt_config.dgm_yaml_path) 60 | 61 | # Create signals for the base. 62 | self._signal_base_pos = VectorConstant("bullet_bolt_base_pos") 63 | self._signal_base_vel = VectorConstant("bullet_bolt_base_vel") 64 | self._signal_base_vel_world = VectorConstant( 65 | "bullet_bolt_base_vel_world" 66 | ) 67 | # Sync the current robot state to the graph input signals. 68 | self._sim2signal() 69 | 70 | super(DgBoltBaseRobot, self).__init__( 71 | self._bolt_config.robot_name, self.device 72 | ) 73 | 74 | def base_signals(self): 75 | return self._signal_base_pos.sout, self._signal_base_vel.sout 76 | 77 | def _sim2signal(self): 78 | """Reads the state from the simulator and fills 79 | the corresponding signals.""" 80 | 81 | q, dq = self._simulated_robot.get_state() 82 | 83 | # Fill in the device. 84 | self.device.joint_positions.value = q[7:] 85 | self.device.joint_velocities.value = dq[6:] 86 | self.device.slider_positions.value = np.array( 87 | [ 88 | self._simulated_robot.get_slider_position("a"), 89 | self._simulated_robot.get_slider_position("b"), 90 | self._simulated_robot.get_slider_position("c"), 91 | self._simulated_robot.get_slider_position("d"), 92 | ] 93 | ) 94 | if self.device.hasSignal("contact_sensors"): 95 | self.device.contact_sensors.value = np.array(4 * [0.0]) 96 | 97 | # Base related signals 98 | self._signal_base_pos.sout.value = q[0:7] 99 | self._signal_base_vel.sout.value = dq[0:6] 100 | self._signal_base_vel_world.sout.value = ( 101 | self._simulated_robot.get_base_velocity_world() 102 | ) 103 | 104 | def run(self, steps=1, sleep=False): 105 | """ Get input from Device, Step the simulation, feed the Device. """ 106 | 107 | for _ in range(steps): 108 | self.device.execute_graph() 109 | self._simulated_robot.send_joint_command( 110 | self.device.ctrl_joint_torques.value 111 | ) 112 | self._bullet_env.step(sleep=sleep) 113 | self._sim2signal() 114 | 115 | def reset_state(self, q, dq): 116 | """Reset the simulator and robot state. """ 117 | 118 | self._simulated_robot.reset_state(q, dq) 119 | self._sim2signal() 120 | 121 | def add_ros_and_trace( 122 | self, client_name, signal_name, topic_name=None, topic_type=None 123 | ): 124 | # for vicon entity 125 | self.signal_name = signal_name 126 | 127 | def start_video_recording(self, file_name): 128 | self._bullet_env.start_video_recording(file_name) 129 | 130 | def stop_video_recording(self): 131 | self._bullet_env.stop_video_recording() 132 | -------------------------------------------------------------------------------- /python/bolt/dg_bolt_bullet.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | License: BSD 3-Clause License 4 | Copyright (C) 2018-2021, New York University , Max Planck Gesellschaft 5 | Copyright note valid unless otherwise stated in individual files. 6 | All rights reserved. 7 | """ 8 | 9 | from robot_properties_bolt.config import BoltConfig 10 | from bolt.dg_bolt_base_bullet import DgBoltBaseRobot 11 | 12 | 13 | class BoltBulletRobot(DgBoltBaseRobot): 14 | def __init__( 15 | self, 16 | use_fixed_base=False, 17 | init_sliders_pose=4 * [0.5], 18 | ): 19 | super(BoltBulletRobot, self).__init__( 20 | BoltConfig(), 21 | use_fixed_base=use_fixed_base, 22 | init_sliders_pose=init_sliders_pose, 23 | ) 24 | 25 | self.q0[2] = 0.26487417 26 | # self.q0[5] = -0.1736482 27 | self.q0[6] = 1.0 28 | self.q0[7] = 0.0 29 | self.q0[8] = 0.78539816 30 | self.q0[9] = -1.57079633 31 | self.q0[10] = 0.0 32 | self.q0[11] = 0.78539816 33 | self.q0[12] = -1.57079633 34 | 35 | # Sync the current robot state to the graph input signals. 36 | self._sim2signal() 37 | 38 | 39 | def get_bolt_robot(use_fixed_base=False, init_sliders_pose=4 * [0.5]): 40 | return BoltBulletRobot(use_fixed_base, init_sliders_pose) 41 | -------------------------------------------------------------------------------- /readme.md: -------------------------------------------------------------------------------- 1 | # bolt 2 | 3 | ## What it is 4 | 5 | Drivers of the bolt robot. 6 | 7 | ## Getting started: 8 | 9 | dependance on pip packages 10 | 11 | ``` 12 | python3 -m pip install xacro m2r sphinxcontrib.moderncmakedomain sphinx breathe pybullet 13 | ``` 14 | dependance on sources 15 | 16 | ``` 17 | cd $HOME 18 | mkdir devel 19 | cd devel 20 | git clone git@github.com:machines-in-motion/treep_machines_in_motion.git 21 | treep --clone BOLT 22 | ``` 23 | 24 | After these command you should have a workspace folder containing a src folder containing the bolt package and all it's dependencies. 25 | 26 | You can use `colcon build` to compile all. 27 | 28 | ## Authors 29 | 30 | - Maximilien Naveau 31 | - Julian Viereck 32 | - Majid Khadiv 33 | - Elham Daneshmand 34 | 35 | ## Copyrights 36 | 37 | Copyright(c) 2018-2019 Max Planck Gesellschaft, New York University 38 | 39 | ## License 40 | 41 | BSD 3-Clause License 42 | 43 | ## Max-Planck Institute: Test defective master_board 44 | 45 | - master board V1 46 | - SPI 0: ok 47 | - SPI 1: ok 48 | - SPI 2: ok 49 | - SPI 3: ok 50 | - SPI 4: ok 51 | - SPI 5: ok 52 | 53 | - master board V2 54 | - SPI 0: not ok 55 | - SPI 1: not ok 56 | - SPI 2: not ok 57 | - SPI 3: not ok 58 | - SPI 4: not ok 59 | - SPI 5: not ok 60 | 61 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from distutils.core import setup 2 | from catkin_pkg.python_setup import generate_distutils_setup 3 | 4 | d = generate_distutils_setup(packages=["bolt"], package_dir={"": "python"}) 5 | 6 | setup(**d) 7 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(all_lib_targets) 2 | set(all_exe_targets) 3 | 4 | # 5 | # Declare the project library. 6 | # 7 | 8 | add_library(bolt SHARED bolt.cpp bolt_humanoid.cpp) 9 | # Add includes 10 | target_include_directories( 11 | bolt PUBLIC $ 12 | $) 13 | # Link the dependencies. 14 | target_link_libraries(bolt odri_control_interface::odri_control_interface) 15 | target_link_libraries(bolt slider_box::slider_box) 16 | target_link_libraries(bolt real_time_tools::real_time_tools) 17 | target_link_libraries(bolt yaml_utils::yaml_utils) 18 | target_link_libraries(bolt Eigen3::Eigen) 19 | # YAML parameters. 20 | string( 21 | CONCAT odri_control_interface_yaml_path 22 | "${PythonModules_robot_properties_bolt_PATH}/" 23 | "robot_properties_bolt/resources/odri_control_interface/" 24 | "bolt_driver.yaml") 25 | target_compile_definitions( 26 | bolt 27 | PUBLIC ODRI_CONTROL_INTERFACE_YAML_PATH="${odri_control_interface_yaml_path}") 28 | 29 | string( 30 | CONCAT odri_control_interface_humanoid_yaml_path 31 | "${PythonModules_robot_properties_bolt_PATH}/" 32 | "robot_properties_bolt/resources/odri_control_interface/" 33 | "bolt_humanoid_driver.yaml") 34 | target_compile_definitions( 35 | bolt 36 | PUBLIC ODRI_CONTROL_INTERFACE_HUMANOID_YAML_PATH="${odri_control_interface_humanoid_yaml_path}") 37 | 38 | # Export the target. 39 | list(APPEND all_lib_targets bolt) 40 | 41 | # 42 | # Hardware calibration program. 43 | # 44 | add_executable(bolt_hardware_calibration programs/hardware_calibration.cpp) 45 | # Add the include dependencies. 46 | target_include_directories( 47 | bolt_hardware_calibration 48 | PUBLIC $ 49 | $) 50 | # Link the dependencies. 51 | target_link_libraries(bolt_hardware_calibration bolt) 52 | # Export the target. 53 | list(APPEND all_exe_targets bolt_hardware_calibration) 54 | 55 | add_executable(bolt_humanoid_hardware_calibration programs/hardware_humanoid_calibration.cpp) 56 | # Add the include dependencies. 57 | target_include_directories( 58 | bolt_humanoid_hardware_calibration 59 | PUBLIC $ 60 | $) 61 | # Link the dependencies. 62 | target_link_libraries(bolt_humanoid_hardware_calibration bolt) 63 | # Export the target. 64 | list(APPEND all_exe_targets bolt_humanoid_hardware_calibration) 65 | 66 | # 67 | # Optionally compile the dynamic_graph_manager Bolt wrapper. 68 | # 69 | if(${dynamic_graph_manager_FOUND}) 70 | # 71 | # create library 72 | # 73 | add_library(dgm_bolt SHARED dgm_bolt.cpp dgm_bolt_humanoid.cpp) 74 | # Add the include dependencies. 75 | target_include_directories( 76 | dgm_bolt PUBLIC $ 77 | $) 78 | # Link the dependencies. 79 | target_link_libraries(dgm_bolt bolt) 80 | target_link_libraries(dgm_bolt dynamic_graph_manager::dynamic_graph_manager) 81 | # Export the target. 82 | list(APPEND all_lib_targets dgm_bolt) 83 | 84 | # 85 | # Create the robot executable. 86 | # 87 | add_executable(dg_main_bolt programs/dg_main_bolt.cpp) 88 | # Add the include dependencies. 89 | target_include_directories( 90 | dg_main_bolt 91 | PUBLIC $ 92 | ${dynamic_graph_manager_INCLUDE_DIR} $) 93 | # Link the dependencies. 94 | target_link_libraries(dg_main_bolt dgm_bolt) 95 | # YAML parameters. 96 | string( 97 | CONCAT dynamic_graph_manager_yaml_file 98 | "${PythonModules_robot_properties_bolt_PATH}/" 99 | "robot_properties_bolt/resources/dynamic_graph_manager/" 100 | "dgm_parameters_bolt.yaml") 101 | target_compile_definitions( 102 | dg_main_bolt 103 | PUBLIC DYNAMIC_GRAPH_MANAGER_YAML_PATH="${dynamic_graph_manager_yaml_file}" 104 | ODRI_CONTROL_INTERFACE_YAML_PATH="${odri_control_interface_yaml_path}") 105 | # Export. 106 | list(APPEND all_exe_targets dg_main_bolt) 107 | 108 | # 109 | # Create the robot executable. 110 | # 111 | add_executable(dg_main_bolt_humanoid programs/dg_main_bolt_humanoid.cpp) 112 | # Add the include dependencies. 113 | target_include_directories( 114 | dg_main_bolt_humanoid 115 | PUBLIC $ 116 | ${dynamic_graph_manager_INCLUDE_DIR} $) 117 | # Link the dependencies. 118 | target_link_libraries(dg_main_bolt_humanoid dgm_bolt) 119 | # YAML parameters. 120 | string( 121 | CONCAT dynamic_graph_manager_humanoid_yaml_file 122 | "${PythonModules_robot_properties_bolt_PATH}/" 123 | "robot_properties_bolt/resources/dynamic_graph_manager/" 124 | "dgm_parameters_bolt_humanoid.yaml") 125 | target_compile_definitions( 126 | dg_main_bolt_humanoid 127 | PUBLIC DYNAMIC_GRAPH_MANAGER_HUMANOID_YAML_PATH="${dynamic_graph_manager_humanoid_yaml_file}" 128 | ODRI_CONTROL_INTERFACE_HUMANOID_YAML_PATH="${odri_control_interface_humanoid_yaml_path}") 129 | # Export. 130 | list(APPEND all_exe_targets dg_main_bolt_humanoid) 131 | 132 | # 133 | # Create the robot executable. 134 | # 135 | add_executable(bolt_dg programs/bolt_dg.cpp) 136 | # Add the include dependencies. 137 | target_include_directories( 138 | bolt_dg 139 | PUBLIC $ 140 | ${dynamic_graph_manager_INCLUDE_DIR} $) 141 | # Link the dependencies. 142 | target_link_libraries(bolt_dg dgm_bolt) 143 | # YAML parameters. 144 | string( 145 | CONCAT dynamic_graph_manager_yaml_file 146 | "${PythonModules_robot_properties_bolt_PATH}/" 147 | "robot_properties_bolt/resources/dynamic_graph_manager/" 148 | "dgm_parameters_bolt.yaml") 149 | target_compile_definitions( 150 | bolt_dg 151 | PUBLIC DYNAMIC_GRAPH_MANAGER_YAML_PATH="${dynamic_graph_manager_yaml_file}" 152 | ODRI_CONTROL_INTERFACE_YAML_PATH="${odri_control_interface_yaml_path}") 153 | # Export. 154 | list(APPEND all_exe_targets bolt_dg) 155 | 156 | # 157 | # Create the robot executable. 158 | # 159 | add_executable(bolt_hw programs/bolt_hw.cpp) 160 | # Add the include dependencies. 161 | target_include_directories( 162 | bolt_hw 163 | PUBLIC $ 164 | ${dynamic_graph_manager_INCLUDE_DIR} $) 165 | # Link the dependencies. 166 | target_link_libraries(bolt_hw dgm_bolt) 167 | # YAML parameters. 168 | string( 169 | CONCAT dynamic_graph_manager_yaml_file 170 | "${PythonModules_robot_properties_bolt_PATH}/" 171 | "robot_properties_bolt/resources/dynamic_graph_manager/" 172 | "dgm_parameters_bolt.yaml") 173 | target_compile_definitions( 174 | bolt_hw 175 | PUBLIC DYNAMIC_GRAPH_MANAGER_YAML_PATH="${dynamic_graph_manager_yaml_file}" 176 | ODRI_CONTROL_INTERFACE_YAML_PATH="${odri_control_interface_yaml_path}") 177 | # Export. 178 | list(APPEND all_exe_targets bolt_hw) 179 | 180 | endif(${dynamic_graph_manager_FOUND}) 181 | 182 | # 183 | # Install and Export the libraries an programs. 184 | # 185 | install( 186 | TARGETS ${all_lib_targets} ${all_exe_targets} 187 | EXPORT export_${PROJECT_NAME} 188 | LIBRARY DESTINATION lib 189 | ARCHIVE DESTINATION lib 190 | RUNTIME DESTINATION lib/${PROJECT_NAME} 191 | INCLUDES 192 | DESTINATION include) 193 | 194 | foreach(target ${all_exe_targets}) 195 | # install a symlink of the executable in lib/bolt 196 | string( 197 | CONCAT symlink_command 198 | "execute_process(" 199 | " COMMAND ${CMAKE_COMMAND} -E make_directory " 200 | " ${CMAKE_INSTALL_PREFIX}/bin/)\n" 201 | "execute_process(" 202 | " COMMAND ${CMAKE_COMMAND} -E create_symlink " 203 | " ${CMAKE_INSTALL_PREFIX}/lib/${PROJECT_NAME}/${target}" 204 | " ${CMAKE_INSTALL_PREFIX}/bin/${target} )" 205 | ) 206 | install(CODE ${symlink_command}) 207 | endforeach() 208 | -------------------------------------------------------------------------------- /src/bolt.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @license BSD 3-clause 4 | * @copyright Copyright (c) 2021, New York University and Max Planck 5 | * Gesellschaft 6 | * 7 | * @brief Implement the Bolt class. 8 | */ 9 | 10 | #include "bolt/bolt.hpp" 11 | 12 | #include 13 | 14 | #include "odri_control_interface/utils.hpp" 15 | 16 | namespace bolt 17 | { 18 | Bolt::Bolt() 19 | { 20 | /** 21 | * Hardware status 22 | */ 23 | for (unsigned i = 0; i < motor_enabled_.size(); ++i) 24 | { 25 | motor_enabled_[i] = false; 26 | motor_ready_[i] = false; 27 | } 28 | for (unsigned i = 0; i < motor_board_enabled_.size(); ++i) 29 | { 30 | motor_board_enabled_[0] = false; 31 | motor_board_errors_[0] = 0; 32 | } 33 | 34 | /** 35 | * Joint data 36 | */ 37 | joint_positions_.setZero(); 38 | joint_velocities_.setZero(); 39 | joint_torques_.setZero(); 40 | joint_target_torques_.setZero(); 41 | 42 | /** 43 | * Additional data 44 | */ 45 | 46 | // Network infos. 47 | network_id_ = ""; 48 | 49 | // Slider bos infos. 50 | slider_positions_.setZero(); 51 | active_estop_ = true; // By default assume the estop is active. 52 | slider_box_data_.resize(BOLT_NB_SLIDER + 1, 0); // 4 sliders + 1 e-stop. 53 | 54 | // imu infos 55 | base_accelerometer_.setZero(); 56 | base_gyroscope_.setZero(); 57 | base_attitude_.setZero(); 58 | base_linear_acceleration_.setZero(); 59 | 60 | // Finite state machine for the control 61 | control_state_ = BoltControlState::initial; 62 | calibrate_request_ = false; 63 | nb_time_we_acquired_sensors_ = 0; 64 | } 65 | 66 | void Bolt::initialize(const std::string& network_id) 67 | { 68 | // Network info. 69 | network_id_ = network_id; 70 | 71 | // Main driver interface. 72 | robot_ = odri_control_interface::RobotFromYamlFile( 73 | network_id_, ODRI_CONTROL_INTERFACE_YAML_PATH); 74 | 75 | calib_ctrl_ = odri_control_interface::JointCalibratorFromYamlFile( 76 | ODRI_CONTROL_INTERFACE_YAML_PATH, robot_->joints); 77 | 78 | // Use a serial port to read slider values. 79 | serial_reader_ = std::make_shared( 80 | "serial_port", BOLT_NB_SLIDER + 1); 81 | 82 | // Initialize the robot. 83 | robot_->Init(); 84 | } 85 | 86 | void Bolt::wait_until_ready() 87 | { 88 | robot_->WaitUntilReady(); 89 | } 90 | 91 | void Bolt::acquire_sensors() 92 | { 93 | // Acquire the data. 94 | robot_->ParseSensorData(); 95 | 96 | /** 97 | * Joint data 98 | */ 99 | // acquire the joint position 100 | joint_positions_ = robot_->joints->GetPositions(); 101 | // acquire the joint velocities 102 | joint_velocities_ = robot_->joints->GetVelocities(); 103 | // acquire the joint torques 104 | joint_torques_ = robot_->joints->GetMeasuredTorques(); 105 | // acquire the target joint torques 106 | joint_target_torques_ = robot_->joints->GetSentTorques(); 107 | 108 | /** 109 | * Additional data 110 | */ 111 | base_accelerometer_ = robot_->imu->GetAccelerometer(); 112 | base_gyroscope_ = robot_->imu->GetGyroscope(); 113 | base_attitude_ = robot_->imu->GetAttitudeQuaternion(); 114 | base_linear_acceleration_ = robot_->imu->GetLinearAcceleration(); 115 | 116 | // acquire the slider positions 117 | if (serial_reader_->fill_vector(slider_box_data_) > 10) 118 | { 119 | robot_->ReportError(); 120 | if(nb_time_we_acquired_sensors_ % 2000 == 0) 121 | { 122 | robot_->ReportError( 123 | "The slider box is not responding correctly, " 124 | "10 iteration are missing."); 125 | } 126 | } 127 | for (unsigned i = 0; i < slider_positions_.size(); ++i) 128 | { 129 | // acquire the slider 130 | slider_positions_(i) = double(slider_box_data_[i + 1]) / 1024.; 131 | } 132 | // acquire the e-stop from the slider box 133 | active_estop_ = slider_box_data_[0] == 0; 134 | 135 | /** 136 | * The different status. 137 | */ 138 | 139 | // motor board status 140 | motor_board_enabled_ = robot_->joints->GetMotorDriverEnabled(); 141 | motor_board_errors_ = robot_->joints->GetMotorDriverErrors(); 142 | // motors status 143 | motor_enabled_ = robot_->joints->GetEnabled(); 144 | motor_ready_ = robot_->joints->GetReady(); 145 | 146 | /* 147 | * Safety check 148 | */ 149 | // if (!(base_attitude_(1) < 0.6 && base_attitude_(1) > -0.6 && 150 | // (base_attitude_(0) < -2.5 || base_attitude_(0) > 2.5))) 151 | // { 152 | // robot_->ReportError(); 153 | // if(nb_time_we_acquired_sensors_ % 2000 == 0) 154 | // { 155 | // robot_->ReportError("Base attitude not in the defined parameter."); 156 | // } 157 | // } 158 | ++nb_time_we_acquired_sensors_; 159 | } 160 | 161 | void Bolt::send_target_joint_torque( 162 | const Eigen::Ref target_joint_torque) 163 | { 164 | robot_->joints->SetTorques(target_joint_torque); 165 | 166 | switch (control_state_) 167 | { 168 | case BoltControlState::initial: 169 | robot_->joints->SetZeroCommands(); 170 | if (!robot_->IsTimeout() && !robot_->IsAckMsgReceived()) 171 | { 172 | robot_->SendInit(); 173 | } 174 | else if (!robot_->IsReady()) 175 | { 176 | robot_->SendCommand(); 177 | } 178 | else 179 | { 180 | control_state_ = BoltControlState::ready; 181 | robot_->SendCommand(); 182 | } 183 | break; 184 | 185 | case BoltControlState::ready: 186 | if (calibrate_request_) 187 | { 188 | calibrate_request_ = false; 189 | control_state_ = BoltControlState::calibrate; 190 | robot_->joints->SetZeroCommands(); 191 | } 192 | robot_->SendCommand(); 193 | break; 194 | 195 | case BoltControlState::calibrate: 196 | // calib_ctrl_ set the robot_->joints torque commands; 197 | if (calib_ctrl_->Run()) 198 | { 199 | control_state_ = BoltControlState::ready; 200 | } 201 | robot_->SendCommand(); 202 | break; 203 | } 204 | } 205 | 206 | void Bolt::request_calibration( 207 | const Eigen::Ref home_offset_rad) 208 | { 209 | printf("Bolt::calibrate called\n"); 210 | calib_ctrl_->UpdatePositionOffsets(home_offset_rad); 211 | calibrate_request_ = true; 212 | } 213 | 214 | void Bolt::request_calibration() 215 | { 216 | printf("Bolt::calibrate called\n"); 217 | calibrate_request_ = true; 218 | } 219 | 220 | } // namespace bolt 221 | -------------------------------------------------------------------------------- /src/bolt_humanoid.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @license BSD 3-clause 4 | * @copyright Copyright (c) 2021, New York University and Max Planck 5 | * Gesellschaft 6 | * 7 | * @brief Implement the Bolt Humanoid class. 8 | */ 9 | 10 | #include "bolt/bolt_humanoid.hpp" 11 | 12 | #include 13 | 14 | #include "odri_control_interface/utils.hpp" 15 | 16 | namespace bolt 17 | { 18 | BoltHumanoid::BoltHumanoid() 19 | { 20 | /** 21 | * Hardware status 22 | */ 23 | for (unsigned i = 0; i < motor_enabled_.size(); ++i) 24 | { 25 | motor_enabled_[i] = false; 26 | motor_ready_[i] = false; 27 | } 28 | for (unsigned i = 0; i < motor_board_enabled_.size(); ++i) 29 | { 30 | motor_board_enabled_[0] = false; 31 | motor_board_errors_[0] = 0; 32 | } 33 | 34 | /** 35 | * Joint data 36 | */ 37 | joint_positions_.setZero(); 38 | joint_velocities_.setZero(); 39 | joint_torques_.setZero(); 40 | joint_target_torques_.setZero(); 41 | 42 | /** 43 | * Additional data 44 | */ 45 | 46 | // Network infos. 47 | network_id_ = ""; 48 | 49 | // Slider bos infos. 50 | slider_positions_.setZero(); 51 | active_estop_ = true; // By default assume the estop is active. 52 | 53 | // 4 sliders + 1 e-stop. 54 | slider_box_data_.resize(BOLT_HUMANOID_NB_SLIDER + 1, 0); 55 | 56 | // imu infos 57 | base_accelerometer_.setZero(); 58 | base_gyroscope_.setZero(); 59 | base_attitude_.setZero(); 60 | base_linear_acceleration_.setZero(); 61 | 62 | // Finite state machine for the control 63 | control_state_ = BoltControlState::initial; 64 | calibrate_request_ = false; 65 | nb_time_we_acquired_sensors_ = 0; 66 | } 67 | 68 | void BoltHumanoid::initialize(const std::string& network_id, 69 | const std::string& slider_box_port) 70 | { 71 | // Network info. 72 | network_id_ = network_id; 73 | 74 | // Main driver interface. 75 | robot_ = odri_control_interface::RobotFromYamlFile( 76 | network_id_, ODRI_CONTROL_INTERFACE_HUMANOID_YAML_PATH); 77 | 78 | calib_ctrl_ = odri_control_interface::JointCalibratorFromYamlFile( 79 | ODRI_CONTROL_INTERFACE_HUMANOID_YAML_PATH, robot_->joints); 80 | 81 | // Use a serial port to read slider values. 82 | // only initialize serial reader if 83 | if (!slider_box_port.empty() and slider_box_port != SLIDER_BOX_DISABLED) 84 | { 85 | // Use a serial port to read slider values. 86 | serial_reader_ = std::make_shared( 87 | slider_box_port, BOLT_HUMANOID_NB_SLIDER + 1); 88 | } 89 | else 90 | { 91 | // if no slider box is used, disable estop 92 | active_estop_ = false; 93 | } 94 | 95 | // Initialize the robot. 96 | robot_->Init(); 97 | } 98 | 99 | void BoltHumanoid::set_max_current(const double& max_current) 100 | { 101 | robot_->joints->SetMaximumCurrents(max_current); 102 | } 103 | 104 | void BoltHumanoid::wait_until_ready() 105 | { 106 | robot_->WaitUntilReady(); 107 | } 108 | 109 | bool BoltHumanoid::is_ready() 110 | { 111 | return control_state_ == BoltControlState::ready; 112 | } 113 | 114 | void BoltHumanoid::acquire_sensors() 115 | { 116 | // Acquire the data. 117 | robot_->ParseSensorData(); 118 | 119 | /** 120 | * Joint data 121 | */ 122 | // acquire the joint position 123 | joint_positions_ = robot_->joints->GetPositions(); 124 | // acquire the joint velocities 125 | joint_velocities_ = robot_->joints->GetVelocities(); 126 | // acquire the joint torques 127 | joint_torques_ = robot_->joints->GetMeasuredTorques(); 128 | // acquire the target joint torques 129 | joint_target_torques_ = robot_->joints->GetSentTorques(); 130 | 131 | /** 132 | * Additional data 133 | */ 134 | base_accelerometer_ = robot_->imu->GetAccelerometer(); 135 | base_gyroscope_ = robot_->imu->GetGyroscope(); 136 | base_attitude_ = robot_->imu->GetAttitudeQuaternion(); 137 | base_linear_acceleration_ = robot_->imu->GetLinearAcceleration(); 138 | 139 | if (serial_reader_) 140 | { 141 | // acquire the slider positions 142 | if (serial_reader_->fill_vector(slider_box_data_) > 10) 143 | { 144 | robot_->ReportError(); 145 | if (nb_time_we_acquired_sensors_ % 2000 == 0) 146 | { 147 | robot_->ReportError( 148 | "The slider box is not responding correctly, " 149 | "10 iteration are missing."); 150 | } 151 | } 152 | for (unsigned i = 0; i < slider_positions_.size(); ++i) 153 | { 154 | // acquire the slider 155 | slider_positions_(i) = double(slider_box_data_[i + 1]) / 1024.; 156 | } 157 | // acquire the e-stop from the slider box 158 | active_estop_ = slider_box_data_[0] == 0; 159 | } 160 | 161 | /** 162 | * The different status. 163 | */ 164 | 165 | // motor board status 166 | motor_board_enabled_ = robot_->joints->GetMotorDriverEnabled(); 167 | motor_board_errors_ = robot_->joints->GetMotorDriverErrors(); 168 | // motors status 169 | motor_enabled_ = robot_->joints->GetEnabled(); 170 | motor_ready_ = robot_->joints->GetReady(); 171 | 172 | /* 173 | * Safety check 174 | */ 175 | // if (!(base_attitude_(1) < 0.6 && base_attitude_(1) > -0.6 && 176 | // (base_attitude_(0) < -2.5 || base_attitude_(0) > 2.5))) 177 | // { 178 | // robot_->ReportError(); 179 | // if(nb_time_we_acquired_sensors_ % 2000 == 0) 180 | // { 181 | // robot_->ReportError("Base attitude not in the defined 182 | // parameter."); 183 | // } 184 | // } 185 | ++nb_time_we_acquired_sensors_; 186 | } 187 | 188 | void BoltHumanoid::send_target_joint_torque( 189 | const Eigen::Ref target_joint_torque) 190 | { 191 | robot_->joints->SetTorques(target_joint_torque); 192 | 193 | switch (control_state_) 194 | { 195 | case BoltControlState::initial: 196 | robot_->joints->SetZeroCommands(); 197 | if (!robot_->IsTimeout() && !robot_->IsAckMsgReceived()) 198 | { 199 | robot_->SendInit(); 200 | } 201 | else if (!robot_->IsReady()) 202 | { 203 | robot_->SendCommand(); 204 | } 205 | else 206 | { 207 | control_state_ = BoltControlState::ready; 208 | robot_->SendCommand(); 209 | } 210 | break; 211 | 212 | case BoltControlState::ready: 213 | if (calibrate_request_) 214 | { 215 | calibrate_request_ = false; 216 | control_state_ = BoltControlState::calibrate; 217 | robot_->joints->SetZeroCommands(); 218 | } 219 | robot_->SendCommand(); 220 | break; 221 | 222 | case BoltControlState::calibrate: 223 | // calib_ctrl_ set the robot_->joints torque commands; 224 | if (calib_ctrl_->Run()) 225 | { 226 | control_state_ = BoltControlState::ready; 227 | } 228 | robot_->SendCommand(); 229 | break; 230 | } 231 | } 232 | 233 | void BoltHumanoid::send_target_joint_position( 234 | const Eigen::Ref target_joint_position) 235 | { 236 | robot_->joints->SetDesiredPositions(target_joint_position); 237 | } 238 | 239 | void BoltHumanoid::send_target_joint_velocity( 240 | const Eigen::Ref target_joint_velocity) 241 | { 242 | robot_->joints->SetDesiredVelocities(target_joint_velocity); 243 | } 244 | 245 | void BoltHumanoid::send_target_joint_position_gains( 246 | const Eigen::Ref target_joint_position_gains) 247 | { 248 | robot_->joints->SetPositionGains(target_joint_position_gains); 249 | } 250 | 251 | void BoltHumanoid::send_target_joint_velocity_gains( 252 | const Eigen::Ref target_joint_velocity_gains) 253 | { 254 | robot_->joints->SetVelocityGains(target_joint_velocity_gains); 255 | } 256 | 257 | void BoltHumanoid::request_calibration( 258 | const Eigen::Ref home_offset_rad) 259 | { 260 | printf("Bolt::calibrate called\n"); 261 | calib_ctrl_->UpdatePositionOffsets(home_offset_rad); 262 | calibrate_request_ = true; 263 | } 264 | 265 | void BoltHumanoid::request_calibration() 266 | { 267 | printf("Bolt::calibrate called\n"); 268 | calibrate_request_ = true; 269 | } 270 | 271 | } // namespace bolt 272 | -------------------------------------------------------------------------------- /src/dgm_bolt.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file dgm_bolt.cpp 3 | * \brief The hardware wrapper of the bolt robot 4 | * \author Maximilien Naveau 5 | * \date 2018 6 | * 7 | * This file defines the TestBench8Motors class. 8 | */ 9 | 10 | #include "bolt/dgm_bolt.hpp" 11 | 12 | #include "dynamic_graph_manager/ros.hpp" 13 | 14 | namespace bolt 15 | { 16 | DGMBolt::DGMBolt() 17 | { 18 | was_in_safety_mode_ = false; 19 | } 20 | 21 | DGMBolt::~DGMBolt() 22 | { 23 | } 24 | 25 | bool DGMBolt::is_in_safety_mode() 26 | { 27 | // Check if any card is in an error state. 28 | if (bolt_.has_error()) { 29 | was_in_safety_mode_ = true; 30 | static int counter = 0; 31 | if (counter % 2000 == 0) { 32 | printf("DGMBolt: Going into safe mode as motor card reports error.\n"); 33 | } 34 | counter += 1; 35 | } 36 | 37 | if (was_in_safety_mode_ || DynamicGraphManager::is_in_safety_mode()) 38 | { 39 | static int counter = 0; 40 | was_in_safety_mode_ = true; 41 | if (counter % 2000 == 0) 42 | { 43 | printf("DGMBolt: is_in_safety_mode.\n"); 44 | } 45 | counter++; 46 | } 47 | return was_in_safety_mode_; 48 | } 49 | 50 | 51 | void DGMBolt::initialize_hardware_communication_process() 52 | { 53 | /** 54 | * Load the calibration parameters 55 | */ 56 | 57 | // get the hardware communication ros node handle 58 | dynamic_graph_manager::RosNodePtr ros_node_handle = 59 | dynamic_graph_manager::get_ros_node( 60 | dynamic_graph_manager::HWC_ROS_NODE_NAME); 61 | 62 | /** initialize the user commands */ 63 | ros_user_commands_.push_back( 64 | ros_node_handle->create_service( 65 | "calibrate_joint_position", 66 | std::bind(&DGMBolt::calibrate_joint_position_callback, 67 | this, 68 | std::placeholders::_1, 69 | std::placeholders::_2))); 70 | 71 | std::string network_id; 72 | YAML::ReadParameter( 73 | params_["hardware_communication"], "network_id", network_id); 74 | 75 | bolt_.initialize(network_id); 76 | } 77 | 78 | void DGMBolt::get_sensors_to_map(dynamic_graph_manager::VectorDGMap& map) 79 | { 80 | bolt_.acquire_sensors(); 81 | 82 | /** 83 | * Joint data 84 | */ 85 | map.at("joint_positions") = bolt_.get_joint_positions(); 86 | map.at("joint_velocities") = bolt_.get_joint_velocities(); 87 | map.at("joint_torques") = bolt_.get_joint_torques(); 88 | map.at("joint_target_torques") = bolt_.get_joint_target_torques(); 89 | 90 | /** 91 | * Additional data 92 | */ 93 | map.at("base_accelerometer") = bolt_.get_base_accelerometer(); 94 | map.at("base_gyroscope") = bolt_.get_base_gyroscope(); 95 | map.at("base_attitude") = bolt_.get_base_attitude(); 96 | map.at("base_linear_acceleration") = bolt_.get_base_linear_acceleration(); 97 | map.at("slider_positions") = bolt_.get_slider_positions(); 98 | 99 | /** 100 | * Robot status 101 | */ 102 | dynamicgraph::Vector& map_motor_enabled = map.at("motor_enabled"); 103 | dynamicgraph::Vector& map_motor_ready = map.at("motor_ready"); 104 | dynamicgraph::Vector& map_motor_board_enabled = 105 | map.at("motor_board_enabled"); 106 | dynamicgraph::Vector& map_motor_board_errors = map.at("motor_board_errors"); 107 | Eigen::Ref > motor_enabled = 108 | bolt_.get_motor_enabled(); 109 | Eigen::Ref > motor_ready = 110 | bolt_.get_motor_ready(); 111 | Eigen::Ref > 112 | motor_board_enabled = bolt_.get_motor_board_enabled(); 113 | Eigen::Ref > 114 | motor_board_errors = bolt_.get_motor_board_errors(); 115 | 116 | for (unsigned i = 0; i < BOLT_NB_MOTOR; ++i) 117 | { 118 | map_motor_enabled[i] = motor_enabled[i]; 119 | map_motor_ready[i] = motor_ready[i]; 120 | } 121 | for (unsigned i = 0; i < BOLT_NB_MOTOR_BOARD; ++i) 122 | { 123 | map_motor_board_enabled[i] = motor_board_enabled[i]; 124 | map_motor_board_errors[i] = motor_board_errors[i]; 125 | } 126 | } 127 | 128 | void DGMBolt::set_motor_controls_from_map( 129 | const dynamic_graph_manager::VectorDGMap& map) 130 | { 131 | try 132 | { 133 | // here we need to perform and internal copy. Otherwise the compilator 134 | // complains 135 | ctrl_joint_torques_ = map.at("ctrl_joint_torques"); 136 | // Actually send the control to the robot 137 | bolt_.send_target_joint_torque(ctrl_joint_torques_); 138 | } 139 | catch (const std::exception& e) 140 | { 141 | rt_printf( 142 | "DGMBolt::set_motor_controls_from_map: " 143 | "Error sending controls, %s\n", 144 | e.what()); 145 | } 146 | } 147 | 148 | void DGMBolt::calibrate_joint_position_callback( 149 | mim_msgs::srv::JointCalibration::Request::SharedPtr, 150 | mim_msgs::srv::JointCalibration::Response::SharedPtr res) 151 | { 152 | // parse and register the command for further call. 153 | add_user_command(std::bind(&DGMBolt::calibrate_joint_position, this)); 154 | 155 | // return whatever the user want 156 | res->sanity_check = true; 157 | } 158 | 159 | void DGMBolt::calibrate_joint_position() 160 | { 161 | bolt_.request_calibration(); 162 | } 163 | 164 | } // namespace bolt 165 | -------------------------------------------------------------------------------- /src/dgm_bolt_humanoid.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file dgm_bolt.cpp 3 | * \brief The hardware wrapper of the bolt robot 4 | * \author Maximilien Naveau 5 | * \date 2018 6 | * 7 | * This file defines the TestBench8Motors class. 8 | */ 9 | 10 | #include "bolt/dgm_bolt_humanoid.hpp" 11 | 12 | #include "dynamic_graph_manager/ros.hpp" 13 | 14 | namespace bolt 15 | { 16 | DGMBoltHumanoid::DGMBoltHumanoid() 17 | { 18 | was_in_safety_mode_ = false; 19 | } 20 | 21 | DGMBoltHumanoid::~DGMBoltHumanoid() 22 | { 23 | } 24 | 25 | bool DGMBoltHumanoid::is_in_safety_mode() 26 | { 27 | // Check if any card is in an error state. 28 | if (bolt_.has_error()) { 29 | was_in_safety_mode_ = true; 30 | static int counter = 0; 31 | if (counter % 2000 == 0) { 32 | printf("DGMBoltHumanoid: Going into safe mode as motor card reports error.\n"); 33 | } 34 | counter += 1; 35 | } 36 | 37 | if (was_in_safety_mode_ || DynamicGraphManager::is_in_safety_mode()) 38 | { 39 | static int counter = 0; 40 | was_in_safety_mode_ = true; 41 | if (counter % 2000 == 0) 42 | { 43 | printf("DGMBoltHumanoid: is_in_safety_mode.\n"); 44 | } 45 | counter++; 46 | } 47 | return was_in_safety_mode_; 48 | } 49 | 50 | 51 | void DGMBoltHumanoid::initialize_hardware_communication_process() 52 | { 53 | /** 54 | * Load the calibration parameters 55 | */ 56 | 57 | // get the hardware communication ros node handle 58 | dynamic_graph_manager::RosNodePtr ros_node_handle = 59 | dynamic_graph_manager::get_ros_node( 60 | dynamic_graph_manager::HWC_ROS_NODE_NAME); 61 | 62 | /** initialize the user commands */ 63 | ros_user_commands_.push_back( 64 | ros_node_handle->create_service( 65 | "calibrate_joint_position", 66 | std::bind(&DGMBoltHumanoid::calibrate_joint_position_callback, 67 | this, 68 | std::placeholders::_1, 69 | std::placeholders::_2))); 70 | 71 | std::string network_id; 72 | YAML::ReadParameter( 73 | params_["hardware_communication"], "network_id", network_id); 74 | 75 | bolt_.initialize(network_id); 76 | } 77 | 78 | void DGMBoltHumanoid::get_sensors_to_map(dynamic_graph_manager::VectorDGMap& map) 79 | { 80 | bolt_.acquire_sensors(); 81 | 82 | /** 83 | * Joint data 84 | */ 85 | map.at("joint_positions") = bolt_.get_joint_positions(); 86 | map.at("joint_velocities") = bolt_.get_joint_velocities(); 87 | map.at("joint_torques") = bolt_.get_joint_torques(); 88 | map.at("joint_target_torques") = bolt_.get_joint_target_torques(); 89 | 90 | /** 91 | * Additional data 92 | */ 93 | map.at("base_accelerometer") = bolt_.get_base_accelerometer(); 94 | map.at("base_gyroscope") = bolt_.get_base_gyroscope(); 95 | map.at("base_attitude") = bolt_.get_base_attitude(); 96 | map.at("base_linear_acceleration") = bolt_.get_base_linear_acceleration(); 97 | map.at("slider_positions") = bolt_.get_slider_positions(); 98 | 99 | /** 100 | * Robot status 101 | */ 102 | dynamicgraph::Vector& map_motor_enabled = map.at("motor_enabled"); 103 | dynamicgraph::Vector& map_motor_ready = map.at("motor_ready"); 104 | dynamicgraph::Vector& map_motor_board_enabled = 105 | map.at("motor_board_enabled"); 106 | dynamicgraph::Vector& map_motor_board_errors = map.at("motor_board_errors"); 107 | Eigen::Ref > motor_enabled = 108 | bolt_.get_motor_enabled(); 109 | Eigen::Ref > motor_ready = 110 | bolt_.get_motor_ready(); 111 | Eigen::Ref > 112 | motor_board_enabled = bolt_.get_motor_board_enabled(); 113 | Eigen::Ref > 114 | motor_board_errors = bolt_.get_motor_board_errors(); 115 | 116 | for (unsigned i = 0; i < BOLT_HUMANOID_NB_MOTOR; ++i) 117 | { 118 | map_motor_enabled[i] = motor_enabled[i]; 119 | map_motor_ready[i] = motor_ready[i]; 120 | } 121 | for (unsigned i = 0; i < BOLT_HUMANOID_NB_MOTOR_BOARD; ++i) 122 | { 123 | map_motor_board_enabled[i] = motor_board_enabled[i]; 124 | map_motor_board_errors[i] = motor_board_errors[i]; 125 | } 126 | } 127 | 128 | void DGMBoltHumanoid::set_motor_controls_from_map( 129 | const dynamic_graph_manager::VectorDGMap& map) 130 | { 131 | try 132 | { 133 | // here we need to perform and internal copy. Otherwise the compilator 134 | // complains 135 | ctrl_joint_torques_ = map.at("ctrl_joint_torques"); 136 | // Actually send the control to the robot 137 | bolt_.send_target_joint_torque(ctrl_joint_torques_); 138 | } 139 | catch (const std::exception& e) 140 | { 141 | rt_printf( 142 | "DGMBoltHumanoid::set_motor_controls_from_map: " 143 | "Error sending controls, %s\n", 144 | e.what()); 145 | } 146 | } 147 | 148 | void DGMBoltHumanoid::calibrate_joint_position_callback( 149 | mim_msgs::srv::JointCalibration::Request::SharedPtr, 150 | mim_msgs::srv::JointCalibration::Response::SharedPtr res) 151 | { 152 | // parse and register the command for further call. 153 | add_user_command(std::bind(&DGMBoltHumanoid::calibrate_joint_position, this)); 154 | 155 | // return whatever the user want 156 | res->sanity_check = true; 157 | } 158 | 159 | void DGMBoltHumanoid::calibrate_joint_position() 160 | { 161 | bolt_.request_calibration(); 162 | } 163 | 164 | } // namespace bolt 165 | -------------------------------------------------------------------------------- /src/programs/bolt_dg.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file bolt.cpp 3 | * \brief Execute the main program to control the bolt 4 | * \author Maximilien Naveau 5 | * \date 2018 6 | * 7 | * DynamicGraphManager for bolt main executable. 8 | */ 9 | 10 | #include 11 | #include "bolt/dgm_bolt.hpp" 12 | 13 | int main(int, char*[]) 14 | { 15 | sleep(2); 16 | bool ready = false; 17 | while(!ready) 18 | { 19 | try{ 20 | shared_memory::get("bolt", "ready", ready); 21 | }catch(...) 22 | { 23 | ready = false; 24 | } 25 | sleep(0.5); 26 | std::cout << "Wait for the shared memory to be ready." << std::endl; 27 | } 28 | std::cout << "Shared memory ready." << std::endl; 29 | 30 | // Get the dynamic_graph_manager config file. 31 | std::string yaml_path = DYNAMIC_GRAPH_MANAGER_YAML_PATH; 32 | 33 | std::cout << "Loading parameters from " << yaml_path << std::endl; 34 | 35 | std::ifstream f(yaml_path.c_str()); 36 | if (!f.good()) 37 | { 38 | throw std::runtime_error("Error: " + yaml_path + " not found!"); 39 | } 40 | YAML::Node param = YAML::LoadFile(yaml_path); 41 | // Create the dgm. 42 | bolt::DGMBolt dgm; 43 | 44 | // Initialize and run it. 45 | dgm.initialize(param); 46 | 47 | dgm.initialize_dynamic_graph_process(); 48 | dgm.run_dynamic_graph_process(); 49 | dynamic_graph_manager::ros_spin(); 50 | dynamic_graph_manager::ros_shutdown(); 51 | std::cout << "DG: End of the dynamic graph process." << std::endl; 52 | } -------------------------------------------------------------------------------- /src/programs/bolt_hw.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file bolt.cpp 3 | * \brief Execute the main program to control the bolt 4 | * \author Maximilien Naveau 5 | * \date 2018 6 | * 7 | * DynamicGraphManager for bolt main executable. 8 | */ 9 | 10 | #include 11 | #include "bolt/dgm_bolt.hpp" 12 | 13 | int main(int, char*[]) 14 | { 15 | // Bolt ready 16 | shared_memory::clear_shared_memory("bolt"); 17 | shared_memory::set("bolt", "ready", false); 18 | // ShM name 19 | shared_memory::clear_shared_memory("dgm_shm_name"); 20 | shared_memory::set( 21 | "dgm_shm_name", "shared_memory_name", dynamic_graph_manager::DynamicGraphManager::shared_memory_name_); 22 | // clean the shared memory 23 | shared_memory::clear_shared_memory(dynamic_graph_manager::DynamicGraphManager::shared_memory_name_); 24 | // we create and destroy the condition variable to free the shared memory 25 | // and therefore the associated mutex which must be lockable at this state. 26 | { 27 | shared_memory::LockedConditionVariable(dynamic_graph_manager::DynamicGraphManager::cond_var_name_, true); 28 | } 29 | shared_memory::set("bolt", "ready", true); 30 | 31 | // Get the dynamic_graph_manager config file. 32 | std::string yaml_path = DYNAMIC_GRAPH_MANAGER_YAML_PATH; 33 | 34 | std::cout << "Loading parameters from " << yaml_path << std::endl; 35 | 36 | std::ifstream f(yaml_path.c_str()); 37 | if (!f.good()) 38 | { 39 | throw std::runtime_error("Error: " + yaml_path + " not found!"); 40 | } 41 | YAML::Node param = YAML::LoadFile(yaml_path); 42 | // Create the dgm. 43 | bolt::DGMBolt dgm; 44 | 45 | // Initialize and run it. 46 | dgm.initialize(param); 47 | 48 | dgm.initialize_hardware_communication_process(); 49 | dgm.run_hardware_communication_process(); 50 | 51 | std::cout << "Wait for shutdown, press CTRL+C to close." << std::endl; 52 | dynamic_graph_manager::ros_spin(); 53 | dynamic_graph_manager::ros_shutdown(); 54 | } -------------------------------------------------------------------------------- /src/programs/dg_main_bolt.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file bolt.cpp 3 | * \brief Execute the main program to control the bolt 4 | * \author Maximilien Naveau 5 | * \date 2018 6 | * 7 | * DynamicGraphManager for bolt main executable. 8 | */ 9 | 10 | #include 11 | #include "bolt/dgm_bolt.hpp" 12 | 13 | int main(int, char*[]) 14 | { 15 | // Get the dynamic_graph_manager config file. 16 | std::string yaml_path = DYNAMIC_GRAPH_MANAGER_YAML_PATH; 17 | 18 | std::cout << "Loading parameters from " << yaml_path << std::endl; 19 | 20 | std::ifstream f(yaml_path.c_str()); 21 | if (!f.good()) 22 | { 23 | throw std::runtime_error("Error: " + yaml_path + " not found!"); 24 | } 25 | YAML::Node param = YAML::LoadFile(yaml_path); 26 | // Create the dgm. 27 | bolt::DGMBolt dgm; 28 | 29 | // Initialize and run it. 30 | dgm.initialize(param); 31 | dgm.run(); 32 | // dgm.run_single_process(); 33 | } -------------------------------------------------------------------------------- /src/programs/dg_main_bolt_humanoid.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file bolt.cpp 3 | * \brief Execute the main program to control the bolt 4 | * \author Maximilien Naveau 5 | * \date 2018 6 | * 7 | * DynamicGraphManager for bolt main executable. 8 | */ 9 | 10 | #include 11 | #include "bolt/dgm_bolt_humanoid.hpp" 12 | 13 | int main(int, char*[]) 14 | { 15 | // Get the dynamic_graph_manager config file. 16 | std::string yaml_path = DYNAMIC_GRAPH_MANAGER_HUMANOID_YAML_PATH; 17 | 18 | std::cout << "Loading parameters from " << yaml_path << std::endl; 19 | 20 | std::ifstream f(yaml_path.c_str()); 21 | if (!f.good()) 22 | { 23 | throw std::runtime_error("Error: " + yaml_path + " not found!"); 24 | } 25 | YAML::Node param = YAML::LoadFile(yaml_path); 26 | // Create the dgm. 27 | bolt::DGMBoltHumanoid dgm; 28 | 29 | // Initialize and run it. 30 | dgm.initialize(param); 31 | dgm.run(); 32 | // dgm.run_single_process(); 33 | } -------------------------------------------------------------------------------- /src/programs/hardware_calibration.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @license BSD 3-clause 4 | * @copyright Copyright (c) 2020, New York University and Max Planck 5 | * Gesellschaft 6 | * 7 | * @brief This program is used to perform the hardware calibration of the robot. 8 | * Procedure: 9 | * - You must start the robot in a position that is close to the "zero" pose. 10 | * - Then start the demo: roscd && sudo ./lib/bolt/hardware_calibration 11 | * The "network_id" is the left column of the output of `ifconfig` 12 | * in bash. 13 | * - The robot is going to find the closest index in a specific direction and 14 | * go idle (0 torques sent). 15 | * - In the terminal you have displayed the distance between the zero and the 16 | * found index. Therefore once you placed the joints to the proper zero 17 | * configuration you can just kill the application with ctrl+c and copy the 18 | * date to you favorite configuration file. 19 | */ 20 | 21 | #include "bolt/utils.hpp" 22 | #include "bolt/bolt.hpp" 23 | 24 | using namespace bolt; 25 | 26 | static THREAD_FUNCTION_RETURN_TYPE control_loop(void* robot_void_ptr) 27 | { 28 | Bolt& robot = *(static_cast(robot_void_ptr)); 29 | 30 | Eigen::Vector6d joint_index_to_zero = Eigen::Vector6d::Zero(); 31 | Eigen::Vector6d dummy_command = Eigen::Vector6d::Zero(); 32 | 33 | robot.wait_until_ready(); 34 | 35 | robot.request_calibration(joint_index_to_zero); 36 | 37 | real_time_tools::Spinner spinner; 38 | spinner.set_period(0.001); 39 | rt_printf("Running calibration...\n"); 40 | while (!CTRL_C_DETECTED && robot.is_calibrating()) 41 | { 42 | robot.acquire_sensors(); 43 | robot.send_target_joint_torque(dummy_command); 44 | spinner.spin(); 45 | } 46 | 47 | // re-send 0 torque command 48 | robot.send_target_joint_torque(dummy_command); 49 | 50 | rt_printf("Running calibration... Done.\n"); 51 | int count = 0; 52 | spinner.set_period(0.001); 53 | while (!CTRL_C_DETECTED) 54 | { 55 | robot.acquire_sensors(); 56 | if (count % 100 == 0){ 57 | print_vector("Joint Positions", -robot.get_joint_positions()); 58 | } 59 | // re-send 0 torque command 60 | robot.send_target_joint_torque(dummy_command); 61 | spinner.spin(); 62 | ++count; 63 | } 64 | 65 | return THREAD_FUNCTION_RETURN_VALUE; 66 | } // end control_loop 67 | 68 | int main(int argc, char** argv) 69 | { 70 | if (argc != 2) 71 | { 72 | throw std::runtime_error( 73 | "Wrong number of argument: `sudo ./hardware_calibration " 74 | "network_id`."); 75 | } 76 | 77 | enable_ctrl_c(); 78 | 79 | real_time_tools::RealTimeThread thread; 80 | 81 | Bolt robot; 82 | robot.initialize(argv[1]); 83 | 84 | rt_printf("Controller is set up.\n"); 85 | // rt_printf("Press enter to launch the calibration.\n"); 86 | // char str[256]; 87 | // std::cin.get(str, 256); // get c-string 88 | 89 | thread.create_realtime_thread(&control_loop, &robot); 90 | 91 | // Wait until the application is killed. 92 | thread.join(); 93 | 94 | rt_printf("Exit cleanly.\n"); 95 | 96 | return 0; 97 | } 98 | -------------------------------------------------------------------------------- /src/programs/hardware_humanoid_calibration.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @license BSD 3-clause 4 | * @copyright Copyright (c) 2020, New York University and Max Planck 5 | * Gesellschaft 6 | * 7 | * @brief This program is used to perform the hardware calibration of the robot. 8 | * Procedure: 9 | * - You must start the robot in a position that is close to the "zero" pose. 10 | * - Then start the demo: roscd && sudo ./lib/bolt/hardware_calibration 11 | * The "network_id" is the left column of the output of `ifconfig` 12 | * in bash. 13 | * - The robot is going to find the closest index in a specific direction and 14 | * go idle (0 torques sent). 15 | * - In the terminal you have displayed the distance between the zero and the 16 | * found index. Therefore once you placed the joints to the proper zero 17 | * configuration you can just kill the application with ctrl+c and copy the 18 | * date to you favorite configuration file. 19 | */ 20 | 21 | #include "bolt/utils.hpp" 22 | #include "bolt/bolt_humanoid.hpp" 23 | 24 | using namespace bolt; 25 | 26 | static THREAD_FUNCTION_RETURN_TYPE control_loop(void* robot_void_ptr) 27 | { 28 | BoltHumanoid& robot = *(static_cast(robot_void_ptr)); 29 | 30 | Eigen::Vector9d joint_index_to_zero = Eigen::Vector9d::Zero(); 31 | Eigen::Vector9d dummy_command = Eigen::Vector9d::Zero(); 32 | 33 | robot.wait_until_ready(); 34 | 35 | robot.request_calibration(joint_index_to_zero); 36 | 37 | real_time_tools::Spinner spinner; 38 | spinner.set_period(0.001); 39 | rt_printf("Running calibration...\n"); 40 | while (!CTRL_C_DETECTED && robot.is_calibrating()) 41 | { 42 | robot.acquire_sensors(); 43 | robot.send_target_joint_torque(dummy_command); 44 | spinner.spin(); 45 | } 46 | 47 | // re-send 0 torque command 48 | robot.send_target_joint_torque(dummy_command); 49 | 50 | rt_printf("Running calibration... Done.\n"); 51 | int count = 0; 52 | spinner.set_period(0.001); 53 | while (!CTRL_C_DETECTED) 54 | { 55 | robot.acquire_sensors(); 56 | if (count % 100 == 0){ 57 | print_vector("Joint Positions", -robot.get_joint_positions()); 58 | } 59 | // re-send 0 torque command 60 | robot.send_target_joint_torque(dummy_command); 61 | spinner.spin(); 62 | ++count; 63 | } 64 | 65 | return THREAD_FUNCTION_RETURN_VALUE; 66 | } // end control_loop 67 | 68 | int main(int argc, char** argv) 69 | { 70 | if (argc != 2) 71 | { 72 | throw std::runtime_error( 73 | "Wrong number of argument: `sudo ./hardware_calibration " 74 | "network_id`."); 75 | } 76 | 77 | enable_ctrl_c(); 78 | 79 | real_time_tools::RealTimeThread thread; 80 | 81 | BoltHumanoid robot; 82 | robot.initialize(argv[1]); 83 | 84 | rt_printf("Controller is set up.\n"); 85 | // rt_printf("Press enter to launch the calibration.\n"); 86 | // char str[256]; 87 | // std::cin.get(str, 256); // get c-string 88 | 89 | thread.create_realtime_thread(&control_loop, &robot); 90 | 91 | // Wait until the application is killed. 92 | thread.join(); 93 | 94 | rt_printf("Exit cleanly.\n"); 95 | 96 | return 0; 97 | } 98 | -------------------------------------------------------------------------------- /srcpy/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # Python bindings. 3 | # 4 | 5 | # Python wrapper 6 | set(wrapper_name bolt_cpp) 7 | 8 | add_library(${wrapper_name} MODULE py_bolt.cpp) 9 | 10 | target_link_libraries(${wrapper_name} PRIVATE pybind11::module) 11 | 12 | target_link_libraries(${wrapper_name} PRIVATE bolt) 13 | 14 | set_target_properties(${wrapper_name} PROPERTIES PREFIX "" SUFFIX 15 | "${PYTHON_MODULE_EXTENSION}") 16 | 17 | target_include_directories( 18 | ${wrapper_name} 19 | PUBLIC BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include> 20 | $ SYSTEM 21 | PUBLIC ${PYTHON_INCLUDE_DIRS}) 22 | 23 | get_python_install_dir(python_install_dir) 24 | 25 | install(TARGETS ${wrapper_name} DESTINATION ${python_install_dir}) 26 | -------------------------------------------------------------------------------- /srcpy/py_bolt.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright [2017] Max Planck Society. All rights reserved. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | */ 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | 24 | namespace py = pybind11; 25 | using namespace bolt; 26 | 27 | PYBIND11_MODULE(bolt, m) 28 | { 29 | py::class_(m, "Bolt") 30 | .def(py::init<>()) 31 | .def("initialize", &Bolt::initialize) 32 | .def("acquire_sensors", &Bolt::acquire_sensors) 33 | .def("send_target_joint_torque", 34 | &Bolt::send_target_joint_torque, 35 | py::arg("target_joint_torque")) 36 | .def("get_motor_board_errors", &Bolt::get_motor_board_errors) 37 | .def("get_motor_board_enabled", &Bolt::get_motor_board_enabled) 38 | .def("get_motor_enabled", &Bolt::get_motor_enabled) 39 | .def("get_motor_ready", &Bolt::get_motor_ready) 40 | .def("get_joint_positions", &Bolt::get_joint_positions) 41 | .def("get_joint_velocities", &Bolt::get_joint_velocities); 42 | } 43 | -------------------------------------------------------------------------------- /tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # Add unit tests. 3 | # 4 | 5 | # Use gtest 6 | find_package(ament_cmake_gtest) 7 | 8 | # Create a usefull macro to build different unit tests suits. 9 | macro(create_blmc_robots_unittest test_name) 10 | 11 | # create the executable 12 | ament_add_gtest(${test_name}_ut main.cpp test_${test_name}.cpp) 13 | # link the dependecies to it 14 | target_link_libraries(${test_name}_ut ${PROJECT_NAME}) 15 | # add some preprocessor variable 16 | target_compile_definitions( 17 | ${test_name}_ut 18 | PUBLIC TEST_CONFIG_PATH="${PROJECT_SOURCE_DIR}/tests/config/") 19 | 20 | endmacro(create_blmc_robots_unittest test_name) 21 | -------------------------------------------------------------------------------- /tests/main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file main.cpp 3 | * \brief gtest main 4 | * \author Maximilien Naveau 5 | * \date 2018 6 | * 7 | * Main file that runs all unittest using gtest 8 | * @see 9 | * https://git-amd.tuebingen.mpg.de/amd-clmc/ci_example/wikis/catkin:-how-to-implement-unit-tests 10 | */ 11 | 12 | #include 13 | 14 | int main(int argc, char **argv) 15 | { 16 | ::testing::InitGoogleTest(&argc, argv); 17 | return RUN_ALL_TESTS(); 18 | } 19 | --------------------------------------------------------------------------------