├── .github └── workflows │ └── demos.yaml ├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── demos ├── config_solo12.yaml ├── config_testbench.yaml ├── demo_solo12.cpp ├── demo_solo12.py ├── demo_solo12_imu.py ├── demo_testbech_joint_calibrator.py ├── demo_testbench.cpp └── robot_calibrator.py ├── documentation ├── calibration.md └── configuration_file.md ├── include └── odri_control_interface │ ├── calibration.hpp │ ├── common.hpp │ ├── device.hpp │ ├── imu.hpp │ ├── joint_modules.hpp │ ├── robot.hpp │ └── utils.hpp ├── license.txt ├── package.xml ├── readme.md ├── schemas └── robot.schema.json ├── src ├── calibration.cpp ├── imu.cpp ├── joint_modules.cpp ├── robot.cpp └── utils.cpp └── srcpy ├── bindings.cpp └── bindings.h /.github/workflows/demos.yaml: -------------------------------------------------------------------------------- 1 | name: Demos 2 | 3 | on: [push, pull_request] 4 | 5 | jobs: 6 | validate-configs: 7 | name: Validate configurations 8 | runs-on: ubuntu-latest 9 | steps: 10 | - uses: actions/checkout@v4 11 | - name: json-yaml-validate 12 | uses: GrantBirki/json-yaml-validate@v3.3.0 13 | with: 14 | json_schema: ./schemas/robot.schema.json 15 | yaml_as_json: true 16 | files: | 17 | demos/config_solo12.yaml 18 | demos/config_testbench.yaml 19 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | 3 | *build* 4 | 5 | *.user 6 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "jrl-cmakemodules"] 2 | path = cmake 3 | url = https://github.com/jrl-umi3218/jrl-cmakemodules 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2020, New York University and Max Planck Gesellschaft. 3 | # 4 | # License BSD-3 clause 5 | # 6 | 7 | cmake_minimum_required(VERSION 3.10) 8 | 9 | # ---------------------------------------------------- 10 | # --- CXX FLAGS -------------------------------------- 11 | # ---------------------------------------------------- 12 | 13 | set(CXX_DISABLE_WERROR True) 14 | set(CMAKE_VERBOSE_MAKEFILE True) 15 | 16 | # These variables have to be defined before running SETUP_PROJECT 17 | set(PROJECT_NAME odri_control_interface) 18 | set(PROJECT_DESCRIPTION 19 | "Common interface for controlling robots build with the odri master board.") 20 | set(PROJECT_URL 21 | https://github.com/open-dynamic-robot-initiative/odri_control_interface) 22 | set(PROJECT_USE_CMAKE_EXPORT TRUE) 23 | 24 | # Check if the submodule cmake have been initialized 25 | set(JRL_CMAKE_MODULES "${CMAKE_CURRENT_LIST_DIR}/cmake") 26 | if(EXISTS "${JRL_CMAKE_MODULES}/base.cmake") 27 | message(STATUS "JRL cmakemodules found in 'cmake/' git submodule") 28 | else() 29 | find_package(jrl-cmakemodules QUIET CONFIG) 30 | if(jrl-cmakemodules_FOUND) 31 | get_property( 32 | JRL_CMAKE_MODULES 33 | TARGET jrl-cmakemodules::jrl-cmakemodules 34 | PROPERTY INTERFACE_INCLUDE_DIRECTORIES) 35 | message(STATUS "JRL cmakemodules found on system at ${JRL_CMAKE_MODULES}") 36 | elseif(${CMAKE_VERSION} VERSION_LESS "3.14.0") 37 | message( 38 | FATAL_ERROR 39 | "\nCan't find jrl-cmakemodules. Please either:\n" 40 | " - use git submodule: 'git submodule update --init'\n" 41 | " - or install https://github.com/jrl-umi3218/jrl-cmakemodules\n" 42 | " - or upgrade your CMake version to >= 3.14 to allow automatic fetching\n" 43 | ) 44 | else() 45 | message(STATUS "JRL cmakemodules not found. Let's fetch it.") 46 | include(FetchContent) 47 | FetchContent_Declare( 48 | "jrl-cmakemodules" 49 | GIT_REPOSITORY "https://github.com/jrl-umi3218/jrl-cmakemodules.git") 50 | FetchContent_MakeAvailable("jrl-cmakemodules") 51 | FetchContent_GetProperties("jrl-cmakemodules" SOURCE_DIR JRL_CMAKE_MODULES) 52 | endif() 53 | endif() 54 | 55 | # --- OPTIONS ---------------------------------------- 56 | option(BUILD_PYTHON_INTERFACE "Build the python binding" ON) 57 | option(PYTHON_STANDARD_LAYOUT "Enable standard Python package layout" ON) 58 | option(PYTHON_DEB_LAYOUT "Enable Debian-style Python package layout" OFF) 59 | 60 | include("${JRL_CMAKE_MODULES}/base.cmake") 61 | include("${JRL_CMAKE_MODULES}/boost.cmake") 62 | include("${JRL_CMAKE_MODULES}/python.cmake") 63 | include("${JRL_CMAKE_MODULES}/ide.cmake") 64 | include("${JRL_CMAKE_MODULES}/apple.cmake") 65 | 66 | compute_project_args(PROJECT_ARGS LANGUAGES CXX) 67 | project(${PROJECT_NAME} ${PROJECT_ARGS}) 68 | apply_default_apple_configuration() 69 | check_minimal_cxx_standard(11 ENFORCE) 70 | 71 | # ---------------------------------------------------- 72 | # --- DEPENDENCIES ----------------------------------- 73 | # ---------------------------------------------------- 74 | 75 | add_project_dependency(yaml-cpp CONFIG REQUIRED) 76 | add_project_dependency(Eigen3 REQUIRED PKG_CONFIG_REQUIRES "eigen3 >= 3.0.5") 77 | add_project_dependency(master_board_sdk REQUIRED) 78 | 79 | # Set component to fetch from boost Get the python interface for the bindings 80 | if(BUILD_PYTHON_INTERFACE) 81 | set(PYTHON_COMPONENTS Interpreter Development.Module) 82 | add_project_dependency(eigenpy 2.7.10 REQUIRED PKG_CONFIG_REQUIRES 83 | "eigenpy >= 2.7.10") 84 | set_boost_default_options() 85 | endif(BUILD_PYTHON_INTERFACE) 86 | 87 | # ---------------------------------------------------- 88 | # --- INCLUDE ---------------------------------------- 89 | # ---------------------------------------------------- 90 | 91 | # --- MAIN LIBRARY ------------------------------------------------------------- 92 | set(ODRI_CONTROL_INTERFACE_SRC src/joint_modules.cpp src/imu.cpp src/robot.cpp 93 | src/calibration.cpp src/utils.cpp) 94 | add_library(${PROJECT_NAME} SHARED ${ODRI_CONTROL_INTERFACE_SRC}) 95 | if(TARGET yaml-cpp::yaml-cpp) 96 | target_link_libraries(${PROJECT_NAME} yaml-cpp::yaml-cpp) 97 | else() 98 | target_link_libraries(${PROJECT_NAME} yaml-cpp) 99 | endif() 100 | target_link_libraries(${PROJECT_NAME} master_board_sdk::master_board_sdk) 101 | target_link_libraries(${PROJECT_NAME} Eigen3::Eigen) 102 | target_include_directories( 103 | ${PROJECT_NAME} PUBLIC $ 104 | $) 105 | install( 106 | TARGETS ${PROJECT_NAME} 107 | EXPORT ${TARGETS_EXPORT_NAME} 108 | DESTINATION lib) 109 | install(DIRECTORY include/ DESTINATION include) 110 | 111 | # --- BINDINGS ----------------------------------------------------------------- 112 | 113 | if(BUILD_PYTHON_INTERFACE) 114 | # --- Setup the wrapper name and source files --- # 115 | set(PYWRAP ${PROJECT_NAME}_pywrap) 116 | set(${PYWRAP}_HEADERS srcpy/bindings.h) 117 | set(${PYWRAP}_SOURCES srcpy/bindings.cpp) 118 | 119 | # --- Build the wrapper library --- # 120 | set(${PYWRAP}_INSTALL_DIR ${PYTHON_SITELIB}) 121 | add_library(${PYWRAP} SHARED ${${PYWRAP}_SOURCES} ${${PYWRAP}_HEADERS}) 122 | target_include_directories(${PYWRAP} SYSTEM PRIVATE ${PYTHON_INCLUDE_DIR}) 123 | 124 | # target_link_libraries(${PYWRAP} ${PROJECT_NAME} eigenpy::eigenpy) 125 | # target_link_boost_python(${PYWRAP}) 126 | # set_target_properties(${PYWRAP} PROPERTIES INSTALL_RPATH 127 | # "${CMAKE_INSTALL_PREFIX}/lib") 128 | set_target_properties(${PYWRAP} PROPERTIES SUFFIX ${PYTHON_EXT_SUFFIX}) 129 | target_link_libraries(${PYWRAP} PUBLIC ${PROJECT_NAME} eigenpy::eigenpy) 130 | target_link_boost_python(${PYWRAP} PUBLIC) 131 | if(UNIX) 132 | get_relative_rpath(${${PYWRAP}_INSTALL_DIR} ${PYWRAP}_INSTALL_RPATH) 133 | set_target_properties(${PYWRAP} PROPERTIES INSTALL_RPATH 134 | "${${PYWRAP}_INSTALL_RPATH}") 135 | endif() 136 | install(TARGETS ${PYWRAP} DESTINATION ${${PYWRAP}_INSTALL_DIR}) 137 | 138 | # --- Allow to do: make python --- # 139 | add_custom_target(python) 140 | set_target_properties(python PROPERTIES EXCLUDE_FROM_DEFAULT_BUILD True) 141 | add_dependencies(python ${PYWRAP}) 142 | 143 | endif(BUILD_PYTHON_INTERFACE) 144 | 145 | # --- CHECK DEMOS ----------------------------------------------------------- 146 | macro(create_demo source) 147 | 148 | set(demo_name ${PROJECT_NAME}_${source}) 149 | add_executable(${demo_name} demos/${source}.cpp) 150 | target_link_libraries(${demo_name} ${PROJECT_NAME}) 151 | target_compile_definitions( 152 | ${demo_name} 153 | PUBLIC CONFIG_SOLO12_YAML="${PROJECT_SOURCE_DIR}/demos/config_solo12.yaml" 154 | PUBLIC CONFIG_TESTBENCH_YAML="${PROJECT_SOURCE_DIR}/demos/config_testbench.yaml") 155 | install(TARGETS ${demo_name} DESTINATION bin) 156 | 157 | endmacro(create_demo source) 158 | 159 | create_demo(demo_solo12) 160 | create_demo(demo_testbench) 161 | -------------------------------------------------------------------------------- /demos/config_solo12.yaml: -------------------------------------------------------------------------------- 1 | # yaml-language-server: $schema=https://raw.githubusercontent.com/open-dynamic-robot-initiative/odri_control_interface/main/schemas/robot.schema.json 2 | 3 | robot: 4 | interface: enp5s0f1 5 | joint_modules: 6 | motor_numbers: [0, 3, 2, 1, 5, 4, 6, 9, 8, 7, 11, 10] 7 | motor_constants: 0.025 8 | gear_ratios: 9. 9 | max_currents: 8. 10 | reverse_polarities: [ 11 | false, true, true, true, false, false, 12 | false, true, true, true, false, false 13 | ] 14 | lower_joint_limits: [ 15 | -0.9, -1.45, -2.8, -0.9, -1.45, -2.8, 16 | -0.9, -1.45, -2.8, -0.9, -1.45, -2.8 17 | ] 18 | upper_joint_limits: [ 19 | 0.9, 1.45, 2.8, 0.9, 1.45, 2.8, 20 | 0.9, 1.45, 2.8, 0.9, 1.45, 2.8 21 | ] 22 | max_joint_velocities: 80. 23 | safety_damping: 0.2 24 | imu: 25 | rotate_vector: [1, 2, 3] 26 | orientation_vector: [1, 2, 3, 4] 27 | joint_calibrator: 28 | # Can be either POS, NEG, ALT or AUTO 29 | search_methods: [ 30 | POS, POS, POS, NEG, POS, POS, 31 | POS, POS, POS, NEG, POS, POS 32 | ] 33 | position_offsets: [ 34 | 0.179, 0.497, 0.360, 35 | -0.594, 0.219, 0.602, 36 | 0.543, 0.539, 0.436, 37 | -0.241, 0.440, 0.210 38 | ] 39 | calib_order: [1, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0] 40 | calib_pos: [ 41 | 0.0, 1.2, -2.4, 42 | 0.0, 1.2, -2.4, 43 | 0.0, -1.2, 2.4, 44 | 0.0, -1.2, 2.4 45 | ] 46 | Kp: 5. 47 | Kd: 0.05 48 | T: 1. 49 | dt: 0.001 50 | -------------------------------------------------------------------------------- /demos/config_testbench.yaml: -------------------------------------------------------------------------------- 1 | # yaml-language-server: $schema=https://raw.githubusercontent.com/open-dynamic-robot-initiative/odri_control_interface/main/schemas/robot.schema.json 2 | 3 | robot: 4 | interface: enp2s0 5 | joint_modules: 6 | motor_numbers: [0, 1] 7 | motor_constants: 0.025 8 | gear_ratios: 1. 9 | max_currents: 1. 10 | reverse_polarities: [ 11 | false, false 12 | ] 13 | lower_joint_limits: [ 14 | -3.4, -3.4 15 | ] 16 | upper_joint_limits: [ 17 | +3.4, +3.4 18 | ] 19 | max_joint_velocities: 80. 20 | safety_damping: 0.5 21 | imu: 22 | rotate_vector: [1, 2, 3] 23 | orientation_vector: [1, 2, 3, 4] 24 | joint_calibrator: 25 | # Can be either POS, NEG, ALT or AUTO 26 | search_methods: [ 27 | AUTO, AUTO 28 | ] 29 | position_offsets: [ 30 | 2.18667, -0.889898 31 | ] 32 | calib_order: [1, 1] 33 | calib_pos: [0.78, 0.78] 34 | Kp: 0.125 35 | Kd: 0.0025 36 | T: 2.5 37 | dt: 0.001 38 | -------------------------------------------------------------------------------- /demos/demo_solo12.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | using namespace odri_control_interface; 7 | 8 | #include 9 | #include 10 | 11 | typedef Eigen::Matrix Vector12d; 12 | 13 | int main() 14 | { 15 | nice(-20); // Give the process a high priority. 16 | 17 | // Define the robot from a yaml file. 18 | auto robot = RobotFromYamlFile(CONFIG_SOLO12_YAML); 19 | 20 | // Store initial position data. 21 | Vector12d des_pos; 22 | des_pos << 0.0, 0.7, -1.4, -0.0, 0.7, -1.4, 0.0, -0.7, +1.4, -0.0, -0.7, 23 | +1.4; 24 | 25 | // Initialize the communication, session, joints, wait for motors to be ready 26 | // and run the joint calibration. 27 | robot->Initialize(des_pos); 28 | 29 | // Initialize simple pd controller. 30 | Vector12d torques; 31 | double kp = 3.; 32 | double kd = 0.05; 33 | int c = 0; 34 | while (!robot->IsTimeout()) 35 | { 36 | robot->ParseSensorData(); 37 | 38 | // Run the main controller. 39 | auto pos = robot->joints->GetPositions(); 40 | auto vel = robot->joints->GetVelocities(); 41 | 42 | // Compute PD control on the zero position. 43 | for (int i = 0; i < 12; i++) 44 | { 45 | torques[i] = kp * (des_pos[i] - pos[i]) - kd * vel[i]; 46 | } 47 | robot->joints->SetTorques(torques); 48 | 49 | // Checks if the robot is in error state (that is, if any component 50 | // returns an error). If there is an error, the commands to send 51 | // are changed to send the safety control. 52 | robot->SendCommandAndWaitEndOfCycle(0.001); 53 | 54 | c++; 55 | if (c % 1000 == 0) 56 | { 57 | std::cout << "Joints: "; 58 | robot->joints->PrintVector(robot->joints->GetPositions()); 59 | std::cout << std::endl; 60 | } 61 | } 62 | 63 | printf("Timeout detected\n"); 64 | 65 | return 0; 66 | } 67 | -------------------------------------------------------------------------------- /demos/demo_solo12.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | # Run a PD controller to keep the zero position. 4 | 5 | import pathlib 6 | 7 | import numpy as np 8 | np.set_printoptions(suppress=True, precision=2) 9 | 10 | import libodri_control_interface_pywrap as oci 11 | 12 | # Create the robot object from yaml. 13 | robot = oci.robot_from_yaml_file("config_solo12.yaml") 14 | 15 | # Store initial position data. 16 | des_pos = np.array( 17 | [0.0, 0.7, -1.4, -0.0, 0.7, -1.4, 0.0, -0.7, +1.4, -0.0, -0.7, +1.4]) 18 | 19 | # Initialize the communication, session, joints, wait for motors to be ready 20 | # and run the joint calibration. 21 | robot.initialize(des_pos) 22 | 23 | c = 0 24 | while not robot.is_timeout: 25 | robot.parse_sensor_data() 26 | 27 | imu_attitude = robot.imu.attitude_euler 28 | positions = robot.joints.positions 29 | velocities = robot.joints.velocities 30 | 31 | # Compute the PD control. 32 | torques = 3.0 * (des_pos - positions) - 0.05 * velocities 33 | robot.joints.set_torques(torques) 34 | 35 | robot.send_command_and_wait_end_of_cycle(0.001) 36 | c += 1 37 | 38 | if c % 2000 == 0: 39 | print("joint pos: ", positions) 40 | print("joint vel: ", velocities) 41 | print("torques: ", torques) 42 | robot.robot_interface.PrintStats() 43 | -------------------------------------------------------------------------------- /demos/demo_solo12_imu.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # 3 | # Similar to `demo_solo12.py`. Uses the IMU attitude to control the desired 4 | # joint positions. 5 | 6 | import pathlib 7 | 8 | import numpy as np 9 | np.set_printoptions(suppress=True, precision=2) 10 | 11 | import libodri_control_interface_pywrap as oci 12 | 13 | # Create the robot object from yaml. 14 | robot = oci.robot_from_yaml_file("config_solo12.yaml") 15 | 16 | # Store initial position data. 17 | init_imu_attitude = robot.imu.attitude_euler.copy() 18 | des_pos = np.zeros(12) 19 | 20 | # Initialize the communication, session, joints, wait for motors to be ready 21 | # and run the joint calibration.ArithmeticError() 22 | robot.initialize(des_pos) 23 | 24 | c = 0 25 | while not robot.is_timeout and not robot.has_error: 26 | robot.parse_sensor_data() 27 | 28 | imu_attitude = robot.imu.attitude_euler 29 | positions = robot.joints.positions 30 | velocities = robot.joints.velocities 31 | 32 | des_pos[:] = imu_attitude[2] - init_imu_attitude[2] 33 | torques = 5.0 * (des_pos - positions) - 0.1 * velocities 34 | robot.joints.set_torques(torques) 35 | 36 | robot.send_command_and_wait_end_of_cycle(0.001) 37 | c += 1 38 | 39 | if c % 2000 == 0: 40 | print("IMU attitude:", imu_attitude) 41 | print("joint pos: ", positions) 42 | print("joint vel: ", velocities) 43 | print("torques: ", torques) 44 | robot.robot_interface.PrintStats() 45 | -------------------------------------------------------------------------------- /demos/demo_testbech_joint_calibrator.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | import numpy as np 4 | 5 | np.set_printoptions(suppress=True, precision=2) 6 | 7 | import time 8 | 9 | import libmaster_board_sdk_pywrap as mbs 10 | import libodri_control_interface_pywrap as oci 11 | 12 | # Testbench 13 | robot_if = mbs.MasterBoardInterface("ens3") 14 | joints = oci.JointModules( 15 | robot_if, 16 | np.array([0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11]), 17 | 0.025, 18 | 1.0, 19 | 1.0, 20 | np.array( 21 | [ 22 | False, 23 | False, 24 | False, 25 | False, 26 | False, 27 | False, 28 | False, 29 | False, 30 | False, 31 | False, 32 | False, 33 | False, 34 | ] 35 | ), 36 | np.array( 37 | [ 38 | -10.0, 39 | -10.0, 40 | -10.0, 41 | -10.0, 42 | -10.0, 43 | -10.0, 44 | -10.0, 45 | -10.0, 46 | -10.0, 47 | -10.0, 48 | -10.0, 49 | -10.0, 50 | ] 51 | ), 52 | np.array( 53 | [ 54 | 10.0, 55 | 10.0, 56 | 10.0, 57 | 10.0, 58 | 10.0, 59 | 10.0, 60 | 10.0, 61 | 10.0, 62 | 10.0, 63 | 10.0, 64 | 10.0, 65 | 10.0, 66 | ] 67 | ), 68 | 80.0, 69 | 0.5, 70 | ) 71 | 72 | 73 | imu = oci.IMU(robot_if) 74 | 75 | robot = oci.Robot(robot_if, joints, imu) 76 | robot.start() 77 | robot.wait_until_ready() 78 | 79 | # As the data is returned by reference, it's enough 80 | # to get hold of the data one. It will update after 81 | # each call to `robot.parse_sensor_data`. 82 | imu_attitude = imu.attitude_euler 83 | positions = joints.positions 84 | velocities = joints.velocities 85 | 86 | des_pos = np.zeros(12) 87 | 88 | # Setup the calibration method 89 | joint_offsets = np.array( 90 | [ 91 | -1.58, 92 | 1.24, 93 | 0.0, 94 | 0.0, 95 | 0.0, 96 | 0.0, 97 | 0.0, 98 | 0.0, 99 | 0.0, 100 | 0.0, 101 | 0.0, 102 | 0.0, 103 | ] 104 | ) 105 | sdir = oci.CalibrationMethod.positive 106 | joint_calibrator = oci.JointCalibrator( 107 | joints, 12 * [sdir], joint_offsets, 108 | np.zeros(12, dtype=int), np.zeros(12), 109 | 5.0 / 20.0, 0.05 / 20.0, 2.0, 0.001 110 | ) 111 | 112 | c = 0 113 | dt = 0.001 114 | calibration_done = False 115 | next_tick = time.time() 116 | while not robot.has_error: 117 | if time.time() >= next_tick: 118 | next_tick = next_tick + dt 119 | robot.parse_sensor_data() 120 | 121 | if c % 2000 == 0: 122 | print("IMU attitude:", imu_attitude) 123 | print("joint pos :", positions) 124 | print("joint vel :", velocities) 125 | 126 | if not calibration_done: 127 | if joint_calibrator.run(): 128 | calibration_done = True 129 | joints.set_position_gains(5.0 * np.ones(12)) 130 | joints.set_velocity_gains(0.001 * np.ones(12)) 131 | else: 132 | des_pos[:] = imu_attitude[2] 133 | joints.set_desired_positions(des_pos) 134 | 135 | robot.send_command() 136 | c += 1 137 | else: 138 | time.sleep(0.0001) 139 | -------------------------------------------------------------------------------- /demos/demo_testbench.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | using namespace odri_control_interface; 7 | 8 | #include 9 | #include 10 | 11 | typedef Eigen::Matrix Vector2d; 12 | 13 | int main() 14 | { 15 | nice(-20); // Give the process a high priority. 16 | 17 | // Define the robot from a yaml file. 18 | auto robot = RobotFromYamlFile(CONFIG_TESTBENCH_YAML); 19 | 20 | // Store initial position data. 21 | Vector2d des_pos(3.1415 * 0.5, - 3.1415 * 0.5); 22 | 23 | // Initialize the communication, session, joints, wait for motors to be ready 24 | // and run the joint calibration. 25 | robot->Initialize(des_pos); 26 | 27 | // Initialize simple pd controller. 28 | Vector2d torques; 29 | double kp = 0.125; 30 | double kd = 0.0025; 31 | int c = 0; 32 | while (!robot->IsTimeout()) 33 | { 34 | robot->ParseSensorData(); 35 | 36 | // Run the main controller. 37 | auto pos = robot->joints->GetPositions(); 38 | auto vel = robot->joints->GetVelocities(); 39 | 40 | // Compute PD control on the zero position. 41 | for (int i = 0; i < 2; i++) 42 | { 43 | torques[i] = kp * (des_pos[i] - pos[i]) - kd * vel[i]; 44 | } 45 | robot->joints->SetTorques(torques); 46 | 47 | // Checks if the robot is in error state (that is, if any component 48 | // returns an error). If there is an error, the commands to send 49 | // are changed to send the safety control. 50 | robot->SendCommandAndWaitEndOfCycle(0.001); 51 | 52 | c++; 53 | if (c % 1000 == 0) 54 | { 55 | std::cout << "Joints: "; 56 | robot->joints->PrintVector(robot->joints->GetPositions()); 57 | std::cout << std::endl; 58 | } 59 | } 60 | 61 | printf("Timeout detected\n"); 62 | 63 | return 0; 64 | } 65 | -------------------------------------------------------------------------------- /demos/robot_calibrator.py: -------------------------------------------------------------------------------- 1 | # coding: utf8 2 | 3 | import argparse 4 | import threading 5 | import libodri_control_interface_pywrap as oci 6 | import numpy as np 7 | 8 | 9 | def get_input(): 10 | keystrk = input() 11 | # thread doesn't continue until key is pressed 12 | # and so it remains alive 13 | 14 | 15 | def control_loop(config_file_path): 16 | """Main function that initializes communication with the robot then waits 17 | until the user presses the Enter key to output joint positions offsets 18 | 19 | Args: 20 | config_file_path (string): path to the yaml config file of the robot 21 | """ 22 | 23 | # Load information about the robot from the yaml config file 24 | device = oci.robot_from_yaml_file(config_file_path) 25 | joint_calibrator = oci.joint_calibrator_from_yaml_file( 26 | config_file_path, device.joints 27 | ) 28 | 29 | # Initiate communication with the device and calibrate encoders 30 | device.initialize(np.zeros(device.joints.number_motors)) 31 | device.joints.set_zero_commands() 32 | device.parse_sensor_data() 33 | 34 | i = threading.Thread(target=get_input) 35 | i.start() 36 | print("Set all actuators to zero positions and press Enter") 37 | 38 | # Control loop 39 | while not device.is_timeout and i.is_alive(): 40 | 41 | # Update sensor data (IMU, encoders, Motion capture) 42 | device.parse_sensor_data() 43 | 44 | # Send command to the robot 45 | device.send_command_and_wait_end_of_cycle(joint_calibrator.dt) 46 | 47 | m = np.pi / device.joints.gear_ratios 48 | offsets = ( 49 | np.mod(joint_calibrator.position_offsets - device.joints.positions + m, 2 * m) 50 | - m 51 | ) 52 | diff = np.mod(joint_calibrator.position_offsets - offsets + m, 2 * m) - m 53 | 54 | print("Difference with the previous position_offsets values:") 55 | print(np.array2string(np.abs(diff), precision=3, separator=", ")) 56 | 57 | print("Values to copy-paste in the position_offsets field of the yaml config file:") 58 | print(np.array2string(offsets, precision=4, separator=", ")) 59 | 60 | return 0 61 | 62 | 63 | def main(): 64 | """Main function""" 65 | 66 | parser = argparse.ArgumentParser( 67 | description="Calibration script to get the joint position offsets of a robot." 68 | ) 69 | parser.add_argument( 70 | "-c", 71 | "--config", 72 | required=True, 73 | help="Path of the config yaml file that contains information about the robot.", 74 | ) 75 | control_loop(parser.parse_args().config) 76 | quit() 77 | 78 | 79 | if __name__ == "__main__": 80 | main() 81 | -------------------------------------------------------------------------------- /documentation/calibration.md: -------------------------------------------------------------------------------- 1 | # Calibration procedure 2 | 3 | * After assembly, a calibration procedure must be performed to retrieve the offsets between the **zero positions of the motors** (position of the indexes on the [coding wheels](https://github.com/open-dynamic-robot-initiative/open_robot_actuator_hardware/blob/master/mechanics/actuator_module_v1/actuator_module_v1.1.md#description-odri-encoder-kit)) and the **positions you want to consider as zero** for your control. 4 | 5 | * The [calibration script](https://github.com/open-dynamic-robot-initiative/odri_control_interface/blob/main/demos/robot_calibrator.py) fetches information from a [yaml configuration file](https://github.com/open-dynamic-robot-initiative/odri_control_interface/blob/main/demos/config_solo12.yaml), it can thus be used to calibrate all robots as long as the information contained in it is correct. Please check the [associated documentation](https://github.com/open-dynamic-robot-initiative/odri_control_interface/blob/main/documentation/configuration_file.dm) before proceeding further. 6 | 7 | ### Launching the script 8 | 9 | * First, turn on the robot and roughtly place its joints in the configuration you want to consider as zero. 10 | 11 |
*Solo 12 quadruped with its joints in zero positions.*
12 | 13 | * You can then launch the calibration script. The robot will slightly move its joints to find the indexes of the coding wheels. The calibration script can be launched as follows: 14 | ``` 15 | sudo -E python3 robot_calibrator.py -c path_to_yaml_config_file 16 | ``` 17 | 18 | * `path_to_yaml_config_file` is the path to the configuration file of the robot you want to calibrate. It can be for instance `config_solo12.yaml` if your script has been launched from the [demos folder](https://github.com/open-dynamic-robot-initiative/odri_control_interface/tree/main/demos) of this repository. 19 | 20 | * Once all indexes have been found, the robot keeps running with **no commands sent to its joints** to let you manually place the joints in the configuration you want to consider as zero. If available, you can use the provided [calibration tools](https://github.com/open-dynamic-robot-initiative/open_robot_actuator_hardware/blob/master/mechanics/general/robot_calibration.md#robot-calibration) to **mechanically** place the robot in zero positions and avoid relying on visual estimation which can lead to small alignement errors. 21 | 22 |
*Calibration tools placed on the Solo 12 quadruped.*
23 | 24 | * When you are satisfied with the joint placement, press the **Enter** key on your keyboard. This will stop the calibration script and output position offsets that you can copy-paste in the position_offsets field of the [yaml configuration file](https://github.com/open-dynamic-robot-initiative/odri_control_interface/blob/main/demos/config_solo12.yaml). These new offsets will be automatically taken into account by the communication interface after you restart the robot. 25 | 26 | * For information purpose, the absolute difference between the offsets contained in the configuration file and the offsets found by the calibration will be displayed as well. 27 | -------------------------------------------------------------------------------- /documentation/configuration_file.md: -------------------------------------------------------------------------------- 1 | # Configuration file 2 | 3 | ### Robot 4 | 5 | The robot node contains general information about the robot such as motor ordering, joint limits or orientation of the IMU with respect to the body frame. 6 | 7 | |Field|Meaning|Example value
(Solo 12)| 8 | |---|---|---| 9 | |interface|Name of the interface to communicate with the robot (you can check with `ifconfig` if you are unsure).|enp5s0f1| 10 | |motor_numbers|Mapping between the [hardware order of the motors and their control order](https://github.com/open-dynamic-robot-initiative/open_robot_actuator_hardware/blob/master/mechanics/quadruped_robot_12dof_v1/README.md#micro-driver-stack-motor-assignment).|[0, 3, 2, 1, 5, 4, 6, 9, 8, 7, 11, 10]| 11 | |motor_constants|Torque constant of the motors in **Nm/A**.|0.025| 12 | |gear_ratios|Gear ratio of the actuator modules.|9| 13 | |max_currents|Absolute limit for the current sent to the motors in **A**.|8| 14 | |reverse_polarities|Whether motors have been assembled on the robot in such a way that their hardware positive direction of rotation is actually reversed compared to the control positive direction of rotation.|[false, true, true, true, false, false, false, true, true, true, false, false]| 15 | |lower_joint_limits|Lower joint limits for the actuators, under which the low-level control automatically switches to a security mode which applies damping on all joints.|[-0.9, -1.45, -2.8, -0.9, -1.45, -2.8, -0.9, -1.45, -2.8, -0.9, -1.45, -2.8]| 16 | |upper_joint_limits|Upper joint limits for the actuators, above which the low-level control automatically switches to a security mode which applies damping on all joints.|[0.9, 1.45, 2.8, 0.9, 1.45, 2.8, 0.9, 1.45, 2.8, 0.9, 1.45, 2.8]| 17 | |max_joint_velocities|Absolute limit for the joint velocities, above which the low-level control automatically switches to a security mode which applies damping on all joints.|80| 18 | |safety_damping|Value of the damping coefficient for the security mode, in **Nm/(rad/s)**.|0.2| 19 | |rotate_vector|Mapping from IMU axes to body axes.|[1, 2, 3]| 20 | |orientation_vector|Mapping from IMU orientation (as a quaternion) to body orientation. Can be useful if the IMU is mounted upside-down on the robot.|[1, 2, 3, 4]| 21 | 22 | ### Joint Calibrator 23 | 24 | The joint calibrator node contains information for the calibration step at startup during which the robot slightly moves its joints to find the indexes of the coding wheels. 25 | 26 | |Field|Meaning|Example value
(Solo 12)| 27 | |---|---|---| 28 | |search_methods|The index search strategy for each motor, can be POS, NEG, ALT, AUTO. If POS, the motor will search the index by going in the positive direction from its starting position until it reaches it. If NEG, it will go in the negative direction. If ALT, the motor will do back-and-forth movements of increasing amplitude around its starting position till the index is reached. AUTO automatically chooses the strategy based on the position offset value.|[AUTO, AUTO, AUTO, AUTO, AUTO, AUTO, AUTO, AUTO, AUTO, AUTO, AUTO, AUTO]| 29 | |position_offsets|Angular position offset between the position of the index on the coding wheel and the zero position of the actuator.|[0.142, -0.251, 0.298, -0.240, -0.247, -0.267, -0.155, -0.109, -0.095, 0.098, 0.271, -0.2476]| 30 | |calib_order|Optional calibration order for the joints. Joints with the lowest values start looking for the indexes first while the others are not moving. Once they have found it, they wait in their associated position in `calib_pos` field. You can set all values to 0 if you want to calibrate all joints at the same time. With the example values, the HFE and Knee joints of the leg are calibrated first, then the leg fold up. That way there is less risk to hit something while the HAA joints are moving to find their indexes.|[1, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0]| 31 | |calib_pos|The waiting position for joints after they have found their indexes (used with `calib_order`).|[0.0, 1.2, -2.4, 0.0, 1.2, -2.4, 0.0, -1.2, 2.4, 0.0, -1.2, 2.4]| 32 | |Kp|The proportional gain of the PD controller used during the calibration process, in **Nm/rad**.|3| 33 | |Kd|The derivative gain of the PD controller used during the calibration process, in **Nm/(rad/s)**.|0.05| 34 | |T|The period of the sinus movement used when looking for indexes, in **seconds**.|1| 35 | |dt|The control time step during the calibration process, in **seconds**.|0.001| 36 | -------------------------------------------------------------------------------- /include/odri_control_interface/calibration.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file joint_modules.hpp 3 | * @author Julian Viereck (jviereck@tuebingen.mpg.de) 4 | * license License BSD-3-Clause 5 | * @copyright Copyright (c) 2020, New York University and Max Planck 6 | * Gesellschaft. 7 | * @date 2020-11-27 8 | * 9 | * @brief Class for calibrating the joints. 10 | */ 11 | 12 | #pragma once 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | #include "master_board_sdk/defines.h" 19 | #include "master_board_sdk/master_board_interface.h" 20 | 21 | #include 22 | #include 23 | 24 | namespace odri_control_interface 25 | { 26 | const int SEARCHING = 0; 27 | const int WAITING = 1; 28 | const int GOTO = 2; 29 | const double AMPLITUDE = 1.5; 30 | 31 | enum CalibrationMethod 32 | { 33 | AUTO, 34 | POSITIVE, 35 | NEGATIVE, 36 | ALTERNATIVE 37 | }; 38 | 39 | /** 40 | * @brief 41 | */ 42 | class JointCalibrator 43 | { 44 | protected: 45 | std::shared_ptr joints_; 46 | std::vector search_methods_; 47 | VectorXd position_offsets_; 48 | VectorXi calib_order_; 49 | VectorXd calib_pos_; 50 | VectorXd initial_positions_; 51 | VectorXd target_positions_; 52 | VectorXb found_index_; 53 | VectorXd gear_ratios_; 54 | VectorXd pos_command_; 55 | VectorXd vel_command_; 56 | VectorXd kp_command_; 57 | VectorXd kd_command_; 58 | VectorXd zero_vector_; 59 | double Kp_; 60 | double Kd_; 61 | double T_; 62 | double T_wait_; 63 | double dt_; 64 | double t_; 65 | int n_; 66 | bool already_calibrated_; 67 | int calib_state_; 68 | int step_number_; 69 | int step_number_max_; 70 | bool step_indexes_detected_; 71 | double t_step_indexes_detected_; 72 | double t_step_end_; 73 | 74 | public: 75 | 76 | JointCalibrator(const std::shared_ptr& joints, 77 | const std::vector& search_methods, 78 | RefVectorXd position_offsets, 79 | RefVectorXi calib_order, 80 | RefVectorXd calib_pos, 81 | double Kp, 82 | double Kd, 83 | double T, 84 | double dt); 85 | 86 | void UpdatePositionOffsets(ConstRefVectorXd position_offsets); 87 | 88 | /** 89 | * Return the joint position offsets. 90 | */ 91 | const VectorXd& GetPositionOffsets(); 92 | 93 | /** 94 | * Return the dt used by the joint calibrator. 95 | */ 96 | const double& dt(); 97 | 98 | /** 99 | * @brief Runs the calibration procedure. Returns true if the calibration is 100 | * done. Legs are placed in zero position at the end. 101 | */ 102 | bool Run(); 103 | 104 | /** 105 | * @brief Runs the calibration procedure. Returns true if the calibration is 106 | * done. Legs are placed at the target position at the end. 107 | * 108 | * @param target_positions target positions for the legs at the end of the calibration 109 | */ 110 | bool RunAndGoTo(VectorXd const& target_positions); 111 | 112 | /** 113 | * @brief Search the index using the desired 114 | * search method 115 | * 116 | * @param i the searching motor number 117 | */ 118 | void SearchIndex(int i); 119 | 120 | /** 121 | * @brief Start the calibration waiting time by setting 122 | * gains to zero and enable index compensation. 123 | */ 124 | void SwitchToWaiting(); 125 | }; 126 | 127 | } // namespace odri_control_interface 128 | -------------------------------------------------------------------------------- /include/odri_control_interface/common.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file common.hpp 3 | * @author Julian Viereck (jviereck@tuebingen.mpg.de) 4 | * license License BSD-3-Clause 5 | * @copyright Copyright (c) 2020, New York University and Max Planck 6 | * Gesellschaft. 7 | * @date 2020-11-27 8 | * 9 | * @brief Common definitions used across the library. 10 | */ 11 | 12 | #include 13 | 14 | namespace odri_control_interface 15 | { 16 | typedef Eigen::VectorXd VectorXd; 17 | typedef Eigen::Matrix VectorXi; 18 | typedef Eigen::Matrix VectorXl; 19 | typedef Eigen::Matrix VectorXb; 20 | 21 | typedef Eigen::Ref > RefVectorXi; 22 | typedef Eigen::Ref > RefVectorXl; 23 | typedef Eigen::Ref > RefVectorXb; 24 | typedef Eigen::Ref > RefVectorXd; 25 | typedef Eigen::Ref RefVector3d; 26 | typedef Eigen::Ref RefVector4d; 27 | 28 | typedef const Eigen::Ref > 29 | ConstRefVectorXi; 30 | typedef const Eigen::Ref > 31 | ConstRefVectorXl; 32 | typedef const Eigen::Ref > 33 | ConstRefVectorXb; 34 | typedef const Eigen::Ref > 35 | ConstRefVectorXd; 36 | typedef const Eigen::Ref ConstRefVector3d; 37 | typedef const Eigen::Ref ConstRefVector4d; 38 | 39 | } // namespace odri_control_interface 40 | -------------------------------------------------------------------------------- /include/odri_control_interface/device.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file device.hpp 3 | * @author Julian Viereck (jviereck@tuebingen.mpg.de) 4 | * license License BSD-3-Clause 5 | * @copyright Copyright (c) 2020, New York University and Max Planck 6 | * Gesellschaft. 7 | * @date 2020-11-27 8 | * 9 | * @brief Abstract device class used in framework. 10 | */ 11 | 12 | #pragma once 13 | 14 | #include 15 | 16 | #include "master_board_sdk/defines.h" 17 | #include "master_board_sdk/master_board_interface.h" 18 | 19 | namespace odri_control_interface 20 | { 21 | /** 22 | * @brief Abstract device class used in framework. 23 | */ 24 | class Device 25 | { 26 | public: 27 | virtual bool HasError() = 0; 28 | }; 29 | 30 | } // namespace odri_control_interface 31 | -------------------------------------------------------------------------------- /include/odri_control_interface/imu.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file imu.hpp 3 | * @author Julian Viereck (jviereck@tuebingen.mpg.de) 4 | * license License BSD-3-Clause 5 | * @copyright Copyright (c) 2020, New York University and Max Planck 6 | * Gesellschaft. 7 | * @date 2020-12-05 8 | * 9 | * @brief IMU abstraction. 10 | */ 11 | 12 | #pragma once 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include "master_board_sdk/defines.h" 20 | #include "master_board_sdk/master_board_interface.h" 21 | 22 | #include 23 | 24 | namespace odri_control_interface 25 | { 26 | /** 27 | * @brief Class for dealing with the IMU. 28 | */ 29 | class IMU 30 | { 31 | protected: 32 | std::shared_ptr robot_if_; 33 | std::array rotate_vector_; 34 | std::array orientation_vector_; 35 | 36 | // Cache for the results. 37 | Eigen::Vector3d gyroscope_; 38 | Eigen::Vector3d accelerometer_; 39 | Eigen::Vector3d linear_acceleration_; 40 | Eigen::Vector3d attitude_euler_; 41 | Eigen::Vector4d attitude_quaternion_; 42 | 43 | public: 44 | IMU(const std::shared_ptr& robot_if, 45 | RefVectorXl rotate_vector, 46 | RefVectorXl orientation_vector); 47 | 48 | IMU(const std::shared_ptr& robot_if); 49 | 50 | // If needed, add some error handling for the IMU as well. 51 | // For instance, check for bounds on the maximum linear acceleration 52 | // or the maximum angular velocity that should be detected as an error. 53 | bool HasError() 54 | { 55 | return false; 56 | } 57 | 58 | void ParseSensorData(); 59 | 60 | const std::shared_ptr& GetMasterBoardInterface(); 61 | const Eigen::Vector3d& GetGyroscope(); 62 | const Eigen::Vector3d& GetAccelerometer(); 63 | const Eigen::Vector3d& GetLinearAcceleration(); 64 | const Eigen::Vector3d& GetAttitudeEuler(); 65 | const Eigen::Vector4d& GetAttitudeQuaternion(); 66 | }; 67 | 68 | } // namespace odri_control_interface 69 | -------------------------------------------------------------------------------- /include/odri_control_interface/joint_modules.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file joint_modules.hpp 3 | * @author Julian Viereck (jviereck@tuebingen.mpg.de) 4 | * license License BSD-3-Clause 5 | * @copyright Copyright (c) 2020, New York University and Max Planck 6 | * Gesellschaft. 7 | * @date 2020-11-27 8 | * 9 | * @brief Joint module abstraction for 10 | */ 11 | 12 | #pragma once 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include "master_board_sdk/defines.h" 20 | #include "master_board_sdk/master_board_interface.h" 21 | 22 | #include 23 | 24 | namespace odri_control_interface 25 | { 26 | /** 27 | * @brief Class abstracting the blmc motors to modules. 28 | */ 29 | class JointModules 30 | { 31 | protected: 32 | std::shared_ptr robot_if_; 33 | std::vector motors_; 34 | 35 | Eigen::VectorXd gear_ratios_; 36 | Eigen::VectorXd motor_constants_; 37 | VectorXi polarities_; 38 | Eigen::VectorXd lower_joint_limits_; 39 | Eigen::VectorXd upper_joint_limits_; 40 | Eigen::VectorXd safety_damping_; 41 | 42 | // Cache for results. 43 | Eigen::VectorXd positions_; 44 | Eigen::VectorXd velocities_; 45 | Eigen::VectorXd sent_torques_; 46 | Eigen::VectorXd measured_torques_; 47 | 48 | // Cache for status bits. 49 | VectorXb index_been_detected_; 50 | VectorXb ready_; 51 | VectorXb enabled_; 52 | VectorXb motor_driver_enabled_; 53 | VectorXi motor_driver_errors_; 54 | 55 | Eigen::VectorXd zero_vector_; 56 | 57 | double max_joint_velocities_; 58 | 59 | int n_; 60 | int nd_; 61 | 62 | bool check_joint_limits_; 63 | 64 | std::ostream& msg_out_ = std::cout; 65 | 66 | public: 67 | JointModules(const std::shared_ptr& robot_if, 68 | ConstRefVectorXi motor_numbers, 69 | double motor_constants, 70 | double gear_ratios, 71 | double max_currents, 72 | ConstRefVectorXb reverse_polarities, 73 | ConstRefVectorXd lower_joint_limits, 74 | ConstRefVectorXd upper_joint_limits, 75 | double max_joint_velocities, 76 | double safety_damping); 77 | virtual ~JointModules(); 78 | 79 | void Enable(); 80 | 81 | void ParseSensorData(); 82 | 83 | void SetTorques(ConstRefVectorXd desired_torques); 84 | void SetDesiredPositions(ConstRefVectorXd desired_positions); 85 | void SetDesiredVelocities(ConstRefVectorXd desired_velocities); 86 | void SetPositionGains(ConstRefVectorXd desired_gains); 87 | void SetVelocityGains(ConstRefVectorXd desired_gains); 88 | void SetMaximumCurrents(double max_currents); 89 | 90 | /** 91 | * @brief Disables the position and velocity gains by setting them to zero. 92 | */ 93 | void SetZeroGains(); 94 | void SetZeroCommands(); 95 | 96 | /** 97 | * @brief Overwrites the control commands for a default safety controller. 98 | * The safety controller applies a D control to all the joints based 99 | * on the provided `safety_damping`. 100 | */ 101 | virtual void RunSafetyController(); 102 | 103 | // Used for calibration. 104 | void SetPositionOffsets(ConstRefVectorXd position_offsets); 105 | 106 | void EnableIndexOffsetCompensation(); 107 | void EnableIndexOffsetCompensation(int); 108 | 109 | const VectorXb& HasIndexBeenDetected(); 110 | const VectorXb& GetReady(); 111 | const VectorXb& GetEnabled(); 112 | const VectorXb& GetMotorDriverEnabled(); 113 | const VectorXi& GetMotorDriverErrors(); 114 | 115 | bool SawAllIndices(); 116 | 117 | /** 118 | * @brief Returns true once all motors are enabled and report ready. 119 | */ 120 | bool IsReady(); 121 | 122 | const VectorXd& GetPositions(); 123 | const VectorXd& GetVelocities(); 124 | const VectorXd& GetSentTorques(); 125 | const VectorXd& GetMeasuredTorques(); 126 | 127 | const VectorXd& GetGearRatios(); 128 | 129 | int GetNumberMotors(); 130 | 131 | void DisableJointLimitCheck(); 132 | void EnableJointLimitCheck(); 133 | 134 | /** 135 | * @brief Checks for errors and prints them 136 | */ 137 | bool HasError(); 138 | 139 | void PrintVector(ConstRefVectorXd vector); 140 | 141 | protected: 142 | int upper_joint_limits_counter_; 143 | int lower_joint_limits_counter_; 144 | int velocity_joint_limits_counter_; 145 | int motor_drivers_error_counter; 146 | }; 147 | 148 | } // namespace odri_control_interface 149 | -------------------------------------------------------------------------------- /include/odri_control_interface/robot.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file robot.hpp 3 | * @author Julian Viereck (jviereck@tuebingen.mpg.de) 4 | * license License BSD-3-Clause 5 | * @copyright Copyright (c) 2020, New York University and Max Planck 6 | * Gesellschaft. 7 | * @date 2020-11-27 8 | * 9 | * @brief Main robot interface from the package. 10 | */ 11 | 12 | #pragma once 13 | 14 | #include 15 | 16 | #include "master_board_sdk/defines.h" 17 | #include "master_board_sdk/master_board_interface.h" 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | namespace odri_control_interface 24 | { 25 | /** 26 | * @brief Class abstracting the blmc motors to modules. 27 | */ 28 | class Robot 29 | { 30 | public: 31 | std::shared_ptr robot_if; 32 | std::shared_ptr joints; 33 | std::shared_ptr imu; 34 | std::shared_ptr calibrator; 35 | 36 | protected: 37 | int timeout_counter_; 38 | bool saw_error_; 39 | std::ostream& msg_out_ = std::cout; 40 | std::chrono::time_point last_time_; 41 | 42 | public: 43 | Robot(const std::shared_ptr& robot_if, 44 | const std::shared_ptr& joint_modules, 45 | const std::shared_ptr& imu, 46 | const std::shared_ptr& calibrator); 47 | 48 | /** 49 | * @brief Returns the underlying robot interface 50 | */ 51 | const std::shared_ptr& GetRobotInterface(); 52 | 53 | /** 54 | * @brief Returns the joint module. 55 | */ 56 | const std::shared_ptr& GetJoints(); 57 | 58 | /** 59 | * @brief Return the IMU. 60 | */ 61 | const std::shared_ptr& GetIMU(); 62 | 63 | /** 64 | * @brief Initializes the connection. Use `SendInit` to initialize the 65 | * session. 66 | */ 67 | void Init(); 68 | 69 | /** 70 | * @brief Establishes the session with the robot interface. 71 | */ 72 | void SendInit(); 73 | 74 | /** 75 | * @brief Initializes the session and blocks until either the package 76 | * got acknowledged or the communication timed out. 77 | */ 78 | void Start(); 79 | 80 | /** 81 | * @brief Initializes the robot. This inclues establishing the communication 82 | * to the robot, wait until the joints are all ready, running the 83 | * calibration procedure and waiting in the desired initial configuration. 84 | * 85 | * This command corresponds to running `Start()`, `WaitUntilReady()` and 86 | * `RunCalibration(target_positions)` in sequence. 87 | */ 88 | void Initialize(VectorXd const& target_positions); 89 | 90 | /** 91 | * @brief If no error happend, send the previously specified commands 92 | * to the robot. If an error was detected, go into safety mode 93 | * and apply the safety control from the joint_module. 94 | */ 95 | bool SendCommand(); 96 | 97 | /** 98 | * @brief Same as SendCommand but waits till the end of the control cycle. 99 | */ 100 | bool SendCommandAndWaitEndOfCycle(double dt); 101 | 102 | /** 103 | * @brief Parses the sensor data and calls ParseSensorData on all devices. 104 | */ 105 | void ParseSensorData(); 106 | 107 | /** 108 | * @brief Run the calibration procedure and blocks. Returns true if the 109 | * calibration procedure finished successfully. Otherwise (e.g. when an 110 | * error occurred or the communication timed-out) return false. 111 | */ 112 | bool RunCalibration(const std::shared_ptr& calibrator, 113 | VectorXd const& target_positions); 114 | 115 | /** 116 | * @brief Runs the calibration procedure for the calibrator passed in 117 | * during initialization and blocks. * calibration procedure finished 118 | * successfully. Otherwise (e.g. when an error occurred or the communication 119 | * timed-out) return false. 120 | */ 121 | bool RunCalibration(VectorXd const& target_positions); 122 | 123 | /** 124 | * @brief Returns true if all connected devices report ready. 125 | */ 126 | bool IsReady(); 127 | 128 | /** 129 | * @brief Returns true if the connection timed out. 130 | */ 131 | bool IsTimeout(); 132 | 133 | /** 134 | * @brief Returns true if during the session initialization the message got 135 | * acknowledged. 136 | */ 137 | bool IsAckMsgReceived(); 138 | 139 | /** 140 | * @brief Blocks until all devices report ready. 141 | */ 142 | bool WaitUntilReady(); 143 | 144 | /** 145 | * @brief Checks all connected devices for errors. Also checks 146 | * if there is a timeout. 147 | */ 148 | bool HasError(); 149 | 150 | /** 151 | * @brief Way to report an external error. Causes the robot to go into 152 | * safety mode. 153 | */ 154 | void ReportError(const std::string& error); 155 | 156 | /** 157 | * @brief Way to report an external error quietly. Causes the robot to go into 158 | * safety mode. 159 | */ 160 | void ReportError(); 161 | }; 162 | 163 | } // namespace odri_control_interface 164 | -------------------------------------------------------------------------------- /include/odri_control_interface/utils.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file utils.cpp 3 | * @author Julian Viereck (jviereck@tuebingen.mpg.de) 4 | * license License BSD-3-Clause 5 | * @copyright Copyright (c) 2020, New York University and Max Planck 6 | * Gesellschaft. 7 | * @date 2020-12-08 8 | * 9 | * @brief Different utils for using the framework. 10 | */ 11 | 12 | #pragma once 13 | 14 | #include "odri_control_interface/calibration.hpp" 15 | #include "odri_control_interface/robot.hpp" 16 | 17 | namespace odri_control_interface 18 | { 19 | std::shared_ptr RobotFromYamlFile(const std::string& if_name, 20 | const std::string& file_path); 21 | std::shared_ptr RobotFromYamlFile(const std::string& file_path); 22 | std::shared_ptr JointCalibratorFromYamlFile( 23 | const std::string& file_path, std::shared_ptr joints); 24 | std::shared_ptr CreateMasterBoardInterface( 25 | const std::string &if_name, bool listener_mode = false); 26 | } // namespace odri_control_interface 27 | -------------------------------------------------------------------------------- /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 | 5 | 6 | 7 | 8 | odri_control_interface 9 | 1.0.1 10 | 11 | 12 | Common interface for controlling robots build with the odri master board. 13 | 14 | 15 | Julian Viereck 16 | 17 | BSD 3-clause 18 | 19 | cmake 20 | mpi_cmake_modules 21 | 22 | 23 | pybind11 24 | Threads 25 | master_board_sdk 26 | Boost 27 | Eigen3 28 | eigenpy 29 | 30 | 31 | -------------------------------------------------------------------------------- /readme.md: -------------------------------------------------------------------------------- 1 | odri_control_interface 2 | ---------------------- 3 | 4 | ### What it is 5 | 6 | Common interface for controlling robots build with the odri master board. 7 | 8 | ### Installation 9 | 10 | #### Download the package: 11 | 12 | We use `treep` to download the required packages. Make sure your ssh key is unlocked. Then 13 | 14 | ``` 15 | mkdir -p ~/devel 16 | pip install treep # This installs treep 17 | cd ~/devel 18 | git clone git@github.com:machines-in-motion/treep_machines_in_motion.git 19 | treep --clone master-board 20 | treep --clone odri_control_interface 21 | ``` 22 | 23 | #### Build the package 24 | 25 | We use [colcon](https://github.com/machines-in-motion/machines-in-motion.github.io/wiki/use_colcon) 26 | to build this package: 27 | ``` 28 | cd mkdir -p ~/devel/workspace 29 | colcon build 30 | ``` 31 | ### Usage: 32 | 33 | #### Demos/Examples 34 | 35 | You find examples in forms of demos under the `demos/` folder. The demos show how to use the python and C++ interface for talking to the library and setting up a robot. 36 | 37 | ##### Python examples 38 | 39 | If you're using nix, you can get into a shell with all required packages available, including the C++-backed python packages 40 | 41 | ``` 42 | nix develop .#python 43 | ``` 44 | 45 | Since the SDK forges raw ethernet frames, you need to run these scripts as root (run it in a shell that has the dependencies installed and PYTHONPATH set): 46 | 47 | ``` 48 | sudo -HE env PATH=$PATH PYTHONPATH=$PYTHONPATH python demos/demo_testbech_joint_calibrator.py 49 | ``` 50 | 51 | Note that running within a network-mounted directory (e.g. using NFS) might result in a "Permission denied" error when trying to make python execute the script as root, even when all users have the read permission set. 52 | 53 | _(LAAS members: this is the case for your home directory when using office computers)_ 54 | 55 | ##### C++ examples 56 | 57 | C++ demos also need to be run as root. Compilation produces binaries named `odri_control_interface_*`. (nix: you can do `nix run .#testbench` or `nix run .#solo12`) 58 | 59 | Change the Ethernet interface the master board is plugged to by editing the relevant `config_*.yaml` file, then _re-build the binaries_ 60 | 61 | You can get the ethernet interface by runnning `ip a` 62 | 63 | ### License and Copyrights 64 | 65 | License BSD-3-Clause 66 | Copyright (c) 2021, New York University and Max Planck Gesellschaft. 67 | -------------------------------------------------------------------------------- /schemas/robot.schema.json: -------------------------------------------------------------------------------- 1 | { 2 | "$schema": "http://json-schema.org/draft-07/schema#", 3 | "$ref": "#/definitions/FullConfiguration", 4 | "definitions": { 5 | "FullConfiguration": { 6 | "type": "object", 7 | "additionalProperties": false, 8 | "properties": { 9 | "robot": { 10 | "$ref": "#/definitions/Robot" 11 | }, 12 | "joint_calibrator": { 13 | "$ref": "#/definitions/JointCalibrator" 14 | } 15 | }, 16 | "required": ["joint_calibrator", "robot"], 17 | "title": "Full configuration", 18 | "description": "Include the robot and joint calibrator configurations" 19 | }, 20 | "JointCalibrator": { 21 | "type": "object", 22 | "additionalProperties": false, 23 | "properties": { 24 | "search_methods": { 25 | "type": "array", 26 | "items": { 27 | "$ref": "#/definitions/SearchMethod" 28 | } 29 | }, 30 | "position_offsets": { 31 | "type": "array", 32 | "description": "Angular position offset between the position of the index on the coding wheel and the zero position of the actuator.", 33 | "items": { 34 | "type": "number" 35 | } 36 | }, 37 | "calib_order": { 38 | "type": "array", 39 | "description": "Optional calibration order for the joints. Joints with the lowest values start looking for the indexes first while the others are not moving. Once they have found it, they wait in their associated position in calib_pos field. You can set all values to 0 if you want to calibrate all joints at the same time. With the example values, the HFE and Knee joints of the leg are calibrated first, then the leg fold up. That way there is less risk to hit something while the HAA joints are moving to find their indexes.", 40 | "items": { 41 | "type": "integer" 42 | } 43 | }, 44 | "calib_pos": { 45 | "type": "array", 46 | "description": "The waiting position for joints after they have found their indexes (used with calib_order).", 47 | "items": { 48 | "type": "number" 49 | } 50 | }, 51 | "Kp": { 52 | "description": "The proportional gain of the PD controller used during the calibration process, in Nm/rad.", 53 | "type": "number" 54 | }, 55 | "Kd": { 56 | "description": "The derivative gain of the PD controller used during the calibration process, in Nm/(rad/s).", 57 | "type": "number" 58 | }, 59 | "T": { 60 | "description": "The period of the sinus movement used when looking for indexes, in seconds.", 61 | "type": "number" 62 | }, 63 | "dt": { 64 | "description": "The control time step during the calibration process, in seconds.", 65 | "type": "number" 66 | } 67 | }, 68 | "required": [ 69 | "Kd", 70 | "Kp", 71 | "T", 72 | "calib_order", 73 | "calib_pos", 74 | "dt", 75 | "position_offsets", 76 | "search_methods" 77 | ], 78 | "title": "JointCalibrator" 79 | }, 80 | "Robot": { 81 | "type": "object", 82 | "description": "general information about the robot such as motor ordering, joint limits or orientation of the IMU with respect to the body frame", 83 | "additionalProperties": false, 84 | "properties": { 85 | "interface": { 86 | "type": "string", 87 | "description": "Name of the interface to communicate with the robot (you can check with ifconfig if you are unsure)." 88 | }, 89 | "joint_modules": { 90 | "$ref": "#/definitions/JointModules" 91 | }, 92 | "imu": { 93 | "$ref": "#/definitions/Imu" 94 | } 95 | }, 96 | "required": ["imu", "interface", "joint_modules"], 97 | "title": "Robot" 98 | }, 99 | "Imu": { 100 | "type": "object", 101 | "additionalProperties": false, 102 | "properties": { 103 | "rotate_vector": { 104 | "type": "array", 105 | "description": "Mapping from IMU axes to body axes.", 106 | "items": { 107 | "type": "integer" 108 | } 109 | }, 110 | "orientation_vector": { 111 | "type": "array", 112 | "description": "Mapping from IMU orientation (as a quaternion) to body orientation. Can be useful if the IMU is mounted upside-down on the robot.", 113 | "items": { 114 | "type": "integer" 115 | } 116 | } 117 | }, 118 | "required": ["orientation_vector", "rotate_vector"], 119 | "title": "Imu" 120 | }, 121 | "JointModules": { 122 | "type": "object", 123 | "additionalProperties": false, 124 | "properties": { 125 | "motor_numbers": { 126 | "type": "array", 127 | "description": "Mapping between the hardware order of the motors and their control order", 128 | "items": { 129 | "type": "integer" 130 | } 131 | }, 132 | "motor_constants": { 133 | "type": "number", 134 | "description": "Torque constant of the motors in Nm/A" 135 | }, 136 | "gear_ratios": { 137 | "type": "number", 138 | "description": "Gear ratio of the actuator modules" 139 | }, 140 | "max_currents": { 141 | "type": "number", 142 | "description": "Absolute limit for the current sent to the motors in A" 143 | }, 144 | "reverse_polarities": { 145 | "type": "array", 146 | "description": "Whether motors have been assembled on the robot in such a way that their hardware positive direction of rotation is actually reversed compared to the control positive direction of rotation.", 147 | "items": { 148 | "type": "boolean" 149 | } 150 | }, 151 | "lower_joint_limits": { 152 | "type": "array", 153 | "description": "Lower joint limits for the actuators, under which the low-level control automatically switches to a security mode which applies damping on all joints.", 154 | "items": { 155 | "type": "number" 156 | } 157 | }, 158 | "upper_joint_limits": { 159 | "type": "array", 160 | "description": "Upper joint limits for the actuators, above which the low-level control automatically switches to a security mode which applies damping on all joints.", 161 | "items": { 162 | "type": "number" 163 | } 164 | }, 165 | "max_joint_velocities": { 166 | "type": "number", 167 | "description": "Absolute limit for the joint velocities, above which the low-level control automatically switches to a security mode which applies damping on all joints." 168 | }, 169 | "safety_damping": { 170 | "type": "number", 171 | "description": "Value of the damping coefficient for the security mode, in Nm/(rad/s)." 172 | } 173 | }, 174 | "required": [ 175 | "gear_ratios", 176 | "lower_joint_limits", 177 | "max_currents", 178 | "max_joint_velocities", 179 | "motor_constants", 180 | "motor_numbers", 181 | "reverse_polarities", 182 | "safety_damping", 183 | "upper_joint_limits" 184 | ], 185 | "title": "JointModules" 186 | }, 187 | "SearchMethod": { 188 | "type": "string", 189 | "description": "The index search strategy for each motor, can be POS, NEG, ALT, AUTO. If POS, the motor will search the index by going in the positive direction from its starting position until it reaches it. If NEG, it will go in the negative direction. If ALT, the motor will do back-and-forth movements of increasing amplitude around its starting position till the index is reached. AUTO automatically chooses the strategy based on the position offset value.", 190 | "enum": ["POS", "NEG", "ALT", "AUTO"], 191 | "title": "SearchMethod" 192 | } 193 | } 194 | } 195 | -------------------------------------------------------------------------------- /src/calibration.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file calibration.cpp 3 | * @author Julian Viereck (jviereck@tuebingen.mpg.de) 4 | * license License BSD-3-Clause 5 | * @copyright Copyright (c) 2020, New York University and Max Planck 6 | * Gesellschaft. 7 | * @date 2020-12-05 8 | * 9 | * @brief Class for calibrating the joints. 10 | */ 11 | 12 | #include "odri_control_interface/calibration.hpp" 13 | #include 14 | 15 | namespace odri_control_interface 16 | { 17 | JointCalibrator::JointCalibrator( 18 | const std::shared_ptr& joints, 19 | const std::vector& search_methods, 20 | RefVectorXd position_offsets, 21 | RefVectorXi calib_order, 22 | RefVectorXd calib_pos, 23 | double Kp, 24 | double Kd, 25 | double T, 26 | double dt) 27 | : joints_(joints), 28 | search_methods_(search_methods), 29 | position_offsets_(position_offsets), 30 | calib_order_(calib_order), 31 | calib_pos_(calib_pos), 32 | Kp_(Kp), 33 | Kd_(Kd), 34 | T_(T), 35 | T_wait_(0.1), 36 | dt_(dt), 37 | t_(0.), 38 | already_calibrated_(false), 39 | calib_state_(0), 40 | step_number_(0), 41 | step_number_max_(0), 42 | step_indexes_detected_(false), 43 | t_step_indexes_detected_(0.), 44 | t_step_end_(0.) 45 | { 46 | gear_ratios_ = joints->GetGearRatios(); 47 | n_ = static_cast(gear_ratios_.size()); 48 | 49 | if (static_cast(search_methods.size()) != n_) 50 | { 51 | throw std::runtime_error( 52 | "Search methods has different size than motor numbers"); 53 | } 54 | 55 | if (static_cast(position_offsets.size()) != n_) 56 | { 57 | throw std::runtime_error( 58 | "Position offsets has different size than motor numbers"); 59 | } 60 | 61 | if (static_cast(calib_order.size()) != n_) 62 | { 63 | throw std::runtime_error( 64 | "Calibration order has different size than motor numbers"); 65 | } 66 | 67 | step_number_max_ = calib_order.maxCoeff(); 68 | 69 | if (static_cast(calib_pos.size()) != n_) 70 | { 71 | throw std::runtime_error( 72 | "Calibration pos has different size than motor numbers"); 73 | } 74 | 75 | for (int i = 0; i < n_; i++) 76 | { 77 | if (search_methods_[i] == AUTO) 78 | { 79 | if (position_offsets[i] > (M_PI / 2.) / gear_ratios_[i]) 80 | { 81 | search_methods_[i] = POSITIVE; 82 | } 83 | else if (position_offsets[i] < (-M_PI / 2.) / gear_ratios_[i]) 84 | { 85 | search_methods_[i] = NEGATIVE; 86 | } 87 | else 88 | { 89 | search_methods_[i] = ALTERNATIVE; 90 | } 91 | } 92 | } 93 | 94 | initial_positions_.resize(n_); 95 | target_positions_.resize(n_); 96 | 97 | found_index_.resize(n_); 98 | found_index_.fill(false); 99 | 100 | zero_vector_.resize(n_); 101 | zero_vector_.fill(0.); 102 | 103 | pos_command_.resize(n_); 104 | vel_command_.resize(n_); 105 | kp_command_.resize(n_); 106 | kd_command_.resize(n_); 107 | pos_command_.fill(0.); 108 | vel_command_.fill(0.); 109 | kp_command_.fill(Kp); 110 | kd_command_.fill(Kd); 111 | } 112 | 113 | void JointCalibrator::UpdatePositionOffsets(ConstRefVectorXd position_offsets) 114 | { 115 | position_offsets_ = position_offsets; 116 | } 117 | 118 | const VectorXd& JointCalibrator::GetPositionOffsets() 119 | { 120 | return position_offsets_; 121 | } 122 | 123 | const double& JointCalibrator::dt() 124 | { 125 | return dt_; 126 | } 127 | 128 | /** 129 | * @brief Runs the calibration procedure. Returns true if the calibration is 130 | * done. 131 | */ 132 | bool JointCalibrator::Run() 133 | { 134 | return RunAndGoTo(zero_vector_); 135 | } 136 | 137 | /** 138 | * @brief Runs the calibration procedure. Returns true if the calibration is 139 | * done. Legs are placed at the target position at the end. Calibration happens in 140 | * three steps: looking for motor indexes, waiting to be sure index compensation has 141 | * been taken into account and finally going to the desired target positions at the 142 | * end of the calibration. 143 | * 144 | * @param target_positions target positions for the legs at the end of the calibration 145 | */ 146 | bool JointCalibrator::RunAndGoTo(VectorXd const& target_positions) 147 | { 148 | if (t_ == 0.) 149 | { 150 | joints_->SetZeroGains(); 151 | joints_->SetPositionOffsets(position_offsets_); 152 | initial_positions_ = joints_->GetPositions(); 153 | target_positions_ = joints_->GetPositions(); 154 | 155 | // If all the indices are already detected, then we 156 | // do not need to search them. 157 | if (joints_->SawAllIndices()) 158 | { 159 | already_calibrated_ = true; 160 | SwitchToWaiting(); 161 | } 162 | 163 | joints_->DisableJointLimitCheck(); 164 | } 165 | 166 | auto has_index_been_detected = joints_->HasIndexBeenDetected(); 167 | auto positions = joints_->GetPositions(); 168 | auto velocities = joints_->GetVelocities(); 169 | 170 | if (calib_state_ == SEARCHING) 171 | { 172 | bool finished_indexes_search = true; 173 | for (int i = 0; i < n_; i++) 174 | { 175 | if (calib_order_[i] != step_number_) // Stay at current position 176 | { 177 | pos_command_[i] = positions[i]; 178 | vel_command_[i] = 0.0; 179 | } 180 | else if (!found_index_[i]) // As long as the index was not found, search for it. 181 | { 182 | if (has_index_been_detected[i]) 183 | { 184 | found_index_[i] = true; 185 | initial_positions_[i] = positions[i]; 186 | } 187 | else 188 | { 189 | SearchIndex(i); 190 | } 191 | finished_indexes_search = false; 192 | } 193 | else // After the index was found, stay at current position. 194 | { 195 | pos_command_[i] = initial_positions_[i]; 196 | vel_command_[i] = 0.0; 197 | } 198 | } 199 | if (finished_indexes_search) // If all indexes have been found we start the waiting time. 200 | { 201 | SwitchToWaiting(); 202 | } 203 | } 204 | else if (calib_state_ == WAITING) 205 | { 206 | if (t_ - t_step_indexes_detected_ > T_wait_) 207 | { 208 | calib_state_ = GOTO; 209 | joints_->EnableJointLimitCheck(); 210 | 211 | // Refresh initial position of the movement after the waiting time. 212 | for (int i = 0; i < n_; i++) 213 | { 214 | if (step_number_ == step_number_max_ || already_calibrated_) 215 | { 216 | // Go to desired initial position 217 | target_positions_[i] = target_positions[i]; 218 | } 219 | else if (calib_order_[i] == step_number_) 220 | { 221 | // Go to desired intermediate calibration position 222 | target_positions_[i] = calib_pos_[i]; 223 | } 224 | else 225 | { 226 | // Stay at current position 227 | target_positions_[i] = positions[i]; 228 | } 229 | initial_positions_[i] = positions[i]; 230 | pos_command_[i] = initial_positions_[i]; 231 | vel_command_[i] = (target_positions_[i] - initial_positions_[i]) / T_; 232 | kp_command_[i] = Kp_; 233 | kd_command_[i] = Kd_; 234 | } 235 | } 236 | } 237 | else if (calib_state_ == GOTO) 238 | { 239 | // Interpolation between initial positions and target positions. 240 | double alpha = (t_ - t_step_indexes_detected_ - T_wait_) / T_; 241 | if (alpha <= 1.0) 242 | { 243 | for (int i = 0; i < n_; i++) 244 | { 245 | pos_command_[i] = initial_positions_[i] * (1.0 - alpha) + 246 | target_positions_[i] * alpha; 247 | } 248 | } 249 | else // We have reached the target positions (at least for the command). 250 | { 251 | step_number_++; 252 | if (step_number_ > step_number_max_ || already_calibrated_) 253 | { 254 | // Set all command quantities to 0 when the calibration finishes. 255 | joints_->SetZeroCommands(); 256 | return true; 257 | } 258 | calib_state_ = SEARCHING; 259 | t_step_end_ = t_; 260 | joints_->DisableJointLimitCheck(); 261 | } 262 | } 263 | else 264 | { 265 | joints_->SetZeroCommands(); 266 | throw std::runtime_error("Undefined calibration state"); 267 | } 268 | 269 | // Set all command quantities. 270 | joints_->SetTorques(zero_vector_); 271 | joints_->SetDesiredPositions(pos_command_); 272 | joints_->SetDesiredVelocities(vel_command_); 273 | joints_->SetPositionGains(kp_command_); 274 | joints_->SetVelocityGains(kd_command_); 275 | 276 | t_ += dt_; 277 | 278 | // Set all command quantities to 0 when the calibration finishes. 279 | if (step_number_ > step_number_max_) 280 | { 281 | joints_->SetZeroCommands(); 282 | } 283 | return false; 284 | } 285 | 286 | /** 287 | * @brief Search the index using the desired 288 | * search method 289 | * 290 | * @param i the searching motor number 291 | */ 292 | void JointCalibrator::SearchIndex(int i) 293 | { 294 | double des_pos = 0.; 295 | double des_vel = 0.; 296 | if (search_methods_[i] == ALTERNATIVE) 297 | { 298 | if ((t_ - t_step_end_) < T_ / 2.) 299 | { 300 | des_pos = AMPLITUDE * M_PI * 0.5 * 301 | (1. - cos(2. * M_PI * (1. / T_) * (t_ - t_step_end_))); 302 | des_vel = AMPLITUDE * M_PI * 0.5 * 2. * M_PI * (1. / T_) * 303 | sin(2. * M_PI * (1. / T_) * (t_ - t_step_end_)); 304 | } 305 | else 306 | { 307 | des_pos = AMPLITUDE * M_PI * 308 | cos(2. * M_PI * (0.5 / T_) * ((t_ - t_step_end_) - T_ / 2.0)); 309 | des_vel = AMPLITUDE * M_PI * -2. * M_PI * (0.5 / T_) * 310 | sin(2. * M_PI * (0.5 / T_) * ((t_ - t_step_end_) - T_ / 2.0)); 311 | } 312 | } 313 | else if (search_methods_[i] == POSITIVE) 314 | { 315 | des_pos = 2. * AMPLITUDE * M_PI * 316 | (1. - cos(2. * M_PI * (0.5 / T_) * (t_ - t_step_end_))); 317 | des_vel = 2. * AMPLITUDE * M_PI * 2. * M_PI * (0.5 / T_) * 318 | sin(2. * M_PI * (0.5 / T_) * (t_ - t_step_end_)); 319 | } 320 | else 321 | { 322 | des_pos = -2. * AMPLITUDE * M_PI * 323 | (1. - cos(2. * M_PI * (0.5 / T_) * (t_ - t_step_end_))); 324 | des_vel = -2. * AMPLITUDE * M_PI * 2. * M_PI * (0.5 / T_) * 325 | sin(2. * M_PI * (0.5 / T_) * (t_ - t_step_end_)); 326 | } 327 | pos_command_[i] = des_pos / gear_ratios_[i] + initial_positions_[i]; 328 | vel_command_[i] = des_vel / gear_ratios_[i]; 329 | } 330 | 331 | /** 332 | * @brief Start the calibration waiting time by setting 333 | * gains to zero and enable index compensation. 334 | */ 335 | void JointCalibrator::SwitchToWaiting() 336 | { 337 | calib_state_ = WAITING; 338 | t_step_indexes_detected_ = t_; 339 | for (int i = 0; i < n_; i++) 340 | { 341 | if (calib_order_[i] == step_number_ || already_calibrated_) 342 | { 343 | kp_command_[i] = 0.0; // Zero gains during waiting time. 344 | kd_command_[i] = 0.0; 345 | joints_->EnableIndexOffsetCompensation(i); // Enable index compensation. 346 | } 347 | } 348 | } 349 | 350 | } // namespace odri_control_interface 351 | -------------------------------------------------------------------------------- /src/imu.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file imu.cpp 3 | * @author Julian Viereck (jviereck@tuebingen.mpg.de) 4 | * license License BSD-3-Clause 5 | * @copyright Copyright (c) 2020, New York University and Max Planck 6 | * Gesellschaft. 7 | * @date 2020-12-05 8 | * 9 | * @brief IMU abstraction. 10 | */ 11 | 12 | #include "odri_control_interface/imu.hpp" 13 | 14 | #include 15 | 16 | namespace odri_control_interface 17 | { 18 | IMU::IMU(const std::shared_ptr& robot_if, 19 | RefVectorXl rotate_vector, 20 | RefVectorXl orientation_vector) 21 | : robot_if_(robot_if) 22 | { 23 | if (rotate_vector.size() != 3) 24 | { 25 | throw std::runtime_error("Expecting rotate_vector of size 3"); 26 | } 27 | if (orientation_vector.size() != 4) 28 | { 29 | throw std::runtime_error("Expecting orientation_vector of size 4"); 30 | } 31 | 32 | for (int i = 0; i < 3; i++) 33 | { 34 | rotate_vector_[i] = (int)rotate_vector(i); 35 | } 36 | for (int i = 0; i < 4; i++) 37 | { 38 | orientation_vector_[i] = (int)orientation_vector(i); 39 | } 40 | } 41 | 42 | IMU::IMU(const std::shared_ptr& robot_if) 43 | : robot_if_(robot_if), 44 | rotate_vector_({1, 2, 3}), 45 | orientation_vector_({1, 2, 3, 4}) 46 | { 47 | } 48 | 49 | const std::shared_ptr& IMU::GetMasterBoardInterface() 50 | { 51 | return robot_if_; 52 | } 53 | 54 | const Eigen::Vector3d& IMU::GetGyroscope() 55 | { 56 | return gyroscope_; 57 | } 58 | 59 | const Eigen::Vector3d& IMU::GetAccelerometer() 60 | { 61 | return accelerometer_; 62 | } 63 | 64 | const Eigen::Vector3d& IMU::GetLinearAcceleration() 65 | { 66 | return linear_acceleration_; 67 | } 68 | 69 | const Eigen::Vector3d& IMU::GetAttitudeEuler() 70 | { 71 | return attitude_euler_; 72 | } 73 | 74 | const Eigen::Vector4d& IMU::GetAttitudeQuaternion() 75 | { 76 | return attitude_quaternion_; 77 | } 78 | 79 | void IMU::ParseSensorData() 80 | { 81 | int index; 82 | for (int i = 0; i < 3; i++) 83 | { 84 | index = rotate_vector_[i]; 85 | if (index < 0) 86 | { 87 | gyroscope_(i) = -robot_if_->imu_data_gyroscope(-index - 1); 88 | accelerometer_(i) = -robot_if_->imu_data_accelerometer(-index - 1); 89 | linear_acceleration_(i) = 90 | -robot_if_->imu_data_linear_acceleration(-index - 1); 91 | } 92 | else 93 | { 94 | gyroscope_(i) = robot_if_->imu_data_gyroscope(index - 1); 95 | accelerometer_(i) = robot_if_->imu_data_accelerometer(index - 1); 96 | linear_acceleration_(i) = 97 | robot_if_->imu_data_linear_acceleration(index - 1); 98 | } 99 | } 100 | 101 | std::array attitude; 102 | double attitude0 = robot_if_->imu_data_attitude(0); 103 | double attitude1 = robot_if_->imu_data_attitude(1); 104 | double attitude2 = robot_if_->imu_data_attitude(2); 105 | 106 | double sr = sin(attitude0 / 2.); 107 | double cr = cos(attitude0 / 2.); 108 | double sp = sin(attitude1 / 2.); 109 | double cp = cos(attitude1 / 2.); 110 | double sy = sin(attitude2 / 2.); 111 | double cy = cos(attitude2 / 2.); 112 | attitude[0] = sr * cp * cy - cr * sp * sy; 113 | attitude[1] = cr * sp * cy + sr * cp * sy; 114 | attitude[2] = cr * cp * sy - sr * sp * cy; 115 | attitude[3] = cr * cp * cy + sr * sp * sy; 116 | 117 | // Rotate the attitude. 118 | for (int i = 0; i < 4; i++) 119 | { 120 | int index = orientation_vector_[i]; 121 | if (index < 0) 122 | { 123 | attitude_quaternion_(i) = -attitude[-index - 1]; 124 | } 125 | else 126 | { 127 | attitude_quaternion_(i) = attitude[index - 1]; 128 | } 129 | } 130 | 131 | // Convert the rotated quatenion back into euler angles. 132 | // Source: 133 | // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles 134 | double qx = attitude_quaternion_(0); 135 | double qy = attitude_quaternion_(1); 136 | double qz = attitude_quaternion_(2); 137 | double qw = attitude_quaternion_(3); 138 | 139 | double sinr_cosp = 2 * (qw * qx + qy * qz); 140 | double cosr_cosp = 1 - 2 * (qx * qx + qy * qy); 141 | attitude_euler_(0) = std::atan2(sinr_cosp, cosr_cosp); 142 | 143 | double sinp = 2 * (qw * qy - qz * qx); 144 | if (std::abs(sinp) >= 1) 145 | attitude_euler_(1) = 146 | std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range 147 | else 148 | attitude_euler_(1) = std::asin(sinp); 149 | 150 | double siny_cosp = 2 * (qw * qz + qx * qy); 151 | double cosy_cosp = 1 - 2 * (qy * qy + qz * qz); 152 | attitude_euler_(2) = std::atan2(siny_cosp, cosy_cosp); 153 | } 154 | 155 | } // namespace odri_control_interface 156 | -------------------------------------------------------------------------------- /src/joint_modules.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file joint_modules.cpp 3 | * @author Julian Viereck (jviereck@tuebingen.mpg.de) 4 | * license License BSD-3-Clause 5 | * @copyright Copyright (c) 2020, New York University and Max Planck 6 | * Gesellschaft. 7 | * @date 2020-11-27 8 | * 9 | * @brief Joint module abstraction for 10 | */ 11 | 12 | #include "odri_control_interface/joint_modules.hpp" 13 | 14 | namespace odri_control_interface 15 | { 16 | JointModules::JointModules( 17 | const std::shared_ptr& robot_if, 18 | ConstRefVectorXi motor_numbers, 19 | double motor_constants, 20 | double gear_ratios, 21 | double max_currents, 22 | ConstRefVectorXb reverse_polarities, 23 | ConstRefVectorXd lower_joint_limits, 24 | ConstRefVectorXd upper_joint_limits, 25 | double max_joint_velocities, 26 | double safety_damping) 27 | : robot_if_(robot_if), 28 | lower_joint_limits_(lower_joint_limits), 29 | upper_joint_limits_(upper_joint_limits), 30 | max_joint_velocities_(max_joint_velocities), 31 | check_joint_limits_(true), 32 | upper_joint_limits_counter_(0), 33 | lower_joint_limits_counter_(0), 34 | velocity_joint_limits_counter_(0), 35 | motor_drivers_error_counter(0) 36 | 37 | { 38 | n_ = static_cast(motor_numbers.size()); 39 | nd_ = (n_ + 1) / 2; 40 | 41 | // Check input arrays for correct sizes. 42 | if (reverse_polarities.size() != n_) 43 | { 44 | throw std::runtime_error( 45 | "Motor polarities has different size than motor numbers"); 46 | } 47 | 48 | if (lower_joint_limits.size() != n_) 49 | { 50 | throw std::runtime_error( 51 | "Lower joint limits has different size than motor numbers"); 52 | } 53 | 54 | if (upper_joint_limits.size() != n_) 55 | { 56 | throw std::runtime_error( 57 | "Upper joint limits has different size than motor numbers"); 58 | } 59 | 60 | // Resize and fill the vectors. 61 | gear_ratios_.resize(n_); 62 | motor_constants_.resize(n_); 63 | positions_.resize(n_); 64 | velocities_.resize(n_); 65 | sent_torques_.resize(n_); 66 | measured_torques_.resize(n_); 67 | index_been_detected_.resize(n_); 68 | index_been_detected_.fill(false); 69 | polarities_.resize(n_); 70 | ready_.resize(n_); 71 | ready_.fill(false); 72 | enabled_.resize(n_); 73 | enabled_.fill(false); 74 | zero_vector_.resize(n_); 75 | zero_vector_.fill(0.); 76 | safety_damping_.resize(n_); 77 | safety_damping_.fill(safety_damping); 78 | motor_driver_enabled_.resize(nd_); 79 | motor_driver_enabled_.fill(false); 80 | motor_driver_errors_.resize(nd_); 81 | motor_driver_errors_.fill(0); 82 | 83 | gear_ratios_.fill(gear_ratios); 84 | motor_constants_.fill(motor_constants); 85 | 86 | for (int i = 0; i < n_; i++) 87 | { 88 | motors_.push_back(robot_if_->GetMotor(motor_numbers[i])); 89 | polarities_(i) = reverse_polarities(i) ? -1 : 1; 90 | } 91 | 92 | SetMaximumCurrents(max_currents); 93 | } 94 | 95 | JointModules::~JointModules() {} 96 | 97 | const VectorXd& JointModules::GetGearRatios() 98 | { 99 | return gear_ratios_; 100 | } 101 | 102 | int JointModules::GetNumberMotors() 103 | { 104 | return n_; 105 | } 106 | 107 | void JointModules::ParseSensorData() 108 | { 109 | for (int i = 0; i < n_; i++) 110 | { 111 | positions_(i) = 112 | motors_[i]->get_position() * polarities_(i) / gear_ratios_(i); 113 | velocities_(i) = 114 | motors_[i]->get_velocity() * polarities_(i) / gear_ratios_(i); 115 | sent_torques_(i) = (motors_[i]->get_current_ref() * polarities_(i) * 116 | gear_ratios_(i) * motor_constants_(i)); 117 | measured_torques_(i) = (motors_[i]->get_current() * polarities_[i] * 118 | gear_ratios_[i] * motor_constants_[i]); 119 | 120 | index_been_detected_(i) = motors_[i]->HasIndexBeenDetected(); 121 | ready_(i) = motors_[i]->get_is_ready(); 122 | enabled_(i) = motors_[i]->get_is_enabled(); 123 | } 124 | 125 | for (int i = 0; i < nd_; i++) 126 | { 127 | motor_driver_enabled_(i) = robot_if_->motor_drivers[i].IsEnabled(); 128 | motor_driver_errors_(i) = robot_if_->motor_drivers[i].GetErrorCode(); 129 | } 130 | } 131 | 132 | void JointModules::Enable() 133 | { 134 | // TODO: Enable this again. 135 | SetZeroCommands(); 136 | 137 | // Enable all motors and cards. 138 | for (int i = 0; i < (n_ + 1) / 2; i++) 139 | { 140 | robot_if_->motor_drivers[i].motor1->Enable(); 141 | robot_if_->motor_drivers[i].motor2->Enable(); 142 | robot_if_->motor_drivers[i].EnablePositionRolloverError(); 143 | robot_if_->motor_drivers[i].SetTimeout(5); 144 | robot_if_->motor_drivers[i].Enable(); 145 | } 146 | } 147 | 148 | void JointModules::SetTorques(ConstRefVectorXd desired_torques) 149 | { 150 | for (int i = 0; i < n_; i++) 151 | { 152 | motors_[i]->SetCurrentReference( 153 | polarities_(i) * desired_torques(i) / 154 | (gear_ratios_(i) * motor_constants_(i))); 155 | } 156 | } 157 | 158 | void JointModules::SetDesiredPositions(ConstRefVectorXd desired_positions) 159 | { 160 | for (int i = 0; i < n_; i++) 161 | { 162 | motors_[i]->SetPositionReference(polarities_[i] * desired_positions[i] * 163 | gear_ratios_[i]); 164 | } 165 | } 166 | 167 | void JointModules::SetDesiredVelocities(ConstRefVectorXd desired_velocities) 168 | { 169 | for (int i = 0; i < n_; i++) 170 | { 171 | motors_[i]->SetVelocityReference( 172 | polarities_[i] * desired_velocities[i] * gear_ratios_[i]); 173 | } 174 | } 175 | 176 | void JointModules::SetPositionGains(ConstRefVectorXd desired_gains) 177 | { 178 | for (int i = 0; i < n_; i++) 179 | { 180 | motors_[i]->set_kp( 181 | desired_gains[i] / 182 | (gear_ratios_[i] * gear_ratios_[i] * motor_constants_[i])); 183 | } 184 | } 185 | 186 | void JointModules::SetVelocityGains(ConstRefVectorXd desired_gains) 187 | { 188 | for (int i = 0; i < n_; i++) 189 | { 190 | motors_[i]->set_kd( 191 | desired_gains[i] / 192 | (gear_ratios_[i] * gear_ratios_[i] * motor_constants_[i])); 193 | } 194 | } 195 | 196 | void JointModules::SetZeroGains() 197 | { 198 | SetPositionGains(zero_vector_); 199 | SetVelocityGains(zero_vector_); 200 | } 201 | 202 | void JointModules::SetZeroCommands() 203 | { 204 | SetTorques(zero_vector_); 205 | SetDesiredPositions(zero_vector_); 206 | SetDesiredVelocities(zero_vector_); 207 | SetZeroGains(); 208 | } 209 | 210 | void JointModules::RunSafetyController() 211 | { 212 | SetZeroCommands(); 213 | SetVelocityGains(safety_damping_); 214 | } 215 | 216 | void JointModules::SetPositionOffsets(ConstRefVectorXd position_offsets) 217 | { 218 | for (int i = 0; i < n_; i++) 219 | { 220 | // REVIEW: Is the following correct? 221 | motors_[i]->SetPositionOffset(position_offsets(i) * polarities_(i) * 222 | gear_ratios_(i)); 223 | } 224 | // Need to trigger a sensor parsing to update the joint positions. 225 | robot_if_->ParseSensorData(); 226 | ParseSensorData(); 227 | } 228 | 229 | void JointModules::EnableIndexOffsetCompensation() 230 | { 231 | for (int i = 0; i < n_; i++) 232 | { 233 | motors_[i]->set_enable_index_offset_compensation(true); 234 | } 235 | } 236 | 237 | void JointModules::EnableIndexOffsetCompensation(int i) 238 | { 239 | motors_[i]->set_enable_index_offset_compensation(true); 240 | } 241 | 242 | const VectorXb& JointModules::HasIndexBeenDetected() 243 | { 244 | return index_been_detected_; 245 | } 246 | 247 | const VectorXb& JointModules::GetReady() 248 | { 249 | return ready_; 250 | } 251 | 252 | const VectorXb& JointModules::GetEnabled() 253 | { 254 | return enabled_; 255 | } 256 | 257 | const VectorXb& JointModules::GetMotorDriverEnabled() 258 | { 259 | return motor_driver_enabled_; 260 | } 261 | 262 | const VectorXi& JointModules::GetMotorDriverErrors() 263 | { 264 | return motor_driver_errors_; 265 | } 266 | 267 | bool JointModules::SawAllIndices() 268 | { 269 | for (int i = 0; i < n_; i++) 270 | { 271 | if (!motors_[i]->get_has_index_been_detected()) 272 | { 273 | return false; 274 | } 275 | } 276 | return true; 277 | } 278 | 279 | /** 280 | * @brief Returns true once all motors are enabled and report ready. 281 | */ 282 | bool JointModules::IsReady() 283 | { 284 | bool is_ready_ = true; 285 | 286 | for (int i = 0; i < n_; i++) 287 | { 288 | if (!motors_[i]->get_is_enabled() || !motors_[i]->get_is_ready()) 289 | { 290 | is_ready_ = false; 291 | break; 292 | } 293 | } 294 | return is_ready_; 295 | } 296 | 297 | const VectorXd& JointModules::GetPositions() 298 | { 299 | return positions_; 300 | } 301 | 302 | const VectorXd& JointModules::GetVelocities() 303 | { 304 | return velocities_; 305 | } 306 | 307 | const VectorXd& JointModules::GetSentTorques() 308 | { 309 | return sent_torques_; 310 | } 311 | 312 | const VectorXd& JointModules::GetMeasuredTorques() 313 | { 314 | return measured_torques_; 315 | } 316 | 317 | void JointModules::DisableJointLimitCheck() 318 | { 319 | check_joint_limits_ = false; 320 | } 321 | 322 | void JointModules::EnableJointLimitCheck() 323 | { 324 | check_joint_limits_ = true; 325 | } 326 | 327 | /** 328 | * @brief Checks for errors and prints them 329 | */ 330 | bool JointModules::HasError() 331 | { 332 | bool has_error = false; 333 | 334 | if (check_joint_limits_) 335 | { 336 | // Check for lower and upper joint limits. 337 | for (int i = 0; i < n_; i++) 338 | { 339 | if (positions_(i) > upper_joint_limits_(i)) 340 | { 341 | has_error = true; 342 | if (upper_joint_limits_counter_++ % 2000 == 0) 343 | { 344 | msg_out_ << "ERROR: Above joint limits at joint #" << (i) 345 | << std::endl; 346 | msg_out_ << " Joints: "; 347 | PrintVector(positions_); 348 | msg_out_ << std::endl; 349 | msg_out_ << " Limits: "; 350 | PrintVector(upper_joint_limits_); 351 | msg_out_ << std::endl; 352 | } 353 | break; 354 | } 355 | } 356 | 357 | for (int i = 0; i < n_; i++) 358 | { 359 | if (positions_(i) < lower_joint_limits_(i)) 360 | { 361 | has_error = true; 362 | if (lower_joint_limits_counter_++ % 2000 == 0) 363 | { 364 | msg_out_ << "ERROR: Below joint limits at joint #" << (i) 365 | << std::endl; 366 | msg_out_ << " Joints: "; 367 | PrintVector(positions_); 368 | msg_out_ << std::endl; 369 | msg_out_ << " Limits: "; 370 | PrintVector(lower_joint_limits_); 371 | msg_out_ << std::endl; 372 | } 373 | break; 374 | } 375 | } 376 | } 377 | 378 | // Check for joint velocities limtis. 379 | // Check the velocity only after the motors report ready to avoid 380 | // fast motions during the initialization phase detected as error. 381 | if (IsReady()) 382 | { 383 | for (int i = 0; i < n_; i++) 384 | { 385 | if (std::abs(velocities_[i]) > max_joint_velocities_) 386 | { 387 | has_error = true; 388 | if (velocity_joint_limits_counter_++ % 2000 == 0) 389 | { 390 | msg_out_ << "ERROR: Above joint velocity limits at joint #" 391 | << (i) << std::endl; 392 | msg_out_ << " Joints: "; 393 | PrintVector(velocities_); 394 | msg_out_ << std::endl; 395 | msg_out_ << " Limit: " << max_joint_velocities_ 396 | << std::endl; 397 | } 398 | break; 399 | } 400 | } 401 | } 402 | 403 | // Check the status of the cards. 404 | bool print_error = false; 405 | for (int i = 0; i < nd_; i++) 406 | { 407 | if (robot_if_->motor_drivers[i].error_code != 0) 408 | { 409 | if (print_error || motor_drivers_error_counter++ % 2000 == 0) 410 | { 411 | print_error = true; 412 | msg_out_ << "ERROR at motor drivers #" << (i) << ": "; 413 | switch (robot_if_->motor_drivers[i].error_code) 414 | { 415 | case UD_SENSOR_STATUS_ERROR_ENCODER1: 416 | msg_out_ << "Encoder A error"; 417 | break; 418 | case UD_SENSOR_STATUS_ERROR_SPI_RECV_TIMEOUT: 419 | msg_out_ << "SPI Receiver timeout"; 420 | break; 421 | case UD_SENSOR_STATUS_ERROR_CRIT_TEMP: 422 | msg_out_ << "Critical temperature"; 423 | break; 424 | case UD_SENSOR_STATUS_ERROR_POSCONV: 425 | msg_out_ << "SpinTAC Positon module"; 426 | break; 427 | case UD_SENSOR_STATUS_ERROR_POS_ROLLOVER: 428 | msg_out_ << "Position rollover occured"; 429 | break; 430 | case UD_SENSOR_STATUS_ERROR_ENCODER2: 431 | msg_out_ << "Encoder B error"; 432 | break; 433 | /* 434 | case UD_SENSOR_STATUS_CRC_ERROR: 435 | msg_out_ << "CRC error in SPI transaction"; 436 | break; 437 | */ 438 | default: 439 | msg_out_ << "Other error (" << robot_if_->motor_drivers[i].error_code << ")"; 440 | break; 441 | } 442 | msg_out_ << std::endl; 443 | } 444 | has_error = true; 445 | } 446 | } 447 | return has_error; 448 | } 449 | 450 | void JointModules::PrintVector(ConstRefVectorXd vector) 451 | { 452 | Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]"); 453 | msg_out_ << vector.transpose().format(CleanFmt); 454 | } 455 | 456 | void JointModules::SetMaximumCurrents(double max_currents) 457 | { 458 | for (int i = 0; i < n_; i++) 459 | { 460 | motors_[i]->set_current_sat(max_currents); 461 | } 462 | } 463 | 464 | } // namespace odri_control_interface 465 | -------------------------------------------------------------------------------- /src/robot.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file joint_modules.cpp 3 | * @author Julian Viereck (jviereck@tuebingen.mpg.de) 4 | * license License BSD-3-Clause 5 | * @copyright Copyright (c) 2020, New York University and Max Planck 6 | * Gesellschaft. 7 | * @date 2020-12-05 8 | * 9 | * @brief Robot class orchistrating the devices. 10 | */ 11 | 12 | #include // for exception, runtime_error, out_of_range 13 | 14 | #include "odri_control_interface/robot.hpp" 15 | 16 | namespace odri_control_interface 17 | { 18 | Robot::Robot(const std::shared_ptr& robot_if, 19 | const std::shared_ptr& joint_modules, 20 | const std::shared_ptr& imu, 21 | const std::shared_ptr& calibrator) 22 | : robot_if(robot_if), joints(joint_modules), imu(imu), 23 | calibrator(calibrator), timeout_counter_(0), saw_error_(false) 24 | { 25 | last_time_ = std::chrono::system_clock::now(); 26 | } 27 | 28 | const std::shared_ptr& Robot::GetRobotInterface() 29 | { 30 | return robot_if; 31 | } 32 | 33 | const std::shared_ptr& Robot::GetJoints() 34 | { 35 | return joints; 36 | } 37 | 38 | const std::shared_ptr& Robot::GetIMU() 39 | { 40 | return imu; 41 | } 42 | 43 | void Robot::Init() 44 | { 45 | // Init the robot. 46 | robot_if->Init(); 47 | 48 | // Enable the joints. 49 | joints->Enable(); 50 | } 51 | 52 | void Robot::SendInit() 53 | { 54 | robot_if->SendInit(); 55 | } 56 | 57 | /** 58 | * @brief Initializes the session and blocks until either the package 59 | * got acknowledged or the communication timed out. 60 | */ 61 | void Robot::Start() 62 | { 63 | Init(); 64 | 65 | // Initiate the communication session. 66 | std::chrono::time_point last = 67 | std::chrono::system_clock::now(); 68 | while (!robot_if->IsTimeout() && !robot_if->IsAckMsgReceived()) 69 | { 70 | if (((std::chrono::duration)(std::chrono::system_clock::now() - 71 | last)) 72 | .count() > 0.001) 73 | { 74 | last = std::chrono::system_clock::now(); 75 | robot_if->SendInit(); 76 | } 77 | } 78 | 79 | if (robot_if->IsTimeout()) 80 | { 81 | throw std::runtime_error("Timeout during Robot::Start()."); 82 | } 83 | 84 | // Parse the sensor data to make sure all fields are filled properly when 85 | // the user starts using the robot object. 86 | ParseSensorData(); 87 | } 88 | 89 | bool Robot::IsAckMsgReceived() 90 | { 91 | return robot_if->IsAckMsgReceived(); 92 | } 93 | 94 | /** 95 | * @brief If no error happend, send the previously specified commands 96 | * to the robot. If an error was detected, go into safety mode 97 | * and apply the safety control from the joint_module. 98 | */ 99 | bool Robot::SendCommand() 100 | { 101 | HasError(); 102 | if (saw_error_) 103 | { 104 | joints->RunSafetyController(); 105 | } 106 | robot_if->SendCommand(); 107 | return !saw_error_; 108 | } 109 | 110 | /** 111 | * @brief 112 | */ 113 | bool Robot::SendCommandAndWaitEndOfCycle(double dt) 114 | { 115 | bool result = SendCommand(); 116 | 117 | while (((std::chrono::duration)(std::chrono::system_clock::now() - 118 | last_time_)) 119 | .count() < dt) 120 | { 121 | std::this_thread::yield(); 122 | } 123 | last_time_ = std::chrono::system_clock::now(); 124 | 125 | return result; 126 | } 127 | 128 | /** 129 | * 130 | */ 131 | void Robot::ParseSensorData() 132 | { 133 | robot_if->ParseSensorData(); 134 | joints->ParseSensorData(); 135 | 136 | if (imu) 137 | { 138 | imu->ParseSensorData(); 139 | } 140 | } 141 | 142 | bool Robot::RunCalibration(const std::shared_ptr& calibrator, 143 | VectorXd const& target_positions) 144 | { 145 | bool is_done = false; 146 | if (target_positions.size() != joints->GetNumberMotors()) 147 | { 148 | throw std::runtime_error( 149 | "Target position vector has a different size than the " 150 | "number of motors."); 151 | } 152 | while (!IsTimeout()) 153 | { 154 | ParseSensorData(); 155 | 156 | is_done = calibrator->RunAndGoTo(target_positions); 157 | 158 | if (is_done) 159 | { 160 | return true; 161 | } 162 | 163 | if (!SendCommandAndWaitEndOfCycle(calibrator->dt())) 164 | { 165 | throw std::runtime_error("Error during Robot::RunCalibration()."); 166 | } 167 | } 168 | 169 | throw std::runtime_error("Timeout during Robot::RunCalibration()."); 170 | return false; 171 | } 172 | 173 | bool Robot::RunCalibration(VectorXd const& target_positions) 174 | { 175 | return RunCalibration(calibrator, target_positions); 176 | } 177 | 178 | /** 179 | * @brief Way to report an external error. Causes the robot to go into 180 | * safety mode. 181 | */ 182 | void Robot::ReportError(const std::string& error) 183 | { 184 | msg_out_ << "ERROR: " << error << std::endl; 185 | ReportError(); 186 | } 187 | 188 | /** 189 | * @brief Way to report an external error. Causes the robot to go into 190 | * safety mode. 191 | */ 192 | void Robot::ReportError() 193 | { 194 | saw_error_ = true; 195 | } 196 | 197 | /** 198 | * @brief Returns true if all connected devices report ready. 199 | */ 200 | bool Robot::IsReady() 201 | { 202 | return joints->IsReady(); 203 | } 204 | 205 | bool Robot::WaitUntilReady() 206 | { 207 | ParseSensorData(); 208 | joints->SetZeroCommands(); 209 | 210 | std::chrono::time_point last = 211 | std::chrono::system_clock::now(); 212 | while (!IsReady() && !HasError()) 213 | { 214 | if (((std::chrono::duration)(std::chrono::system_clock::now() - 215 | last)) 216 | .count() > 0.001) 217 | { 218 | last += std::chrono::milliseconds(1); 219 | if (!IsAckMsgReceived()) { 220 | SendInit(); 221 | } else { 222 | ParseSensorData(); 223 | SendCommand(); 224 | } 225 | } 226 | else 227 | { 228 | std::this_thread::yield(); 229 | } 230 | } 231 | 232 | if (HasError()) { 233 | if (robot_if->IsTimeout()) 234 | { 235 | throw std::runtime_error("Timeout during Robot::WaitUntilReady()."); 236 | } else { 237 | throw std::runtime_error("Error during Robot::WaitUntilReady()."); 238 | } 239 | } 240 | 241 | return !saw_error_; 242 | } 243 | 244 | void Robot::Initialize(VectorXd const& target_positions) 245 | { 246 | Start(); 247 | WaitUntilReady(); 248 | RunCalibration(target_positions); 249 | } 250 | 251 | bool Robot::IsTimeout() 252 | { 253 | return robot_if->IsTimeout(); 254 | } 255 | 256 | /** 257 | * @brief Checks all connected devices for errors. Also checks 258 | * if there is a timeout. 259 | */ 260 | bool Robot::HasError() 261 | { 262 | saw_error_ |= joints->HasError(); 263 | if (imu) 264 | { 265 | saw_error_ |= imu->HasError(); 266 | } 267 | 268 | if (robot_if->IsTimeout()) 269 | { 270 | if (timeout_counter_++ % 2000 == 0) 271 | { 272 | msg_out_ << "ERROR: Robot communication timedout." << std::endl; 273 | } 274 | saw_error_ = true; 275 | } 276 | 277 | return saw_error_; 278 | } 279 | 280 | } // namespace odri_control_interface 281 | -------------------------------------------------------------------------------- /src/utils.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file utils.cpp 3 | * @author Julian Viereck (jviereck@tuebingen.mpg.de) 4 | * license License BSD-3-Clause 5 | * @copyright Copyright (c) 2020, New York University and Max Planck 6 | * Gesellschaft. 7 | * @date 2020-12-08 8 | * 9 | * @brief Different utils for using the framework. 10 | */ 11 | 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | namespace odri_control_interface 20 | { 21 | #define assert_yaml_parsing(yaml_node, parent_node_name, child_node_name) \ 22 | if (!yaml_node[child_node_name]) \ 23 | { \ 24 | std::ostringstream oss; \ 25 | oss << "Error: Wrong parsing of the YAML file from src file: [" \ 26 | << __FILE__ << "], in function: [" << __FUNCTION__ << "], line: [" \ 27 | << __LINE__ << ". Node [" << child_node_name \ 28 | << "] does not exists under the node [" << parent_node_name \ 29 | << "]."; \ 30 | throw std::runtime_error(oss.str()); \ 31 | } \ 32 | assert(true) 33 | 34 | #define assert_file_exists(filename) \ 35 | std::ifstream f(filename.c_str()); \ 36 | if (!f.good()) \ 37 | { \ 38 | std::ostringstream oss; \ 39 | oss << "Error: Problem opening the file [" << filename \ 40 | << "], from src file: [" << __FILE__ << "], in function: [" \ 41 | << __FUNCTION__ << "], line: [" << __LINE__ \ 42 | << ". The file may not exists."; \ 43 | throw std::runtime_error(oss.str()); \ 44 | } \ 45 | assert(true) 46 | 47 | std::shared_ptr JointModulesFromYaml( 48 | std::shared_ptr robot_if, 49 | const YAML::Node& joint_modules_yaml) 50 | { 51 | assert_yaml_parsing(joint_modules_yaml, "joint_modules", "motor_numbers"); 52 | const YAML::Node& motor_numbers = joint_modules_yaml["motor_numbers"]; 53 | std::size_t n = motor_numbers.size(); 54 | VectorXi motor_numbers_vec; 55 | motor_numbers_vec.resize(n); 56 | for (std::size_t i = 0; i < n; i++) 57 | { 58 | motor_numbers_vec(i) = motor_numbers[i].as(); 59 | } 60 | 61 | assert_yaml_parsing( 62 | joint_modules_yaml, "joint_modules", "reverse_polarities"); 63 | const YAML::Node& rev_polarities = joint_modules_yaml["reverse_polarities"]; 64 | if (rev_polarities.size() != n) 65 | { 66 | throw std::runtime_error( 67 | "Motor polarities has different size than motor " 68 | "numbers"); 69 | } 70 | VectorXb rev_polarities_vec; 71 | rev_polarities_vec.resize(n); 72 | for (std::size_t i = 0; i < n; i++) 73 | { 74 | rev_polarities_vec(i) = rev_polarities[i].as(); 75 | } 76 | 77 | assert_yaml_parsing( 78 | joint_modules_yaml, "joint_modules", "lower_joint_limits"); 79 | const YAML::Node& lower_joint_limits = 80 | joint_modules_yaml["lower_joint_limits"]; 81 | if (lower_joint_limits.size() != n) 82 | { 83 | throw std::runtime_error( 84 | "Lower joint limits has different size than motor " 85 | "numbers"); 86 | } 87 | VectorXd lower_joint_limits_vec; 88 | lower_joint_limits_vec.resize(n); 89 | for (std::size_t i = 0; i < n; i++) 90 | { 91 | lower_joint_limits_vec(i) = lower_joint_limits[i].as(); 92 | } 93 | 94 | assert_yaml_parsing( 95 | joint_modules_yaml, "joint_modules", "upper_joint_limits"); 96 | const YAML::Node& upper_joint_limits = 97 | joint_modules_yaml["upper_joint_limits"]; 98 | if (upper_joint_limits.size() != n) 99 | { 100 | throw std::runtime_error( 101 | "Upper joint limits has different size than motor " 102 | "numbers"); 103 | } 104 | VectorXd upper_joint_limits_vec; 105 | upper_joint_limits_vec.resize(n); 106 | for (std::size_t i = 0; i < n; i++) 107 | { 108 | upper_joint_limits_vec(i) = upper_joint_limits[i].as(); 109 | } 110 | 111 | assert_yaml_parsing(joint_modules_yaml, "joint_modules", "motor_constants"); 112 | assert_yaml_parsing(joint_modules_yaml, "joint_modules", "gear_ratios"); 113 | assert_yaml_parsing(joint_modules_yaml, "joint_modules", "max_currents"); 114 | assert_yaml_parsing( 115 | joint_modules_yaml, "joint_modules", "max_joint_velocities"); 116 | assert_yaml_parsing(joint_modules_yaml, "joint_modules", "safety_damping"); 117 | return std::make_shared( 118 | robot_if, 119 | motor_numbers_vec, 120 | joint_modules_yaml["motor_constants"].as(), 121 | joint_modules_yaml["gear_ratios"].as(), 122 | joint_modules_yaml["max_currents"].as(), 123 | rev_polarities_vec, 124 | lower_joint_limits_vec, 125 | upper_joint_limits_vec, 126 | joint_modules_yaml["max_joint_velocities"].as(), 127 | joint_modules_yaml["safety_damping"].as()); 128 | } 129 | 130 | std::shared_ptr IMUFromYaml(std::shared_ptr robot_if, 131 | const YAML::Node& imu_yaml) 132 | { 133 | assert_yaml_parsing(imu_yaml, "imu", "rotate_vector"); 134 | const YAML::Node& rotate_vector = imu_yaml["rotate_vector"]; 135 | VectorXl rotate_vector_vec; 136 | rotate_vector_vec.resize(3); 137 | if (rotate_vector.size() != 3) 138 | { 139 | throw std::runtime_error("Rotate vector not of size 3."); 140 | } 141 | for (int i = 0; i < 3; i++) 142 | { 143 | rotate_vector_vec(i) = rotate_vector[i].as(); 144 | } 145 | 146 | assert_yaml_parsing(imu_yaml, "imu", "orientation_vector"); 147 | const YAML::Node& orientation_vector = imu_yaml["orientation_vector"]; 148 | VectorXl orientation_vector_vec; 149 | orientation_vector_vec.resize(4); 150 | if (orientation_vector.size() != 4) 151 | { 152 | throw std::runtime_error("Orientation vector not of size 4."); 153 | } 154 | for (int i = 0; i < 4; i++) 155 | { 156 | orientation_vector_vec(i) = orientation_vector[i].as(); 157 | } 158 | 159 | return std::make_shared( 160 | robot_if, rotate_vector_vec, orientation_vector_vec); 161 | } 162 | 163 | std::shared_ptr JointCalibratorFromYaml( 164 | std::shared_ptr joints, const YAML::Node& joint_calibrator) 165 | { 166 | assert_yaml_parsing(joint_calibrator, "joint_calibrator", "search_methods"); 167 | std::vector calib_methods; 168 | for (std::size_t i = 0; i < joint_calibrator["search_methods"].size(); i++) 169 | { 170 | std::string method = 171 | joint_calibrator["search_methods"][i].as(); 172 | if (method == "AUTO") 173 | { 174 | calib_methods.push_back(CalibrationMethod::AUTO); 175 | } 176 | else if (method == "POS") 177 | { 178 | calib_methods.push_back(CalibrationMethod::POSITIVE); 179 | } 180 | else if (method == "NEG") 181 | { 182 | calib_methods.push_back(CalibrationMethod::NEGATIVE); 183 | } 184 | else if (method == "ALT") 185 | { 186 | calib_methods.push_back(CalibrationMethod::ALTERNATIVE); 187 | } 188 | else 189 | { 190 | throw std::runtime_error("Unknown search method '" + method + "'."); 191 | } 192 | } 193 | 194 | assert_yaml_parsing( 195 | joint_calibrator, "joint_calibrator", "position_offsets"); 196 | const YAML::Node& position_offsets = joint_calibrator["position_offsets"]; 197 | std::size_t n = position_offsets.size(); 198 | VectorXd position_offsets_vec; 199 | position_offsets_vec.resize(n); 200 | for (std::size_t i = 0; i < n; i++) 201 | { 202 | position_offsets_vec(i) = position_offsets[i].as(); 203 | } 204 | 205 | VectorXi calib_order_vec; 206 | if (joint_calibrator["calib_order"]) 207 | { 208 | const YAML::Node& calib_order = joint_calibrator["calib_order"]; 209 | std::size_t n_order = calib_order.size(); 210 | calib_order_vec.resize(n_order); 211 | for (std::size_t i = 0; i < n_order; i++) 212 | { 213 | calib_order_vec(i) = calib_order[i].as(); 214 | } 215 | } 216 | else 217 | { 218 | calib_order_vec.resize(n); 219 | calib_order_vec.setZero(); 220 | } 221 | 222 | VectorXd calib_pos_vec; 223 | if (joint_calibrator["calib_pos"]) 224 | { 225 | const YAML::Node& calib_pos = joint_calibrator["calib_pos"]; 226 | std::size_t n_order = calib_pos.size(); 227 | calib_pos_vec.resize(n_order); 228 | for (std::size_t i = 0; i < n_order; i++) 229 | { 230 | calib_pos_vec(i) = calib_pos[i].as(); 231 | } 232 | } 233 | else 234 | { 235 | calib_pos_vec.resize(n); 236 | calib_pos_vec.setZero(); 237 | } 238 | 239 | assert_yaml_parsing(joint_calibrator, "joint_calibrator", "Kp"); 240 | assert_yaml_parsing(joint_calibrator, "joint_calibrator", "Kd"); 241 | assert_yaml_parsing(joint_calibrator, "joint_calibrator", "T"); 242 | assert_yaml_parsing(joint_calibrator, "joint_calibrator", "dt"); 243 | return std::make_shared( 244 | joints, 245 | calib_methods, 246 | position_offsets_vec, 247 | calib_order_vec, 248 | calib_pos_vec, 249 | joint_calibrator["Kp"].as(), 250 | joint_calibrator["Kd"].as(), 251 | joint_calibrator["T"].as(), 252 | joint_calibrator["dt"].as()); 253 | } 254 | 255 | std::shared_ptr RobotFromYamlFile(const std::string& if_name, 256 | const std::string& file_path) 257 | { 258 | assert_file_exists(file_path); 259 | YAML::Node param = YAML::LoadFile(file_path); 260 | 261 | // Parse the robot part. 262 | assert_yaml_parsing(param, file_path, "robot"); 263 | const YAML::Node& robot_node = param["robot"]; 264 | 265 | // 1. Create the robot interface. 266 | std::shared_ptr robot_if = 267 | std::make_shared(if_name); 268 | 269 | // 2. Create the joint modules. 270 | assert_yaml_parsing(robot_node, "robot", "joint_modules"); 271 | std::shared_ptr joints = 272 | JointModulesFromYaml(robot_if, robot_node["joint_modules"]); 273 | 274 | // 3. Create the imu. 275 | assert_yaml_parsing(robot_node, "robot", "imu"); 276 | std::shared_ptr imu = IMUFromYaml(robot_if, robot_node["imu"]); 277 | 278 | // 4. Create the calibrator procedure. 279 | assert_yaml_parsing(param, file_path, "joint_calibrator"); 280 | std::shared_ptr calibrator = 281 | JointCalibratorFromYaml(joints, param["joint_calibrator"]); 282 | 283 | // 5. Create the robot instance from the objects. 284 | return std::make_shared(robot_if, joints, imu, calibrator); 285 | } 286 | 287 | std::shared_ptr RobotFromYamlFile(const std::string& file_path) 288 | { 289 | assert_file_exists(file_path); 290 | YAML::Node param = YAML::LoadFile(file_path); 291 | 292 | // Parse the robot part. 293 | assert_yaml_parsing(param, file_path, "robot"); 294 | assert_yaml_parsing(param["robot"], "robot", "interface"); 295 | std::string if_name = param["robot"]["interface"].as(); 296 | 297 | return RobotFromYamlFile(if_name, file_path); 298 | } 299 | 300 | std::shared_ptr JointCalibratorFromYamlFile( 301 | const std::string& file_path, std::shared_ptr joints) 302 | { 303 | assert_file_exists(file_path); 304 | YAML::Node param = YAML::LoadFile(file_path); 305 | 306 | assert_yaml_parsing(param, file_path, "joint_calibrator"); 307 | return JointCalibratorFromYaml(joints, param["joint_calibrator"]); 308 | } 309 | 310 | std::shared_ptr CreateMasterBoardInterface( 311 | const std::string& if_name, bool listener_mode) 312 | { 313 | return std::make_shared(if_name, listener_mode); 314 | } 315 | 316 | } // namespace odri_control_interface 317 | -------------------------------------------------------------------------------- /srcpy/bindings.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file joint_modules.hpp 3 | * @author Julian Viereck (jviereck@tuebingen.mpg.de) 4 | * license License BSD-3-Clause 5 | * @copyright Copyright (c) 2020, New York University and Max Planck 6 | * Gesellschaft. 7 | * @date 2020-12-04 8 | * 9 | * @brief Python bindings to the library. 10 | */ 11 | 12 | #include "bindings.h" 13 | #include 14 | #include 15 | #include 16 | 17 | using namespace boost::python; 18 | using namespace odri_control_interface; 19 | 20 | typedef Eigen::Matrix VectorXl; 21 | typedef Eigen::Matrix VectorXb; 22 | 23 | std::shared_ptr joint_calibrator_constructor( 24 | std::shared_ptr joints, 25 | boost::python::list search_methods, 26 | RefVectorXd position_offsets, 27 | RefVectorXi calib_order, 28 | RefVectorXd calib_pos, 29 | double Kp, 30 | double Kd, 31 | double T, 32 | double dt) 33 | { 34 | boost::python::ssize_t len = boost::python::len(search_methods); 35 | std::vector search_method_vec; 36 | for (int i = 0; i < len; i++) 37 | { 38 | search_method_vec.push_back( 39 | boost::python::extract(search_methods[i])); 40 | } 41 | return std::make_shared( 42 | joints, search_method_vec, position_offsets, calib_order, 43 | calib_pos, Kp, Kd, T, dt); 44 | } 45 | 46 | std::shared_ptr joint_modules_constructor( 47 | const std::shared_ptr& robot_if, 48 | ConstRefVectorXl motor_numbers, 49 | double motor_constants, 50 | double gear_ratios, 51 | double max_currents, 52 | ConstRefVectorXb reverse_polarities, 53 | ConstRefVectorXd lower_joint_limits, 54 | ConstRefVectorXd upper_joint_limits, 55 | double max_joint_velocities, 56 | double safety_damping) 57 | { 58 | VectorXi motor_numbers_int = VectorXi::Zero(motor_numbers.size()); 59 | for (Eigen::Index i = 0; i < motor_numbers.size(); ++i) 60 | { 61 | motor_numbers_int(i) = static_cast(motor_numbers(i)); 62 | } 63 | return std::make_shared(robot_if, 64 | motor_numbers_int, 65 | motor_constants, 66 | gear_ratios, 67 | max_currents, 68 | reverse_polarities, 69 | lower_joint_limits, 70 | upper_joint_limits, 71 | max_joint_velocities, 72 | safety_damping); 73 | } 74 | 75 | std::shared_ptr CreateMasterBoardInterfaceDefaults( 76 | const std::string& if_name) 77 | { 78 | return CreateMasterBoardInterface(if_name); 79 | } 80 | 81 | std::shared_ptr RobotFromYamlFileAndIfConfig( 82 | const std::string& if_name, const std::string& file_path) 83 | { 84 | return RobotFromYamlFile(if_name, file_path); 85 | } 86 | 87 | std::shared_ptr RobotOnlyFromYamlFile(const std::string& file_path) 88 | { 89 | return RobotFromYamlFile(file_path); 90 | } 91 | 92 | void ReportErrorVerbose(Robot& r, const std::string err_msg) 93 | { 94 | r.ReportError(err_msg); 95 | } 96 | 97 | void ReportErrorQuiet(Robot& r) 98 | { 99 | r.ReportError(); 100 | } 101 | 102 | BOOST_PYTHON_MODULE(libodri_control_interface_pywrap) 103 | { 104 | // Enabling eigenpy support, i.e. numpy/eigen compatibility. 105 | eigenpy::enableEigenPy(); 106 | eigenpy::enableEigenPySpecific(); 107 | eigenpy::enableEigenPySpecific(); 108 | eigenpy::enableEigenPySpecific(); 109 | eigenpy::enableEigenPySpecific(); 110 | 111 | /* Operation on the master_board_sdk 112 | * - Import the python binding of the master_board_sdk 113 | * - Make available the std::shared_ptr type. 114 | */ 115 | boost::python::import("libmaster_board_sdk_pywrap"); 116 | register_ptr_to_python>(); 117 | 118 | void (JointModules::*EnableIndexOffsetCompensation0)(int) = 119 | &JointModules::EnableIndexOffsetCompensation; 120 | void (JointModules::*EnableIndexOffsetCompensation1)() = 121 | &JointModules::EnableIndexOffsetCompensation; 122 | 123 | // JointModules bindings and it's std::shared_ptr. 124 | class_("JointModules", no_init) 125 | .def("__init__", make_constructor(&joint_modules_constructor)) 126 | .def("enable", &JointModules::Enable) 127 | .def("set_torques", &JointModules::SetTorques) 128 | .def("set_desired_positions", &JointModules::SetDesiredPositions) 129 | .def("set_desired_velocities", &JointModules::SetDesiredVelocities) 130 | .def("set_position_gains", &JointModules::SetPositionGains) 131 | .def("set_velocity_gains", &JointModules::SetVelocityGains) 132 | .def("set_zero_gains", &JointModules::SetZeroGains) 133 | .def("set_zero_commands", &JointModules::SetZeroCommands) 134 | .def("set_position_offsets", &JointModules::SetPositionOffsets) 135 | .def("enable_index_offset_compensation", EnableIndexOffsetCompensation0) 136 | .def("enable_index_offset_compensation", EnableIndexOffsetCompensation1) 137 | .def("set_maximum_current", &JointModules::SetMaximumCurrents) 138 | .def("disable_joint_limit_check", &JointModules::DisableJointLimitCheck) 139 | .def("enable_joint_limit_check", &JointModules::EnableJointLimitCheck) 140 | .add_property( 141 | "ready", 142 | make_function(&JointModules::GetReady, 143 | return_value_policy())) 144 | .add_property( 145 | "enabled", 146 | make_function(&JointModules::GetEnabled, 147 | return_value_policy())) 148 | .add_property( 149 | "saw_all_indices", &JointModules::SawAllIndices) 150 | .add_property( 151 | "is_ready", &JointModules::IsReady) 152 | .add_property("has_error", &JointModules::HasError) 153 | .add_property("number_motors", &JointModules::GetNumberMotors) 154 | .add_property( 155 | "positions", 156 | make_function(&JointModules::GetPositions, 157 | return_value_policy())) 158 | .add_property( 159 | "velocities", 160 | make_function(&JointModules::GetVelocities, 161 | return_value_policy())) 162 | .add_property( 163 | "sent_torques", 164 | make_function(&JointModules::GetSentTorques, 165 | return_value_policy())) 166 | .add_property( 167 | "measured_torques", 168 | make_function(&JointModules::GetMeasuredTorques, 169 | return_value_policy())) 170 | .add_property( 171 | "gear_ratios", 172 | make_function(&JointModules::GetGearRatios, 173 | return_value_policy())); 174 | register_ptr_to_python>(); 175 | 176 | // JointModules bindings and it's std::shared_ptr. 177 | class_("IMU", init>()) 178 | .def(init, 179 | RefVectorXl, 180 | RefVectorXl>()) 181 | .add_property("has_error", &IMU::HasError) 182 | .add_property( 183 | "robot_interface", 184 | make_function(&IMU::GetMasterBoardInterface, 185 | return_value_policy())) 186 | .add_property( 187 | "gyroscope", 188 | make_function(&IMU::GetGyroscope, 189 | return_value_policy())) 190 | .add_property( 191 | "accelerometer", 192 | make_function(&IMU::GetAccelerometer, 193 | return_value_policy())) 194 | .add_property( 195 | "linear_acceleration", 196 | make_function(&IMU::GetLinearAcceleration, 197 | return_value_policy())) 198 | .add_property( 199 | "attitude_euler", 200 | make_function(&IMU::GetAttitudeEuler, 201 | return_value_policy())) 202 | .add_property( 203 | "attitude_quaternion", 204 | make_function(&IMU::GetAttitudeQuaternion, 205 | return_value_policy())); 206 | register_ptr_to_python>(); 207 | 208 | // CalibrationMethod enum bindings. 209 | enum_("CalibrationMethod") 210 | .value("auto", CalibrationMethod::AUTO) 211 | .value("positive", CalibrationMethod::POSITIVE) 212 | .value("negative", CalibrationMethod::NEGATIVE) 213 | .value("alternative", CalibrationMethod::ALTERNATIVE); 214 | 215 | bool (Robot::*RunCalibration0)(Eigen::VectorXd const&) = &Robot::RunCalibration; 216 | bool (Robot::*RunCalibration1)(const std::shared_ptr&, 217 | Eigen::VectorXd const&) = &Robot::RunCalibration; 218 | 219 | class_("Robot", 220 | init, 221 | std::shared_ptr, 222 | std::shared_ptr, 223 | std::shared_ptr>()) 224 | .def("init", &Robot::Init) 225 | .def("sendInit", &Robot::SendInit) 226 | .def("start", &Robot::Start) 227 | .def("wait_until_ready", &Robot::WaitUntilReady) 228 | .def("initialize", &Robot::Initialize) 229 | .def("parse_sensor_data", &Robot::ParseSensorData) 230 | .def("send_command", &Robot::SendCommand) 231 | .def("send_command_and_wait_end_of_cycle", 232 | &Robot::SendCommandAndWaitEndOfCycle) 233 | .def("run_calibration", RunCalibration0) 234 | .def("run_calibration", RunCalibration1) 235 | .def("report_error", &ReportErrorVerbose) 236 | .def("report_error", &ReportErrorQuiet) 237 | .add_property( 238 | "robot_interface", 239 | make_function(&Robot::GetRobotInterface, 240 | return_value_policy())) 241 | .add_property( 242 | "joints", 243 | make_function(&Robot::GetJoints, 244 | return_value_policy())) 245 | .add_property( 246 | "imu", 247 | make_function(&Robot::GetIMU, 248 | return_value_policy())) 249 | .add_property("is_ready", &Robot::IsReady) 250 | .add_property("is_timeout", &Robot::IsTimeout) 251 | .add_property("is_ack_msg_received", &Robot::IsAckMsgReceived) 252 | .add_property("has_error", &Robot::HasError); 253 | register_ptr_to_python>(); 254 | 255 | class_("JointCalibrator", no_init) 256 | .def("__init__", make_constructor(&joint_calibrator_constructor)) 257 | .def("run", &JointCalibrator::Run) 258 | .add_property( 259 | "position_offsets", 260 | make_function(&JointCalibrator::GetPositionOffsets, 261 | return_value_policy())) 262 | .add_property( 263 | "dt", 264 | make_function(&JointCalibrator::dt, 265 | return_value_policy())); 266 | register_ptr_to_python>(); 267 | 268 | def("robot_from_yaml_file", &RobotOnlyFromYamlFile); 269 | def("robot_from_yaml_file", &RobotFromYamlFileAndIfConfig); 270 | def("joint_calibrator_from_yaml_file", &JointCalibratorFromYamlFile); 271 | } 272 | -------------------------------------------------------------------------------- /srcpy/bindings.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "odri_control_interface/calibration.hpp" 5 | #include "odri_control_interface/imu.hpp" 6 | #include "odri_control_interface/joint_modules.hpp" 7 | #include "odri_control_interface/robot.hpp" 8 | #include "odri_control_interface/utils.hpp" 9 | 10 | /* make boost::python understand std::shared_ptr */ 11 | namespace boost 12 | { 13 | template 14 | T *get_pointer(std::shared_ptr p) 15 | { 16 | return p.get(); 17 | } 18 | } // namespace boost --------------------------------------------------------------------------------