diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml index 4c3e4c4..19149e3 100644 --- a/.github/workflows/release.yml +++ b/.github/workflows/release.yml @@ -62,6 +62,8 @@ jobs: packages: write steps: + - uses: actions/checkout@v4 + - name: Download the artifacts uses: actions/download-artifact@v4 with: @@ -74,23 +76,23 @@ jobs: $releaseDate = Get-Date -Format "yyyy-MM-dd (at HH:mm:ss UTC)" echo "RELEASE_DATE=$releaseDate" | Out-File -FilePath $env:GITHUB_ENV -Append - - name: Delete old release - uses: dev-drprasad/delete-tag-and-release@v1.0.1 - with: - tag_name: release-${{ github.ref_name }} - github_token: ${{ secrets.GITHUB_TOKEN }} - delete_release: true + - name: Delete release and tag + shell: bash + run: | + gh release delete release-${{ github.ref_name }} --cleanup-tag --yes || true + gh api --method DELETE repos/${{ github.repository }}/git/refs/tags/release-${{ github.ref_name }} || true + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} - name: Create Release if: success() || failure() - uses: softprops/action-gh-release@v2.2.2 - with: - name: ${{ github.ref_name }} - tag_name: release-${{ github.ref_name }} - fail_on_unmatched_files: false - draft: false - body: | - Last updated on ${{ env.RELEASE_DATE }}. - files: | - artifacts/dist/* + shell: bash + run: | + gh release create release-${{ github.ref_name }} \ + --title "${{ github.ref_name }}" \ + --notes "Last updated on ${{ env.RELEASE_DATE }}." \ + --target ${{ github.sha }} \ + artifacts/dist/* \ artifacts/dynamixelmotors-api.md + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/scrapeDynamixel.yml b/.github/workflows/scrapeDynamixel.yml new file mode 100644 index 0000000..b90c5ef --- /dev/null +++ b/.github/workflows/scrapeDynamixel.yml @@ -0,0 +1,27 @@ +name: Scrape Dynamixel Motors ⛏ + +on: + workflow_dispatch: + +jobs: + generate: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + + - uses: actions/setup-python@v5 + with: + python-version: "3.x" + + - name: Scraping Dynamixel website + run: | + python scraper.py + + - name: Commit motors data + shell: bash + run: | + git config user.name "github-actions" + git config user.email "actions@github.com" + git add dynamixelmotorsapi/dynamixel_configs.json + git diff --staged --quiet || git commit -m "chore: refresh motors" + git push diff --git a/README.md b/README.md index d82e7e6..1f54d0e 100644 --- a/README.md +++ b/README.md @@ -13,55 +13,149 @@ python -m pip install git+https://github.com/SofaComplianceRobotics/DynamixelMot The Dynamixel Motors API provides the `DynamixelMotors` class, which can be used to control the Motors. The API provides methods for controlling the robot's motors. You can look at the [motors_example.py](motors_example.py) file for a simple example of how to use the API to control the motors of the motors. +You have several ways to create a `DynamixelMotors` object: +- by using the `from_dicts` method, which takes a list of dictionaries or one dictionnary describing the motors like above +- by using a json file containing the same list of dictionaries, and use the `from_json` method +- by inheriting from `DynamixelMotors` class. This is the recommended way if you need to extend the capabilities of your Dynamixel motors set (e.g. you want to add specific methods or attributes) -Simple example thaht sets the angles of the motors to 0 radians, waits for 1 second, and then prints the status of the robot: + +Simple example thaht sets the angles of 4 *XM430-W210* motors to 0 radians, waits for 1 second, and then prints the status of the robot: ```python +# Standard creation example + import time from dynamixelmotorsapi import DynamixelMotors -# Define your own class of motors that inherits DynamixelMotors class -class MyDynamixelMotors(DynamixelMotors): - _length_to_rad = 1.0 / 20.0 # 1/radius of the pulley - _rad_to_pulse = 4096 / (2 * pi) # the resolution of the Dynamixel xm430 w210 - _pulse_center= 2048 - _max_vel = 1000 # *0.01 rev/min +motors_description = [ + { + "id": [0, 1, 2, 3], + "model": "XM430-W210", + "pulley_radius": 20, # radius of the pulley in mm + "pulse_center": 2048, + "max_vel": 1000, + "baud_rate": 57600 + } + ] + +robot_motors = DynamixelMotors.from_dicts(motors_description) + +if robot_motors.open(): + + robot_motors.printStatus() + initial_pos_pulse = [0] * len(DXL_IDs) + robot_motors.angles = initial_pos_pulse + time.sleep(1) + robot_motors.printStatus() + robot_motors.close() +``` +This is an example of creation using inheritance. Note that you keep the list of dictionnaries notation you use in the previous example. +```python +# Inheritance creation example + +import time +from dynamixelmotorsapi import DynamixelMotors + +class MyRobot(DynamixelMotors): def __init__(self): - super().__init__() # Check if all parameters have been set + super().__init__([{ + "id": [0, 1, 2, 3], + "model": "XM430-W210", + "pulley_radius": 20, + "pulse_center": 2048, + "max_vel": 1000, + "baud_rate": 1000000 + }]) + + self.foo_bar = "foo" -robot_motors = MyDynamixelMotors() + def toggleAttribute(self): + self.foo_bar = "foo"if self.foo_bar=="bar" else "foo" + +myrobot = MyRobot() if robot_motors.open(): - + print("Foo Bar attribute: ", myrobot.foo_bar) robot_motors.printStatus() initial_pos_pulse = [0] * len(DXL_IDs) robot_motors.angles = initial_pos_pulse time.sleep(1) robot_motors.printStatus() + print("Foo Bar attribute: ", myrobot.foo_bar) robot_motors.close() ``` -By default, there are four motors with IDS 0, 1, 2, and 3. You can change this in the [_dynamixelmotorsparameters.py](dynamixelmotorsapi/_dynamixelmotorsparameters.py) at line 21: -``` python -21 | DXL_IDs = (0, 1, 2, 3) -``` +The catalog of Dynamixel motors has been compiled into the file [dynamixel_configs.json](dynamixelmotorsapi\dynamixel_configs.json). +They've been extracted from the Dynamixel website. -To change the motors parameters based on your motors, like the addresses and values, you can also change them into the file [_dynamixelmotorsparameters.py](dynamixelmotorsapi/_dynamixelmotorsparameters.py) if needed. +Except for the `id` parameter, which must be a list of unique IDs for each motor, the other parameters can be shared among motors of the same model or can also be lists of values for each motor, as long as they are consistent with the number of motors described in the `id` parameter. + +Several syntaxes are possible for the motor description dictionaries, as long as they contain the required information: id, model, pulley_radius (mm), pulse_center, max_vel (rev/min). -The parameters already include some Dynamixel motor series so you can easily switch by commenting and uncommenting the right line: ```python - 4 | #***** (Use only one definition at a time) ***** - 5 | MY_DXL = 'X_SERIES' # X330 (5.0 V recommended), X430, X540, 2X430 - 6 | # MY_DXL = 'MX_SERIES' # MX series with 2.0 firmware update. - 7 | # MY_DXL = 'PRO_SERIES' # H54, H42, M54, M42, L54, L42 - 8 | # MY_DXL = 'PRO_A_SERIES' # PRO series with (A) firmware update. - 9 | # MY_DXL = 'P_SERIES' # PH54, PH42, PM54 -10 | # MY_DXL = 'XL320' # [WARNING] Operating Voltage : 7.4V +# Example of a motor description dictionary for 4 XM430-W210 motors, note that the IDs must be unique for each motor, but the other parameters can be shared among motors of the same model. +{ + "id": [0, 1, 2, 3], + "model": "XM430-W210", + "pulley_radius": 20, + "pulse_center": 2048, + "max_vel": 1000, + "baud_rate": 57600 +} + +# Equivalent to the above, but with the parameters as lists of values for each motor, which can be useful if you have motors of different models or with different configurations. +[ + { + "id": [0, 1, 2, 3], + "model": ["XM430-W210"]*4, + "pulley_radius": [20]*4, + "pulse_center": [2048, 2048, 2048, 2048], + "max_vel": [1000]*4, + "baud_rate": [57600]*4 + } +] + +# Equivalent to the above, but with each motor described as a separate dictionary in the list, which can be useful if you want to have more flexibility in the configuration of each motor. +[ + { + "id": 0, + "model": "XM430-W210", + "pulley_radius": 20, + "pulse_center": 2048, + "max_vel": 1000, + "baud_rate": 57600 + }, + { + "id": 1, + "model": "XM430-W210", + "pulley_radius": 20, + "pulse_center": 2048, + "max_vel": 1000, + "baud_rate": 57600 + }, + { + "id": 2, + "model": "XM430-W210", + "pulley_radius": 20, + "pulse_center": 2048, + "max_vel": 1000, + "baud_rate": 57600 + }, + { + "id": 3, + "model": "XM430-W210", + "pulley_radius": 20, + "pulse_center": 2048, + "max_vel": 1000, + "baud_rate": 57600 + } +] ``` -⚠️ For now, the API assumes that all the motors are the same + +You can also create a `DynamixelMotors` object by directly passing a list of `MotorConfig` objects to the constructor. ## For Developers The documentation is generated using [pydoc-markdown](https://pypi.org/project/pydoc-markdown/). To generate the documentation, you need to install `pydoc-markdown`: diff --git a/all_data_names.py b/all_data_names.py new file mode 100644 index 0000000..646986d --- /dev/null +++ b/all_data_names.py @@ -0,0 +1,601 @@ + +all_data_names = set(['acceleration limit', +'alarm led', +'backup ready', +'baud rate', +'baud rate (bus)', +'brake delay', +'bus watchdog', +'ccw angle limit', +'ccw compliance margin', +'ccw compliance slope', +'control mode', +'controller state', +'current', +'current limit', +'current offset', +'cw angle limit', +'cw compliance margin', +'cw compliance slope', +'d gain', +'drive mode', +'electronic gearratio denominator', +'electronic gearratio numerator', +'error code', +'error code history 1', +'error code history 15', +'error code history 16', +'error code history 2', +'feedforward 1st gain', +'feedforward 2nd gain', +'firmware version', +'following error threshold', +'goal acceleration', +'goal current', +'goal current lpf frequency', +'goal position', +'goal pwm', +'goal torque', +'goal update delay', +'goal velocity', +'hardware error status', +'homing offset', +'hybrid save', +'i gain', +'id', +'in-position threshold', +'inverter temperature limit', +'led', +'led blue', +'led green', +'led red', +'lock', +'max position limit', +'max torque', +'max voltage limit', +'min position limit', +'min voltage limit', +'model information', +'model number', +'motor temperature limit', +'moving', +'moving speed', +'moving status', +'moving threshold', +'multi turn offset', +'normal excitation voltage', +'operating mode', +'overexcitation time', +'overexcitation voltage', +'p gain', +'position d gain', +'position ff gain', +'position ff lpf time', +'position i gain', +'position limit threshold', +'position p gain', +'position trajectory', +'present current', +'present input voltage', +'present inverter temperature', +'present load', +'present motor temperature', +'present position', +'present pwm', +'present speed', +'present temperature', +'present velocity', +'present velocity lpf frequency', +'present voltage', +'profile acceleration', +'profile acceleration time', +'profile time', +'profile velocity', +'protocol type', +'punch', +'pwm limit', +'pwm offset', +'pwm slope', +'realtime tick', +'registered', +'registered instruction', +'resolution divider', +'return delay time', +'safe stop time', +'secondary id', +'secondary(shadow) id', +'sensed current', +'shutdown', +'startup configuration', +'status return level', +'temperature limit', +'torque ctrl mode enable', +'torque enable', +'torque limit', +'velocity ff gain', +'velocity ff lpf time', +'velocity i gain', +'velocity limit', +'velocity offset', +'velocity p gain', +'velocity trajectory'] +) + + + +all_data_properties = set( +['addr_acceleration_limit', +'addr_alarm_led', +'addr_backup_ready', +'addr_baud_rate', +'addr_baud_rate_bus', +'addr_brake_delay', +'addr_bus_watchdog', +'addr_ccw_angle_limit', +'addr_ccw_compliance_margin', +'addr_ccw_compliance_slope', +'addr_control_mode', +'addr_controller_state', +'addr_current', +'addr_current_limit', +'addr_current_offset', +'addr_cw_angle_limit', +'addr_cw_compliance_margin', +'addr_cw_compliance_slope', +'addr_d_gain', +'addr_drive_mode', +'addr_electronic_gearratio_denominator', +'addr_electronic_gearratio_numerator', +'addr_error_code', +'addr_error_code_history_1', +'addr_error_code_history_15', +'addr_error_code_history_16', +'addr_error_code_history_2', +'addr_external_port_data_1', +'addr_external_port_data_2', +'addr_external_port_data_3', +'addr_external_port_data_4', +'addr_external_port_mode_1', +'addr_external_port_mode_2', +'addr_external_port_mode_3', +'addr_external_port_mode_4', +'addr_feedforward_1st_gain', +'addr_feedforward_2nd_gain', +'addr_firmware_version', +'addr_following_error_threshold', +'addr_goal_acceleration', +'addr_goal_current', +'addr_goal_current_lpf_frequency', +'addr_goal_position', +'addr_goal_pwm', +'addr_goal_torque', +'addr_goal_update_delay', +'addr_goal_velocity', +'addr_hardware_error_status', +'addr_homing_offset', +'addr_hybrid_save', +'addr_i_gain', +'addr_id', +'addr_in_position_threshold', +'addr_indirect_address_1', +'addr_indirect_address_127', +'addr_indirect_address_128', +'addr_indirect_address_2', +'addr_indirect_address_256', +'addr_indirect_address_26', +'addr_indirect_address_27', +'addr_indirect_address_28', +'addr_indirect_address_29', +'addr_indirect_address_3', +'addr_indirect_address_30', +'addr_indirect_address_31', +'addr_indirect_address_54', +'addr_indirect_address_55', +'addr_indirect_address_56', +'addr_indirect_data_1', +'addr_indirect_data_127', +'addr_indirect_data_128', +'addr_indirect_data_2', +'addr_indirect_data_256', +'addr_indirect_data_26', +'addr_indirect_data_27', +'addr_indirect_data_28', +'addr_indirect_data_29', +'addr_indirect_data_3', +'addr_indirect_data_30', +'addr_indirect_data_31', +'addr_indirect_data_54', +'addr_indirect_data_55', +'addr_indirect_data_56', +'addr_inverter_temperature_limit', +'addr_led', +'addr_led_blue', +'addr_led_green', +'addr_led_red', +'addr_lock', +'addr_max_position_limit', +'addr_max_torque', +'addr_max_voltage_limit', +'addr_min_position_limit', +'addr_min_voltage_limit', +'addr_model_information', +'addr_model_number', +'addr_motor_temperature_limit', +'addr_moving', +'addr_moving_speed', +'addr_moving_status', +'addr_moving_threshold', +'addr_multi_turn_offset', +'addr_normal_excitation_voltage', +'addr_operating_mode', +'addr_overexcitation_time', +'addr_overexcitation_voltage', +'addr_p_gain', +'addr_position_d_gain', +'addr_position_ff_gain', +'addr_position_ff_lpf_time', +'addr_position_i_gain', +'addr_position_limit_threshold', +'addr_position_p_gain', +'addr_position_trajectory', +'addr_present_current', +'addr_present_input_voltage', +'addr_present_inverter_temperature', +'addr_present_load', +'addr_present_motor_temperature', +'addr_present_position', +'addr_present_pwm', +'addr_present_speed', +'addr_present_temperature', +'addr_present_velocity', +'addr_present_velocity_lpf_frequency', +'addr_present_voltage', +'addr_profile_acceleration', +'addr_profile_acceleration_time', +'addr_profile_time', +'addr_profile_velocity', +'addr_protocol_type', +'addr_punch', +'addr_pwm_limit', +'addr_pwm_offset', +'addr_pwm_slope', +'addr_realtime_tick', +'addr_registered', +'addr_registered_instruction', +'addr_resolution_divider', +'addr_return_delay_time', +'addr_safe_stop_time', +'addr_secondary_id', +'addr_secondary_shadow_id', +'addr_sensed_current', +'addr_shutdown', +'addr_startup_configuration', +'addr_status_return_level', +'addr_temperature_limit', +'addr_torque_ctrl_mode_enable', +'addr_torque_enable', +'addr_torque_limit', +'addr_velocity_ff_gain', +'addr_velocity_ff_lpf_time', +'addr_velocity_i_gain', +'addr_velocity_limit', +'addr_velocity_offset', +'addr_velocity_p_gain', +'addr_velocity_trajectory', +'initial_acceleration_limit', +'initial_alarm_led', +'initial_backup_ready', +'initial_baud_rate', +'initial_baud_rate_bus', +'initial_brake_delay', +'initial_bus_watchdog', +'initial_ccw_angle_limit', +'initial_ccw_compliance_margin', +'initial_ccw_compliance_slope', +'initial_control_mode', +'initial_controller_state', +'initial_current', +'initial_current_limit', +'initial_current_offset', +'initial_cw_angle_limit', +'initial_cw_compliance_margin', +'initial_cw_compliance_slope', +'initial_d_gain', +'initial_drive_mode', +'initial_electronic_gearratio_denominator', +'initial_electronic_gearratio_numerator', +'initial_error_code', +'initial_error_code_history_1', +'initial_error_code_history_15', +'initial_error_code_history_16', +'initial_error_code_history_2', +'initial_external_port_data_1', +'initial_external_port_data_2', +'initial_external_port_data_3', +'initial_external_port_data_4', +'initial_external_port_mode_1', +'initial_external_port_mode_2', +'initial_external_port_mode_3', +'initial_external_port_mode_4', +'initial_feedforward_1st_gain', +'initial_feedforward_2nd_gain', +'initial_firmware_version', +'initial_following_error_threshold', +'initial_goal_acceleration', +'initial_goal_current', +'initial_goal_current_lpf_frequency', +'initial_goal_position', +'initial_goal_pwm', +'initial_goal_torque', +'initial_goal_update_delay', +'initial_goal_velocity', +'initial_hardware_error_status', +'initial_homing_offset', +'initial_hybrid_save', +'initial_i_gain', +'initial_id', +'initial_in_position_threshold', +'initial_indirect_address_1', +'initial_indirect_address_127', +'initial_indirect_address_128', +'initial_indirect_address_2', +'initial_indirect_address_256', +'initial_indirect_address_26', +'initial_indirect_address_27', +'initial_indirect_address_28', +'initial_indirect_address_29', +'initial_indirect_address_3', +'initial_indirect_address_30', +'initial_indirect_address_31', +'initial_indirect_address_54', +'initial_indirect_address_55', +'initial_indirect_address_56', +'initial_indirect_data_1', +'initial_indirect_data_127', +'initial_indirect_data_128', +'initial_indirect_data_2', +'initial_indirect_data_256', +'initial_indirect_data_26', +'initial_indirect_data_27', +'initial_indirect_data_28', +'initial_indirect_data_29', +'initial_indirect_data_3', +'initial_indirect_data_30', +'initial_indirect_data_31', +'initial_indirect_data_54', +'initial_indirect_data_55', +'initial_indirect_data_56', +'initial_inverter_temperature_limit', +'initial_led', +'initial_led_blue', +'initial_led_green', +'initial_led_red', +'initial_lock', +'initial_max_position_limit', +'initial_max_torque', +'initial_max_voltage_limit', +'initial_min_position_limit', +'initial_min_voltage_limit', +'initial_model_information', +'initial_model_number', +'initial_motor_temperature_limit', +'initial_moving', +'initial_moving_speed', +'initial_moving_status', +'initial_moving_threshold', +'initial_multi_turn_offset', +'initial_normal_excitation_voltage', +'initial_operating_mode', +'initial_overexcitation_time', +'initial_overexcitation_voltage', +'initial_p_gain', +'initial_position_d_gain', +'initial_position_ff_gain', +'initial_position_ff_lpf_time', +'initial_position_i_gain', +'initial_position_limit_threshold', +'initial_position_p_gain', +'initial_position_trajectory', +'initial_present_current', +'initial_present_input_voltage', +'initial_present_inverter_temperature', +'initial_present_load', +'initial_present_motor_temperature', +'initial_present_position', +'initial_present_pwm', +'initial_present_speed', +'initial_present_temperature', +'initial_present_velocity', +'initial_present_velocity_lpf_frequency', +'initial_present_voltage', +'initial_profile_acceleration', +'initial_profile_acceleration_time', +'initial_profile_time', +'initial_profile_velocity', +'initial_protocol_type', +'initial_punch', +'initial_pwm_limit', +'initial_pwm_offset', +'initial_pwm_slope', +'initial_realtime_tick', +'initial_registered', +'initial_registered_instruction', +'initial_resolution_divider', +'initial_return_delay_time', +'initial_safe_stop_time', +'initial_secondary_id', +'initial_secondary_shadow_id', +'initial_sensed_current', +'initial_shutdown', +'initial_startup_configuration', +'initial_status_return_level', +'initial_temperature_limit', +'initial_torque_ctrl_mode_enable', +'initial_torque_enable', +'initial_torque_limit', +'initial_velocity_ff_gain', +'initial_velocity_ff_lpf_time', +'initial_velocity_i_gain', +'initial_velocity_limit', +'initial_velocity_offset', +'initial_velocity_p_gain', +'initial_velocity_trajectory', +'len_acceleration_limit', +'len_alarm_led', +'len_backup_ready', +'len_baud_rate', +'len_baud_rate_bus', +'len_brake_delay', +'len_bus_watchdog', +'len_ccw_angle_limit', +'len_ccw_compliance_margin', +'len_ccw_compliance_slope', +'len_control_mode', +'len_controller_state', +'len_current', +'len_current_limit', +'len_current_offset', +'len_cw_angle_limit', +'len_cw_compliance_margin', +'len_cw_compliance_slope', +'len_d_gain', +'len_drive_mode', +'len_electronic_gearratio_denominator', +'len_electronic_gearratio_numerator', +'len_error_code', +'len_error_code_history_1', +'len_error_code_history_15', +'len_error_code_history_16', +'len_error_code_history_2', +'len_external_port_data_1', +'len_external_port_data_2', +'len_external_port_data_3', +'len_external_port_data_4', +'len_external_port_mode_1', +'len_external_port_mode_2', +'len_external_port_mode_3', +'len_external_port_mode_4', +'len_feedforward_1st_gain', +'len_feedforward_2nd_gain', +'len_firmware_version', +'len_following_error_threshold', +'len_goal_acceleration', +'len_goal_current', +'len_goal_current_lpf_frequency', +'len_goal_position', +'len_goal_pwm', +'len_goal_torque', +'len_goal_update_delay', +'len_goal_velocity', +'len_hardware_error_status', +'len_homing_offset', +'len_hybrid_save', +'len_i_gain', +'len_id', +'len_in_position_threshold', +'len_indirect_address_1', +'len_indirect_address_127', +'len_indirect_address_128', +'len_indirect_address_2', +'len_indirect_address_256', +'len_indirect_address_26', +'len_indirect_address_27', +'len_indirect_address_28', +'len_indirect_address_29', +'len_indirect_address_3', +'len_indirect_address_30', +'len_indirect_address_31', +'len_indirect_address_54', +'len_indirect_address_55', +'len_indirect_address_56', +'len_indirect_data_1', +'len_indirect_data_127', +'len_indirect_data_128', +'len_indirect_data_2', +'len_indirect_data_256', +'len_indirect_data_26', +'len_indirect_data_27', +'len_indirect_data_28', +'len_indirect_data_29', +'len_indirect_data_3', +'len_indirect_data_30', +'len_indirect_data_31', +'len_indirect_data_54', +'len_indirect_data_55', +'len_indirect_data_56', +'len_inverter_temperature_limit', +'len_led', +'len_led_blue', +'len_led_green', +'len_led_red', +'len_lock', +'len_max_position_limit', +'len_max_torque', +'len_max_voltage_limit', +'len_min_position_limit', +'len_min_voltage_limit', +'len_model_information', +'len_model_number', +'len_motor_temperature_limit', +'len_moving', +'len_moving_speed', +'len_moving_status', +'len_moving_threshold', +'len_multi_turn_offset', +'len_normal_excitation_voltage', +'len_operating_mode', +'len_overexcitation_time', +'len_overexcitation_voltage', +'len_p_gain', +'len_position_d_gain', +'len_position_ff_gain', +'len_position_ff_lpf_time', +'len_position_i_gain', +'len_position_limit_threshold', +'len_position_p_gain', +'len_position_trajectory', +'len_present_current', +'len_present_input_voltage', +'len_present_inverter_temperature', +'len_present_load', +'len_present_motor_temperature', +'len_present_position', +'len_present_pwm', +'len_present_speed', +'len_present_temperature', +'len_present_velocity', +'len_present_velocity_lpf_frequency', +'len_present_voltage', +'len_profile_acceleration', +'len_profile_acceleration_time', +'len_profile_time', +'len_profile_velocity', +'len_protocol_type', +'len_punch', +'len_pwm_limit', +'len_pwm_offset', +'len_pwm_slope', +'len_realtime_tick', +'len_registered', +'len_registered_instruction', +'len_resolution_divider', +'len_return_delay_time', +'len_safe_stop_time', +'len_secondary_id', +'len_secondary_shadow_id', +'len_sensed_current', +'len_shutdown', +'len_startup_configuration', +'len_status_return_level', +'len_temperature_limit', +'len_torque_ctrl_mode_enable', +'len_torque_enable', +'len_torque_limit', +'len_velocity_ff_gain', +'len_velocity_ff_lpf_time', +'len_velocity_i_gain', +'len_velocity_limit', +'len_velocity_offset', +'len_velocity_p_gain', +'len_velocity_trajectory'] +) \ No newline at end of file diff --git a/dynamixelmotorsapi/__init__.py b/dynamixelmotorsapi/__init__.py index 62c3c84..6a66097 100644 --- a/dynamixelmotorsapi/__init__.py +++ b/dynamixelmotorsapi/__init__.py @@ -1,4 +1,17 @@ +from importlib.resources import files + from .dynamixelmotors import DynamixelMotors, motorgroup +from dynamixelmotorsapi._dynamixelmotorsconfigs import register_model_from_json, MODELS_CONFIGS +from dynamixelmotorsapi._logging_config import logger + +try: + _config_path = files("dynamixelmotorsapi").joinpath("dynamixel_configs.json") + loaded_models = register_model_from_json(_config_path, overwrite=True) + if len(loaded_models): + logger.info(f"Loaded {len(MODELS_CONFIGS)} motor configs") +except Exception as e: + logger.error(f"Failed to load motor configs from JSON: {e}") + @staticmethod def listFTDIDevices() -> list: diff --git a/dynamixelmotorsapi/_dynamixelmotorsconfigs.py b/dynamixelmotorsapi/_dynamixelmotorsconfigs.py new file mode 100644 index 0000000..e047f58 --- /dev/null +++ b/dynamixelmotorsapi/_dynamixelmotorsconfigs.py @@ -0,0 +1,423 @@ +from dataclasses import dataclass +from math import pi +from typing import Dict, List, Optional, Tuple + + +# DYNAMIXEL Protocol Version (1.0 / 2.0) +# https://emanual.robotis.com/docs/en/dxl/protocol2/ +PROTOCOL_VERSION = 2.0 + +BAUDRATE = 1000000 + +# Generic motor parameters +TORQUE_ENABLE = 1 +TORQUE_DISABLE = 0 +VELOCITY_MODE = 1 +POSITION_MODE = 3 +EXT_POSITION_MODE = 4 +PWM_MODE = 16 +CURRENT_MODE = 0 + + +@dataclass(frozen=True) +class ModelConfig: + """ + Hardware-level register map and resolution for a Dynamixel motor model. + Frozen because these are fixed hardware constants — never mutated at runtime. + + To add a custom model at runtime, use register_model() rather than + editing this file. + """ + model: str + series: str + url: str + torque_points: Optional[List[Tuple[float, float]]] = None + resolution: Optional[int] = None + + + addr_baud_rate: Optional[int] = None + len_baud_rate: Optional[int] = None + addr_torque_enable: Optional[int] = None + addr_operating_mode: Optional[int] = None + addr_goal_position: Optional[int] = None + len_goal_position: Optional[int] = None + addr_goal_velocity: Optional[int] = None + len_goal_velocity: Optional[int] = None + addr_present_position: Optional[int] = None + len_present_position: Optional[int] = None + addr_present_velocity: Optional[int] = None + len_present_velocity: Optional[int] = None + addr_moving: Optional[int] = None + addr_moving_status: Optional[int] = None + addr_velocity_profile: Optional[int] = None + len_velocity_profile: Optional[int] = None + addr_velocity_limit: Optional[int] = None + len_velocity_limit: Optional[int] = None + addr_velocity_trajectory: Optional[int] = None + len_velocity_trajectory: Optional[int] = None + addr_velocity_p_gain: Optional[int] = None + len_velocity_p_gain: Optional[int] = None + addr_velocity_i_gain: Optional[int] = None + len_velocity_i_gain: Optional[int] = None + addr_position_trajectory: Optional[int] = None + len_position_trajectory: Optional[int] = None + addr_position_p_gain: Optional[int] = None + len_position_p_gain: Optional[int] = None + addr_position_i_gain: Optional[int] = None + len_position_i_gain: Optional[int] = None + addr_position_d_gain: Optional[int] = None + len_position_d_gain: Optional[int] = None + addr_present_current: Optional[int] = None + len_present_current: Optional[int] = None + addr_goal_pwm: Optional[int] = None + len_goal_pwm: Optional[int] = None + addr_present_pwm: Optional[int] = None + len_present_pwm: Optional[int] = None + addr_pwm_limit: Optional[int] = None + len_pwm_limit: Optional[int] = None + addr_min_position_limit: Optional[int] = None + len_min_position_limit: Optional[int] = None + initial_min_position_limit: Optional[int] = None + addr_max_position_limit: Optional[int] = None + len_max_position_limit: Optional[int] = None + initial_max_position_limit: Optional[int] = None + current_unit: Optional[float] = None + + @property + def rad_to_pulse(self) -> float: + """Conversion factor from radians to pulses, derived from resolution.""" + return self.resolution / (2 * pi) + + +# Registry of all supported model configs. +# Add new model here, or at runtime via register_model(). +MODELS_CONFIGS: Dict[str, ModelConfig] = { + + "X_SERIES": ModelConfig( + model = "X_SERIES", + series = "X_SERIES", + url = "https://emanual.robotis.com/docs/en/dxl/x/xm430-w210/", + resolution = 4096, + addr_baud_rate = 8, + len_baud_rate = 1, + addr_torque_enable = 64, + addr_operating_mode = 11, + addr_goal_position = 116, + len_goal_position = 4, + addr_goal_velocity = 104, + len_goal_velocity = 4, + addr_present_position = 132, + len_present_position = 4, + addr_present_velocity = 128, + len_present_velocity = 4, + addr_velocity_profile = 112, + addr_moving = 122, + addr_moving_status = 123, + addr_velocity_trajectory = 136, + len_velocity_trajectory = 4, + addr_position_trajectory = 140, + len_position_trajectory = 4, + addr_position_p_gain = 84, + len_position_p_gain = 2, + addr_position_i_gain = 82, + len_position_i_gain = 2, + addr_position_d_gain = 80, + len_position_d_gain = 2, + addr_present_current = 126, + len_present_current = 2, + current_unit = 2.69, # mA per unit + addr_goal_pwm = 524, + len_goal_pwm = 2, + addr_present_pwm = 544, + len_present_pwm = 2, + addr_pwm_limit = 64, + len_pwm_limit = 2, + initial_min_position_limit = 0, + initial_max_position_limit = 4095, + ), + + "MX_SERIES": ModelConfig( + # MX series with 2.0 firmware — same register map as X_SERIES + model = "MX_SERIES", + series = "MX_SERIES", + url = "https://emanual.robotis.com/docs/en/dxl/mx/mx-106-2/", + resolution = 4096, + addr_baud_rate = 8, + len_baud_rate = 1, + addr_torque_enable = 64, + addr_operating_mode = 11, + addr_goal_position = 116, + len_goal_position = 4, + addr_goal_velocity = 104, + len_goal_velocity = 4, + addr_present_position = 132, + len_present_position = 4, + addr_present_velocity = 128, + len_present_velocity = 4, + addr_velocity_profile = 112, + addr_moving = 122, + addr_moving_status = 123, + addr_velocity_trajectory = 136, + len_velocity_trajectory = 4, + addr_position_trajectory = 140, + len_position_trajectory = 4, + addr_position_p_gain = 84, + len_position_p_gain = 2, + addr_position_i_gain = 82, + len_position_i_gain = 2, + addr_position_d_gain = 80, + len_position_d_gain = 2, + addr_present_current = 126, + len_present_current = 2, + current_unit = 3.36, # mA per unit + initial_min_position_limit = 0, + initial_max_position_limit = 4095, + ), + + "P_SERIES": ModelConfig( + model = "P_SERIES", + series = "P_SERIES", + url = "https://emanual.robotis.com/docs/en/dxl/p/pm42-010-s260-r/", + resolution = 501923, + addr_baud_rate = 8, + len_baud_rate = 1, + addr_torque_enable = 512, + addr_operating_mode = 11, + addr_goal_position = 564, + len_goal_position = 4, + addr_goal_velocity = 552, + len_goal_velocity = 4, + addr_present_position = 580, + len_present_position = 4, + addr_present_velocity = 576, + len_present_velocity = 4, + addr_velocity_profile = 560, + addr_moving = 570, + addr_moving_status = 571, + addr_velocity_trajectory = 584, + len_velocity_trajectory = 4, + addr_position_trajectory = 588, + len_position_trajectory = 4, + addr_position_p_gain = 532, + len_position_p_gain = 2, + addr_position_i_gain = 530, + len_position_i_gain = 2, + addr_position_d_gain = 528, + len_position_d_gain = 2, + addr_present_current = 574, + len_present_current = 2, + current_unit = 1, # mA per unit + addr_goal_pwm = 548, + len_goal_pwm = 2, + addr_present_pwm = 572, + len_present_pwm = 2, + addr_pwm_limit = 36, + len_pwm_limit = 2, + initial_min_position_limit = -150000, + initial_max_position_limit = 150000, + ), +} + + +def register_model(config: ModelConfig, overwrite: bool = False) -> None: + """ + Register a custom modelConfig in the global registry at runtime. + + Use this for motor model not listed in this file, without needing to + modify the library source. + + Args: + config: A fully constructed modelConfig instance. + overwrite: If False (default), raises ValueError if the model name + is already registered. Set to True to replace an existing entry. + + Example: + ```python + from dynamixelmotorsapi._dynamixelmotorsparameters import modelConfig, register_model + + my_model = modelConfig(model="MY_CUSTOM_model", resolution=8192, ...) + register_model(my_model) + ``` + """ + if config.model in MODELS_CONFIGS and not overwrite: + raise ValueError( + f"model '{config.model}' is already registered. " + f"Pass overwrite=True to replace it." + ) + MODELS_CONFIGS[config.model] = config + + +def register_model_from_dict(data: dict, overwrite: bool = False) -> ModelConfig: + """ + Build a modelConfig from a dict and register it. + + Validates that all required fields are present and reports all missing + fields at once rather than failing on the first one. + + Args: + data: dict containing all ModelConfig fields. + overwrite: passed through to register_model. + + Returns: + The registered ModelConfig instance. + + Raises: + ValueError: if any required fields are missing, listing all of them. + """ + import dataclasses + required_fields = {f.name for f in dataclasses.fields(ModelConfig)} + missing = required_fields - data.keys() + if missing: + raise ValueError( + f"Missing required fields for ModelConfig: {sorted(missing)}" + ) + config = ModelConfig(**{k: data[k] for k in required_fields}) + register_model(config, overwrite=overwrite) + return config + + +def register_model_from_json(path: str, overwrite: bool = False) -> list: + """ + Load a JSON file containing a list of model config dicts and register + each one into model_CONFIGS. + + Args: + path: path to a JSON file containing a list of ModelConfig dicts. + overwrite: passed through to register_model for each entry. + + Returns: + List of registered ModelConfig instances. + + Example JSON: + ```json + [ + { + "model": "MY_CUSTOM_MODEL", + "resolution": 8192, + "addr_torque_enable": 64, + "addr_operating_mode": 11, + ... + } + ] + ``` + """ + import json + with open(path) as f: + entries = json.load(f) + if not isinstance(entries, list): + raise ValueError( + f"Expected a JSON array of model configs, got {type(entries).__name__}." + ) + [register_model_from_dict(entry, overwrite=overwrite) for entry in entries] + return MODELS_CONFIGS + +@dataclass +class MotorConfig: + """ + Per-motor configuration. Holds the varying parameters for a single motor + alongside a reference to its model hardware config. + + The model can be specified in two ways: + - By name (str): looked up in the global model_CONFIGS registry. + - Directly (ModelConfig): for one-off or custom model without registering globally. + + If both are provided, they must refer to the same model name. + + Example (from name): + ```python + MotorConfig(id=0, model="XM430-W210", pulley_radius=20, pulse_center=2048, max_vel=1000) + ``` + + Example (ModelConfig directly): + ```python + MotorConfig(id=0, model_config=my_config, pulley_radius=20, pulse_center=2048, max_vel=1000) + ``` + + Example JSON (name-based, for from_dict / from_json): + ```json + { + "id": 0, + "model": "XM430-W210", + "pulley_radius": 20, + "pulse_center": 2048, + "max_vel": 1000 + } + ``` + """ + id: int + pulley_radius: float + pulse_center: int + max_vel: float + baud_rate: int + + # Provide one of these — not both (unless consistent), not neither + model: Optional[str] = None + model_config: Optional[ModelConfig] = None + torque_points: Optional[List[Tuple[float]]] = None + + def __post_init__(self): + if self.model_config is not None and self.model is not None: + if self.model_config.model != self.model: + raise ValueError( + f"Conflicting model: model='{self.model}' but " + f"model_config.model='{self.model_config.model}'. " + f"Provide one or the other, or ensure they match." + ) + # Both given and consistent — model_config takes precedence + return + + if self.model_config is not None: + # ModelConfig provided directly — set model name for reference + self.model = self.model_config.model + return + + if self.model is not None: + # Model name provided — look up in registry + if self.model not in MODELS_CONFIGS: + raise ValueError( + f"Unknown motor model '{self.model}'. " + f"Available: {list(MODELS_CONFIGS.keys())}. " + f"Use register_model() to add a custom model, " + f"or pass a ModelConfig directly." + ) + self.model_config = MODELS_CONFIGS[self.model] + + if self.torque_points is None: + self.torque_points = self.model_config.torque_points + + return + + raise ValueError( + "Either 'model' (str) or 'model_config' (ModelConfig) must be provided." + ) + + @property + def length_to_rad(self) -> float: + """Conversion factor for length to radians, derived from the pulley radius.""" + return 1./self.pulley_radius + + @property + def rad_to_pulse(self) -> float: + """Conversion factor from radians to pulses, derived from the motor model resolution.""" + return self.model_config.rad_to_pulse + + @property + def length_to_pulse(self) -> float: + """Conversion factor to convert a length (e.g. linear position of a joint) to motor pulses, derived from pulley radius and motor resolution.""" + return self.length_to_rad * self.rad_to_pulse + + @classmethod + def from_dict(cls, data: dict) -> "MotorConfig": + """ + Instantiate from a dict. The model must be specified as a string name + referring to a registered entry in MODEL_CONFIGS. + """ + return cls( + id = data["id"], + model = data["model"], + pulley_radius = data["pulley_radius"], + pulse_center = data["pulse_center"], + max_vel = data["max_vel"], + baud_rate = data["baud_rate"], + torque_points = data["torque_points"] if "torque_points" in data.keys() else None + ) \ No newline at end of file diff --git a/dynamixelmotorsapi/_dynamixelmotorsparameters.py b/dynamixelmotorsapi/_dynamixelmotorsparameters.py deleted file mode 100644 index f381fa0..0000000 --- a/dynamixelmotorsapi/_dynamixelmotorsparameters.py +++ /dev/null @@ -1,92 +0,0 @@ -#********* DYNAMIXEL Model definition ********* -# https://emanual.robotis.com/docs/en/dxl/p/pm42-010-s260-r/ - -#***** (Use only one definition at a time) ***** -MY_DXL = 'X_SERIES' # X330 (5.0 V recommended), X430, X540, 2X430 -# MY_DXL = 'MX_SERIES' # MX series with 2.0 firmware update. -# MY_DXL = 'PRO_SERIES' # H54, H42, M54, M42, L54, L42 -# MY_DXL = 'PRO_A_SERIES' # PRO series with (A) firmware update. -# MY_DXL = 'P_SERIES' # PH54, PH42, PM54 -# MY_DXL = 'XL320' # [WARNING] Operating Voltage : 7.4V - - - -# DYNAMIXEL Protocol Version (1.0 / 2.0) -# https://emanual.robotis.com/docs/en/dxl/protocol2/ -PROTOCOL_VERSION = 2.0 - -# Make sure that each DYNAMIXEL ID should have unique ID. - - -DXL_IDs = (0, 1, 2, 3) - -import serial.tools.list_ports as list_ports - -# Use the actual port assigned to the U2D2. -# ex) Windows: "COM*", Linux: "/dev/ttyUSB*", Mac: "/dev/tty.usbserial-*" -BAUDRATE = 1000000 - - -# Generic motor parameters - -TORQUE_ENABLE = 1 # Value for enabling the torque -TORQUE_DISABLE = 0 # Value for disabling the torque -VELOCITY_MODE = 1 -POSITION_MODE = 3 -EXT_POSITION_MODE = 4 -if MY_DXL == 'X_SERIES' or MY_DXL == 'MX_SERIES': #from https://emanual.robotis.com/docs/en/dxl/x/xm430-w210/ - ADDR_TORQUE_ENABLE = 64 - ADDR_GOAL_POSITION = 116 - LEN_GOAL_POSITION = 4 # Data Byte Length - ADDR_GOAL_VELOCITY = 104 - LEN_GOAL_VELOCITY = 4 # Data Byte Length - ADDR_PRESENT_POSITION = 132 - LEN_PRESENT_POSITION = 4 # Data Byte Length - ADDR_PRESENT_VELOCITY = 128 - LEN_PRESENT_VELOCITY = 4 # Data Byte Length - DXL_MINIMUM_POSITION_VALUE = 0 # Refer to the Minimum Position Limit of product eManual - DXL_MAXIMUM_POSITION_VALUE = 4095 # Refer to the Maximum Position Limit of product eManual - ADDR_OPERATING_MODE = 11 - ADDR_VELOCITY_PROFILE = 112 - ADDR_MOVING = 122 - ADDR_MOVING_STATUS = 123 - ADDR_VELOCITY_TRAJECTORY = 136 - LEN_VELOCITY_TRAJECTORY = 4 - ADDR_POSITION_TRAJECTORY = 140 - LEN_POSITION_TRAJECTORY = 4 - ADDR_POSITION_P_GAIN = 84 - LEN_POSITION_P_GAIN = 2 - ADDR_POSITION_I_GAIN = 82 - LEN_POSITION_I_GAIN = 2 - ADDR_POSITION_D_GAIN = 80 - LEN_POSITION_D_GAIN = 2 - -elif MY_DXL == 'PRO_SERIES': - ADDR_TORQUE_ENABLE = 562 # Control table address is different in DYNAMIXEL model - ADDR_GOAL_POSITION = 596 - LEN_GOAL_POSITION = 4 - ADDR_PRESENT_POSITION = 611 - LEN_PRESENT_POSITION = 4 - DXL_MINIMUM_POSITION_VALUE = -150000 # Refer to the Minimum Position Limit of product eManual - DXL_MAXIMUM_POSITION_VALUE = 150000 # Refer to the Maximum Position Limit of product eManual - raise Exception("not defined yet") - -elif MY_DXL == 'P_SERIES' or MY_DXL == 'PRO_A_SERIES': #from https://emanual.robotis.com/docs/en/dxl/p/pm42-010-s260-r/ - ADDR_TORQUE_ENABLE = 512 # Control table address is different in DYNAMIXEL model - ADDR_GOAL_POSITION = 564 - ADDR_GOAL_VELOCITY = 552 - LEN_GOAL_POSITION = 4 # Data Byte Length - ADDR_PRESENT_POSITION = 580 - LEN_PRESENT_POSITION = 4 # Data Byte Length - ADDR_PRESENT_VELOCITY = 576 - LEN_PRESENT_VELOCITY = 4 # Data Byte Length - DXL_MINIMUM_POSITION_VALUE = -150000 # Refer to the Minimum Position Limit of product eManual - DXL_MAXIMUM_POSITION_VALUE = 150000 # Refer to the Maximum Position Limit of product eManual - ADDR_OPERATING_MODE = 11 - ADDR_VELOCITY_PROFILE = 560 - ADDR_MOVING = 570 - ADDR_MOVING_STATUS = 571 - ADDR_VELOCITY_TRAJECTORY = 584 - LEN_VELOCITY_TRAJECTORY = 4 - ADDR_POSITION_TRAJECTORY = 588 - LEN_POSITION_TRAJECTORY = 4 diff --git a/dynamixelmotorsapi/_motorgroup.py b/dynamixelmotorsapi/_motorgroup.py index 6f83808..b66d572 100644 --- a/dynamixelmotorsapi/_motorgroup.py +++ b/dynamixelmotorsapi/_motorgroup.py @@ -1,10 +1,20 @@ import ctypes +from threading import Lock +from math import pi +from typing import List, Dict +from itertools import groupby + +import serial.tools.list_ports as list_ports from dynamixel_sdk import * -import dynamixelmotorsapi._dynamixelmotorsparameters as MotorsParametersTemplate +from dynamixelmotorsapi._dynamixelmotorsconfigs import ( + MotorConfig, ModelConfig, PROTOCOL_VERSION, TORQUE_ENABLE, TORQUE_DISABLE, + VELOCITY_MODE, POSITION_MODE, EXT_POSITION_MODE, PWM_MODE, CURRENT_MODE +) from dynamixelmotorsapi._logging_config import logger + def listMotors(): """ List all the FTDI devices connected to the computer. @@ -23,62 +33,60 @@ def listMotors(): elif p.serial_number is not None and "FTDI" in p.serial_number: ports.append(p.device) - if ports is None or len(ports) == 0: + if not ports: logger.warning("No motor found. Please check the connection.") - return ports return ports def getDevicePort(entry, method="manufacturer"): - """ - Get the device port based on the device name and method. This will get the first FTDI device found. - - Args: - entry (str): The name of the device to search for. - method (str): The method to use for searching (default is "manufacturer"). - Returns: - The first port of the device if found, otherwise None. - """ - ports = [] - comports = serial.tools.list_ports.comports() - - if comports is None or len(comports) == 0: - logger.error("Serial ports check failed, list of ports is empty.") - return - - if method == "manufacturer": - ports = [p for p in comports if p.manufacturer is not None and entry in p.manufacturer] - if method == "description": - ports = [p for p in comports if p.description is not None and entry in p.description] - if method == "serial_number": - ports = [p for p in comports if p.serial_number is not None and entry in p.serial_number] - - if not ports: - logger.error("No serial port found with " + method + " = " + entry) - return - - if len(ports) > 1: - logger.warning("Multiple port found with " + method + " = " + entry + ". Using the first.") - - logger.debug("Found port with " + method + " = " + entry + ": \n" + - "device : " + ports[0].device + "\n" + - "manufacturer : " + ports[0].manufacturer + "\n" + - "description : " + ports[0].description + "\n" + - "serial number : " + ports[0].serial_number - ) - return ports[0].device - + """ + Get the device port based on the device name and method. -def _valToArray( val): - """Convert a 32-bit integer to a list of 4 bytes. Args: - val (int): The 32-bit integer to convert. + entry (str): The name of the device to search for. + method (str): The method to use for searching (default is "manufacturer"). + Returns: - list of bytes: The list of 4 bytes representing the integer. + The first port of the device if found, otherwise None. """ - return [DXL_LOBYTE(DXL_LOWORD(val)), DXL_HIBYTE(DXL_LOWORD(val)), DXL_LOBYTE(DXL_HIWORD(val)), - DXL_HIBYTE(DXL_HIWORD(val))] + comports = serial.tools.list_ports.comports() + + if not comports: + logger.error("Serial ports check failed, list of ports is empty.") + return None + + if method == "manufacturer": + ports = [p for p in comports if p.manufacturer is not None and entry in p.manufacturer] + elif method == "description": + ports = [p for p in comports if p.description is not None and entry in p.description] + elif method == "serial_number": + ports = [p for p in comports if p.serial_number is not None and entry in p.serial_number] + else: + ports = [] + + if not ports: + logger.error(f"No serial port found with {method} = {entry}") + return None + + if len(ports) > 1: + logger.warning(f"Multiple ports found with {method} = {entry}. Using the first.") + + p = ports[0] + logger.debug( + f"Found port with {method} = {entry}:\n" + f" device: {p.device}\n" + f" manufacturer: {p.manufacturer}\n" + f" description: {p.description}\n" + f" serial number: {p.serial_number}" + ) + return p.device + + +def _valToArray(val): + """Convert a 32-bit integer to a list of 4 bytes.""" + return [DXL_LOBYTE(DXL_LOWORD(val)), DXL_HIBYTE(DXL_LOWORD(val)), + DXL_LOBYTE(DXL_HIWORD(val)), DXL_HIBYTE(DXL_HIWORD(val))] def _valTo2Bytes(val): @@ -87,434 +95,564 @@ def _valTo2Bytes(val): class DisconnectedException(Exception): - """Custom exception for disconnected motors.""" + """Raised when an operation is attempted on a disconnected motor group.""" def __init__(self): - message = "MotorGroup is not connected. It is either disconnected or permission denied." - super().__init__(message) + super().__init__( + "MotorGroup is not connected. It is either disconnected or permission denied." + ) + class MotorGroup: + """ + Controls a group of Dynamixel motors, supporting heterogeneous models. + + Motors are grouped internally by models so that GroupSyncRead/Write — + which require a uniform address and data length — operate correctly even + when motors from different models (with different register maps) are mixed. - def __init__(self, parameters: MotorsParametersTemplate) -> None: + All public read methods return values ordered by the original motor_configs + list, regardless of internal grouping. + """ - self.parameters = parameters + def __init__(self, motor_configs: List[MotorConfig]) -> None: + self.motorsConfig = motor_configs self.deviceName = None - self.packetHandler = PacketHandler(self.parameters.PROTOCOL_VERSION) + self.packetHandler = PacketHandler(PROTOCOL_VERSION) self.portHandler = PortHandler(self.deviceName) - self.groupReaders = {} - self.groupWriters = {} - - self.groupReaders["position"] = GroupSyncRead(self.portHandler, self.packetHandler, - self.parameters.ADDR_PRESENT_POSITION, - self.parameters.LEN_PRESENT_POSITION) - self.groupReaders["velocity"] = GroupSyncRead(self.portHandler, self.packetHandler, - self.parameters.ADDR_PRESENT_VELOCITY, - self.parameters.LEN_PRESENT_VELOCITY) - self.groupReaders["goal_position"] = GroupSyncRead(self.portHandler, self.packetHandler, - self.parameters.ADDR_GOAL_POSITION, - self.parameters.LEN_GOAL_POSITION) - self.groupReaders["goal_velocity"] = GroupSyncRead(self.portHandler, self.packetHandler, - self.parameters.ADDR_GOAL_VELOCITY, - self.parameters.LEN_GOAL_VELOCITY) - self.groupReaders["moving"] = GroupSyncRead(self.portHandler, self.packetHandler, - self.parameters.ADDR_MOVING, - 1) - self.groupReaders["moving_status"] = GroupSyncRead(self.portHandler, self.packetHandler, - self.parameters.ADDR_MOVING_STATUS, - 1) - self.groupReaders["velocity_trajectory"] = GroupSyncRead(self.portHandler, self.packetHandler, - self.parameters.ADDR_VELOCITY_TRAJECTORY, - self.parameters.LEN_VELOCITY_TRAJECTORY) - self.groupReaders["position_trajectory"] = GroupSyncRead(self.portHandler, self.packetHandler, - self.parameters.ADDR_POSITION_TRAJECTORY, - self.parameters.LEN_POSITION_TRAJECTORY) - self.groupReaders["position_p_gain"] = GroupSyncRead(self.portHandler, self.packetHandler, - self.parameters.ADDR_POSITION_P_GAIN, - self.parameters.LEN_POSITION_P_GAIN) - self.groupReaders["position_i_gain"] = GroupSyncRead(self.portHandler, self.packetHandler, - self.parameters.ADDR_POSITION_I_GAIN, - self.parameters.LEN_POSITION_I_GAIN) - self.groupReaders["position_d_gain"] = GroupSyncRead(self.portHandler, self.packetHandler, - self.parameters.ADDR_POSITION_D_GAIN, - self.parameters.LEN_POSITION_D_GAIN) - - self.groupWriters["goal_position"] = GroupSyncWrite(self.portHandler, self.packetHandler, - self.parameters.ADDR_GOAL_POSITION, - self.parameters.LEN_GOAL_POSITION) - self.groupWriters["goal_velocity"] = GroupSyncWrite(self.portHandler, self.packetHandler, - self.parameters.ADDR_GOAL_VELOCITY, - self.parameters.LEN_GOAL_POSITION) - self.groupWriters["velocity_profile"] = GroupSyncWrite(self.portHandler, self.packetHandler, - self.parameters.ADDR_VELOCITY_PROFILE, - self.parameters.LEN_GOAL_POSITION) - self.groupWriters["position_p_gain"] = GroupSyncWrite(self.portHandler, self.packetHandler, - self.parameters.ADDR_POSITION_P_GAIN, - self.parameters.LEN_POSITION_P_GAIN) - self.groupWriters["position_i_gain"] = GroupSyncWrite(self.portHandler, self.packetHandler, - self.parameters.ADDR_POSITION_I_GAIN, - self.parameters.LEN_POSITION_I_GAIN) - self.groupWriters["position_d_gain"] = GroupSyncWrite(self.portHandler, self.packetHandler, - self.parameters.ADDR_POSITION_D_GAIN, - self.parameters.LEN_POSITION_D_GAIN) - - for DXL_ID in self.parameters.DXL_IDs: - for group in self.groupReaders.values(): - group.addParam(DXL_ID) + # Group motors by models so each GroupSyncRead/Write uses a consistent + # address and length. Dict key is model name, value is list of MotorConfigs. + self._models_groups: Dict[str, List[MotorConfig]] = {} + for cfg in motor_configs: + self._models_groups.setdefault(cfg.model, []).append(cfg) + + # Per-models sync read/write groups: {model_name: {reader_name: GroupSyncRead}} + self.groupReaders: Dict[str, Dict[str, GroupSyncRead]] = {} + self.groupWriters: Dict[str, Dict[str, GroupSyncWrite]] = {} + + self._initGroups() + + # ------------------------------------------------------------------ # + # Initialisation helpers # + # ------------------------------------------------------------------ # + + def _initGroups(self): + """Create GroupSyncRead/Write objects for each models.""" + for model_name, configs in self._models_groups.items(): + sc: ModelConfig = configs[0].model_config # all share the same ModelConfig + + readers = { + "position": GroupSyncRead( + self.portHandler, self.packetHandler, + sc.addr_present_position, sc.len_present_position), + "velocity": GroupSyncRead( + self.portHandler, self.packetHandler, + sc.addr_present_velocity, sc.len_present_velocity), + "goal_position": GroupSyncRead( + self.portHandler, self.packetHandler, + sc.addr_goal_position, sc.len_goal_position), + "goal_velocity": GroupSyncRead( + self.portHandler, self.packetHandler, + sc.addr_goal_velocity, sc.len_goal_velocity), + "velocity_profile": GroupSyncRead( + self.portHandler, self.packetHandler, + sc.addr_velocity_profile, sc.len_goal_velocity), + "moving": GroupSyncRead( + self.portHandler, self.packetHandler, + sc.addr_moving, 1), + "moving_status": GroupSyncRead( + self.portHandler, self.packetHandler, + sc.addr_moving_status, 1), + "velocity_trajectory": GroupSyncRead( + self.portHandler, self.packetHandler, + sc.addr_velocity_trajectory, sc.len_velocity_trajectory), + "position_trajectory": GroupSyncRead( + self.portHandler, self.packetHandler, + sc.addr_position_trajectory, sc.len_position_trajectory), + "position_p_gain": GroupSyncRead( + self.portHandler, self.packetHandler, + sc.addr_position_p_gain, sc.len_position_p_gain), + "position_i_gain": GroupSyncRead( + self.portHandler, self.packetHandler, + sc.addr_position_i_gain, sc.len_position_i_gain), + "position_d_gain": GroupSyncRead( + self.portHandler, self.packetHandler, + sc.addr_position_d_gain, sc.len_position_d_gain), + "present_current": GroupSyncRead( + self.portHandler, self.packetHandler, + sc.addr_present_current, sc.len_present_current), + "pwm": GroupSyncRead(self.portHandler, self.packetHandler, + sc.addr_present_pwm, + sc.len_present_pwm), + "pwm_limit": GroupSyncRead(self.portHandler, self.packetHandler, + sc.addr_pwm_limit, + sc.len_pwm_limit), + "goal_pwm": GroupSyncRead(self.portHandler, self.packetHandler, + sc.addr_goal_pwm, + sc.len_goal_pwm) + } + + writers = { + "goal_position": GroupSyncWrite( + self.portHandler, self.packetHandler, + sc.addr_goal_position, sc.len_goal_position), + "goal_velocity": GroupSyncWrite( + self.portHandler, self.packetHandler, + sc.addr_goal_velocity, sc.len_goal_velocity), + "velocity_profile": GroupSyncWrite( + self.portHandler, self.packetHandler, + sc.addr_velocity_profile, sc.len_goal_velocity), + "position_p_gain": GroupSyncWrite( + self.portHandler, self.packetHandler, + sc.addr_position_p_gain, sc.len_position_p_gain), + "position_i_gain": GroupSyncWrite( + self.portHandler, self.packetHandler, + sc.addr_position_i_gain, sc.len_position_i_gain), + "position_d_gain": GroupSyncWrite( + self.portHandler, self.packetHandler, + sc.addr_position_d_gain, sc.len_position_d_gain), + "present_current": GroupSyncWrite( + self.portHandler, self.packetHandler, + sc.addr_present_current, sc.len_present_current), + "pwm": GroupSyncWrite(self.portHandler, self.packetHandler, + sc.addr_present_pwm, + sc.len_present_pwm), + "pwm_limit": GroupSyncWrite(self.portHandler, self.packetHandler, + sc.addr_pwm_limit, + sc.len_pwm_limit), + "goal_pwm": GroupSyncWrite(self.portHandler, self.packetHandler, + sc.addr_goal_pwm, + sc.len_goal_pwm) + } + + # Register all motor IDs in this model with every reader + for cfg in configs: + for reader in readers.values(): + reader.addParam(cfg.id) + + self.groupReaders[model_name] = readers + self.groupWriters[model_name] = writers + + def _updateGroups(self): + """Propagate the new portHandler to all sync groups after reconnection.""" + for readers in self.groupReaders.values(): + for group in readers.values(): + group.port = self.portHandler + group.ph = self.packetHandler + for writers in self.groupWriters.values(): + for group in writers.values(): + group.port = self.portHandler + group.ph = self.packetHandler + + # ------------------------------------------------------------------ # + # Connection management # + # ------------------------------------------------------------------ # @property def isConnected(self): """Check if the motor group is connected.""" try: - if self.portHandler and self.portHandler.is_open and self._isDeviceDetected(): - return True + return bool(self.portHandler and self.portHandler.is_open and self._isDeviceDetected()) except Exception as e: logger.exception(f"Failed to check connection: {e}") return False - def _updateGroups(self): - """ - Update the port handler with the new device name. - """ - for group in self.groupReaders.values(): - group.port = self.portHandler - group.ph = self.packetHandler - - for group in self.groupWriters.values(): - group.port = self.portHandler - group.ph = self.packetHandler - - - def updateDeviceName(self, device_name: str=None): - """ - Update the device name based on the available ports. This will get the first FTDI device found if no device name is provided. - If no device is found, the device name will be None. - """ - self.deviceName = device_name if device_name is not None else getDevicePort("FTDI", method="manufacturer") - self.portHandler = PortHandler(self.deviceName) - self._updateGroups() - - logger.debug(f"Device name updated to: {self.deviceName}") - - return - - def _isDeviceDetected(self): for port in serial.tools.list_ports.comports(): if port.device == self.deviceName: return True return False + + def updateDeviceName(self, device_name: str = None): + """ + Update the device name. If not provided, uses the first FTDI device found. + """ + self.deviceName = device_name if device_name is not None else getDevicePort("FTDI") + self.portHandler = PortHandler(self.deviceName) + self._updateGroups() + logger.debug(f"Device name updated to: {self.deviceName}") - def _readSyncMotorsData(self, groupSyncRead:GroupSyncRead): - """Read data from the motor. - Args: - DXL_ID (int): The ID of the motor. - addr (int): The address to read from. - length (int): The length of the data to read. + def openPort(self) -> None: + """Open the port""" + try: + self.portHandler.openPort() + logger.debug(f"Port opened") + except Exception as e: + raise Exception(f"Failed to open port: {e}") + - Returns: - int: The value read from the motor. + def close(self) -> None: + """Disable torque on all motors and close the port.""" + try: + for cfg in self.motorsConfig: + self.packetHandler.write1ByteTxRx( + self.portHandler, cfg.id, + cfg.model_config.addr_torque_enable, TORQUE_DISABLE + ) + self.portHandler.closePort() + self.deviceName = None + + logger.debug(f"Closed port and disabled torque") + except Exception as e: + raise Exception(f"Failed to close port: {e}") + - Raises: - Exception: If the motor group is not connected or if the read operation fails. - """ + def clearPort(self) -> None: + """Clear the port buffer.""" if not self.isConnected: raise DisconnectedException() + if self.portHandler: + self.portHandler.clearPort() + logger.debug(f"Port cleared") + + # ------------------------------------------------------------------ # + # Torque and operating mode # + # ------------------------------------------------------------------ # - dxl_comm_result = groupSyncRead.txRxPacket() - if dxl_comm_result != COMM_SUCCESS: - raise Exception(f"Failed to read data from motor: {self.packetHandler.getTxRxResult(dxl_comm_result)}") - result = list() + def enableTorque(self): + """Enable torque on all motors.""" + self._write1ByteAll(lambda cfg: cfg.model_config.addr_torque_enable, TORQUE_ENABLE) + logger.debug(f"Torque enabled") - for DXL_ID in self.parameters.DXL_IDs: - dxl_getdata_result = groupSyncRead.isAvailable(DXL_ID, groupSyncRead.start_address, groupSyncRead.data_length) - if dxl_getdata_result != True: - return None - angle = ctypes.c_int(groupSyncRead.getData(DXL_ID, groupSyncRead.start_address, groupSyncRead.data_length)) - result.append(angle.value) + def disableTorque(self): + """Disable torque on all motors.""" + self._write1ByteAll(lambda cfg: cfg.model_config.addr_torque_enable, TORQUE_DISABLE) + logger.debug(f"Torque disabled") + + + def isTorqueEnable(self) -> list: + """Return the torque enable state for each motor.""" + result = [] + for cfg in self.motorsConfig: + self.portHandler.setBaudRate(cfg.baud_rate) + torque, dxl_comm_result, dxl_error = self.packetHandler.read1ByteTxRx( + self.portHandler, cfg.id, cfg.model_config.addr_torque_enable + ) + if dxl_comm_result != COMM_SUCCESS: + raise Exception(f"Failed to read torque (motor {cfg.id}): " + f"{self.packetHandler.getTxRxResult(dxl_comm_result)}") + if dxl_error != 0: + raise Exception(f"Failed to read torque (motor {cfg.id}): " + f"{self.packetHandler.getRxPacketError(dxl_error)}") + + result.append(torque) return result - def __writeSyncMotorsData(self, group: GroupSyncWrite, values): - """Helper function to write data to the motors. - Args: - group (GroupSyncWrite): The group sync write object. - values (list of numbers): The values to write to the motors. + def __setOperatingMode(self, mode: int, ids: int|List[int]=None): """ - if not self.isConnected: - raise DisconnectedException() - - group.clearParam() - for index, DXL_ID in enumerate(self.parameters.DXL_IDs): - if group.data_length == 2: - data = _valTo2Bytes(values[index]) - elif group.data_length == 4: - data = _valToArray(values[index]) - else: - raise Exception(f"Unsupported data length: {group.data_length}") - group.addParam(DXL_ID, data) - group.txPacket() - - - def __write1Byte(self, paramAddress, paramValue): - if not self.isConnected: - raise DisconnectedException() - - for DXL_ID in self.parameters.DXL_IDs: - dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx(self.portHandler, DXL_ID, paramAddress, paramValue) - if dxl_comm_result != COMM_SUCCESS: - raise Exception(f"Failed to change operating mode: {self.packetHandler.getTxRxResult(dxl_comm_result)}") - elif dxl_error != 0: - raise Exception(f"Failed to change operating mode: {self.packetHandler.getTxRxResult(dxl_error)}") - else: - logger.debug("Motor Data %s changed to %s (%s,%s)" % (paramAddress, paramValue, self.deviceName, DXL_ID)) - + Set the operating mode on all motors. - def __setOperatingMode(self, mode): - """Set the operating mode of the motors. Args: - mode (int): The operating mode to set. - 0: Current Control Mode - 1: Velocity Control Mode - 3: (Default) Position Control Mode - 4: Extended Position Control Mode - 5: Current-bqsed Position Control Mode - 16: PWM Control Mode - - See https://emanual.robotis.com/docs/en/dxl/x/xc330-t288/#operating-mode for more details. + mode: 1=Velocity, 3=Position, 4=Extended Position. + See https://emanual.robotis.com/docs/en/dxl/x/xc330-t288/#operating-mode """ - if not self.isConnected: - raise DisconnectedException() - - self.__write1Byte(self.parameters.ADDR_OPERATING_MODE, mode) - + if isinstance(mode, int): + self._write1ByteAll(lambda cfg: cfg.model_config.addr_operating_mode, mode) + else: + for cfg in self.motorsConfig: + if ids is None or cfg.id in ids: + self.portHandler.setBaudRate(cfg.baud_rate) + dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx( + self.portHandler, cfg.id, cfg.model_config.addr_operating_mode, mode + ) + if dxl_comm_result != COMM_SUCCESS: + raise Exception(f"Failed to write operating mode (motor {cfg.id}): " + f"{self.packetHandler.getTxRxResult(dxl_comm_result)}") + if dxl_error != 0: + raise Exception(f"Failed to write operating mode (motor {cfg.id}): " + f"{self.packetHandler.getRxPacketError(dxl_error)}") - def enableVelocityMode(self): + def enableVelocityMode(self, ids: List[int]=None): torques = self.isTorqueEnable() + if any(t == 1 for t in torques): + self.disableTorque() + self.__setOperatingMode(VELOCITY_MODE, ids) + if any(t == 1 for t in torques): + self.enableTorque() + logger.debug(f"Enabled Velocity Mode") - if any(t==1 for t in torques): + + def enablePositionMode(self, ids: List[int]=None): + torques = self.isTorqueEnable() + if any(t == 1 for t in torques): self.disableTorque() - self.__setOperatingMode(self.parameters.VELOCITY_MODE) - if any(t==1 for t in torques): + self.__setOperatingMode(POSITION_MODE, ids) + if any(t == 1 for t in torques): self.enableTorque() + logger.debug(f"Enabled Position Mode") - def enableExtendedPositionMode(self): + def enableExtendedPositionMode(self, ids: List[int]=None): + torques = self.isTorqueEnable() + if any(t == 1 for t in torques): + self.disableTorque() + self.__setOperatingMode(EXT_POSITION_MODE, ids) + if any(t == 1 for t in torques): + self.enableTorque() + logger.debug(f"Enabled Extended Position Mode") + + + def enablePWMMode(self, ids: List[int]=None): torques = self.isTorqueEnable() if any(t==1 for t in torques): self.disableTorque() - self.__setOperatingMode(self.parameters.EXT_POSITION_MODE) + self.__setOperatingMode(PWM_MODE, ids) if any(t==1 for t in torques): self.enableTorque() - def enablePositionMode(self): + def enableCurrentMode(self, ids: List[int]=None): torques = self.isTorqueEnable() if any(t==1 for t in torques): self.disableTorque() - self.__setOperatingMode(self.parameters.POSITION_MODE) + self.__setOperatingMode(CURRENT_MODE, ids) if any(t==1 for t in torques): self.enableTorque() - def setGoalVelocity(self, speeds): - """Set the goal velocity + def getBaudRate(self) -> list: + """Get the baud rate for each motor.""" + result = [] + for cfg in self.motorsConfig: + baud_rate, dxl_comm_result, dxl_error = self.packetHandler.read1ByteTxRx( + self.portHandler, cfg.id, cfg.model_config.addr_baud_rate + ) + if dxl_comm_result != COMM_SUCCESS: + raise Exception(f"Failed to read baud rate (motor {cfg.id}): " + f"{self.packetHandler.getTxRxResult(dxl_comm_result)}") + if dxl_error != 0: + raise Exception(f"Failed to read baud rate (motor {cfg.id}): " + f"{self.packetHandler.getRxPacketError(dxl_error)}") + + result.append(baud_rate) + return result + + # ------------------------------------------------------------------ # + # Low-level read / write helpers # + # ------------------------------------------------------------------ # + + def _write1ByteAll(self, addr_fn, value: int): + """ + Write a single byte to all motors, using each motor's own address. Args: - speeds (list of numbers): unit depends on motor type + addr_fn: callable(MotorConfig) -> int, returns the register address for that motor. + value: the byte value to write. """ - self.__writeSyncMotorsData(self.groupWriters["goal_velocity"] , speeds) + for cfg in self.motorsConfig: + addr = addr_fn(cfg) + + self.portHandler.setBaudRate(cfg.baud_rate) + dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx( + self.portHandler, cfg.id, addr, value + ) + if dxl_comm_result != COMM_SUCCESS: + raise Exception(f"Failed to write byte {value} to motor ID {cfg.id} at addr {addr}: " + f"{self.packetHandler.getTxRxResult(dxl_comm_result)}") + if dxl_error != 0: + raise Exception(f"Error on motor {cfg.id} at addr {addr}: " + f"{self.packetHandler.getRxPacketError(dxl_error)}") + + logger.debug(f"Motor {cfg.id}: addr {addr} set to {value}") - def setGoalPosition(self, positions): - """Set the goal position + def _readSyncMotorsData(self, reader_name: str) -> list: + """ + Read data from all motors using GroupSyncRead, per model, and return + results in the original motor_configs order. Args: - positions (list of numbers): unit = 1 pulse + reader_name: key into self.groupReaders[model]. + + Returns: + List of values in motor_configs order. """ - self.__writeSyncMotorsData(self.groupWriters["goal_position"], positions) + # Collect results keyed by motor ID across all models + results_by_id = {} + for model_name, readers in self.groupReaders.items(): + + self.portHandler.setBaudRate(self._models_groups[model_name][0].baud_rate) + + group = readers[reader_name] + dxl_comm_result = group.txRxPacket() + if dxl_comm_result != COMM_SUCCESS: + raise Exception( + f"Failed to read '{reader_name}' for model '{model_name}': " + f"{self.packetHandler.getTxRxResult(dxl_comm_result)}" + ) + + for cfg in self._models_groups[model_name]: + if not group.isAvailable(cfg.id, group.start_address, group.data_length): + raise Exception( + f"Data not available for motor {cfg.id} (model '{model_name}', " + f"reader '{reader_name}')" + ) + raw = group.getData(cfg.id, group.start_address, group.data_length) + results_by_id[cfg.id] = ctypes.c_int(raw).value + # Return in original motor_configs order + return [results_by_id[cfg.id] for cfg in self.motorsConfig] + - def setVelocityProfile(self, max_vel): - """Set the maximum velocities in position mode + def __writeSyncMotorsData(self, writer_name: str, values: list): + """ + Write data to all motors using GroupSyncWrite, per models. Args: - positions (list of numbers): unit depends on the motor type + writer_name: key into self.groupWriters[model]. + values: list of values in motor_configs order. """ - self.__writeSyncMotorsData(self.groupWriters["velocity_profile"], max_vel) + # Map motor ID -> value from the ordered values list + values_by_id = {cfg.id: values[i] for i, cfg in enumerate(self.motorsConfig)} + + for model_name, writers in self.groupWriters.items(): # iterate through motor models in groupwriters + self.portHandler.setBaudRate(self._models_groups[model_name][0].baud_rate) + group = writers[writer_name] + group.clearParam() + for cfg in self._models_groups[model_name]: # get the MotorConfigs for model model_name + val = values_by_id[cfg.id] + if group.data_length == 2: + data = _valTo2Bytes(val) + elif group.data_length == 4: + data = _valToArray(val) + else: + raise Exception(f"Unsupported data length: {group.data_length}") + group.addParam(cfg.id, data) + + + dxl_comm_result = group.txPacket() + if dxl_comm_result != COMM_SUCCESS: + raise Exception( + f"Failed to read '{writer_name}' for model '{model_name}': " + f"{self.packetHandler.getTxRxResult(dxl_comm_result)}" + ) - def setPositionPGain(self, p_gains): - """Set the position P gains + # ------------------------------------------------------------------ # + # Public read / write API # + # ------------------------------------------------------------------ # - Args: - p_gains (list of numbers): unit depends on the motor type - """ - self.__writeSyncMotorsData(self.groupWriters["position_p_gain"], p_gains) + def setGoalPosition(self, positions: list): + """Set the goal position (pulses) for each motor.""" + self.__writeSyncMotorsData("goal_position", positions) + logger.debug(f"Goal Position set to {positions}") + def setGoalVelocity(self, speeds: list): + """Set the goal velocity for each motor.""" + self.__writeSyncMotorsData("goal_velocity", speeds) + logger.debug(f"Goal Velocity set to {speeds}") - def setPositionIGain(self, i_gains): - """Set the position I gains + def setGoalPWM(self, pwms: list): + """Set the goal PWM for each motor. Args: - i_gains (list of numbers): unit depends on the motor type + pwms (list of numbers): unit depends on motor type """ - self.__writeSyncMotorsData(self.groupWriters["position_i_gain"], i_gains) + self.__writeSyncMotorsData(self.groupWriters["goal_pwm"] , pwms) - - def setPositionDGain(self, d_gains): - """Set the position D gains + def setPWMLimit(self, limits: list): + """Set the PWM limit for each motor. Args: - d_gains (list of numbers): unit depends on the motor type + limits (list of numbers): unit depends on motor type """ - self.__writeSyncMotorsData(self.groupWriters["position_d_gain"], d_gains) + self.__writeSyncMotorsData(self.groupWriters["pwm_limit"] , limits) + def setVelocityProfile(self, max_vel: list): + """Set the maximum velocity profile (position mode) for each motor.""" + self.__writeSyncMotorsData("velocity_profile", max_vel) + logger.debug(f"Velocity profile set to {max_vel}") - def getCurrentPosition(self) -> list: - """Get the current position of the motors - Returns: - list of numbers: unit = 1 pulse - """ - return self._readSyncMotorsData(self.groupReaders["position"]) + def setPositionPGain(self, p_gains: list): + """Set the position P gain for each motor.""" + self.__writeSyncMotorsData("position_p_gain", p_gains) + logger.debug(f"Position P gains set to {p_gains}") + def setPositionIGain(self, i_gains: list): + """Set the position I gain for each motor.""" + self.__writeSyncMotorsData("position_i_gain", i_gains) + logger.debug(f"Position I gains set to {i_gains}") - def getGoalPosition(self) -> list: - """Get the goal position of the motors - Returns: - list of numbers: unit = 1 pulse - """ - return self._readSyncMotorsData(self.groupReaders["goal_position"]) + def setPositionDGain(self, d_gains: list): + """Set the position D gain for each motor.""" + self.__writeSyncMotorsData("position_d_gain", d_gains) + logger.debug(f"Position D gains set to {d_gains}") - def getGoalVelocity(self) -> list: - """Get the goal velocity of the motors - Returns: - list of velocities: unit is rev/min - """ - return self._readSyncMotorsData(self.groupReaders["goal_velocity"]) + def setPresentCurrent(self, currents: list): + """Set the present current for each motor.""" + self.__writeSyncMotorsData("present_current", currents) + logger.debug(f"Present Current set to {currents}") + def getPresentPosition(self) -> list: + """Get the current position (pulses) for each motor.""" + return self._readSyncMotorsData("position") - def getCurrentVelocity(self) -> list: - """Get the current velocity of the motors - Returns: - list of velocities: unit is rev/min - """ - return self._readSyncMotorsData(self.groupReaders["velocity"]) + def getGoalPosition(self) -> list: + """Get the goal position (pulses) for each motor.""" + return self._readSyncMotorsData("goal_position") + + def getGoalVelocity(self) -> list: + """Get the goal velocity for each motor.""" + return self._readSyncMotorsData("goal_velocity") + + def getGoalPWM(self) -> list: + """Get the goal PWM for each motor.""" + return self._readSyncMotorsData("goal_pwm") + + def getPWMLimit(self) -> list: + """Get the PWM limit for each motor.""" + return self._readSyncMotorsData("pwm_limit") + def getPresentVelocity(self) -> list: + """Get the current velocity for each motor.""" + return self._readSyncMotorsData("velocity") + + def getVelocityProfile(self) -> list: + """Get the velocity profile (position mode) for each motor.""" + return self._readSyncMotorsData("velocity_profile") def isMoving(self) -> list: - """Check if the motors are moving - Returns: - list of booleans: True if the motor is moving, False otherwise - """ - return self._readSyncMotorsData(self.groupReaders["moving"]) - + """Return True for each motor that is currently moving.""" + return self._readSyncMotorsData("moving") def getMovingStatus(self) -> list: - """Get the moving status of the motors - Returns: - list of booleans: True if the motor is moving, False otherwise - """ - return self._readSyncMotorsData(self.groupReaders["moving_status"]) - + """Get the moving status byte for each motor.""" + return self._readSyncMotorsData("moving_status") def getVelocityTrajectory(self) -> list: - """Get the velocity trajectory of the motors - Returns: - list of velocities: unit is rev/min - """ - return self._readSyncMotorsData(self.groupReaders["velocity_trajectory"]) - + """Get the velocity trajectory for each motor.""" + return self._readSyncMotorsData("velocity_trajectory") def getPositionTrajectory(self) -> list: - """Get the position trajectory of the motors - Returns: - list of positions: unit = 1 pulse - """ - return self._readSyncMotorsData(self.groupReaders["position_trajectory"]) - + """Get the position trajectory for each motor.""" + return self._readSyncMotorsData("position_trajectory") def getPositionPGain(self) -> list: - """Get the position P gains of the motors - Returns: - list of P gains: unit depends on the motor type - """ - return self._readSyncMotorsData(self.groupReaders["position_p_gain"]) - + """Get the position P gain for each motor.""" + return self._readSyncMotorsData("position_p_gain") def getPositionIGain(self) -> list: - """Get the position I gains of the motors - Returns: - list of I gains: unit depends on the motor type - """ - return self._readSyncMotorsData(self.groupReaders["position_i_gain"]) - + """Get the position I gain for each motor.""" + return self._readSyncMotorsData("position_i_gain") def getPositionDGain(self) -> list: - """Get the position D gains of the motors - Returns: - list of D gains: unit depends on the motor type - """ - return self._readSyncMotorsData(self.groupReaders["position_d_gain"]) - - - def open(self) -> None: - """Open the port and set the baud rate. - Raises: - Exception: If the port cannot be opened or the baud rate cannot be set. - """ - try: - self.portHandler.openPort() - self.portHandler.setBaudRate(self.parameters.BAUDRATE) - except Exception as e: - raise Exception(f"Failed to open port: {e}") - - - def enableTorque(self): - """Enable the torque of the motors.""" - self.__write1Byte(self.parameters.ADDR_TORQUE_ENABLE, self.parameters.TORQUE_ENABLE) - - - def disableTorque(self): - """Disable the torque of the motors.""" - self.__write1Byte(self.parameters.ADDR_TORQUE_ENABLE, self.parameters.TORQUE_DISABLE) - - - def isTorqueEnable(self): - torques = [] - for DXL_ID in self.parameters.DXL_IDs: - torque, dxl_comm_result, dxl_error = self.packetHandler.read1ByteTxRx(self.portHandler, DXL_ID, self.parameters.ADDR_TORQUE_ENABLE) - if dxl_comm_result != COMM_SUCCESS: - raise Exception(f"Failed to read torque: {self.packetHandler.getTxRxResult(dxl_comm_result)}") - elif dxl_error != 0: - raise Exception(f"Failed to read torque: {self.packetHandler.getTxRxResult(dxl_error)}") - torques.append(torque) - return torques + """Get the position D gain for each motor.""" + return self._readSyncMotorsData("position_d_gain") - - def close(self) -> None: - """Close the port and disable the torque of the motors.""" - try: - for DXL_ID in self.parameters.DXL_IDs: - self.packetHandler.write1ByteTxRx(self.portHandler, DXL_ID, self.parameters.ADDR_TORQUE_ENABLE, - self.parameters.TORQUE_DISABLE) - self.portHandler.closePort() - self.deviceName = None - except Exception as e: - raise Exception(f"Failed to close port: {e}") - - def clearPort(self) -> None: - """Clear the port.""" - if not self.isConnected: - raise DisconnectedException() - - if self.portHandler: - self.portHandler.clearPort() + def getPresentCurrent(self) -> list: + """Get the present current for each motor.""" + return self._readSyncMotorsData("present_current") + + def getPresentPWM(self) -> list: + """Get the present PWM for each motor.""" + return self._readSyncMotorsData("pwm") \ No newline at end of file diff --git a/dynamixelmotorsapi/dynamixel_configs.json b/dynamixelmotorsapi/dynamixel_configs.json new file mode 100644 index 0000000..176a2f2 --- /dev/null +++ b/dynamixelmotorsapi/dynamixel_configs.json @@ -0,0 +1,6375 @@ +[ + { + "model": "YM070-210-M001-RH", + "series": "Y_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/y/ym070-210-m001-rh/", + "torque_points": [ + [-0.036, 1.778], + [0.007, 2.606], + [0.051, 3.49], + [0.094, 4.318], + [0.139, 5.256], + [0.181, 6.084], + [0.226, 6.967], + [0.268, 7.851], + [0.313, 8.734], + [0.355, 9.617], + [0.4, 10.556], + [0.443, 11.439], + [0.488, 12.377], + [0.53, 13.316], + [0.575, 14.31], + [0.617, 15.248], + [0.662, 16.297], + [0.704, 17.291], + [0.749, 18.45], + [0.794, 19.609] + ], + "resolution": 524288, + "addr_baud_rate": 12, + "len_baud_rate": 1, + "addr_torque_enable": 512, + "addr_operating_mode": 33, + "addr_goal_position": 532, + "len_goal_position": 4, + "addr_goal_velocity": 528, + "len_goal_velocity": 4, + "addr_present_position": 552, + "len_present_position": 4, + "addr_present_velocity": 132, + "len_present_velocity": 2, + "addr_moving": 48, + "addr_moving_status": 541, + "addr_velocity_profile": 244, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 564, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 216, + "len_velocity_p_gain": 4, + "addr_velocity_i_gain": 212, + "len_velocity_i_gain": 4, + "addr_position_trajectory": 560, + "len_position_trajectory": 4, + "addr_position_p_gain": 216, + "len_position_p_gain": 4, + "addr_position_i_gain": 212, + "len_position_i_gain": 4, + "addr_position_d_gain": 224, + "len_position_d_gain": 4, + "addr_present_current": 546, + "len_present_current": 2, + "addr_goal_pwm": 524, + "len_goal_pwm": 2, + "addr_present_pwm": 544, + "len_present_pwm": 2, + "addr_pwm_limit": 64, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": -262144, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 10.0, + "errors": [] + }, + { + "model": "YM070-210-B001-RH", + "series": "Y_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/y/ym070-210-b001-rh/", + "torque_points": [ + [-0.03, 1.613], + [0.012, 2.441], + [0.054, 3.269], + [0.099, 4.152], + [0.141, 4.98], + [0.184, 5.808], + [0.229, 6.691], + [0.271, 7.519], + [0.313, 8.347], + [0.358, 9.231], + [0.4, 10.059], + [0.445, 10.942], + [0.488, 11.825], + [0.53, 12.709], + [0.575, 13.647], + [0.617, 14.586], + [0.659, 15.524], + [0.704, 16.573], + [0.747, 17.622], + [0.792, 18.726] + ], + "resolution": 524288, + "addr_baud_rate": 12, + "len_baud_rate": 1, + "addr_torque_enable": 512, + "addr_operating_mode": 33, + "addr_goal_position": 532, + "len_goal_position": 4, + "addr_goal_velocity": 528, + "len_goal_velocity": 4, + "addr_present_position": 552, + "len_present_position": 4, + "addr_present_velocity": 132, + "len_present_velocity": 2, + "addr_moving": 48, + "addr_moving_status": 541, + "addr_velocity_profile": 244, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 564, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 216, + "len_velocity_p_gain": 4, + "addr_velocity_i_gain": 212, + "len_velocity_i_gain": 4, + "addr_position_trajectory": 560, + "len_position_trajectory": 4, + "addr_position_p_gain": 216, + "len_position_p_gain": 4, + "addr_position_i_gain": 212, + "len_position_i_gain": 4, + "addr_position_d_gain": 224, + "len_position_d_gain": 4, + "addr_present_current": 546, + "len_present_current": 2, + "addr_goal_pwm": 524, + "len_goal_pwm": 2, + "addr_present_pwm": 544, + "len_present_pwm": 2, + "addr_pwm_limit": 64, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": -262144, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 10.0, + "errors": [] + }, + { + "model": "YM070-210-R051-RH", + "series": "Y_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/y/ym070-210-r051-rh/", + "torque_points": [ + [1.514, 5.406], + [2.711, 5.782], + [3.907, 6.193], + [5.178, 6.671], + [6.375, 7.081], + [7.646, 7.526], + [8.843, 7.97], + [10.114, 8.414], + [11.31, 8.859], + [12.582, 9.303], + [13.778, 9.748], + [15.05, 10.226], + [16.246, 10.671], + [17.517, 11.149], + [18.714, 11.594], + [19.985, 12.106], + [21.182, 12.585], + [22.453, 13.098], + [23.649, 13.61], + [24.921, 14.157] + ], + "resolution": 26738688, + "addr_baud_rate": 12, + "len_baud_rate": 1, + "addr_torque_enable": 512, + "addr_operating_mode": 33, + "addr_goal_position": 532, + "len_goal_position": 4, + "addr_goal_velocity": 528, + "len_goal_velocity": 4, + "addr_present_position": 552, + "len_present_position": 4, + "addr_present_velocity": 132, + "len_present_velocity": 2, + "addr_moving": 48, + "addr_moving_status": 541, + "addr_velocity_profile": 244, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 564, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 216, + "len_velocity_p_gain": 4, + "addr_velocity_i_gain": 212, + "len_velocity_i_gain": 4, + "addr_position_trajectory": 560, + "len_position_trajectory": 4, + "addr_position_p_gain": 216, + "len_position_p_gain": 4, + "addr_position_i_gain": 212, + "len_position_i_gain": 4, + "addr_position_d_gain": 224, + "len_position_d_gain": 4, + "addr_present_current": 546, + "len_present_current": 2, + "addr_goal_pwm": 524, + "len_goal_pwm": 2, + "addr_present_pwm": 544, + "len_present_pwm": 2, + "addr_pwm_limit": 64, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": -262144, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 10.0, + "errors": [] + }, + { + "model": "YM070-210-R099-RH", + "series": "Y_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/y/ym070-210-r099-rh/", + "torque_points": [ + [0.441, 5.954], + [2.512, 6.292], + [4.713, 6.662], + [6.914, 7.032], + [8.985, 7.371], + [11.186, 7.741], + [13.387, 8.11], + [15.588, 8.48], + [17.659, 8.819], + [19.86, 9.22], + [22.061, 9.589], + [24.262, 9.959], + [26.333, 10.329], + [28.534, 10.729], + [30.735, 11.099], + [33.065, 11.561], + [35.137, 11.931], + [37.337, 12.332], + [39.538, 12.794], + [41.739, 13.256] + ], + "resolution": 51904512, + "addr_baud_rate": 12, + "len_baud_rate": 1, + "addr_torque_enable": 512, + "addr_operating_mode": 33, + "addr_goal_position": 532, + "len_goal_position": 4, + "addr_goal_velocity": 528, + "len_goal_velocity": 4, + "addr_present_position": 552, + "len_present_position": 4, + "addr_present_velocity": 132, + "len_present_velocity": 2, + "addr_moving": 48, + "addr_moving_status": 541, + "addr_velocity_profile": 244, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 564, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 216, + "len_velocity_p_gain": 4, + "addr_velocity_i_gain": 212, + "len_velocity_i_gain": 4, + "addr_position_trajectory": 560, + "len_position_trajectory": 4, + "addr_position_p_gain": 216, + "len_position_p_gain": 4, + "addr_position_i_gain": 212, + "len_position_i_gain": 4, + "addr_position_d_gain": 224, + "len_position_d_gain": 4, + "addr_present_current": 546, + "len_present_current": 2, + "addr_goal_pwm": 524, + "len_goal_pwm": 2, + "addr_present_pwm": 544, + "len_present_pwm": 2, + "addr_pwm_limit": 64, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": -262144, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 10.0, + "errors": [] + }, + { + "model": "YM070-210-A051-RH", + "series": "Y_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/y/ym070-210-a051-rh/", + "torque_points": [ + [1.514, 5.406], + [2.711, 5.782], + [3.907, 6.193], + [5.178, 6.671], + [6.375, 7.081], + [7.646, 7.526], + [8.843, 7.97], + [10.114, 8.414], + [11.31, 8.859], + [12.582, 9.303], + [13.778, 9.748], + [15.05, 10.226], + [16.246, 10.671], + [17.517, 11.149], + [18.714, 11.594], + [19.985, 12.106], + [21.182, 12.585], + [22.453, 13.098], + [23.649, 13.61], + [24.921, 14.157] + ], + "resolution": 26738688, + "addr_baud_rate": 12, + "len_baud_rate": 1, + "addr_torque_enable": 512, + "addr_operating_mode": 33, + "addr_goal_position": 532, + "len_goal_position": 4, + "addr_goal_velocity": 528, + "len_goal_velocity": 4, + "addr_present_position": 552, + "len_present_position": 4, + "addr_present_velocity": 132, + "len_present_velocity": 2, + "addr_moving": 48, + "addr_moving_status": 541, + "addr_velocity_profile": 244, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 564, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 216, + "len_velocity_p_gain": 4, + "addr_velocity_i_gain": 212, + "len_velocity_i_gain": 4, + "addr_position_trajectory": 560, + "len_position_trajectory": 4, + "addr_position_p_gain": 216, + "len_position_p_gain": 4, + "addr_position_i_gain": 212, + "len_position_i_gain": 4, + "addr_position_d_gain": 224, + "len_position_d_gain": 4, + "addr_present_current": 546, + "len_present_current": 2, + "addr_goal_pwm": 524, + "len_goal_pwm": 2, + "addr_present_pwm": 544, + "len_present_pwm": 2, + "addr_pwm_limit": 64, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": -262144, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 10.0, + "errors": [] + }, + { + "model": "YM070-210-A099-RH", + "series": "Y_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/y/ym070-210-a099-rh/", + "torque_points": [ + [0.441, 5.954], + [2.512, 6.292], + [4.713, 6.662], + [6.914, 7.032], + [8.985, 7.371], + [11.186, 7.741], + [13.387, 8.11], + [15.588, 8.48], + [17.659, 8.819], + [19.86, 9.22], + [22.061, 9.589], + [24.262, 9.959], + [26.333, 10.329], + [28.534, 10.729], + [30.735, 11.099], + [33.065, 11.561], + [35.137, 11.931], + [37.337, 12.332], + [39.538, 12.794], + [41.739, 13.256] + ], + "resolution": 51904512, + "addr_baud_rate": 12, + "len_baud_rate": 1, + "addr_torque_enable": 512, + "addr_operating_mode": 33, + "addr_goal_position": 532, + "len_goal_position": 4, + "addr_goal_velocity": 528, + "len_goal_velocity": 4, + "addr_present_position": 552, + "len_present_position": 4, + "addr_present_velocity": 132, + "len_present_velocity": 2, + "addr_moving": 48, + "addr_moving_status": 541, + "addr_velocity_profile": 244, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 564, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 216, + "len_velocity_p_gain": 4, + "addr_velocity_i_gain": 212, + "len_velocity_i_gain": 4, + "addr_position_trajectory": 560, + "len_position_trajectory": 4, + "addr_position_p_gain": 216, + "len_position_p_gain": 4, + "addr_position_i_gain": 212, + "len_position_i_gain": 4, + "addr_position_d_gain": 224, + "len_position_d_gain": 4, + "addr_present_current": 546, + "len_present_current": 2, + "addr_goal_pwm": 524, + "len_goal_pwm": 2, + "addr_present_pwm": 544, + "len_present_pwm": 2, + "addr_pwm_limit": 64, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": -262144, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 10.0, + "errors": [] + }, + { + "model": "YM080-230-M001-RH", + "series": "Y_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/y/ym080-230-m001-rh/", + "torque_points": [ + [-0.074, 18.593], + [0.011, 18.329], + [0.096, 18.048], + [0.181, 17.767], + [0.265, 17.487], + [0.35, 17.206], + [0.435, 16.942], + [0.52, 16.661], + [0.604, 16.38], + [0.689, 16.1], + [0.774, 15.836], + [0.859, 15.555], + [0.943, 15.274], + [1.028, 14.994], + [1.113, 14.713], + [1.198, 14.432], + [1.283, 14.119], + [1.367, 13.788], + [1.452, 13.442], + [1.542, 13.029] + ], + "resolution": 524288, + "addr_baud_rate": 12, + "len_baud_rate": 1, + "addr_torque_enable": 512, + "addr_operating_mode": 33, + "addr_goal_position": 532, + "len_goal_position": 4, + "addr_goal_velocity": 528, + "len_goal_velocity": 4, + "addr_present_position": 552, + "len_present_position": 4, + "addr_present_velocity": 132, + "len_present_velocity": 2, + "addr_moving": 48, + "addr_moving_status": 541, + "addr_velocity_profile": 244, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 564, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 216, + "len_velocity_p_gain": 4, + "addr_velocity_i_gain": 212, + "len_velocity_i_gain": 4, + "addr_position_trajectory": 560, + "len_position_trajectory": 4, + "addr_position_p_gain": 216, + "len_position_p_gain": 4, + "addr_position_i_gain": 212, + "len_position_i_gain": 4, + "addr_position_d_gain": 224, + "len_position_d_gain": 4, + "addr_present_current": 546, + "len_present_current": 2, + "addr_goal_pwm": 524, + "len_goal_pwm": 2, + "addr_present_pwm": 544, + "len_present_pwm": 2, + "addr_pwm_limit": 64, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": -262144, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 10.0, + "errors": [] + }, + { + "model": "YM080-230-B001-RH", + "series": "Y_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/y/ym080-230-b001-rh/", + "torque_points": [ + [-0.074, 4.153], + [0.011, 5.013], + [0.096, 5.933], + [0.181, 6.793], + [0.265, 7.713], + [0.35, 8.573], + [0.435, 9.493], + [0.52, 10.414], + [0.604, 11.396], + [0.689, 12.378], + [0.774, 13.361], + [0.859, 14.343], + [0.943, 15.386], + [1.028, 16.491], + [1.113, 17.596], + [1.198, 18.762], + [1.283, 19.99], + [1.367, 21.279], + [1.452, 22.568], + [1.542, 24.041] + ], + "resolution": 524288, + "addr_baud_rate": 12, + "len_baud_rate": 1, + "addr_torque_enable": 512, + "addr_operating_mode": 33, + "addr_goal_position": 532, + "len_goal_position": 4, + "addr_goal_velocity": 528, + "len_goal_velocity": 4, + "addr_present_position": 552, + "len_present_position": 4, + "addr_present_velocity": 132, + "len_present_velocity": 2, + "addr_moving": 48, + "addr_moving_status": 541, + "addr_velocity_profile": 244, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 564, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 216, + "len_velocity_p_gain": 4, + "addr_velocity_i_gain": 212, + "len_velocity_i_gain": 4, + "addr_position_trajectory": 560, + "len_position_trajectory": 4, + "addr_position_p_gain": 216, + "len_position_p_gain": 4, + "addr_position_i_gain": 212, + "len_position_i_gain": 4, + "addr_position_d_gain": 224, + "len_position_d_gain": 4, + "addr_present_current": 546, + "len_present_current": 2, + "addr_goal_pwm": 524, + "len_goal_pwm": 2, + "addr_present_pwm": 544, + "len_present_pwm": 2, + "addr_pwm_limit": 64, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": -262144, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 10.0, + "errors": [] + }, + { + "model": "YM080-230-R051-RH", + "series": "Y_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/y/ym080-230-r051-rh/", + "torque_points": [ + [1.655, 5.954], + [4.019, 6.373], + [6.134, 6.732], + [8.124, 7.06], + [10.239, 7.419], + [12.23, 7.778], + [14.344, 8.137], + [16.335, 8.495], + [18.45, 8.854], + [20.44, 9.213], + [22.555, 9.601], + [24.546, 9.93], + [26.661, 10.319], + [28.651, 10.678], + [30.766, 11.066], + [32.756, 11.425], + [34.871, 11.814], + [36.862, 12.202], + [38.977, 12.621], + [41.092, 13.069] + ], + "resolution": 26738688, + "addr_baud_rate": 12, + "len_baud_rate": 1, + "addr_torque_enable": 512, + "addr_operating_mode": 33, + "addr_goal_position": 532, + "len_goal_position": 4, + "addr_goal_velocity": 528, + "len_goal_velocity": 4, + "addr_present_position": 552, + "len_present_position": 4, + "addr_present_velocity": 132, + "len_present_velocity": 2, + "addr_moving": 48, + "addr_moving_status": 541, + "addr_velocity_profile": 244, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 564, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 216, + "len_velocity_p_gain": 4, + "addr_velocity_i_gain": 212, + "len_velocity_i_gain": 4, + "addr_position_trajectory": 560, + "len_position_trajectory": 4, + "addr_position_p_gain": 216, + "len_position_p_gain": 4, + "addr_position_i_gain": 212, + "len_position_i_gain": 4, + "addr_position_d_gain": 224, + "len_position_d_gain": 4, + "addr_present_current": 546, + "len_present_current": 2, + "addr_goal_pwm": 524, + "len_goal_pwm": 2, + "addr_present_pwm": 544, + "len_present_pwm": 2, + "addr_pwm_limit": 64, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": -262144, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 10.0, + "errors": [] + }, + { + "model": "YM080-230-R099-RH", + "series": "Y_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/y/ym080-230-r099-rh/", + "torque_points": [ + [-5.367, 5.172], + [-0.883, 5.621], + [3.922, 6.099], + [8.406, 6.548], + [13.211, 7.027], + [18.016, 7.505], + [22.5, 7.924], + [27.305, 8.403], + [32.109, 8.821], + [36.594, 9.21], + [41.398, 9.659], + [45.883, 10.078], + [50.687, 10.467], + [55.492, 10.856], + [59.976, 11.214], + [64.781, 11.603], + [69.586, 11.962], + [74.07, 12.261], + [78.875, 12.561], + [83.679, 12.86] + ], + "resolution": 51904512, + "addr_baud_rate": 12, + "len_baud_rate": 1, + "addr_torque_enable": 512, + "addr_operating_mode": 33, + "addr_goal_position": 532, + "len_goal_position": 4, + "addr_goal_velocity": 528, + "len_goal_velocity": 4, + "addr_present_position": 552, + "len_present_position": 4, + "addr_present_velocity": 132, + "len_present_velocity": 2, + "addr_moving": 48, + "addr_moving_status": 541, + "addr_velocity_profile": 244, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 564, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 216, + "len_velocity_p_gain": 4, + "addr_velocity_i_gain": 212, + "len_velocity_i_gain": 4, + "addr_position_trajectory": 560, + "len_position_trajectory": 4, + "addr_position_p_gain": 216, + "len_position_p_gain": 4, + "addr_position_i_gain": 212, + "len_position_i_gain": 4, + "addr_position_d_gain": 224, + "len_position_d_gain": 4, + "addr_present_current": 546, + "len_present_current": 2, + "addr_goal_pwm": 524, + "len_goal_pwm": 2, + "addr_present_pwm": 544, + "len_present_pwm": 2, + "addr_pwm_limit": 64, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": -262144, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 10.0, + "errors": [] + }, + { + "model": "YM080-230-A051-RH", + "series": "Y_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/y/ym080-230-a051-rh/", + "torque_points": [ + [1.655, 5.954], + [4.019, 6.373], + [6.134, 6.732], + [8.124, 7.06], + [10.239, 7.419], + [12.23, 7.778], + [14.344, 8.137], + [16.335, 8.495], + [18.45, 8.854], + [20.44, 9.213], + [22.555, 9.601], + [24.546, 9.93], + [26.661, 10.319], + [28.651, 10.678], + [30.766, 11.066], + [32.756, 11.425], + [34.871, 11.814], + [36.862, 12.202], + [38.977, 12.621], + [41.092, 13.069] + ], + "resolution": 26738688, + "addr_baud_rate": 12, + "len_baud_rate": 1, + "addr_torque_enable": 512, + "addr_operating_mode": 33, + "addr_goal_position": 532, + "len_goal_position": 4, + "addr_goal_velocity": 528, + "len_goal_velocity": 4, + "addr_present_position": 552, + "len_present_position": 4, + "addr_present_velocity": 132, + "len_present_velocity": 2, + "addr_moving": 48, + "addr_moving_status": 541, + "addr_velocity_profile": 244, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 564, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 216, + "len_velocity_p_gain": 4, + "addr_velocity_i_gain": 212, + "len_velocity_i_gain": 4, + "addr_position_trajectory": 560, + "len_position_trajectory": 4, + "addr_position_p_gain": 216, + "len_position_p_gain": 4, + "addr_position_i_gain": 212, + "len_position_i_gain": 4, + "addr_position_d_gain": 224, + "len_position_d_gain": 4, + "addr_present_current": 546, + "len_present_current": 2, + "addr_goal_pwm": 524, + "len_goal_pwm": 2, + "addr_present_pwm": 544, + "len_present_pwm": 2, + "addr_pwm_limit": 64, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": -262144, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 10.0, + "errors": [] + }, + { + "model": "YM080-230-A099-RH", + "series": "Y_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/y/ym080-230-a099-rh/", + "torque_points": [ + [-5.367, 5.172], + [-0.883, 5.621], + [3.922, 6.099], + [8.406, 6.548], + [13.211, 7.027], + [18.016, 7.505], + [22.5, 7.924], + [27.305, 8.403], + [32.109, 8.821], + [36.594, 9.21], + [41.398, 9.659], + [45.883, 10.078], + [50.687, 10.467], + [55.492, 10.856], + [59.976, 11.214], + [64.781, 11.603], + [69.586, 11.962], + [74.07, 12.261], + [78.875, 12.561], + [83.679, 12.86] + ], + "resolution": 51904512, + "addr_baud_rate": 12, + "len_baud_rate": 1, + "addr_torque_enable": 512, + "addr_operating_mode": 33, + "addr_goal_position": 532, + "len_goal_position": 4, + "addr_goal_velocity": 528, + "len_goal_velocity": 4, + "addr_present_position": 552, + "len_present_position": 4, + "addr_present_velocity": 132, + "len_present_velocity": 2, + "addr_moving": 48, + "addr_moving_status": 541, + "addr_velocity_profile": 244, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 564, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 216, + "len_velocity_p_gain": 4, + "addr_velocity_i_gain": 212, + "len_velocity_i_gain": 4, + "addr_position_trajectory": 560, + "len_position_trajectory": 4, + "addr_position_p_gain": 216, + "len_position_p_gain": 4, + "addr_position_i_gain": 212, + "len_position_i_gain": 4, + "addr_position_d_gain": 224, + "len_position_d_gain": 4, + "addr_present_current": 546, + "len_present_current": 2, + "addr_goal_pwm": 524, + "len_goal_pwm": 2, + "addr_present_pwm": 544, + "len_present_pwm": 2, + "addr_pwm_limit": 64, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": -262144, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 10.0, + "errors": [] + }, + { + "model": "PH54-200-S500-R", + "series": "P_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/p/ph54-200-s500-r/", + "torque_points": [ + [-0.745, 1.715], + [3.21, 2.221], + [7.164, 2.769], + [11.119, 3.318], + [15.074, 3.908], + [19.028, 4.499], + [22.983, 5.089], + [26.937, 5.68], + [30.892, 6.27], + [34.847, 6.903], + [38.801, 7.536], + [42.756, 8.169], + [46.71, 8.843], + [50.665, 9.518], + [54.619, 10.193], + [58.574, 10.91], + [62.529, 11.627], + [66.483, 12.429], + [70.438, 13.23], + [74.612, 14.074] + ], + "resolution": 1003846, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 512, + "addr_operating_mode": 11, + "addr_goal_position": 564, + "len_goal_position": 4, + "addr_goal_velocity": 552, + "len_goal_velocity": 4, + "addr_present_position": 580, + "len_present_position": 4, + "addr_present_velocity": 576, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 571, + "addr_velocity_profile": 560, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 584, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 526, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 524, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 588, + "len_position_trajectory": 4, + "addr_position_p_gain": 526, + "len_position_p_gain": 2, + "addr_position_i_gain": 524, + "len_position_i_gain": 2, + "addr_position_d_gain": 528, + "len_position_d_gain": 2, + "addr_present_current": 574, + "len_present_current": 2, + "addr_goal_pwm": 548, + "len_goal_pwm": 2, + "addr_present_pwm": 572, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": -501433, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 1.0, + "errors": [] + }, + { + "model": "PH54-100-S500-R", + "series": "P_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/p/ph54-100-s500-r/", + "torque_points": [ + [-0.305, 1.42], + [3.21, 2.01], + [6.725, 2.601], + [10.24, 3.191], + [13.755, 3.782], + [17.49, 4.457], + [21.005, 5.089], + [24.521, 5.68], + [28.036, 6.313], + [31.551, 6.987], + [35.286, 7.662], + [38.801, 8.337], + [42.316, 9.012], + [45.831, 9.687], + [49.347, 10.362], + [53.082, 11.121], + [56.597, 11.881], + [60.112, 12.64], + [63.627, 13.483], + [67.362, 14.411] + ], + "resolution": 1003846, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 512, + "addr_operating_mode": 11, + "addr_goal_position": 564, + "len_goal_position": 4, + "addr_goal_velocity": 552, + "len_goal_velocity": 4, + "addr_present_position": 580, + "len_present_position": 4, + "addr_present_velocity": 576, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 571, + "addr_velocity_profile": 560, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 584, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 526, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 524, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 588, + "len_position_trajectory": 4, + "addr_position_p_gain": 526, + "len_position_p_gain": 2, + "addr_position_i_gain": 524, + "len_position_i_gain": 2, + "addr_position_d_gain": 528, + "len_position_d_gain": 2, + "addr_present_current": 574, + "len_present_current": 2, + "addr_goal_pwm": 548, + "len_goal_pwm": 2, + "addr_present_pwm": 572, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": -501433, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 1.0, + "errors": [] + }, + { + "model": "PH42-020-S300-R", + "series": "P_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/p/ph42-020-s300-r/", + "torque_points": [ + [-0.567, 1.867], + [0.525, 2.038], + [1.695, 2.257], + [2.865, 2.477], + [4.035, 2.697], + [5.205, 2.917], + [6.376, 3.136], + [7.546, 3.356], + [8.716, 3.6], + [9.886, 3.82], + [11.056, 4.064], + [12.226, 4.284], + [13.396, 4.528], + [14.566, 4.772], + [15.737, 5.04], + [16.907, 5.285], + [18.077, 5.553], + [19.247, 5.846], + [20.417, 6.163], + [21.587, 6.578] + ], + "resolution": 607500, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 512, + "addr_operating_mode": 11, + "addr_goal_position": 564, + "len_goal_position": 4, + "addr_goal_velocity": 552, + "len_goal_velocity": 4, + "addr_present_position": 580, + "len_present_position": 4, + "addr_present_velocity": 576, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 571, + "addr_velocity_profile": 560, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 584, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 526, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 524, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 588, + "len_position_trajectory": 4, + "addr_position_p_gain": 526, + "len_position_p_gain": 2, + "addr_position_i_gain": 524, + "len_position_i_gain": 2, + "addr_position_d_gain": 528, + "len_position_d_gain": 2, + "addr_present_current": 574, + "len_present_current": 2, + "addr_goal_pwm": 548, + "len_goal_pwm": 2, + "addr_present_pwm": 572, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": -303454, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 1.0, + "errors": [] + }, + { + "model": "PM54-060-S250-R", + "series": "P_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/p/pm54-060-s250-r/", + "torque_points": [ + [-1.813, 1.17], + [0.304, 1.467], + [2.555, 1.763], + [4.805, 2.06], + [6.923, 2.357], + [9.173, 2.696], + [11.423, 3.077], + [13.541, 3.374], + [15.792, 3.755], + [18.042, 4.136], + [20.16, 4.518], + [22.41, 4.942], + [24.66, 5.365], + [26.91, 5.832], + [29.161, 6.34], + [31.543, 6.891], + [33.661, 7.4], + [35.911, 8.035], + [38.162, 8.798], + [40.412, 9.688] + ], + "resolution": 502834, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 512, + "addr_operating_mode": 11, + "addr_goal_position": 564, + "len_goal_position": 4, + "addr_goal_velocity": 552, + "len_goal_velocity": 4, + "addr_present_position": 580, + "len_present_position": 4, + "addr_present_velocity": 576, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 571, + "addr_velocity_profile": 560, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 584, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 526, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 524, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 588, + "len_position_trajectory": 4, + "addr_position_p_gain": 526, + "len_position_p_gain": 2, + "addr_position_i_gain": 524, + "len_position_i_gain": 2, + "addr_position_d_gain": 528, + "len_position_d_gain": 2, + "addr_present_current": 574, + "len_present_current": 2, + "addr_goal_pwm": 548, + "len_goal_pwm": 2, + "addr_present_pwm": 572, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": -251173, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 1.0, + "errors": [] + }, + { + "model": "PM54-040-S250-R", + "series": "P_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/p/pm54-040-s250-r/", + "torque_points": [ + [-1.151, 2.128], + [-0.349, 2.247], + [0.453, 2.355], + [1.322, 2.475], + [2.124, 2.594], + [2.992, 2.726], + [3.794, 2.845], + [4.663, 2.977], + [5.465, 3.108], + [6.334, 3.252], + [7.136, 3.383], + [8.005, 3.539], + [8.807, 3.682], + [9.676, 3.838], + [10.478, 3.993], + [11.347, 4.161], + [12.149, 4.328], + [13.018, 4.519], + [13.82, 4.723], + [14.688, 4.938] + ], + "resolution": 502834, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 512, + "addr_operating_mode": 11, + "addr_goal_position": 564, + "len_goal_position": 4, + "addr_goal_velocity": 552, + "len_goal_velocity": 4, + "addr_present_position": 580, + "len_present_position": 4, + "addr_present_velocity": 576, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 571, + "addr_velocity_profile": 560, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 584, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 526, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 524, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 588, + "len_position_trajectory": 4, + "addr_position_p_gain": 526, + "len_position_p_gain": 2, + "addr_position_i_gain": 524, + "len_position_i_gain": 2, + "addr_position_d_gain": 528, + "len_position_d_gain": 2, + "addr_present_current": 574, + "len_present_current": 2, + "addr_goal_pwm": 548, + "len_goal_pwm": 2, + "addr_present_pwm": 572, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": -251173, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 1.0, + "errors": [] + }, + { + "model": "PM42-010-S260-R", + "series": "P_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/p/pm42-010-s260-r/", + "torque_points": [ + [12.733, 49.832], + [12.461, 49.836], + [12.189, 49.84], + [11.917, 49.844], + [11.621, 49.849], + [11.349, 49.855], + [11.077, 49.86], + [10.781, 49.866], + [10.509, 49.871], + [10.237, 49.879], + [9.941, 49.886], + [9.669, 49.894], + [9.397, 49.903], + [9.101, 49.912], + [8.829, 49.922], + [8.557, 49.933], + [8.261, 49.946], + [7.989, 49.959], + [7.717, 49.974], + [7.421, 49.993] + ], + "resolution": 526374, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 512, + "addr_operating_mode": 11, + "addr_goal_position": 564, + "len_goal_position": 4, + "addr_goal_velocity": 552, + "len_goal_velocity": 4, + "addr_present_position": 580, + "len_present_position": 4, + "addr_present_velocity": 576, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 571, + "addr_velocity_profile": 560, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 584, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 526, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 524, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 588, + "len_position_trajectory": 4, + "addr_position_p_gain": 526, + "len_position_p_gain": 2, + "addr_position_i_gain": 524, + "len_position_i_gain": 2, + "addr_position_d_gain": 528, + "len_position_d_gain": 2, + "addr_present_current": 574, + "len_present_current": 2, + "addr_goal_pwm": 548, + "len_goal_pwm": 2, + "addr_present_pwm": 572, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": -262931, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 1.0, + "errors": [] + }, + { + "model": "XL320", + "series": "X_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/x/xl320/", + "torque_points": null, + "resolution": null, + "addr_baud_rate": 4, + "len_baud_rate": 1, + "addr_torque_enable": 24, + "addr_operating_mode": null, + "addr_goal_position": 30, + "len_goal_position": 2, + "addr_goal_velocity": null, + "len_goal_velocity": null, + "addr_present_position": 37, + "len_present_position": 2, + "addr_present_velocity": null, + "len_present_velocity": null, + "addr_moving": 32, + "addr_moving_status": null, + "addr_velocity_profile": null, + "len_velocity_profile": null, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": null, + "len_velocity_trajectory": null, + "addr_velocity_p_gain": null, + "len_velocity_p_gain": null, + "addr_velocity_i_gain": null, + "len_velocity_i_gain": null, + "addr_position_trajectory": null, + "len_position_trajectory": null, + "addr_position_p_gain": 29, + "len_position_p_gain": 1, + "addr_position_i_gain": 28, + "len_position_i_gain": 1, + "addr_position_d_gain": 27, + "len_position_d_gain": 1, + "addr_present_current": null, + "len_present_current": null, + "addr_goal_pwm": null, + "len_goal_pwm": null, + "addr_present_pwm": null, + "len_present_pwm": null, + "addr_pwm_limit": null, + "len_pwm_limit": null, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": null, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": null, + "errors": [] + }, + { + "model": "XW540-T140", + "series": "X_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/x/xw540-t140/", + "torque_points": [ + [0.297, 37.184], + [0.493, 36.934], + [0.702, 36.601], + [0.911, 36.226], + [1.12, 35.851], + [1.316, 35.518], + [1.525, 35.101], + [1.734, 34.685], + [1.943, 34.268], + [2.151, 33.81], + [2.347, 33.394], + [2.556, 32.936], + [2.778, 32.394], + [2.987, 31.895], + [3.209, 31.312], + [3.405, 30.77], + [3.614, 30.187], + [3.823, 29.562], + [4.032, 28.896], + [4.241, 28.271] + ], + "resolution": 4096, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 64, + "addr_operating_mode": 11, + "addr_goal_position": 116, + "len_goal_position": 4, + "addr_goal_velocity": 104, + "len_goal_velocity": 4, + "addr_present_position": 132, + "len_present_position": 4, + "addr_present_velocity": 128, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 123, + "addr_velocity_profile": 112, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 136, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 78, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 76, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 140, + "len_position_trajectory": 4, + "addr_position_p_gain": 78, + "len_position_p_gain": 2, + "addr_position_i_gain": 76, + "len_position_i_gain": 2, + "addr_position_d_gain": 80, + "len_position_d_gain": 2, + "addr_present_current": 126, + "len_present_current": 2, + "addr_goal_pwm": 100, + "len_goal_pwm": 2, + "addr_present_pwm": 124, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": 0, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 2.69, + "errors": [] + }, + { + "model": "XW540-T260", + "series": "X_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/x/xw540-t260/", + "torque_points": [ + [0.187, 1.162], + [0.587, 1.335], + [0.987, 1.529], + [1.387, 1.723], + [1.786, 1.939], + [2.186, 2.133], + [2.586, 2.349], + [2.986, 2.586], + [3.386, 2.802], + [3.785, 3.039], + [4.209, 3.277], + [4.608, 3.536], + [5.008, 3.773], + [5.408, 4.032], + [5.808, 4.291], + [6.207, 4.55], + [6.607, 4.83], + [7.007, 5.111], + [7.407, 5.391], + [7.83, 5.693] + ], + "resolution": 4096, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 64, + "addr_operating_mode": 11, + "addr_goal_position": 116, + "len_goal_position": 4, + "addr_goal_velocity": 104, + "len_goal_velocity": 4, + "addr_present_position": 132, + "len_present_position": 4, + "addr_present_velocity": 128, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 123, + "addr_velocity_profile": 112, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 136, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 78, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 76, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 140, + "len_position_trajectory": 4, + "addr_position_p_gain": 78, + "len_position_p_gain": 2, + "addr_position_i_gain": 76, + "len_position_i_gain": 2, + "addr_position_d_gain": 80, + "len_position_d_gain": 2, + "addr_present_current": 126, + "len_present_current": 2, + "addr_goal_pwm": 100, + "len_goal_pwm": 2, + "addr_present_pwm": 124, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": 0, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 2.69, + "errors": [] + }, + { + "model": "XW430-T200", + "series": "X_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/x/xw430-t200/", + "torque_points": [ + [-0.004, 0.545], + [0.074, 0.952], + [0.152, 1.359], + [0.236, 1.834], + [0.314, 2.274], + [0.393, 2.715], + [0.476, 3.224], + [0.555, 3.732], + [0.633, 4.207], + [0.716, 4.783], + [0.795, 5.292], + [0.878, 5.868], + [0.957, 6.444], + [1.035, 7.021], + [1.119, 7.665], + [1.197, 8.275], + [1.275, 8.919], + [1.359, 9.665], + [1.437, 10.377], + [1.521, 11.157] + ], + "resolution": 4096, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 64, + "addr_operating_mode": 11, + "addr_goal_position": 116, + "len_goal_position": 4, + "addr_goal_velocity": 104, + "len_goal_velocity": 4, + "addr_present_position": 132, + "len_present_position": 4, + "addr_present_velocity": 128, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 123, + "addr_velocity_profile": 112, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 136, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 78, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 76, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 140, + "len_position_trajectory": 4, + "addr_position_p_gain": 78, + "len_position_p_gain": 2, + "addr_position_i_gain": 76, + "len_position_i_gain": 2, + "addr_position_d_gain": 80, + "len_position_d_gain": 2, + "addr_present_current": 126, + "len_present_current": 2, + "addr_goal_pwm": 100, + "len_goal_pwm": 2, + "addr_present_pwm": 124, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": 0, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 2.69, + "errors": [] + }, + { + "model": "XW430-T333", + "series": "X_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/x/xw430-t333/", + "torque_points": [ + [0.043, 2.273], + [0.163, 2.507], + [0.29, 2.805], + [0.41, 3.082], + [0.538, 3.381], + [0.665, 3.7], + [0.785, 3.999], + [0.913, 4.319], + [1.04, 4.66], + [1.16, 4.979], + [1.287, 5.342], + [1.407, 5.661], + [1.535, 6.024], + [1.662, 6.386], + [1.782, 6.748], + [1.91, 7.132], + [2.037, 7.516], + [2.157, 7.878], + [2.284, 8.283], + [2.412, 8.666] + ], + "resolution": 4096, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 64, + "addr_operating_mode": 11, + "addr_goal_position": 116, + "len_goal_position": 4, + "addr_goal_velocity": 104, + "len_goal_velocity": 4, + "addr_present_position": 132, + "len_present_position": 4, + "addr_present_velocity": 128, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 123, + "addr_velocity_profile": 112, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 136, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 78, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 76, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 140, + "len_position_trajectory": 4, + "addr_position_p_gain": 78, + "len_position_p_gain": 2, + "addr_position_i_gain": 76, + "len_position_i_gain": 2, + "addr_position_d_gain": 80, + "len_position_d_gain": 2, + "addr_present_current": 126, + "len_present_current": 2, + "addr_goal_pwm": 100, + "len_goal_pwm": 2, + "addr_present_pwm": 124, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": 0, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 2.69, + "errors": [] + }, + { + "model": "XD540-T270", + "series": "X_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/x/xd540-t270/", + "torque_points": [ + [0.263, 1.029], + [0.668, 1.192], + [1.073, 1.355], + [1.5, 1.535], + [1.905, 1.715], + [2.31, 1.894], + [2.737, 2.09], + [3.142, 2.286], + [3.547, 2.466], + [3.974, 2.678], + [4.379, 2.874], + [4.829, 3.119], + [5.234, 3.315], + [5.639, 3.528], + [6.066, 3.756], + [6.471, 3.985], + [6.876, 4.214], + [7.304, 4.459], + [7.709, 4.704], + [8.136, 4.949] + ], + "resolution": 4096, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 64, + "addr_operating_mode": 11, + "addr_goal_position": 116, + "len_goal_position": 4, + "addr_goal_velocity": 104, + "len_goal_velocity": 4, + "addr_present_position": 132, + "len_present_position": 4, + "addr_present_velocity": 128, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 123, + "addr_velocity_profile": 112, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 136, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 78, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 76, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 140, + "len_position_trajectory": 4, + "addr_position_p_gain": 78, + "len_position_p_gain": 2, + "addr_position_i_gain": 76, + "len_position_i_gain": 2, + "addr_position_d_gain": 80, + "len_position_d_gain": 2, + "addr_present_current": 126, + "len_present_current": 2, + "addr_goal_pwm": 100, + "len_goal_pwm": 2, + "addr_present_pwm": 124, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": 0, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 2.69, + "errors": [] + }, + { + "model": "XD540-T150", + "series": "X_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/x/xd540-t150/", + "torque_points": [ + [0.344, 0.536], + [0.557, 0.693], + [0.769, 0.866], + [0.982, 1.04], + [1.194, 1.231], + [1.406, 1.422], + [1.619, 1.613], + [1.831, 1.804], + [2.043, 2.013], + [2.256, 2.204], + [2.468, 2.412], + [2.681, 2.638], + [2.893, 2.846], + [3.105, 3.072], + [3.318, 3.298], + [3.53, 3.541], + [3.742, 3.784], + [3.955, 4.027], + [4.167, 4.288], + [4.38, 4.549] + ], + "resolution": 4096, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 64, + "addr_operating_mode": 11, + "addr_goal_position": 116, + "len_goal_position": 4, + "addr_goal_velocity": 104, + "len_goal_velocity": 4, + "addr_present_position": 132, + "len_present_position": 4, + "addr_present_velocity": 128, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 123, + "addr_velocity_profile": 112, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 136, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 78, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 76, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 140, + "len_position_trajectory": 4, + "addr_position_p_gain": 78, + "len_position_p_gain": 2, + "addr_position_i_gain": 76, + "len_position_i_gain": 2, + "addr_position_d_gain": 80, + "len_position_d_gain": 2, + "addr_present_current": 126, + "len_present_current": 2, + "addr_goal_pwm": 100, + "len_goal_pwm": 2, + "addr_present_pwm": 124, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": 0, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 2.69, + "errors": [] + }, + { + "model": "XD430-T350", + "series": "X_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/x/xd430-t350/", + "torque_points": [ + [0.034, 0.72], + [0.168, 0.83], + [0.303, 0.948], + [0.437, 1.058], + [0.572, 1.169], + [0.714, 1.287], + [0.849, 1.397], + [0.983, 1.508], + [1.118, 1.618], + [1.253, 1.728], + [1.395, 1.839], + [1.529, 1.949], + [1.664, 2.052], + [1.798, 2.163], + [1.933, 2.266], + [2.075, 2.377], + [2.21, 2.48], + [2.344, 2.583], + [2.479, 2.678], + [2.621, 2.782] + ], + "resolution": 4096, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 64, + "addr_operating_mode": 11, + "addr_goal_position": 116, + "len_goal_position": 4, + "addr_goal_velocity": 104, + "len_goal_velocity": 4, + "addr_present_position": 132, + "len_present_position": 4, + "addr_present_velocity": 128, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 123, + "addr_velocity_profile": 112, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 136, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 78, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 76, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 140, + "len_position_trajectory": 4, + "addr_position_p_gain": 78, + "len_position_p_gain": 2, + "addr_position_i_gain": 76, + "len_position_i_gain": 2, + "addr_position_d_gain": 80, + "len_position_d_gain": 2, + "addr_present_current": 126, + "len_present_current": 2, + "addr_goal_pwm": 100, + "len_goal_pwm": 2, + "addr_present_pwm": 124, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": 0, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 2.69, + "errors": [] + }, + { + "model": "XD430-T210", + "series": "X_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/x/xd430-t210/", + "torque_points": [ + [0.024, 2.039], + [0.108, 2.106], + [0.198, 2.183], + [0.288, 2.26], + [0.378, 2.338], + [0.468, 2.421], + [0.558, 2.498], + [0.648, 2.581], + [0.738, 2.67], + [0.828, 2.758], + [0.917, 2.847], + [1.007, 2.935], + [1.097, 3.029], + [1.187, 3.129], + [1.277, 3.228], + [1.367, 3.333], + [1.457, 3.444], + [1.547, 3.56], + [1.636, 3.693], + [1.726, 3.825] + ], + "resolution": 4096, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 64, + "addr_operating_mode": 11, + "addr_goal_position": 116, + "len_goal_position": 4, + "addr_goal_velocity": 104, + "len_goal_velocity": 4, + "addr_present_position": 132, + "len_present_position": 4, + "addr_present_velocity": 128, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 123, + "addr_velocity_profile": 112, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 136, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 78, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 76, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 140, + "len_position_trajectory": 4, + "addr_position_p_gain": 78, + "len_position_p_gain": 2, + "addr_position_i_gain": 76, + "len_position_i_gain": 2, + "addr_position_d_gain": 80, + "len_position_d_gain": 2, + "addr_present_current": 126, + "len_present_current": 2, + "addr_goal_pwm": 100, + "len_goal_pwm": 2, + "addr_present_pwm": 124, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": 0, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 2.69, + "errors": [] + }, + { + "model": "XH540-W150", + "series": "X_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/x/xh540-w150/", + "torque_points": [ + [0.344, 0.536], + [0.557, 0.693], + [0.769, 0.866], + [0.982, 1.04], + [1.194, 1.231], + [1.406, 1.422], + [1.619, 1.613], + [1.831, 1.804], + [2.043, 2.013], + [2.256, 2.204], + [2.468, 2.412], + [2.681, 2.638], + [2.893, 2.846], + [3.105, 3.072], + [3.318, 3.298], + [3.53, 3.541], + [3.742, 3.784], + [3.955, 4.027], + [4.167, 4.288], + [4.38, 4.549] + ], + "resolution": 4096, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 64, + "addr_operating_mode": 11, + "addr_goal_position": 116, + "len_goal_position": 4, + "addr_goal_velocity": 104, + "len_goal_velocity": 4, + "addr_present_position": 132, + "len_present_position": 4, + "addr_present_velocity": 128, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 123, + "addr_velocity_profile": 112, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 136, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 78, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 76, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 140, + "len_position_trajectory": 4, + "addr_position_p_gain": 78, + "len_position_p_gain": 2, + "addr_position_i_gain": 76, + "len_position_i_gain": 2, + "addr_position_d_gain": 80, + "len_position_d_gain": 2, + "addr_present_current": 126, + "len_present_current": 2, + "addr_goal_pwm": 100, + "len_goal_pwm": 2, + "addr_present_pwm": 124, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": 0, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 2.69, + "errors": [] + }, + { + "model": "XH540-W270", + "series": "X_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/x/xh540-w270/", + "torque_points": [ + [0.263, 1.029], + [0.668, 1.192], + [1.073, 1.355], + [1.5, 1.535], + [1.905, 1.715], + [2.31, 1.894], + [2.737, 2.09], + [3.142, 2.286], + [3.547, 2.466], + [3.974, 2.678], + [4.379, 2.874], + [4.829, 3.119], + [5.234, 3.315], + [5.639, 3.528], + [6.066, 3.756], + [6.471, 3.985], + [6.876, 4.214], + [7.304, 4.459], + [7.709, 4.704], + [8.136, 4.949] + ], + "resolution": 4096, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 64, + "addr_operating_mode": 11, + "addr_goal_position": 116, + "len_goal_position": 4, + "addr_goal_velocity": 104, + "len_goal_velocity": 4, + "addr_present_position": 132, + "len_present_position": 4, + "addr_present_velocity": 128, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 123, + "addr_velocity_profile": 112, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 136, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 78, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 76, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 140, + "len_position_trajectory": 4, + "addr_position_p_gain": 78, + "len_position_p_gain": 2, + "addr_position_i_gain": 76, + "len_position_i_gain": 2, + "addr_position_d_gain": 80, + "len_position_d_gain": 2, + "addr_present_current": 126, + "len_present_current": 2, + "addr_goal_pwm": 100, + "len_goal_pwm": 2, + "addr_present_pwm": 124, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": 0, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 2.69, + "errors": [] + }, + { + "model": "XH540-V150", + "series": "X_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/x/xh540-v150/", + "torque_points": [ + [0.555, -0.95], + [0.765, 1.665], + [0.989, 4.861], + [1.214, 8.057], + [1.439, 11.543], + [1.663, 15.03], + [1.888, 18.807], + [2.112, 22.584], + [2.337, 26.361], + [2.562, 30.138], + [2.771, 33.625], + [2.996, 37.692], + [3.221, 41.76], + [3.445, 45.827], + [3.67, 50.185], + [3.894, 54.253], + [4.119, 58.611], + [4.344, 62.969], + [4.568, 67.618], + [4.793, 71.686] + ], + "resolution": 4096, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 64, + "addr_operating_mode": 11, + "addr_goal_position": 116, + "len_goal_position": 4, + "addr_goal_velocity": 104, + "len_goal_velocity": 4, + "addr_present_position": 132, + "len_present_position": 4, + "addr_present_velocity": 128, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 123, + "addr_velocity_profile": 112, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 136, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 78, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 76, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 140, + "len_position_trajectory": 4, + "addr_position_p_gain": 78, + "len_position_p_gain": 2, + "addr_position_i_gain": 76, + "len_position_i_gain": 2, + "addr_position_d_gain": 80, + "len_position_d_gain": 2, + "addr_present_current": 126, + "len_present_current": 2, + "addr_goal_pwm": 100, + "len_goal_pwm": 2, + "addr_present_pwm": 124, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": 0, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 2.69, + "errors": [] + }, + { + "model": "XH540-V270", + "series": "X_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/x/xh540-v270/", + "torque_points": [ + [0.099, -4.282], + [0.501, -2.575], + [0.927, -0.526], + [1.354, 1.523], + [1.756, 3.572], + [2.182, 5.792], + [2.609, 8.011], + [3.036, 10.231], + [3.437, 12.451], + [3.864, 14.841], + [4.29, 17.403], + [4.717, 19.622], + [5.119, 22.013], + [5.545, 24.745], + [5.972, 27.306], + [6.399, 30.209], + [6.8, 32.941], + [7.227, 35.844], + [7.653, 38.917], + [8.08, 41.99] + ], + "resolution": 4096, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 64, + "addr_operating_mode": 11, + "addr_goal_position": 116, + "len_goal_position": 4, + "addr_goal_velocity": 104, + "len_goal_velocity": 4, + "addr_present_position": 132, + "len_present_position": 4, + "addr_present_velocity": 128, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 123, + "addr_velocity_profile": 112, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 136, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 78, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 76, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 140, + "len_position_trajectory": 4, + "addr_position_p_gain": 78, + "len_position_p_gain": 2, + "addr_position_i_gain": 76, + "len_position_i_gain": 2, + "addr_position_d_gain": 80, + "len_position_d_gain": 2, + "addr_present_current": 126, + "len_present_current": 2, + "addr_goal_pwm": 100, + "len_goal_pwm": 2, + "addr_present_pwm": 124, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": 0, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 2.69, + "errors": [] + }, + { + "model": "XH430-W210", + "series": "X_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/x/xh430-w210/", + "torque_points": [ + [0.024, 2.039], + [0.108, 2.106], + [0.198, 2.183], + [0.288, 2.26], + [0.378, 2.338], + [0.468, 2.421], + [0.558, 2.498], + [0.648, 2.581], + [0.738, 2.67], + [0.828, 2.758], + [0.917, 2.847], + [1.007, 2.935], + [1.097, 3.029], + [1.187, 3.129], + [1.277, 3.228], + [1.367, 3.333], + [1.457, 3.444], + [1.547, 3.56], + [1.636, 3.693], + [1.726, 3.825] + ], + "resolution": 4096, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 64, + "addr_operating_mode": 11, + "addr_goal_position": 116, + "len_goal_position": 4, + "addr_goal_velocity": 104, + "len_goal_velocity": 4, + "addr_present_position": 132, + "len_present_position": 4, + "addr_present_velocity": 128, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 123, + "addr_velocity_profile": 112, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 136, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 78, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 76, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 140, + "len_position_trajectory": 4, + "addr_position_p_gain": 78, + "len_position_p_gain": 2, + "addr_position_i_gain": 76, + "len_position_i_gain": 2, + "addr_position_d_gain": 80, + "len_position_d_gain": 2, + "addr_present_current": 126, + "len_present_current": 2, + "addr_goal_pwm": 100, + "len_goal_pwm": 2, + "addr_present_pwm": 124, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": 0, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 2.69, + "errors": [] + }, + { + "model": "XH430-W350", + "series": "X_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/x/xh430-w350/", + "torque_points": [ + [0.034, 0.72], + [0.168, 0.83], + [0.303, 0.948], + [0.437, 1.058], + [0.572, 1.169], + [0.714, 1.287], + [0.849, 1.397], + [0.983, 1.508], + [1.118, 1.618], + [1.253, 1.728], + [1.395, 1.839], + [1.529, 1.949], + [1.664, 2.052], + [1.798, 2.163], + [1.933, 2.266], + [2.075, 2.377], + [2.21, 2.48], + [2.344, 2.583], + [2.479, 2.678], + [2.621, 2.782] + ], + "resolution": 4096, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 64, + "addr_operating_mode": 11, + "addr_goal_position": 116, + "len_goal_position": 4, + "addr_goal_velocity": 104, + "len_goal_velocity": 4, + "addr_present_position": 132, + "len_present_position": 4, + "addr_present_velocity": 128, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 123, + "addr_velocity_profile": 112, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 136, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 78, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 76, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 140, + "len_position_trajectory": 4, + "addr_position_p_gain": 78, + "len_position_p_gain": 2, + "addr_position_i_gain": 76, + "len_position_i_gain": 2, + "addr_position_d_gain": 80, + "len_position_d_gain": 2, + "addr_present_current": 126, + "len_present_current": 2, + "addr_goal_pwm": 100, + "len_goal_pwm": 2, + "addr_present_pwm": 124, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": 0, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 2.69, + "errors": [] + }, + { + "model": "XH430-V210", + "series": "X_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/x/xh430-v210/", + "torque_points": [ + [-0.007, 1.341], + [0.074, 1.37], + [0.155, 1.404], + [0.24, 1.438], + [0.321, 1.472], + [0.406, 1.512], + [0.487, 1.552], + [0.572, 1.592], + [0.653, 1.632], + [0.738, 1.678], + [0.819, 1.723], + [0.904, 1.769], + [0.985, 1.814], + [1.071, 1.866], + [1.156, 1.922], + [1.241, 1.968], + [1.322, 2.019], + [1.407, 2.076], + [1.488, 2.133], + [1.573, 2.19] + ], + "resolution": 4096, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 64, + "addr_operating_mode": 11, + "addr_goal_position": 116, + "len_goal_position": 4, + "addr_goal_velocity": 104, + "len_goal_velocity": 4, + "addr_present_position": 132, + "len_present_position": 4, + "addr_present_velocity": 128, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 123, + "addr_velocity_profile": 112, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 136, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 78, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 76, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 140, + "len_position_trajectory": 4, + "addr_position_p_gain": 78, + "len_position_p_gain": 2, + "addr_position_i_gain": 76, + "len_position_i_gain": 2, + "addr_position_d_gain": 80, + "len_position_d_gain": 2, + "addr_present_current": 126, + "len_present_current": 2, + "addr_goal_pwm": 100, + "len_goal_pwm": 2, + "addr_present_pwm": 124, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": 0, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 1.34, + "errors": [] + }, + { + "model": "XH430-V350", + "series": "X_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/x/xh430-v350/", + "torque_points": [ + [0.002, 0.034], + [0.121, 0.091], + [0.239, 0.171], + [0.358, 0.24], + [0.477, 0.32], + [0.595, 0.388], + [0.714, 0.469], + [0.833, 0.549], + [0.951, 0.629], + [1.07, 0.709], + [1.196, 0.8], + [1.314, 0.88], + [1.433, 0.972], + [1.552, 1.063], + [1.67, 1.155], + [1.796, 1.246], + [1.915, 1.349], + [2.033, 1.441], + [2.152, 1.555], + [2.278, 1.669] + ], + "resolution": 4096, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 64, + "addr_operating_mode": 11, + "addr_goal_position": 116, + "len_goal_position": 4, + "addr_goal_velocity": 104, + "len_goal_velocity": 4, + "addr_present_position": 132, + "len_present_position": 4, + "addr_present_velocity": 128, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 123, + "addr_velocity_profile": 112, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 136, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 78, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 76, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 140, + "len_position_trajectory": 4, + "addr_position_p_gain": 78, + "len_position_p_gain": 2, + "addr_position_i_gain": 76, + "len_position_i_gain": 2, + "addr_position_d_gain": 80, + "len_position_d_gain": 2, + "addr_present_current": 126, + "len_present_current": 2, + "addr_goal_pwm": 100, + "len_goal_pwm": 2, + "addr_present_pwm": 124, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": 0, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 1.34, + "errors": [] + }, + { + "model": "XM540-W150", + "series": "X_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/x/xm540-w150/", + "torque_points": [ + [0.301, 1.973], + [0.555, 2.057], + [0.81, 2.153], + [1.079, 2.261], + [1.334, 2.357], + [1.588, 2.465], + [1.858, 2.585], + [2.112, 2.705], + [2.367, 2.824], + [2.637, 2.944], + [2.891, 3.076], + [3.161, 3.208], + [3.43, 3.352], + [3.685, 3.496], + [3.954, 3.64], + [4.209, 3.796], + [4.463, 3.951], + [4.733, 4.119], + [4.987, 4.287], + [5.257, 4.467] + ], + "resolution": 4096, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 64, + "addr_operating_mode": 11, + "addr_goal_position": 116, + "len_goal_position": 4, + "addr_goal_velocity": 104, + "len_goal_velocity": 4, + "addr_present_position": 132, + "len_present_position": 4, + "addr_present_velocity": 128, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 123, + "addr_velocity_profile": 112, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 136, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 78, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 76, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 140, + "len_position_trajectory": 4, + "addr_position_p_gain": 78, + "len_position_p_gain": 2, + "addr_position_i_gain": 76, + "len_position_i_gain": 2, + "addr_position_d_gain": 80, + "len_position_d_gain": 2, + "addr_present_current": 126, + "len_present_current": 2, + "addr_goal_pwm": 100, + "len_goal_pwm": 2, + "addr_present_pwm": 124, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": 0, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 2.69, + "errors": [] + }, + { + "model": "XM540-W270", + "series": "X_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/x/xm540-w270/", + "torque_points": [ + [0.35, 2.392], + [0.772, 2.454], + [1.194, 2.534], + [1.641, 2.614], + [2.064, 2.693], + [2.511, 2.782], + [2.933, 2.87], + [3.381, 2.968], + [3.803, 3.056], + [4.25, 3.154], + [4.673, 3.251], + [5.12, 3.357], + [5.567, 3.472], + [6.039, 3.587], + [6.461, 3.702], + [6.909, 3.826], + [7.331, 3.941], + [7.778, 4.074], + [8.2, 4.198], + [8.648, 4.34] + ], + "resolution": 4096, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 64, + "addr_operating_mode": 11, + "addr_goal_position": 116, + "len_goal_position": 4, + "addr_goal_velocity": 104, + "len_goal_velocity": 4, + "addr_present_position": 132, + "len_present_position": 4, + "addr_present_velocity": 128, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 123, + "addr_velocity_profile": 112, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 136, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 78, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 76, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 140, + "len_position_trajectory": 4, + "addr_position_p_gain": 78, + "len_position_p_gain": 2, + "addr_position_i_gain": 76, + "len_position_i_gain": 2, + "addr_position_d_gain": 80, + "len_position_d_gain": 2, + "addr_present_current": 126, + "len_present_current": 2, + "addr_goal_pwm": 100, + "len_goal_pwm": 2, + "addr_present_pwm": 124, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": 0, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 2.69, + "errors": [] + }, + { + "model": "XM430-W210", + "series": "X_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/x/xm430-w210/", + "torque_points": [ + [0.051, -10.13], + [0.136, -7.401], + [0.226, -4.369], + [0.316, -1.64], + [0.406, 1.392], + [0.496, 4.424], + [0.586, 7.76], + [0.671, 10.489], + [0.761, 13.521], + [0.851, 16.856], + [0.941, 19.888], + [1.031, 23.224], + [1.121, 26.559], + [1.211, 30.198], + [1.301, 33.533], + [1.391, 37.172], + [1.481, 40.811], + [1.571, 44.753], + [1.661, 48.998], + [1.751, 53.546] + ], + "resolution": 4096, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 64, + "addr_operating_mode": 11, + "addr_goal_position": 116, + "len_goal_position": 4, + "addr_goal_velocity": 104, + "len_goal_velocity": 4, + "addr_present_position": 132, + "len_present_position": 4, + "addr_present_velocity": 128, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 123, + "addr_velocity_profile": 112, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 136, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 78, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 76, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 140, + "len_position_trajectory": 4, + "addr_position_p_gain": 78, + "len_position_p_gain": 2, + "addr_position_i_gain": 76, + "len_position_i_gain": 2, + "addr_position_d_gain": 80, + "len_position_d_gain": 2, + "addr_present_current": 126, + "len_present_current": 2, + "addr_goal_pwm": 100, + "len_goal_pwm": 2, + "addr_present_pwm": 124, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": 0, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 2.69, + "errors": [] + }, + { + "model": "XM430-W350", + "series": "X_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/x/xm430-w350/", + "torque_points": [ + [-0.091, -0.213], + [0.065, -0.066], + [0.229, 0.081], + [0.394, 0.241], + [0.55, 0.387], + [0.715, 0.534], + [0.879, 0.694], + [1.035, 0.841], + [1.2, 1.001], + [1.365, 1.147], + [1.52, 1.294], + [1.685, 1.454], + [1.859, 1.614], + [2.015, 1.761], + [2.179, 1.921], + [2.344, 2.067], + [2.5, 2.214], + [2.664, 2.374], + [2.829, 2.534], + [2.994, 2.681] + ], + "resolution": 4096, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 64, + "addr_operating_mode": 11, + "addr_goal_position": 116, + "len_goal_position": 4, + "addr_goal_velocity": 104, + "len_goal_velocity": 4, + "addr_present_position": 132, + "len_present_position": 4, + "addr_present_velocity": 128, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 123, + "addr_velocity_profile": 112, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 136, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 78, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 76, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 140, + "len_position_trajectory": 4, + "addr_position_p_gain": 78, + "len_position_p_gain": 2, + "addr_position_i_gain": 76, + "len_position_i_gain": 2, + "addr_position_d_gain": 80, + "len_position_d_gain": 2, + "addr_present_current": 126, + "len_present_current": 2, + "addr_goal_pwm": 100, + "len_goal_pwm": 2, + "addr_present_pwm": 124, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": 0, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 2.69, + "errors": [] + }, + { + "model": "XM335-T323", + "series": "X_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/x/xm335-t323/", + "torque_points": [ + [0.044, 2.188], + [0.078, 2.257], + [0.112, 2.326], + [0.148, 2.408], + [0.182, 2.484], + [0.218, 2.559], + [0.252, 2.635], + [0.286, 2.71], + [0.323, 2.793], + [0.357, 2.868], + [0.393, 2.951], + [0.431, 3.047], + [0.467, 3.136], + [0.501, 3.219], + [0.535, 3.308], + [0.572, 3.411], + [0.606, 3.514], + [0.642, 3.638], + [0.676, 3.768], + [0.712, 3.913] + ], + "resolution": 4096, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 64, + "addr_operating_mode": 11, + "addr_goal_position": 116, + "len_goal_position": 4, + "addr_goal_velocity": 104, + "len_goal_velocity": 4, + "addr_present_position": 132, + "len_present_position": 4, + "addr_present_velocity": 128, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 123, + "addr_velocity_profile": 112, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 136, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 78, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 76, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 140, + "len_position_trajectory": 4, + "addr_position_p_gain": 78, + "len_position_p_gain": 2, + "addr_position_i_gain": 76, + "len_position_i_gain": 2, + "addr_position_d_gain": 80, + "len_position_d_gain": 2, + "addr_present_current": 126, + "len_present_current": 2, + "addr_goal_pwm": 100, + "len_goal_pwm": 2, + "addr_present_pwm": 124, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": 1024, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 1.0, + "errors": [] + }, + { + "model": "2XC430-W250", + "series": "X_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/x/2xc430-w250/", + "torque_points": [ + [0.067, 16.435], + [0.121, 16.086], + [0.178, 15.693], + [0.232, 15.301], + [0.288, 14.908], + [0.342, 14.472], + [0.399, 14.079], + [0.456, 13.599], + [0.51, 13.163], + [0.566, 12.683], + [0.623, 12.203], + [0.679, 11.68], + [0.733, 11.2], + [0.79, 10.633], + [0.847, 10.11], + [0.901, 9.542], + [0.957, 8.932], + [1.011, 8.321], + [1.068, 7.667], + [1.125, 7.012] + ], + "resolution": 4096, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 64, + "addr_operating_mode": 11, + "addr_goal_position": 116, + "len_goal_position": 4, + "addr_goal_velocity": 104, + "len_goal_velocity": 4, + "addr_present_position": 132, + "len_present_position": 4, + "addr_present_velocity": 128, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 123, + "addr_velocity_profile": 112, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 136, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 78, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 76, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 140, + "len_position_trajectory": 4, + "addr_position_p_gain": 78, + "len_position_p_gain": 2, + "addr_position_i_gain": 76, + "len_position_i_gain": 2, + "addr_position_d_gain": 80, + "len_position_d_gain": 2, + "addr_present_current": null, + "len_present_current": null, + "addr_goal_pwm": 100, + "len_goal_pwm": 2, + "addr_present_pwm": 124, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": 0, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": null, + "errors": [] + }, + { + "model": "XC430-W150", + "series": "X_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/x/xc430-w150/", + "torque_points": [ + [0.048, 1.811], + [0.084, 1.801], + [0.121, 1.79], + [0.157, 1.779], + [0.194, 1.767], + [0.23, 1.755], + [0.267, 1.743], + [0.303, 1.729], + [0.34, 1.715], + [0.376, 1.7], + [0.413, 1.687], + [0.449, 1.671], + [0.486, 1.654], + [0.522, 1.638], + [0.559, 1.62], + [0.595, 1.603], + [0.632, 1.582], + [0.668, 1.562], + [0.705, 1.54], + [0.744, 1.516] + ], + "resolution": 4096, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 64, + "addr_operating_mode": 11, + "addr_goal_position": 116, + "len_goal_position": 4, + "addr_goal_velocity": 104, + "len_goal_velocity": 4, + "addr_present_position": 132, + "len_present_position": 4, + "addr_present_velocity": 128, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 123, + "addr_velocity_profile": 112, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 136, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 78, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 76, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 140, + "len_position_trajectory": 4, + "addr_position_p_gain": 78, + "len_position_p_gain": 2, + "addr_position_i_gain": 76, + "len_position_i_gain": 2, + "addr_position_d_gain": 80, + "len_position_d_gain": 2, + "addr_present_current": null, + "len_present_current": null, + "addr_goal_pwm": 100, + "len_goal_pwm": 2, + "addr_present_pwm": 124, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": 0, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": null, + "errors": [] + }, + { + "model": "XC430-W240", + "series": "X_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/x/xc430-w240/", + "torque_points": [ + [2.285, 1.995], + [2.19, 2.077], + [2.095, 2.167], + [2.0, 2.257], + [1.906, 2.339], + [1.811, 2.438], + [1.716, 2.528], + [1.621, 2.628], + [1.526, 2.727], + [1.432, 2.826], + [1.328, 2.935], + [1.233, 3.034], + [1.138, 3.143], + [1.044, 3.251], + [0.949, 3.359], + [0.854, 3.468], + [0.759, 3.585], + [0.664, 3.702], + [0.57, 3.82], + [0.475, 3.937] + ], + "resolution": 4096, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 64, + "addr_operating_mode": 11, + "addr_goal_position": 116, + "len_goal_position": 4, + "addr_goal_velocity": 104, + "len_goal_velocity": 4, + "addr_present_position": 132, + "len_present_position": 4, + "addr_present_velocity": 128, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 123, + "addr_velocity_profile": 112, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 136, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 78, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 76, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 140, + "len_position_trajectory": 4, + "addr_position_p_gain": 78, + "len_position_p_gain": 2, + "addr_position_i_gain": 76, + "len_position_i_gain": 2, + "addr_position_d_gain": 80, + "len_position_d_gain": 2, + "addr_present_current": null, + "len_present_current": null, + "addr_goal_pwm": 100, + "len_goal_pwm": 2, + "addr_present_pwm": 124, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": 0, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": null, + "errors": [] + }, + { + "model": "XC430-T150BB", + "series": "X_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/x/xc430-t150bb/", + "torque_points": [ + [0.048, 1.811], + [0.084, 1.801], + [0.121, 1.79], + [0.157, 1.779], + [0.194, 1.767], + [0.23, 1.755], + [0.267, 1.743], + [0.303, 1.729], + [0.34, 1.715], + [0.376, 1.7], + [0.413, 1.687], + [0.449, 1.671], + [0.486, 1.654], + [0.522, 1.638], + [0.559, 1.62], + [0.595, 1.603], + [0.632, 1.582], + [0.668, 1.562], + [0.705, 1.54], + [0.744, 1.516] + ], + "resolution": 4096, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 64, + "addr_operating_mode": 11, + "addr_goal_position": 116, + "len_goal_position": 4, + "addr_goal_velocity": 104, + "len_goal_velocity": 4, + "addr_present_position": 132, + "len_present_position": 4, + "addr_present_velocity": 128, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 123, + "addr_velocity_profile": 112, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 136, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 78, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 76, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 140, + "len_position_trajectory": 4, + "addr_position_p_gain": 78, + "len_position_p_gain": 2, + "addr_position_i_gain": 76, + "len_position_i_gain": 2, + "addr_position_d_gain": 80, + "len_position_d_gain": 2, + "addr_present_current": null, + "len_present_current": null, + "addr_goal_pwm": 100, + "len_goal_pwm": 2, + "addr_present_pwm": 124, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": 0, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": null, + "errors": [] + }, + { + "model": "XC430-T240BB", + "series": "X_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/x/xc430-t240bb/", + "torque_points": [ + [2.285, 1.995], + [2.19, 2.077], + [2.095, 2.167], + [2.0, 2.257], + [1.906, 2.339], + [1.811, 2.438], + [1.716, 2.528], + [1.621, 2.628], + [1.526, 2.727], + [1.432, 2.826], + [1.328, 2.935], + [1.233, 3.034], + [1.138, 3.143], + [1.044, 3.251], + [0.949, 3.359], + [0.854, 3.468], + [0.759, 3.585], + [0.664, 3.702], + [0.57, 3.82], + [0.475, 3.937] + ], + "resolution": 4096, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 64, + "addr_operating_mode": 11, + "addr_goal_position": 116, + "len_goal_position": 4, + "addr_goal_velocity": 104, + "len_goal_velocity": 4, + "addr_present_position": 132, + "len_present_position": 4, + "addr_present_velocity": 128, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 123, + "addr_velocity_profile": 112, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 136, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 78, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 76, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 140, + "len_position_trajectory": 4, + "addr_position_p_gain": 78, + "len_position_p_gain": 2, + "addr_position_i_gain": 76, + "len_position_i_gain": 2, + "addr_position_d_gain": 80, + "len_position_d_gain": 2, + "addr_present_current": null, + "len_present_current": null, + "addr_goal_pwm": 100, + "len_goal_pwm": 2, + "addr_present_pwm": 124, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": 0, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": null, + "errors": [] + }, + { + "model": "XC330-T288", + "series": "X_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/x/xc330-t288/", + "torque_points": [ + [0.036, 2.916], + [0.067, 2.934], + [0.1, 2.955], + [0.132, 2.976], + [0.163, 2.996], + [0.196, 3.017], + [0.229, 3.04], + [0.26, 3.06], + [0.293, 3.083], + [0.326, 3.106], + [0.357, 3.129], + [0.389, 3.153], + [0.422, 3.178], + [0.453, 3.203], + [0.486, 3.232], + [0.519, 3.259], + [0.55, 3.288], + [0.583, 3.319], + [0.616, 3.352], + [0.648, 3.383] + ], + "resolution": 4096, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 64, + "addr_operating_mode": 11, + "addr_goal_position": 116, + "len_goal_position": 4, + "addr_goal_velocity": 104, + "len_goal_velocity": 4, + "addr_present_position": 132, + "len_present_position": 4, + "addr_present_velocity": 128, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 123, + "addr_velocity_profile": 112, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 136, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 78, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 76, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 140, + "len_position_trajectory": 4, + "addr_position_p_gain": 78, + "len_position_p_gain": 2, + "addr_position_i_gain": 76, + "len_position_i_gain": 2, + "addr_position_d_gain": 80, + "len_position_d_gain": 2, + "addr_present_current": 126, + "len_present_current": 2, + "addr_goal_pwm": 100, + "len_goal_pwm": 2, + "addr_present_pwm": 124, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": 0, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 1.0, + "errors": [] + }, + { + "model": "XC330-T181", + "series": "X_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/x/xc330-t181/", + "torque_points": [ + [0.073, 0.165], + [0.087, 0.181], + [0.102, 0.2], + [0.116, 0.218], + [0.131, 0.237], + [0.146, 0.258], + [0.16, 0.277], + [0.175, 0.298], + [0.19, 0.32], + [0.204, 0.341], + [0.219, 0.365], + [0.233, 0.386], + [0.248, 0.41], + [0.263, 0.434], + [0.277, 0.459], + [0.292, 0.485], + [0.307, 0.512], + [0.321, 0.541], + [0.336, 0.571], + [0.351, 0.603] + ], + "resolution": 4096, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 64, + "addr_operating_mode": 11, + "addr_goal_position": 116, + "len_goal_position": 4, + "addr_goal_velocity": 104, + "len_goal_velocity": 4, + "addr_present_position": 132, + "len_present_position": 4, + "addr_present_velocity": 128, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 123, + "addr_velocity_profile": 112, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 136, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 78, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 76, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 140, + "len_position_trajectory": 4, + "addr_position_p_gain": 78, + "len_position_p_gain": 2, + "addr_position_i_gain": 76, + "len_position_i_gain": 2, + "addr_position_d_gain": 80, + "len_position_d_gain": 2, + "addr_present_current": 126, + "len_present_current": 2, + "addr_goal_pwm": 100, + "len_goal_pwm": 2, + "addr_present_pwm": 124, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": 0, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 1.0, + "errors": [] + }, + { + "model": "XC330-M288", + "series": "X_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/x/xc330-m288/", + "torque_points": [ + [0.057, 0.749], + [0.084, 0.768], + [0.111, 0.79], + [0.138, 0.812], + [0.165, 0.836], + [0.192, 0.86], + [0.219, 0.887], + [0.246, 0.913], + [0.273, 0.945], + [0.3, 0.974], + [0.327, 1.007], + [0.354, 1.041], + [0.381, 1.078], + [0.408, 1.114], + [0.435, 1.155], + [0.462, 1.196], + [0.489, 1.239], + [0.516, 1.285], + [0.543, 1.336], + [0.57, 1.387] + ], + "resolution": 4096, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 64, + "addr_operating_mode": 11, + "addr_goal_position": 116, + "len_goal_position": 4, + "addr_goal_velocity": 104, + "len_goal_velocity": 4, + "addr_present_position": 132, + "len_present_position": 4, + "addr_present_velocity": 128, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 123, + "addr_velocity_profile": 112, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 136, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 78, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 76, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 140, + "len_position_trajectory": 4, + "addr_position_p_gain": 78, + "len_position_p_gain": 2, + "addr_position_i_gain": 76, + "len_position_i_gain": 2, + "addr_position_d_gain": 80, + "len_position_d_gain": 2, + "addr_present_current": 126, + "len_present_current": 2, + "addr_goal_pwm": 100, + "len_goal_pwm": 2, + "addr_present_pwm": 124, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": 0, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 1.0, + "errors": [] + }, + { + "model": "XC330-M181", + "series": "X_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/x/xc330-m181/", + "torque_points": [ + [0.068, 0.997], + [0.079, 1.041], + [0.091, 1.092], + [0.103, 1.143], + [0.114, 1.194], + [0.125, 1.252], + [0.137, 1.318], + [0.149, 1.383], + [0.159, 1.441], + [0.171, 1.514], + [0.183, 1.587], + [0.194, 1.66], + [0.205, 1.74], + [0.217, 1.82], + [0.229, 1.907], + [0.24, 1.994], + [0.251, 2.082], + [0.263, 2.184], + [0.275, 2.285], + [0.286, 2.387] + ], + "resolution": 4096, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 64, + "addr_operating_mode": 11, + "addr_goal_position": 116, + "len_goal_position": 4, + "addr_goal_velocity": 104, + "len_goal_velocity": 4, + "addr_present_position": 132, + "len_present_position": 4, + "addr_present_velocity": 128, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 123, + "addr_velocity_profile": 112, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 136, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 78, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 76, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 140, + "len_position_trajectory": 4, + "addr_position_p_gain": 78, + "len_position_p_gain": 2, + "addr_position_i_gain": 76, + "len_position_i_gain": 2, + "addr_position_d_gain": 80, + "len_position_d_gain": 2, + "addr_present_current": 126, + "len_present_current": 2, + "addr_goal_pwm": 100, + "len_goal_pwm": 2, + "addr_present_pwm": 124, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": 0, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 1.0, + "errors": [] + }, + { + "model": "2XL430-W250", + "series": "X_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/x/2xl430-w250/", + "torque_points": [ + [0.027, 11.153], + [0.067, 11.026], + [0.107, 10.882], + [0.147, 10.737], + [0.189, 10.574], + [0.229, 10.429], + [0.269, 10.266], + [0.312, 10.085], + [0.352, 9.904], + [0.392, 9.705], + [0.434, 9.524], + [0.474, 9.325], + [0.514, 9.144], + [0.557, 8.927], + [0.597, 8.71], + [0.637, 8.492], + [0.679, 8.257], + [0.719, 8.04], + [0.759, 7.805], + [0.802, 7.569] + ], + "resolution": 4096, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 64, + "addr_operating_mode": 11, + "addr_goal_position": 116, + "len_goal_position": 4, + "addr_goal_velocity": 104, + "len_goal_velocity": 4, + "addr_present_position": 132, + "len_present_position": 4, + "addr_present_velocity": 128, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 123, + "addr_velocity_profile": 112, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 136, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 78, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 76, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 140, + "len_position_trajectory": 4, + "addr_position_p_gain": 78, + "len_position_p_gain": 2, + "addr_position_i_gain": 76, + "len_position_i_gain": 2, + "addr_position_d_gain": 80, + "len_position_d_gain": 2, + "addr_present_current": null, + "len_present_current": null, + "addr_goal_pwm": 100, + "len_goal_pwm": 2, + "addr_present_pwm": 124, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": 0, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": null, + "errors": [] + }, + { + "model": "XL430-W250", + "series": "X_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/x/xl430-w250/", + "torque_points": [ + [0.051, 0.212], + [0.09, 0.251], + [0.132, 0.301], + [0.174, 0.345], + [0.213, 0.39], + [0.255, 0.429], + [0.3, 0.479], + [0.339, 0.523], + [0.381, 0.568], + [0.423, 0.612], + [0.462, 0.657], + [0.504, 0.701], + [0.548, 0.756], + [0.587, 0.8], + [0.629, 0.844], + [0.671, 0.894], + [0.71, 0.943], + [0.752, 0.993], + [0.794, 1.047], + [0.836, 1.092] + ], + "resolution": 4096, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 64, + "addr_operating_mode": 11, + "addr_goal_position": 116, + "len_goal_position": 4, + "addr_goal_velocity": 104, + "len_goal_velocity": 4, + "addr_present_position": 132, + "len_present_position": 4, + "addr_present_velocity": 128, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 123, + "addr_velocity_profile": 112, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 136, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 78, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 76, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 140, + "len_position_trajectory": 4, + "addr_position_p_gain": 78, + "len_position_p_gain": 2, + "addr_position_i_gain": 76, + "len_position_i_gain": 2, + "addr_position_d_gain": 80, + "len_position_d_gain": 2, + "addr_present_current": null, + "len_present_current": null, + "addr_goal_pwm": 100, + "len_goal_pwm": 2, + "addr_present_pwm": 124, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": 0, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": null, + "errors": [] + }, + { + "model": "XL330-M288", + "series": "X_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/x/xl330-m288/", + "torque_points": null, + "resolution": 4096, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 64, + "addr_operating_mode": 11, + "addr_goal_position": 116, + "len_goal_position": 4, + "addr_goal_velocity": 104, + "len_goal_velocity": 4, + "addr_present_position": 132, + "len_present_position": 4, + "addr_present_velocity": 128, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 123, + "addr_velocity_profile": 112, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 136, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 78, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 76, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 140, + "len_position_trajectory": 4, + "addr_position_p_gain": 78, + "len_position_p_gain": 2, + "addr_position_i_gain": 76, + "len_position_i_gain": 2, + "addr_position_d_gain": 80, + "len_position_d_gain": 2, + "addr_present_current": 126, + "len_present_current": 2, + "addr_goal_pwm": 100, + "len_goal_pwm": 2, + "addr_present_pwm": 124, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": 0, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 1.0, + "errors": [] + }, + { + "model": "XL330-M077", + "series": "X_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/x/xl330-m077/", + "torque_points": null, + "resolution": 4096, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 64, + "addr_operating_mode": 11, + "addr_goal_position": 116, + "len_goal_position": 4, + "addr_goal_velocity": 104, + "len_goal_velocity": 4, + "addr_present_position": 132, + "len_present_position": 4, + "addr_present_velocity": 128, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 123, + "addr_velocity_profile": 112, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 136, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 78, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 76, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 140, + "len_position_trajectory": 4, + "addr_position_p_gain": 78, + "len_position_p_gain": 2, + "addr_position_i_gain": 76, + "len_position_i_gain": 2, + "addr_position_d_gain": 80, + "len_position_d_gain": 2, + "addr_present_current": 126, + "len_present_current": 2, + "addr_goal_pwm": 100, + "len_goal_pwm": 2, + "addr_present_pwm": 124, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": 0, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 1.0, + "errors": [] + }, + { + "model": "MX-12W", + "series": "MX_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/mx/mx-12w/", + "torque_points": null, + "resolution": 4096, + "addr_baud_rate": 4, + "len_baud_rate": 1, + "addr_torque_enable": 24, + "addr_operating_mode": null, + "addr_goal_position": 30, + "len_goal_position": 2, + "addr_goal_velocity": null, + "len_goal_velocity": null, + "addr_present_position": 36, + "len_present_position": 2, + "addr_present_velocity": null, + "len_present_velocity": null, + "addr_moving": 32, + "addr_moving_status": null, + "addr_velocity_profile": 73, + "len_velocity_profile": 1, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": null, + "len_velocity_trajectory": null, + "addr_velocity_p_gain": null, + "len_velocity_p_gain": null, + "addr_velocity_i_gain": null, + "len_velocity_i_gain": null, + "addr_position_trajectory": null, + "len_position_trajectory": null, + "addr_position_p_gain": 28, + "len_position_p_gain": 1, + "addr_position_i_gain": 27, + "len_position_i_gain": 1, + "addr_position_d_gain": 26, + "len_position_d_gain": 1, + "addr_present_current": null, + "len_present_current": null, + "addr_goal_pwm": null, + "len_goal_pwm": null, + "addr_present_pwm": null, + "len_present_pwm": null, + "addr_pwm_limit": null, + "len_pwm_limit": null, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": null, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": null, + "errors": [] + }, + { + "model": "MX-106T/R", + "series": "MX_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/mx/mx-106/", + "torque_points": [ + [0.801, 6.086], + [0.885, 6.13], + [0.977, 6.197], + [1.07, 6.264], + [1.154, 6.32], + [1.247, 6.387], + [1.339, 6.465], + [1.432, 6.543], + [1.516, 6.621], + [1.609, 6.71], + [1.701, 6.799], + [1.794, 6.911], + [1.878, 7.011], + [1.97, 7.122], + [2.063, 7.245], + [2.156, 7.39], + [2.24, 7.524], + [2.332, 7.68], + [2.425, 7.858], + [2.711, 7.1] + ], + "resolution": 4096, + "addr_baud_rate": 4, + "len_baud_rate": 1, + "addr_torque_enable": 24, + "addr_operating_mode": null, + "addr_goal_position": 30, + "len_goal_position": 2, + "addr_goal_velocity": null, + "len_goal_velocity": null, + "addr_present_position": 36, + "len_present_position": 2, + "addr_present_velocity": null, + "len_present_velocity": null, + "addr_moving": 32, + "addr_moving_status": null, + "addr_velocity_profile": 73, + "len_velocity_profile": 1, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": null, + "len_velocity_trajectory": null, + "addr_velocity_p_gain": null, + "len_velocity_p_gain": null, + "addr_velocity_i_gain": null, + "len_velocity_i_gain": null, + "addr_position_trajectory": null, + "len_position_trajectory": null, + "addr_position_p_gain": 28, + "len_position_p_gain": 1, + "addr_position_i_gain": 27, + "len_position_i_gain": 1, + "addr_position_d_gain": 26, + "len_position_d_gain": 1, + "addr_present_current": null, + "len_present_current": null, + "addr_goal_pwm": null, + "len_goal_pwm": null, + "addr_present_pwm": null, + "len_present_pwm": null, + "addr_pwm_limit": null, + "len_pwm_limit": null, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": null, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": null, + "errors": [] + }, + { + "model": "MX-64T/R/AT/AR", + "series": "MX_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/mx/mx-64/", + "torque_points": [ + [3.508, 0.95], + [3.508, 1.063], + [3.507, 1.185], + [3.506, 1.298], + [3.505, 1.429], + [3.504, 1.542], + [3.504, 1.672], + [3.503, 1.785], + [3.502, 1.916], + [3.501, 2.038], + [3.5, 2.159], + [3.5, 2.29], + [3.499, 2.42], + [3.498, 2.542], + [3.497, 2.673], + [3.497, 2.803], + [3.496, 2.934], + [3.495, 3.064], + [3.494, 3.203], + [3.493, 3.334] + ], + "resolution": 4096, + "addr_baud_rate": 4, + "len_baud_rate": 1, + "addr_torque_enable": 24, + "addr_operating_mode": null, + "addr_goal_position": 30, + "len_goal_position": 2, + "addr_goal_velocity": null, + "len_goal_velocity": null, + "addr_present_position": 36, + "len_present_position": 2, + "addr_present_velocity": null, + "len_present_velocity": null, + "addr_moving": 32, + "addr_moving_status": null, + "addr_velocity_profile": 73, + "len_velocity_profile": 1, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": null, + "len_velocity_trajectory": null, + "addr_velocity_p_gain": null, + "len_velocity_p_gain": null, + "addr_velocity_i_gain": null, + "len_velocity_i_gain": null, + "addr_position_trajectory": null, + "len_position_trajectory": null, + "addr_position_p_gain": 28, + "len_position_p_gain": 1, + "addr_position_i_gain": 27, + "len_position_i_gain": 1, + "addr_position_d_gain": 26, + "len_position_d_gain": 1, + "addr_present_current": null, + "len_present_current": null, + "addr_goal_pwm": null, + "len_goal_pwm": null, + "addr_present_pwm": null, + "len_present_pwm": null, + "addr_pwm_limit": null, + "len_pwm_limit": null, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": null, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": null, + "errors": [] + }, + { + "model": "MX-28T/R/AT/AR", + "series": "MX_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/mx/mx-28/", + "torque_points": [ + [-0.303, -0.672], + [-0.22, -0.449], + [-0.132, -0.199], + [-0.049, 0.052], + [0.039, 0.33], + [0.122, 0.608], + [0.209, 0.942], + [0.297, 1.249], + [0.38, 1.555], + [0.468, 1.917], + [0.551, 2.251], + [0.639, 2.613], + [0.722, 2.947], + [0.809, 3.336], + [0.897, 3.726], + [0.98, 4.116], + [1.068, 4.533], + [1.151, 4.951], + [1.239, 5.396], + [1.327, 5.814] + ], + "resolution": 4096, + "addr_baud_rate": 4, + "len_baud_rate": 1, + "addr_torque_enable": 24, + "addr_operating_mode": null, + "addr_goal_position": 30, + "len_goal_position": 2, + "addr_goal_velocity": null, + "len_goal_velocity": null, + "addr_present_position": 36, + "len_present_position": 2, + "addr_present_velocity": null, + "len_present_velocity": null, + "addr_moving": 32, + "addr_moving_status": null, + "addr_velocity_profile": 73, + "len_velocity_profile": 1, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": null, + "len_velocity_trajectory": null, + "addr_velocity_p_gain": null, + "len_velocity_p_gain": null, + "addr_velocity_i_gain": null, + "len_velocity_i_gain": null, + "addr_position_trajectory": null, + "len_position_trajectory": null, + "addr_position_p_gain": 28, + "len_position_p_gain": 1, + "addr_position_i_gain": 27, + "len_position_i_gain": 1, + "addr_position_d_gain": 26, + "len_position_d_gain": 1, + "addr_present_current": null, + "len_present_current": null, + "addr_goal_pwm": null, + "len_goal_pwm": null, + "addr_present_pwm": null, + "len_present_pwm": null, + "addr_pwm_limit": null, + "len_pwm_limit": null, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": null, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": null, + "errors": [] + }, + { + "model": "MX-106T/R(2.0)", + "series": "MX_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/mx/mx-106-2/", + "torque_points": [ + [0.801, 6.086], + [0.885, 6.13], + [0.977, 6.197], + [1.07, 6.264], + [1.154, 6.32], + [1.247, 6.387], + [1.339, 6.465], + [1.432, 6.543], + [1.516, 6.621], + [1.609, 6.71], + [1.701, 6.799], + [1.794, 6.911], + [1.878, 7.011], + [1.97, 7.122], + [2.063, 7.245], + [2.156, 7.39], + [2.24, 7.524], + [2.332, 7.68], + [2.425, 7.858], + [2.711, 7.1] + ], + "resolution": 4096, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 64, + "addr_operating_mode": 11, + "addr_goal_position": 116, + "len_goal_position": 4, + "addr_goal_velocity": 104, + "len_goal_velocity": 4, + "addr_present_position": 132, + "len_present_position": 4, + "addr_present_velocity": 128, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 123, + "addr_velocity_profile": 112, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 136, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 78, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 76, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 140, + "len_position_trajectory": 4, + "addr_position_p_gain": 78, + "len_position_p_gain": 2, + "addr_position_i_gain": 76, + "len_position_i_gain": 2, + "addr_position_d_gain": 80, + "len_position_d_gain": 2, + "addr_present_current": 126, + "len_present_current": 2, + "addr_goal_pwm": 100, + "len_goal_pwm": 2, + "addr_present_pwm": 124, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": 0, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 3.36, + "errors": [] + }, + { + "model": "MX-64T/R/AT/AR(2.0)", + "series": "MX_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/mx/mx-64-2/", + "torque_points": [ + [3.508, 0.95], + [3.508, 1.063], + [3.507, 1.185], + [3.506, 1.298], + [3.505, 1.429], + [3.504, 1.542], + [3.504, 1.672], + [3.503, 1.785], + [3.502, 1.916], + [3.501, 2.038], + [3.5, 2.159], + [3.5, 2.29], + [3.499, 2.42], + [3.498, 2.542], + [3.497, 2.673], + [3.497, 2.803], + [3.496, 2.934], + [3.495, 3.064], + [3.494, 3.203], + [3.493, 3.334] + ], + "resolution": 4096, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 64, + "addr_operating_mode": 11, + "addr_goal_position": 116, + "len_goal_position": 4, + "addr_goal_velocity": 104, + "len_goal_velocity": 4, + "addr_present_position": 132, + "len_present_position": 4, + "addr_present_velocity": 128, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 123, + "addr_velocity_profile": 112, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 136, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 78, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 76, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 140, + "len_position_trajectory": 4, + "addr_position_p_gain": 78, + "len_position_p_gain": 2, + "addr_position_i_gain": 76, + "len_position_i_gain": 2, + "addr_position_d_gain": 80, + "len_position_d_gain": 2, + "addr_present_current": 126, + "len_present_current": 2, + "addr_goal_pwm": 100, + "len_goal_pwm": 2, + "addr_present_pwm": 124, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": 0, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 3.36, + "errors": [] + }, + { + "model": "MX-28T/R/AT/AR(2.0)", + "series": "MX_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/mx/mx-28-2/", + "torque_points": [ + [-0.303, -0.672], + [-0.22, -0.449], + [-0.132, -0.199], + [-0.049, 0.052], + [0.039, 0.33], + [0.122, 0.608], + [0.209, 0.942], + [0.297, 1.249], + [0.38, 1.555], + [0.468, 1.917], + [0.551, 2.251], + [0.639, 2.613], + [0.722, 2.947], + [0.809, 3.336], + [0.897, 3.726], + [0.98, 4.116], + [1.068, 4.533], + [1.151, 4.951], + [1.239, 5.396], + [1.327, 5.814] + ], + "resolution": 4096, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 64, + "addr_operating_mode": 11, + "addr_goal_position": 116, + "len_goal_position": 4, + "addr_goal_velocity": 104, + "len_goal_velocity": 4, + "addr_present_position": 132, + "len_present_position": 4, + "addr_present_velocity": 128, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 123, + "addr_velocity_profile": 112, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 136, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 78, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 76, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 140, + "len_position_trajectory": 4, + "addr_position_p_gain": 78, + "len_position_p_gain": 2, + "addr_position_i_gain": 76, + "len_position_i_gain": 2, + "addr_position_d_gain": 80, + "len_position_d_gain": 2, + "addr_present_current": null, + "len_present_current": null, + "addr_goal_pwm": 100, + "len_goal_pwm": 2, + "addr_present_pwm": 124, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": 0, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": null, + "errors": [] + }, + { + "model": "AX-18A", + "series": "AX_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/ax/ax-18a/", + "torque_points": null, + "resolution": null, + "addr_baud_rate": 4, + "len_baud_rate": 1, + "addr_torque_enable": 24, + "addr_operating_mode": null, + "addr_goal_position": 30, + "len_goal_position": 2, + "addr_goal_velocity": null, + "len_goal_velocity": null, + "addr_present_position": 36, + "len_present_position": 2, + "addr_present_velocity": null, + "len_present_velocity": null, + "addr_moving": 32, + "addr_moving_status": null, + "addr_velocity_profile": null, + "len_velocity_profile": null, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": null, + "len_velocity_trajectory": null, + "addr_velocity_p_gain": null, + "len_velocity_p_gain": null, + "addr_velocity_i_gain": null, + "len_velocity_i_gain": null, + "addr_position_trajectory": null, + "len_position_trajectory": null, + "addr_position_p_gain": null, + "len_position_p_gain": null, + "addr_position_i_gain": null, + "len_position_i_gain": null, + "addr_position_d_gain": null, + "len_position_d_gain": null, + "addr_present_current": null, + "len_present_current": null, + "addr_goal_pwm": null, + "len_goal_pwm": null, + "addr_present_pwm": null, + "len_present_pwm": null, + "addr_pwm_limit": null, + "len_pwm_limit": null, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": null, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": null, + "errors": [] + }, + { + "model": "AX-12A", + "series": "AX_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/ax/ax-12a/", + "torque_points": null, + "resolution": null, + "addr_baud_rate": 4, + "len_baud_rate": 1, + "addr_torque_enable": 24, + "addr_operating_mode": null, + "addr_goal_position": 30, + "len_goal_position": 2, + "addr_goal_velocity": null, + "len_goal_velocity": null, + "addr_present_position": 36, + "len_present_position": 2, + "addr_present_velocity": null, + "len_present_velocity": null, + "addr_moving": 32, + "addr_moving_status": null, + "addr_velocity_profile": null, + "len_velocity_profile": null, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": null, + "len_velocity_trajectory": null, + "addr_velocity_p_gain": null, + "len_velocity_p_gain": null, + "addr_velocity_i_gain": null, + "len_velocity_i_gain": null, + "addr_position_trajectory": null, + "len_position_trajectory": null, + "addr_position_p_gain": null, + "len_position_p_gain": null, + "addr_position_i_gain": null, + "len_position_i_gain": null, + "addr_position_d_gain": null, + "len_position_d_gain": null, + "addr_present_current": null, + "len_present_current": null, + "addr_goal_pwm": null, + "len_goal_pwm": null, + "addr_present_pwm": null, + "len_present_pwm": null, + "addr_pwm_limit": null, + "len_pwm_limit": null, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": null, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": null, + "errors": [] + }, + { + "model": "AX-12W", + "series": "AX_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/ax/ax-12w/", + "torque_points": null, + "resolution": null, + "addr_baud_rate": 4, + "len_baud_rate": 1, + "addr_torque_enable": 24, + "addr_operating_mode": null, + "addr_goal_position": 30, + "len_goal_position": 2, + "addr_goal_velocity": null, + "len_goal_velocity": null, + "addr_present_position": 36, + "len_present_position": 2, + "addr_present_velocity": null, + "len_present_velocity": null, + "addr_moving": 32, + "addr_moving_status": null, + "addr_velocity_profile": null, + "len_velocity_profile": null, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": null, + "len_velocity_trajectory": null, + "addr_velocity_p_gain": null, + "len_velocity_p_gain": null, + "addr_velocity_i_gain": null, + "len_velocity_i_gain": null, + "addr_position_trajectory": null, + "len_position_trajectory": null, + "addr_position_p_gain": null, + "len_position_p_gain": null, + "addr_position_i_gain": null, + "len_position_i_gain": null, + "addr_position_d_gain": null, + "len_position_d_gain": null, + "addr_present_current": null, + "len_present_current": null, + "addr_goal_pwm": null, + "len_goal_pwm": null, + "addr_present_pwm": null, + "len_present_pwm": null, + "addr_pwm_limit": null, + "len_pwm_limit": null, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": null, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": null, + "errors": [] + }, + { + "model": "EX-106+", + "series": "EX_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/ex/ex-106+/", + "torque_points": null, + "resolution": null, + "addr_baud_rate": 4, + "len_baud_rate": 1, + "addr_torque_enable": 24, + "addr_operating_mode": null, + "addr_goal_position": 30, + "len_goal_position": 2, + "addr_goal_velocity": null, + "len_goal_velocity": null, + "addr_present_position": 36, + "len_present_position": 2, + "addr_present_velocity": null, + "len_present_velocity": null, + "addr_moving": 32, + "addr_moving_status": null, + "addr_velocity_profile": null, + "len_velocity_profile": null, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": null, + "len_velocity_trajectory": null, + "addr_velocity_p_gain": null, + "len_velocity_p_gain": null, + "addr_velocity_i_gain": null, + "len_velocity_i_gain": null, + "addr_position_trajectory": null, + "len_position_trajectory": null, + "addr_position_p_gain": null, + "len_position_p_gain": null, + "addr_position_i_gain": null, + "len_position_i_gain": null, + "addr_position_d_gain": null, + "len_position_d_gain": null, + "addr_present_current": null, + "len_present_current": null, + "addr_goal_pwm": null, + "len_goal_pwm": null, + "addr_present_pwm": null, + "len_present_pwm": null, + "addr_pwm_limit": null, + "len_pwm_limit": null, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": null, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": null, + "errors": [] + }, + { + "model": "DX-113", + "series": "DX_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/dx/dx-113/", + "torque_points": null, + "resolution": null, + "addr_baud_rate": 4, + "len_baud_rate": 1, + "addr_torque_enable": 24, + "addr_operating_mode": null, + "addr_goal_position": 30, + "len_goal_position": 2, + "addr_goal_velocity": null, + "len_goal_velocity": null, + "addr_present_position": 36, + "len_present_position": 2, + "addr_present_velocity": null, + "len_present_velocity": null, + "addr_moving": 32, + "addr_moving_status": null, + "addr_velocity_profile": null, + "len_velocity_profile": null, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": null, + "len_velocity_trajectory": null, + "addr_velocity_p_gain": null, + "len_velocity_p_gain": null, + "addr_velocity_i_gain": null, + "len_velocity_i_gain": null, + "addr_position_trajectory": null, + "len_position_trajectory": null, + "addr_position_p_gain": null, + "len_position_p_gain": null, + "addr_position_i_gain": null, + "len_position_i_gain": null, + "addr_position_d_gain": null, + "len_position_d_gain": null, + "addr_present_current": null, + "len_present_current": null, + "addr_goal_pwm": null, + "len_goal_pwm": null, + "addr_present_pwm": null, + "len_present_pwm": null, + "addr_pwm_limit": null, + "len_pwm_limit": null, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": null, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": null, + "errors": [] + }, + { + "model": "DX-116", + "series": "DX_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/dx/dx-116/", + "torque_points": null, + "resolution": null, + "addr_baud_rate": 4, + "len_baud_rate": 1, + "addr_torque_enable": 24, + "addr_operating_mode": null, + "addr_goal_position": 30, + "len_goal_position": 2, + "addr_goal_velocity": null, + "len_goal_velocity": null, + "addr_present_position": 36, + "len_present_position": 2, + "addr_present_velocity": null, + "len_present_velocity": null, + "addr_moving": 32, + "addr_moving_status": null, + "addr_velocity_profile": null, + "len_velocity_profile": null, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": null, + "len_velocity_trajectory": null, + "addr_velocity_p_gain": null, + "len_velocity_p_gain": null, + "addr_velocity_i_gain": null, + "len_velocity_i_gain": null, + "addr_position_trajectory": null, + "len_position_trajectory": null, + "addr_position_p_gain": null, + "len_position_p_gain": null, + "addr_position_i_gain": null, + "len_position_i_gain": null, + "addr_position_d_gain": null, + "len_position_d_gain": null, + "addr_present_current": null, + "len_present_current": null, + "addr_goal_pwm": null, + "len_goal_pwm": null, + "addr_present_pwm": null, + "len_present_pwm": null, + "addr_pwm_limit": null, + "len_pwm_limit": null, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": null, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": null, + "errors": [] + }, + { + "model": "DX-117", + "series": "DX_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/dx/dx-117/", + "torque_points": null, + "resolution": null, + "addr_baud_rate": 4, + "len_baud_rate": 1, + "addr_torque_enable": 24, + "addr_operating_mode": null, + "addr_goal_position": 30, + "len_goal_position": 2, + "addr_goal_velocity": null, + "len_goal_velocity": null, + "addr_present_position": 36, + "len_present_position": 2, + "addr_present_velocity": null, + "len_present_velocity": null, + "addr_moving": 32, + "addr_moving_status": null, + "addr_velocity_profile": null, + "len_velocity_profile": null, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": null, + "len_velocity_trajectory": null, + "addr_velocity_p_gain": null, + "len_velocity_p_gain": null, + "addr_velocity_i_gain": null, + "len_velocity_i_gain": null, + "addr_position_trajectory": null, + "len_position_trajectory": null, + "addr_position_p_gain": null, + "len_position_p_gain": null, + "addr_position_i_gain": null, + "len_position_i_gain": null, + "addr_position_d_gain": null, + "len_position_d_gain": null, + "addr_present_current": null, + "len_present_current": null, + "addr_goal_pwm": null, + "len_goal_pwm": null, + "addr_present_pwm": null, + "len_present_pwm": null, + "addr_pwm_limit": null, + "len_pwm_limit": null, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": null, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": null, + "errors": [] + }, + { + "model": "RX-10", + "series": "RX_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/rx/rx-10/", + "torque_points": null, + "resolution": null, + "addr_baud_rate": 4, + "len_baud_rate": 1, + "addr_torque_enable": 24, + "addr_operating_mode": null, + "addr_goal_position": 30, + "len_goal_position": 2, + "addr_goal_velocity": null, + "len_goal_velocity": null, + "addr_present_position": 36, + "len_present_position": 2, + "addr_present_velocity": null, + "len_present_velocity": null, + "addr_moving": 32, + "addr_moving_status": null, + "addr_velocity_profile": null, + "len_velocity_profile": null, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": null, + "len_velocity_trajectory": null, + "addr_velocity_p_gain": null, + "len_velocity_p_gain": null, + "addr_velocity_i_gain": null, + "len_velocity_i_gain": null, + "addr_position_trajectory": null, + "len_position_trajectory": null, + "addr_position_p_gain": null, + "len_position_p_gain": null, + "addr_position_i_gain": null, + "len_position_i_gain": null, + "addr_position_d_gain": null, + "len_position_d_gain": null, + "addr_present_current": null, + "len_present_current": null, + "addr_goal_pwm": null, + "len_goal_pwm": null, + "addr_present_pwm": null, + "len_present_pwm": null, + "addr_pwm_limit": null, + "len_pwm_limit": null, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": null, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": null, + "errors": [] + }, + { + "model": "RX-24F", + "series": "RX_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/rx/rx-24f/", + "torque_points": null, + "resolution": null, + "addr_baud_rate": 4, + "len_baud_rate": 1, + "addr_torque_enable": 24, + "addr_operating_mode": null, + "addr_goal_position": 30, + "len_goal_position": 2, + "addr_goal_velocity": null, + "len_goal_velocity": null, + "addr_present_position": 36, + "len_present_position": 2, + "addr_present_velocity": null, + "len_present_velocity": null, + "addr_moving": 32, + "addr_moving_status": null, + "addr_velocity_profile": null, + "len_velocity_profile": null, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": null, + "len_velocity_trajectory": null, + "addr_velocity_p_gain": null, + "len_velocity_p_gain": null, + "addr_velocity_i_gain": null, + "len_velocity_i_gain": null, + "addr_position_trajectory": null, + "len_position_trajectory": null, + "addr_position_p_gain": null, + "len_position_p_gain": null, + "addr_position_i_gain": null, + "len_position_i_gain": null, + "addr_position_d_gain": null, + "len_position_d_gain": null, + "addr_present_current": null, + "len_present_current": null, + "addr_goal_pwm": null, + "len_goal_pwm": null, + "addr_present_pwm": null, + "len_present_pwm": null, + "addr_pwm_limit": null, + "len_pwm_limit": null, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": null, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": null, + "errors": [] + }, + { + "model": "RX-28", + "series": "RX_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/rx/rx-28/", + "torque_points": null, + "resolution": null, + "addr_baud_rate": 4, + "len_baud_rate": 1, + "addr_torque_enable": 24, + "addr_operating_mode": null, + "addr_goal_position": 30, + "len_goal_position": 2, + "addr_goal_velocity": null, + "len_goal_velocity": null, + "addr_present_position": 36, + "len_present_position": 2, + "addr_present_velocity": null, + "len_present_velocity": null, + "addr_moving": 32, + "addr_moving_status": null, + "addr_velocity_profile": null, + "len_velocity_profile": null, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": null, + "len_velocity_trajectory": null, + "addr_velocity_p_gain": null, + "len_velocity_p_gain": null, + "addr_velocity_i_gain": null, + "len_velocity_i_gain": null, + "addr_position_trajectory": null, + "len_position_trajectory": null, + "addr_position_p_gain": null, + "len_position_p_gain": null, + "addr_position_i_gain": null, + "len_position_i_gain": null, + "addr_position_d_gain": null, + "len_position_d_gain": null, + "addr_present_current": null, + "len_present_current": null, + "addr_goal_pwm": null, + "len_goal_pwm": null, + "addr_present_pwm": null, + "len_present_pwm": null, + "addr_pwm_limit": null, + "len_pwm_limit": null, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": null, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": null, + "errors": [] + }, + { + "model": "RX-64", + "series": "RX_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/rx/rx-64/", + "torque_points": null, + "resolution": null, + "addr_baud_rate": 4, + "len_baud_rate": 1, + "addr_torque_enable": 24, + "addr_operating_mode": null, + "addr_goal_position": 30, + "len_goal_position": 2, + "addr_goal_velocity": null, + "len_goal_velocity": null, + "addr_present_position": 36, + "len_present_position": 2, + "addr_present_velocity": null, + "len_present_velocity": null, + "addr_moving": 32, + "addr_moving_status": null, + "addr_velocity_profile": null, + "len_velocity_profile": null, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": null, + "len_velocity_trajectory": null, + "addr_velocity_p_gain": null, + "len_velocity_p_gain": null, + "addr_velocity_i_gain": null, + "len_velocity_i_gain": null, + "addr_position_trajectory": null, + "len_position_trajectory": null, + "addr_position_p_gain": null, + "len_position_p_gain": null, + "addr_position_i_gain": null, + "len_position_i_gain": null, + "addr_position_d_gain": null, + "len_position_d_gain": null, + "addr_present_current": null, + "len_present_current": null, + "addr_goal_pwm": null, + "len_goal_pwm": null, + "addr_present_pwm": null, + "len_present_pwm": null, + "addr_pwm_limit": null, + "len_pwm_limit": null, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": null, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": null, + "errors": [] + }, + { + "model": "L54-30-S400-R", + "series": "PRO_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/pro/l54-30-s400-r/", + "torque_points": [ + [0.176, 3.034], + [0.926, 3.098], + [1.726, 3.179], + [2.526, 3.259], + [3.326, 3.34], + [4.127, 3.42], + [4.927, 3.517], + [5.727, 3.597], + [6.527, 3.694], + [7.327, 3.79], + [8.128, 3.903], + [8.928, 4.0], + [9.728, 4.112], + [10.528, 4.241], + [11.328, 4.354], + [12.129, 4.499], + [12.929, 4.628], + [13.779, 4.805], + [14.579, 4.982], + [15.379, 5.191] + ], + "resolution": 288395, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 562, + "addr_operating_mode": 11, + "addr_goal_position": 596, + "len_goal_position": 4, + "addr_goal_velocity": 600, + "len_goal_velocity": 4, + "addr_present_position": 611, + "len_present_position": 4, + "addr_present_velocity": 615, + "len_present_velocity": 4, + "addr_moving": 17, + "addr_moving_status": null, + "addr_velocity_profile": 606, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": null, + "len_velocity_trajectory": null, + "addr_velocity_p_gain": 588, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 586, + "len_velocity_i_gain": 2, + "addr_position_trajectory": null, + "len_position_trajectory": null, + "addr_position_p_gain": 588, + "len_position_p_gain": 2, + "addr_position_i_gain": 586, + "len_position_i_gain": 2, + "addr_position_d_gain": null, + "len_position_d_gain": null, + "addr_present_current": 621, + "len_present_current": 2, + "addr_goal_pwm": null, + "len_goal_pwm": null, + "addr_present_pwm": null, + "len_present_pwm": null, + "addr_pwm_limit": null, + "len_pwm_limit": null, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": -144197, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": null, + "errors": [] + }, + { + "model": "L54-30-S500-R", + "series": "PRO_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/pro/l54-30-s500-r/", + "torque_points": [ + [1.27, 2.953], + [2.346, 3.034], + [3.422, 3.098], + [4.498, 3.179], + [5.637, 3.259], + [6.713, 3.356], + [7.789, 3.436], + [8.865, 3.517], + [10.005, 3.613], + [11.081, 3.71], + [12.157, 3.806], + [13.233, 3.919], + [14.373, 4.032], + [15.449, 4.128], + [16.651, 4.273], + [17.727, 4.386], + [18.867, 4.515], + [19.943, 4.66], + [21.019, 4.805], + [22.158, 4.982] + ], + "resolution": 361384, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 562, + "addr_operating_mode": 11, + "addr_goal_position": 596, + "len_goal_position": 4, + "addr_goal_velocity": 600, + "len_goal_velocity": 4, + "addr_present_position": 611, + "len_present_position": 4, + "addr_present_velocity": 615, + "len_present_velocity": 4, + "addr_moving": 17, + "addr_moving_status": null, + "addr_velocity_profile": 606, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": null, + "len_velocity_trajectory": null, + "addr_velocity_p_gain": 588, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 586, + "len_velocity_i_gain": 2, + "addr_position_trajectory": null, + "len_position_trajectory": null, + "addr_position_p_gain": 588, + "len_position_p_gain": 2, + "addr_position_i_gain": 586, + "len_position_i_gain": 2, + "addr_position_d_gain": null, + "len_position_d_gain": null, + "addr_present_current": 621, + "len_present_current": 2, + "addr_goal_pwm": null, + "len_goal_pwm": null, + "addr_present_pwm": null, + "len_present_pwm": null, + "addr_pwm_limit": null, + "len_pwm_limit": null, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": -180692, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": null, + "errors": [] + }, + { + "model": "L54-50-S290-R", + "series": "PRO_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/pro/l54-50-s290-r/", + "torque_points": [ + [1.38, 3.066], + [2.539, 3.162], + [3.698, 3.259], + [4.857, 3.356], + [6.088, 3.484], + [7.247, 3.597], + [8.406, 3.71], + [9.565, 3.823], + [10.796, 3.967], + [11.955, 4.08], + [13.114, 4.225], + [14.273, 4.354], + [15.504, 4.515], + [16.663, 4.66], + [17.822, 4.821], + [18.981, 4.998], + [20.212, 5.191], + [21.371, 5.384], + [22.53, 5.593], + [23.762, 5.835] + ], + "resolution": 207692, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 562, + "addr_operating_mode": 11, + "addr_goal_position": 596, + "len_goal_position": 4, + "addr_goal_velocity": 600, + "len_goal_velocity": 4, + "addr_present_position": 611, + "len_present_position": 4, + "addr_present_velocity": 615, + "len_present_velocity": 4, + "addr_moving": 17, + "addr_moving_status": null, + "addr_velocity_profile": 606, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": null, + "len_velocity_trajectory": null, + "addr_velocity_p_gain": 588, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 586, + "len_velocity_i_gain": 2, + "addr_position_trajectory": null, + "len_position_trajectory": null, + "addr_position_p_gain": 588, + "len_position_p_gain": 2, + "addr_position_i_gain": 586, + "len_position_i_gain": 2, + "addr_position_d_gain": null, + "len_position_d_gain": null, + "addr_present_current": 621, + "len_present_current": 2, + "addr_goal_pwm": null, + "len_goal_pwm": null, + "addr_present_pwm": null, + "len_present_pwm": null, + "addr_pwm_limit": null, + "len_pwm_limit": null, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": -103846, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": null, + "errors": [] + }, + { + "model": "L54-50-S500-R", + "series": "PRO_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/pro/l54-50-s500-r/", + "torque_points": [ + [1.169, 3.737], + [3.108, 3.798], + [5.16, 3.868], + [7.099, 3.93], + [9.151, 4.009], + [11.089, 4.079], + [13.142, 4.149], + [15.08, 4.228], + [17.132, 4.307], + [19.071, 4.395], + [21.123, 4.482], + [23.061, 4.57], + [25.114, 4.667], + [27.052, 4.763], + [29.105, 4.877], + [31.043, 4.991], + [33.095, 5.114], + [35.034, 5.254], + [37.086, 5.421], + [39.138, 5.657] + ], + "resolution": 361384, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 562, + "addr_operating_mode": 11, + "addr_goal_position": 596, + "len_goal_position": 4, + "addr_goal_velocity": 600, + "len_goal_velocity": 4, + "addr_present_position": 611, + "len_present_position": 4, + "addr_present_velocity": 615, + "len_present_velocity": 4, + "addr_moving": 17, + "addr_moving_status": null, + "addr_velocity_profile": 606, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": null, + "len_velocity_trajectory": null, + "addr_velocity_p_gain": 588, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 586, + "len_velocity_i_gain": 2, + "addr_position_trajectory": null, + "len_position_trajectory": null, + "addr_position_p_gain": 588, + "len_position_p_gain": 2, + "addr_position_i_gain": 586, + "len_position_i_gain": 2, + "addr_position_d_gain": null, + "len_position_d_gain": null, + "addr_present_current": 621, + "len_present_current": 2, + "addr_goal_pwm": null, + "len_goal_pwm": null, + "addr_present_pwm": null, + "len_present_pwm": null, + "addr_pwm_limit": null, + "len_pwm_limit": null, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": -180692, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": null, + "errors": [] + }, + { + "model": "L42-10-S300-R", + "series": "PRO_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/pro/l42-10-s300-r/", + "torque_points": [ + [0.537, 2.535], + [0.73, 2.567], + [0.937, 2.631], + [1.145, 2.663], + [1.338, 2.712], + [1.545, 2.76], + [1.753, 2.824], + [1.946, 2.857], + [2.153, 2.905], + [2.361, 2.953], + [2.554, 3.001], + [2.761, 3.05], + [2.969, 3.098], + [3.162, 3.146], + [3.369, 3.195], + [3.577, 3.243], + [3.77, 3.291], + [3.977, 3.34], + [4.2, 3.404], + [4.408, 3.452] + ], + "resolution": 4096, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 562, + "addr_operating_mode": 11, + "addr_goal_position": 596, + "len_goal_position": 4, + "addr_goal_velocity": 600, + "len_goal_velocity": 4, + "addr_present_position": 611, + "len_present_position": 4, + "addr_present_velocity": 615, + "len_present_velocity": 4, + "addr_moving": 17, + "addr_moving_status": null, + "addr_velocity_profile": 606, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": null, + "len_velocity_trajectory": null, + "addr_velocity_p_gain": 588, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 586, + "len_velocity_i_gain": 2, + "addr_position_trajectory": null, + "len_position_trajectory": null, + "addr_position_p_gain": 588, + "len_position_p_gain": 2, + "addr_position_i_gain": 586, + "len_position_i_gain": 2, + "addr_position_d_gain": null, + "len_position_d_gain": null, + "addr_present_current": 621, + "len_present_current": 2, + "addr_goal_pwm": null, + "len_goal_pwm": null, + "addr_present_pwm": null, + "len_present_pwm": null, + "addr_pwm_limit": null, + "len_pwm_limit": null, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": -2047, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": null, + "errors": [] + }, + { + "model": "M54-40-S250-R", + "series": "PRO_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/pro/m54-40-s250-r/", + "torque_points": [ + [-1.151, 2.128], + [-0.349, 2.247], + [0.453, 2.355], + [1.322, 2.475], + [2.124, 2.594], + [2.992, 2.726], + [3.794, 2.845], + [4.663, 2.977], + [5.465, 3.108], + [6.334, 3.252], + [7.136, 3.383], + [8.005, 3.539], + [8.807, 3.682], + [9.676, 3.838], + [10.478, 3.993], + [11.347, 4.161], + [12.149, 4.328], + [13.018, 4.519], + [13.82, 4.723], + [14.688, 4.938] + ], + "resolution": 251417, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 562, + "addr_operating_mode": 11, + "addr_goal_position": 596, + "len_goal_position": 4, + "addr_goal_velocity": 600, + "len_goal_velocity": 4, + "addr_present_position": 611, + "len_present_position": 4, + "addr_present_velocity": 615, + "len_present_velocity": 4, + "addr_moving": 17, + "addr_moving_status": null, + "addr_velocity_profile": 606, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": null, + "len_velocity_trajectory": null, + "addr_velocity_p_gain": 588, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 586, + "len_velocity_i_gain": 2, + "addr_position_trajectory": null, + "len_position_trajectory": null, + "addr_position_p_gain": 588, + "len_position_p_gain": 2, + "addr_position_i_gain": 586, + "len_position_i_gain": 2, + "addr_position_d_gain": null, + "len_position_d_gain": null, + "addr_present_current": 621, + "len_present_current": 2, + "addr_goal_pwm": null, + "len_goal_pwm": null, + "addr_present_pwm": null, + "len_present_pwm": null, + "addr_pwm_limit": null, + "len_pwm_limit": null, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": -125708, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": null, + "errors": [] + }, + { + "model": "M54-60-S250-R", + "series": "PRO_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/pro/m54-60-s250-r/", + "torque_points": [ + [-1.813, 1.17], + [0.304, 1.467], + [2.555, 1.763], + [4.805, 2.06], + [6.923, 2.357], + [9.173, 2.696], + [11.423, 3.077], + [13.541, 3.374], + [15.792, 3.755], + [18.042, 4.136], + [20.16, 4.518], + [22.41, 4.942], + [24.66, 5.365], + [26.91, 5.832], + [29.161, 6.34], + [31.543, 6.891], + [33.661, 7.4], + [35.911, 8.035], + [38.162, 8.798], + [40.412, 9.688] + ], + "resolution": 251417, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 562, + "addr_operating_mode": 11, + "addr_goal_position": 596, + "len_goal_position": 4, + "addr_goal_velocity": 600, + "len_goal_velocity": 4, + "addr_present_position": 611, + "len_present_position": 4, + "addr_present_velocity": 615, + "len_present_velocity": 4, + "addr_moving": 17, + "addr_moving_status": null, + "addr_velocity_profile": 606, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": null, + "len_velocity_trajectory": null, + "addr_velocity_p_gain": 588, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 586, + "len_velocity_i_gain": 2, + "addr_position_trajectory": null, + "len_position_trajectory": null, + "addr_position_p_gain": 588, + "len_position_p_gain": 2, + "addr_position_i_gain": 586, + "len_position_i_gain": 2, + "addr_position_d_gain": null, + "len_position_d_gain": null, + "addr_present_current": 621, + "len_present_current": 2, + "addr_goal_pwm": null, + "len_goal_pwm": null, + "addr_present_pwm": null, + "len_present_pwm": null, + "addr_pwm_limit": null, + "len_pwm_limit": null, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": -125708, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": null, + "errors": [] + }, + { + "model": "M42-10-S260-R", + "series": "PRO_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/pro/m42-10-s260-r/", + "torque_points": [ + [12.733, 49.832], + [12.461, 49.836], + [12.189, 49.84], + [11.917, 49.844], + [11.621, 49.849], + [11.349, 49.855], + [11.077, 49.86], + [10.781, 49.866], + [10.509, 49.871], + [10.237, 49.879], + [9.941, 49.886], + [9.669, 49.894], + [9.397, 49.903], + [9.101, 49.912], + [8.829, 49.922], + [8.557, 49.933], + [8.261, 49.946], + [7.989, 49.959], + [7.717, 49.974], + [7.421, 49.993] + ], + "resolution": 263187, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 562, + "addr_operating_mode": 11, + "addr_goal_position": 596, + "len_goal_position": 4, + "addr_goal_velocity": 600, + "len_goal_velocity": 4, + "addr_present_position": 611, + "len_present_position": 4, + "addr_present_velocity": 615, + "len_present_velocity": 4, + "addr_moving": 17, + "addr_moving_status": null, + "addr_velocity_profile": 606, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": null, + "len_velocity_trajectory": null, + "addr_velocity_p_gain": 588, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 586, + "len_velocity_i_gain": 2, + "addr_position_trajectory": null, + "len_position_trajectory": null, + "addr_position_p_gain": 588, + "len_position_p_gain": 2, + "addr_position_i_gain": 586, + "len_position_i_gain": 2, + "addr_position_d_gain": null, + "len_position_d_gain": null, + "addr_present_current": 621, + "len_present_current": 2, + "addr_goal_pwm": null, + "len_goal_pwm": null, + "addr_present_pwm": null, + "len_present_pwm": null, + "addr_pwm_limit": null, + "len_pwm_limit": null, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": -131593, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": null, + "errors": [] + }, + { + "model": "M54-40-S250-R(A)", + "series": "PRO_A_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/pro/m54-40-s250-ra/", + "torque_points": [ + [-1.151, 2.128], + [-0.349, 2.247], + [0.453, 2.355], + [1.322, 2.475], + [2.124, 2.594], + [2.992, 2.726], + [3.794, 2.845], + [4.663, 2.977], + [5.465, 3.108], + [6.334, 3.252], + [7.136, 3.383], + [8.005, 3.539], + [8.807, 3.682], + [9.676, 3.838], + [10.478, 3.993], + [11.347, 4.161], + [12.149, 4.328], + [13.018, 4.519], + [13.82, 4.723], + [14.688, 4.938] + ], + "resolution": 502834, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 512, + "addr_operating_mode": 11, + "addr_goal_position": 564, + "len_goal_position": 4, + "addr_goal_velocity": 552, + "len_goal_velocity": 4, + "addr_present_position": 580, + "len_present_position": 4, + "addr_present_velocity": 576, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 571, + "addr_velocity_profile": 560, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 584, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 526, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 524, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 588, + "len_position_trajectory": 4, + "addr_position_p_gain": 526, + "len_position_p_gain": 2, + "addr_position_i_gain": 524, + "len_position_i_gain": 2, + "addr_position_d_gain": 528, + "len_position_d_gain": 2, + "addr_present_current": 574, + "len_present_current": 2, + "addr_goal_pwm": 548, + "len_goal_pwm": 2, + "addr_present_pwm": 572, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": -251173, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 1.0, + "errors": [] + }, + { + "model": "M54-60-S250-R(A)", + "series": "PRO_A_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/pro/m54-60-s250-ra/", + "torque_points": [ + [-1.813, 1.17], + [0.304, 1.467], + [2.555, 1.763], + [4.805, 2.06], + [6.923, 2.357], + [9.173, 2.696], + [11.423, 3.077], + [13.541, 3.374], + [15.792, 3.755], + [18.042, 4.136], + [20.16, 4.518], + [22.41, 4.942], + [24.66, 5.365], + [26.91, 5.832], + [29.161, 6.34], + [31.543, 6.891], + [33.661, 7.4], + [35.911, 8.035], + [38.162, 8.798], + [40.412, 9.688] + ], + "resolution": 502834, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 512, + "addr_operating_mode": 11, + "addr_goal_position": 564, + "len_goal_position": 4, + "addr_goal_velocity": 552, + "len_goal_velocity": 4, + "addr_present_position": 580, + "len_present_position": 4, + "addr_present_velocity": 576, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 571, + "addr_velocity_profile": 560, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 584, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 526, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 524, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 588, + "len_position_trajectory": 4, + "addr_position_p_gain": 526, + "len_position_p_gain": 2, + "addr_position_i_gain": 524, + "len_position_i_gain": 2, + "addr_position_d_gain": 528, + "len_position_d_gain": 2, + "addr_present_current": 574, + "len_present_current": 2, + "addr_goal_pwm": 548, + "len_goal_pwm": 2, + "addr_present_pwm": 572, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": -251173, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 1.0, + "errors": [] + }, + { + "model": "M42-10-S260-R(A)", + "series": "PRO_A_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/pro/m42-10-s260-ra/", + "torque_points": [ + [12.733, 49.832], + [12.461, 49.836], + [12.189, 49.84], + [11.917, 49.844], + [11.621, 49.849], + [11.349, 49.855], + [11.077, 49.86], + [10.781, 49.866], + [10.509, 49.871], + [10.237, 49.879], + [9.941, 49.886], + [9.669, 49.894], + [9.397, 49.903], + [9.101, 49.912], + [8.829, 49.922], + [8.557, 49.933], + [8.261, 49.946], + [7.989, 49.959], + [7.717, 49.974], + [7.421, 49.993] + ], + "resolution": 526375, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 512, + "addr_operating_mode": 11, + "addr_goal_position": 564, + "len_goal_position": 4, + "addr_goal_velocity": 552, + "len_goal_velocity": 4, + "addr_present_position": 580, + "len_present_position": 4, + "addr_present_velocity": 576, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 571, + "addr_velocity_profile": 560, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 584, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 526, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 524, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 588, + "len_position_trajectory": 4, + "addr_position_p_gain": 526, + "len_position_p_gain": 2, + "addr_position_i_gain": 524, + "len_position_i_gain": 2, + "addr_position_d_gain": 528, + "len_position_d_gain": 2, + "addr_present_current": 574, + "len_present_current": 2, + "addr_goal_pwm": 548, + "len_goal_pwm": 2, + "addr_present_pwm": 572, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": -262931, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 1.0, + "errors": [] + }, + { + "model": "H54-100-S500-R", + "series": "PRO_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/pro/h54-100-s500-r/", + "torque_points": [ + [-0.305, 1.42], + [3.21, 2.01], + [6.725, 2.601], + [10.24, 3.191], + [13.755, 3.782], + [17.49, 4.457], + [21.005, 5.089], + [24.521, 5.68], + [28.036, 6.313], + [31.551, 6.987], + [35.286, 7.662], + [38.801, 8.337], + [42.316, 9.012], + [45.831, 9.687], + [49.347, 10.362], + [53.082, 11.121], + [56.597, 11.881], + [60.112, 12.64], + [63.627, 13.483], + [67.362, 14.411] + ], + "resolution": 501923, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 562, + "addr_operating_mode": 11, + "addr_goal_position": 596, + "len_goal_position": 4, + "addr_goal_velocity": 600, + "len_goal_velocity": 4, + "addr_present_position": 611, + "len_present_position": 4, + "addr_present_velocity": 615, + "len_present_velocity": 4, + "addr_moving": 17, + "addr_moving_status": null, + "addr_velocity_profile": 606, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": null, + "len_velocity_trajectory": null, + "addr_velocity_p_gain": 588, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 586, + "len_velocity_i_gain": 2, + "addr_position_trajectory": null, + "len_position_trajectory": null, + "addr_position_p_gain": 588, + "len_position_p_gain": 2, + "addr_position_i_gain": 586, + "len_position_i_gain": 2, + "addr_position_d_gain": null, + "len_position_d_gain": null, + "addr_present_current": 621, + "len_present_current": 2, + "addr_goal_pwm": null, + "len_goal_pwm": null, + "addr_present_pwm": null, + "len_present_pwm": null, + "addr_pwm_limit": null, + "len_pwm_limit": null, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": -250961, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": null, + "errors": [] + }, + { + "model": "H54-200-S500-R", + "series": "PRO_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/pro/h54-200-s500-r/", + "torque_points": [ + [-0.745, 1.715], + [3.21, 2.221], + [7.164, 2.769], + [11.119, 3.318], + [15.074, 3.908], + [19.028, 4.499], + [22.983, 5.089], + [26.937, 5.68], + [30.892, 6.27], + [34.847, 6.903], + [38.801, 7.536], + [42.756, 8.169], + [46.71, 8.843], + [50.665, 9.518], + [54.619, 10.193], + [58.574, 10.91], + [62.529, 11.627], + [66.483, 12.429], + [70.438, 13.23], + [74.612, 14.074] + ], + "resolution": 501923, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 562, + "addr_operating_mode": 11, + "addr_goal_position": 596, + "len_goal_position": 4, + "addr_goal_velocity": 600, + "len_goal_velocity": 4, + "addr_present_position": 611, + "len_present_position": 4, + "addr_present_velocity": 615, + "len_present_velocity": 4, + "addr_moving": 17, + "addr_moving_status": null, + "addr_velocity_profile": 606, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": null, + "len_velocity_trajectory": null, + "addr_velocity_p_gain": 588, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 586, + "len_velocity_i_gain": 2, + "addr_position_trajectory": null, + "len_position_trajectory": null, + "addr_position_p_gain": 588, + "len_position_p_gain": 2, + "addr_position_i_gain": 586, + "len_position_i_gain": 2, + "addr_position_d_gain": null, + "len_position_d_gain": null, + "addr_present_current": 621, + "len_present_current": 2, + "addr_goal_pwm": null, + "len_goal_pwm": null, + "addr_present_pwm": null, + "len_present_pwm": null, + "addr_pwm_limit": null, + "len_pwm_limit": null, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": -250961, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": null, + "errors": [] + }, + { + "model": "H42-20-S300-R", + "series": "PRO_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/pro/h42-20-s300-r/", + "torque_points": [ + [-0.567, 1.867], + [0.525, 2.038], + [1.695, 2.257], + [2.865, 2.477], + [4.035, 2.697], + [5.205, 2.917], + [6.376, 3.136], + [7.546, 3.356], + [8.716, 3.6], + [9.886, 3.82], + [11.056, 4.064], + [12.226, 4.284], + [13.396, 4.528], + [14.566, 4.772], + [15.737, 5.04], + [16.907, 5.285], + [18.077, 5.553], + [19.247, 5.846], + [20.417, 6.163], + [21.587, 6.578] + ], + "resolution": 303750, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 562, + "addr_operating_mode": 11, + "addr_goal_position": 596, + "len_goal_position": 4, + "addr_goal_velocity": 600, + "len_goal_velocity": 4, + "addr_present_position": 611, + "len_present_position": 4, + "addr_present_velocity": 615, + "len_present_velocity": 4, + "addr_moving": 17, + "addr_moving_status": null, + "addr_velocity_profile": 606, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": null, + "len_velocity_trajectory": null, + "addr_velocity_p_gain": 588, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 586, + "len_velocity_i_gain": 2, + "addr_position_trajectory": null, + "len_position_trajectory": null, + "addr_position_p_gain": 588, + "len_position_p_gain": 2, + "addr_position_i_gain": 586, + "len_position_i_gain": 2, + "addr_position_d_gain": null, + "len_position_d_gain": null, + "addr_present_current": 621, + "len_present_current": 2, + "addr_goal_pwm": null, + "len_goal_pwm": null, + "addr_present_pwm": null, + "len_present_pwm": null, + "addr_pwm_limit": null, + "len_pwm_limit": null, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": -151875, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": null, + "errors": [] + }, + { + "model": "H54-100-S500-R(A)", + "series": "PRO_A_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/pro/h54-100-s500-ra/", + "torque_points": [ + [-0.305, 1.42], + [3.21, 2.01], + [6.725, 2.601], + [10.24, 3.191], + [13.755, 3.782], + [17.49, 4.457], + [21.005, 5.089], + [24.521, 5.68], + [28.036, 6.313], + [31.551, 6.987], + [35.286, 7.662], + [38.801, 8.337], + [42.316, 9.012], + [45.831, 9.687], + [49.347, 10.362], + [53.082, 11.121], + [56.597, 11.881], + [60.112, 12.64], + [63.627, 13.483], + [67.362, 14.411] + ], + "resolution": 1003846, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 512, + "addr_operating_mode": 11, + "addr_goal_position": 564, + "len_goal_position": 4, + "addr_goal_velocity": 552, + "len_goal_velocity": 4, + "addr_present_position": 580, + "len_present_position": 4, + "addr_present_velocity": 576, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 571, + "addr_velocity_profile": 560, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 584, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 526, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 524, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 588, + "len_position_trajectory": 4, + "addr_position_p_gain": 526, + "len_position_p_gain": 2, + "addr_position_i_gain": 524, + "len_position_i_gain": 2, + "addr_position_d_gain": 528, + "len_position_d_gain": 2, + "addr_present_current": 574, + "len_present_current": 2, + "addr_goal_pwm": 548, + "len_goal_pwm": 2, + "addr_present_pwm": 572, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": -501433, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 1.0, + "errors": [] + }, + { + "model": "H54-200-S500-R(A)", + "series": "PRO_A_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/pro/h54-200-s500-ra/", + "torque_points": [ + [-0.745, 1.715], + [3.21, 2.221], + [7.164, 2.769], + [11.119, 3.318], + [15.074, 3.908], + [19.028, 4.499], + [22.983, 5.089], + [26.937, 5.68], + [30.892, 6.27], + [34.847, 6.903], + [38.801, 7.536], + [42.756, 8.169], + [46.71, 8.843], + [50.665, 9.518], + [54.619, 10.193], + [58.574, 10.91], + [62.529, 11.627], + [66.483, 12.429], + [70.438, 13.23], + [74.612, 14.074] + ], + "resolution": 1003846, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 512, + "addr_operating_mode": 11, + "addr_goal_position": 564, + "len_goal_position": 4, + "addr_goal_velocity": 552, + "len_goal_velocity": 4, + "addr_present_position": 580, + "len_present_position": 4, + "addr_present_velocity": 576, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 571, + "addr_velocity_profile": 560, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 584, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 526, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 524, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 588, + "len_position_trajectory": 4, + "addr_position_p_gain": 526, + "len_position_p_gain": 2, + "addr_position_i_gain": 524, + "len_position_i_gain": 2, + "addr_position_d_gain": 528, + "len_position_d_gain": 2, + "addr_present_current": 574, + "len_present_current": 2, + "addr_goal_pwm": 548, + "len_goal_pwm": 2, + "addr_present_pwm": 572, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": -501433, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 1.0, + "errors": [] + }, + { + "model": "H42-20-S300-R(A)", + "series": "PRO_A_SERIES", + "url": "https://emanual.robotis.com/docs/en/dxl/pro/h42-20-s300-ra/", + "torque_points": [ + [-0.567, 1.867], + [0.525, 2.038], + [1.695, 2.257], + [2.865, 2.477], + [4.035, 2.697], + [5.205, 2.917], + [6.376, 3.136], + [7.546, 3.356], + [8.716, 3.6], + [9.886, 3.82], + [11.056, 4.064], + [12.226, 4.284], + [13.396, 4.528], + [14.566, 4.772], + [15.737, 5.04], + [16.907, 5.285], + [18.077, 5.553], + [19.247, 5.846], + [20.417, 6.163], + [21.587, 6.578] + ], + "resolution": 607500, + "addr_baud_rate": 8, + "len_baud_rate": 1, + "addr_torque_enable": 512, + "addr_operating_mode": 11, + "addr_goal_position": 564, + "len_goal_position": 4, + "addr_goal_velocity": 552, + "len_goal_velocity": 4, + "addr_present_position": 580, + "len_present_position": 4, + "addr_present_velocity": 576, + "len_present_velocity": 4, + "addr_moving": 24, + "addr_moving_status": 571, + "addr_velocity_profile": 560, + "len_velocity_profile": 4, + "addr_velocity_limit": null, + "len_velocity_limit": null, + "addr_velocity_trajectory": 584, + "len_velocity_trajectory": 4, + "addr_velocity_p_gain": 526, + "len_velocity_p_gain": 2, + "addr_velocity_i_gain": 524, + "len_velocity_i_gain": 2, + "addr_position_trajectory": 588, + "len_position_trajectory": 4, + "addr_position_p_gain": 526, + "len_position_p_gain": 2, + "addr_position_i_gain": 524, + "len_position_i_gain": 2, + "addr_position_d_gain": 528, + "len_position_d_gain": 2, + "addr_present_current": 574, + "len_present_current": 2, + "addr_goal_pwm": 548, + "len_goal_pwm": 2, + "addr_present_pwm": 572, + "len_present_pwm": 2, + "addr_pwm_limit": 36, + "len_pwm_limit": 2, + "addr_min_position_limit": null, + "len_min_position_limit": null, + "initial_min_position_limit": -303454, + "addr_max_position_limit": null, + "len_max_position_limit": null, + "initial_max_position_limit": null, + "current_unit": 1.0, + "errors": [] + } +] \ No newline at end of file diff --git a/dynamixelmotorsapi/dynamixelmotors.py b/dynamixelmotorsapi/dynamixelmotors.py index 4028e85..d1f413f 100644 --- a/dynamixelmotorsapi/dynamixelmotors.py +++ b/dynamixelmotorsapi/dynamixelmotors.py @@ -1,69 +1,81 @@ -from abc import ABC, abstractmethod -from dataclasses import field from threading import Lock from math import pi +from typing import List +import atexit + +import numpy as np import dynamixelmotorsapi._motorgroup as motorgroup -import dynamixelmotorsapi._dynamixelmotorsparameters as robotparameters +from dynamixelmotorsapi._dynamixelmotorsconfigs import MotorConfig, MODELS_CONFIGS from dynamixelmotorsapi._logging_config import logger -class DynamixelMotors(ABC): + +class DynamixelMotors: """ - Abstract class to control Dynamixel motors. - The class is designed to be used with the Dynamixel motors plugged into a FTDI USB hub. - The motors are controlled in position mode. The class is thread-safe and can be used in a multi-threaded environment. + Abstract class to control Dynamixel motors, supporting heterogeneous motor groups + where each motor can be a different series with different conversion parameters. + + The motor group is configured via a list of MotorConfig objects, one per motor, + which can be loaded from a dict or JSON file. Example: ```python - from robotsmotorsapi import DynamixelMotors - from robotsmotorsapi._dynamixelmotorsparameters import DXL_IDs - from robotsmotorsapi._logging_config import logger - - - class MyDynamixelMotors(DynamixelMotors): - _length_to_rad = 1.0 / 20.0 # 1/radius of the pulley - _rad_to_pulse = 4096 / (2 * pi) # the resolution of the Dynamixel xm430 w210 - _pulse_center= 2048 - _max_vel = 1000 # *0.01 rev/min + from dynamixelmotorsapi import DynamixelMotors - def __init__(self): - super().__init__() # Check if all parameters have been set + motors = DynamixelMotors.from_json("my_motors.json") - - # Open connection to the motors (optionally specify device name, or it will take the first FTDI device) if motors.open(): - # Print current angles in radians print("Current angles (rad):", motors.angles) - - # Set new goal angles (example values) motors.angles = [0.5, 1.0, -0.5, 1.0] - - # Print status motors.printStatus() - - # Close connection when done motors.close() else: print("Failed to connect to motors.") ``` - """ + JSON format examples: + ```json + [ + { + "id": 0, + "model": "XM430-W210", + "pulley_radius": 20, # radius of the pulley in mm + "pulse_center": 2048, + "max_vel": 1000, + "baud_rate": 57600 + }, + { + "id": 1, + "model": "P_SERIES", + "pulley_radius": 30, # radius of the pulley in mm + "pulse_center": 0, + "max_vel": 500, + "baud_rate": 57600 + } + ] + ``` - _FTDI_list = {} # Dict of all FTDI devices connected to the computer + ```json + { + "id": [0, 1], + "model": ["XM430-W210", "P_SERIES"], + "pulley_radius": [20, 30], + "pulse_center": [2048, 0], + "max_vel": [1000, 500], + "baud_rate": 57600 + } + ``` + """ + _FTDI_list = {} _initialized: bool = False - # To be defined by child class - _length_to_rad: float = None - _rad_to_pulse: int = None - _pulse_center: int = None - _max_vel: float = None - - _length_to_pulse: int = None - - _goal_velocity: list = None - _goal_position: list = None + _motor_configs: List[MotorConfig] = None + _goal_velocities: list = None # store the last commanded velocity in rev/min, per motor + _goal_positions: list = None # store the last commanded position in pulses + _goal_pwms: list = None # store the last commanded PWM, per motor _mg: motorgroup.MotorGroup = None _device_index: int = None + _torque_polys: dict[int, List[float]] = None ##################### @@ -71,80 +83,226 @@ def __init__(self): ##################### - @abstractmethod - def __init__(self): + def __init__(self, motor_configs: List[MotorConfig]|List[dict]): + """ + Args: + motor_configs: list of MotorConfig, one per motor, ordered by motor index. + """ self._lock = Lock() - self._goal_velocity = field(default_factory=lambda: [0] * len(robotparameters.DXL_IDs)) - self._goal_position = field(default_factory=lambda: [0] * len(robotparameters.DXL_IDs)) + + atexit.register(self._atexit_close) + + # Normalize: if dicts were passed, unwrap and convert to MotorConfig + if motor_configs and isinstance(motor_configs[0], dict): + dicts = [] + for d in motor_configs: + dicts.extend(self._unwrap_dict(d)) + self._motor_configs = [MotorConfig.from_dict(m) for m in dicts] + else: + self._motor_configs = motor_configs + + n = len(self._motor_configs) + self._goal_velocities = [0] * n + self._goal_positions = [0] * n + self._goal_pwms = [0] * n if not self._initialized: - self._mg = motorgroup.MotorGroup(robotparameters) + self._mg = motorgroup.MotorGroup(self._motor_configs) self._initialized = True + + for cfg in self._motor_configs: + if cfg.torque_points is None: + break + _I = np.array([pt[1] for pt in cfg.torque_points]) + _T = np.array([pt[0] for pt in cfg.torque_points]) + self._torque_polys = {} if self._torque_polys is None else self._torque_polys + self._torque_polys[cfg.id] = np.polyfit(_I, _T, 2) + + + @staticmethod + def listMotorsModels() -> list: + """List the models of Dynamixel motors supported by this API.""" + return list(MODELS_CONFIGS.keys()) + + + @classmethod + def _unwrap_dict(cls, data: dict) -> List[MotorConfig]: + """Helper function to convert a dict of motor configs into a list of dicts, one per motor.""" + if isinstance(data["id"], list): + motors_count = len(data["id"]) + for key, value in data.items(): + if isinstance(value, list) and len(value) != motors_count: + raise ValueError(f"All list values in the motor config dict must have the same length. Key '{key}' has length {len(value)}, expected {motors_count}.") + elif not isinstance(value, list): + data[key] = [value] * motors_count + return [dict(zip(data.keys(), values)) for values in zip(*data.values())] # zip the dict of lists into a list of dicts, one per motor + else : + return [data] # single motor case - message = "A child class of DynamixelMotors must initialize this parameter" - assert self._length_to_rad is not None, message - assert self._rad_to_pulse is not None, message - assert self._pulse_center is not None, message - assert self._max_vel is not None, message - self._length_to_pulse = self._length_to_rad * self._rad_to_pulse + @classmethod + def from_dicts(cls, data: list) -> "DynamixelMotors": + """ + Instantiate from a list of per-motor config dicts. + + Args: + data: list of dicts, each containing the fields for one MotorConfig. + + Returns: + A configured DynamixelMotors instance (not yet connected). + + Example: + ```json + [ + { + "id": 0, + "model": "XM430-W210", + "pulley_radius": 20, # radius of the pulley in mm + "pulse_center": 2048, + "max_vel": 1000, + "baud_rate": 57600 + }, + { + "id": [1, 2], + "model": ["XM430-W210", "P_SERIES"], + "pulley_radius": [0.05, 0.03], + "pulse_center": [0, 0], + "max_vel": [1000, 500], + "baud_rate": 57600 + } + ] + ``` + """ + return cls(data) + + + @classmethod + def from_dict(cls, data: dict) -> "DynamixelMotors": + """ + Instantiate from a list of per-motor config dicts. + + Args: + data: dict of lists, each list containing the fields for the MotorConfig. + + Returns: + A configured DynamixelMotors instance (not yet connected). + + Example: + ```json + { + "id": 0, + "model": "XM430-W210", + "pulley_radius": 20, # radius of the pulley in mm + "pulse_center": 2048, + "max_vel": 1000, + "baud_rate": 57600 + } + + OR + + { + "id": [0, 1], + "model": ["XM430-W210", "P_SERIES"], + "pulley_radius": [20, 30], + "pulse_center": [2048, 0], + "max_vel": [1000, 500], + "baud_rate": 57600 + } + ``` + """ + return cls([data]) + + + @classmethod + def from_json(cls, path: str) -> "DynamixelMotors": + """ + Instantiate from a JSON file containing a list of per-motor config dicts. + + Args: + path: path to the JSON file. + + Returns: + A configured DynamixelMotors instance (not yet connected). + """ + with open(path) as f: + json = json.load(f) + if isinstance(json, list): + return cls.from_dicts(json) + elif isinstance(json, dict): + return cls.from_dict(json) + + def _config(self, index: int) -> MotorConfig: + """Convenience accessor for the MotorConfig of motor at position `index`.""" + return self._motor_configs[index] - def lengthToPulse(self, displacement: list): + + def lengthToPulse(self, displacement: list) -> list: """ - Convert length (mm) to pulse using the conversion factor `lengthToPulse`. + Convert length (mm) to pulse, per motor. Args: - displacement: list: list of length values in mm for each motor. + displacement: list of length values in mm, one per motor. Returns: A list of pulse values for each motor. """ - return [self._pulse_center - int(item * self._length_to_pulse) for item in displacement] + return [ + cfg.pulse_center - int(d * cfg.length_to_pulse) + for d, cfg in zip(displacement, self._motor_configs) + ] - def pulseToLength(self, pulse: list): + def pulseToLength(self, pulse: list) -> list: """ - Convert pulse to length (mm) using the conversion factor `lengthToPulse`. + Convert pulse to length (mm), per motor. Args: - pulse: list of pulse integer values for each motor. + pulse: list of pulse integer values, one per motor. Returns: A list of length values in mm for each motor. """ - return [(self._pulse_center - float(item)) / self._length_to_pulse for item in pulse] + return [ + (cfg.pulse_center - float(p)) / cfg.length_to_pulse + for p, cfg in zip(pulse, self._motor_configs) + ] - def pulseToRad(self, pulse: list): + def pulseToRad(self, pulse: list) -> list: """ - Convert pulse to radians using the conversion factor `radToPulse`. + Convert pulse to radians, per motor. Args: - pulse: list: list of pulse integer values for each motor. + pulse: list of pulse integer values, one per motor. Returns: A list of angles in radians for each motor. - """ - return [(self._pulse_center - float(item)) / self._rad_to_pulse for item in pulse] + return [ + (cfg.pulse_center - float(p)) / cfg.rad_to_pulse + for p, cfg in zip(pulse, self._motor_configs) + ] - def pulseToDeg(self, pulse: list): + def pulseToDeg(self, pulse: list) -> list: """ - Convert pulse to degrees using the conversion factor `radToPulse`. + Convert pulse to degrees, per motor. Args: - pulse: list: list of pulse values for each motor. + pulse: list of pulse values, one per motor. Returns: A list of angles in degrees for each motor. """ - return [(self._pulse_center - float(item)) / self._rad_to_pulse * 180.0 / pi for item in pulse] + return [ + (cfg.pulse_center - float(p)) / cfg.rad_to_pulse * 180.0 / pi + for p, cfg in zip(pulse, self._motor_configs) + ] - def _openAndConfig(self, device_name: str=None, multi_turn:bool = False) -> bool: - """Open the connection to the motors, configure it for position mode in multi-turn or not and enable torque sensing.""" + def _openAndConfig(self, device_name: str = None, multi_turn: bool = False) -> bool: + """Open the connection to the motors, configure position mode and enable torque.""" with self._lock: try: self._mg.updateDeviceName(device_name) @@ -153,29 +311,36 @@ def _openAndConfig(self, device_name: str=None, multi_turn:bool = False) -> bool logger.error("Device name is None. Please check the connection.") return False - self._mg.open() + self._mg.openPort() self._mg.disableTorque() self._mg.clearPort() if multi_turn: self.enableExtendedPositionMode() else: self.enablePositionMode() + # set the max velocity profile to the configured max velocity for each motor + self._mg.setVelocityProfile([cfg.max_vel for cfg in self._motor_configs]) self._mg.enableTorque() - logger.debug(f"Motor group opened and configured. Device name: {self._mg.deviceName}, Multi turn activated: {multi_turn}") + DynamixelMotors._FTDI_list[self._mg.deviceName] = self # add the connected device to the list of used devices + + logger.debug( + f"Motor group opened and configured. " + f"Device name: {self._mg.deviceName}, Multi turn: {multi_turn}" + ) return True except Exception as e: logger.error(f"Failed to open and configure the motor group: {e}") return False - def open(self, device_name: str=None, multi_turn: bool=False) -> bool: + def open(self, device_name: str = None, multi_turn: bool = False) -> bool: """ Open the connection to the motors. Args: - device_name: str: if set, it will connect to the device with the given name, If not set, the first device will be used. - multi_turn: bool: Whether to enable the multi-turn mode of the motors. In multi-turn mode on, the angles interval is [-256*2π, 256*2π] + device_name: if set, connect to this specific port; otherwise use the first available. + multi_turn: enable multi-turn mode. Angle interval becomes [-256*2π, 256*2π]. """ if self._openAndConfig(device_name, multi_turn): self._device_index = motorgroup.listMotors().index(self._mg.deviceName) @@ -184,135 +349,275 @@ def open(self, device_name: str=None, multi_turn: bool=False) -> bool: return False - def findAndOpen(self, device_name: str=None, multi_turn: bool=False) -> int: + def findAndOpen(self, device_name: str = None, multi_turn: bool = False) -> int: """ - Iterate over the serial ports and try to conenct to the first available FTDI device. + Iterate over serial ports and connect to the first available FTDI device. Args: - device_name: str: If set, It will try to connected to the given device name (port name) - multi_turn: bool: Whether to enable the multi-turn mode of the motors. In multi-turn mode on, the angles interval is [-256*2π, 256*2π] + device_name: if set, try only this port. + multi_turn: enable multi-turn mode. Returns: - the index in the list of port to which it connected. If no connection was possible, returns -1. + Index of the connected port, or -1 if no connection was possible. """ if device_name is not None: try: index = motorgroup.listMotors().index(device_name) logger.info(f"Trying given device number {index} on port: {device_name}.") - return index if len(motorgroup.listMotors())>0 and self.open(device_name, multi_turn) else -1 - except: + return index if len(motorgroup.listMotors()) > 0 and self.open(device_name, multi_turn) else -1 + except Exception: return -1 index = 0 - connected = False try: - - while not connected and index None: + """ + Immediately disable torque on all motors. + Does NOT close the serial port — call close() afterwards if needed. + """ + with self._lock: + try: + self.torque = False + except Exception as e: + logger.error(f"Emergency stop: disableTorque failed: {e}") + logger.warning("EMERGENCY STOP: torque disabled on all motors.") + + + def _atexit_close(self) -> None: + """ + Safety shutdown registered with atexit: fires when the Python interpreter + exits. Immediately disables motor torque so the physical robot is not left in a commanded position. + """ + try: + if not self.is_connected(): + return + self.torque = False + self.close() + except Exception: + pass # at interpreter shutdown other objects may already be partially torn down + + def close(self): """Close the connection to the motors.""" with self._lock: try: self._mg.close() + DynamixelMotors._FTDI_list.pop(self._mg.deviceName, None) logger.info("Motors connection closed.") except Exception as e: - print(e) + logger.error(e) def printStatus(self): - """Print the current position of the motors.""" + """Print the current position of the motors in radians, pulses, and degrees.""" with self._lock: - current_pos = self._mg.getCurrentPosition() - logger.info(f"Current position of the motors in radians: {[ a%pi for a in self.pulseToRad(current_pos)]}\n" - +f"{' '*75} in pulses: {[ a for a in current_pos]}\n" - +f"{' '*75} in degrees: {[ a*180/pi for a in self.pulseToRad(current_pos)]}") + current_pos = self._mg.getPresentPosition() + rads = self.pulseToRad(current_pos) + degrees = self.pulseToDeg(current_pos) + logger.info( + f"Current position of the motors\n" + f" radians: {[round(a, 4) for a in rads]}\n" + f" pulses: {list(current_pos)}\n" + f" degrees: {[round(a, 2) for a in degrees]}" + ) - def enablePositionMode(self): - self._mg.enablePositionMode() + def printConfig(self): + """Print the current configuration of the motors.""" + for i, cfg in enumerate(self._motor_configs): + logger.info( + f"Motor {i} config:\n" + f" ID: {cfg.id}\n" + f" Model: {cfg.model}\n" + f" Length to rad conversion: {cfg.length_to_rad}\n" + f" Pulse center: {cfg.pulse_center}\n" + f" Max velocity: {cfg.max_vel}\n" + f" Torque Points: {cfg.torque_points}\n" + ) - - def enableExtendedPositionMode(self): - self._mg.enableExtendedPositionMode() + def enablePositionMode(self, ids: List[int]=None): + self._mg.enablePositionMode(ids) - #################### - #### PROPERTIES #### - #################### + def enableExtendedPositionMode(self, ids: List[int]=None): + self._mg.enableExtendedPositionMode(ids) - #### Read and Write properties #### - @property - def relativePos(self, init_pos: list, rel_pos: list): + def enableVelocityMode(self, ids: List[int]=None): + self._mg.enableVelocityMode(ids) + + + def enablePWMMode(self, ids: List[int]=None): + self._mg.enablePWMMode(ids) + + + def enableCurrentMode(self, ids: List[int]=None): + self._mg.enableCurrentMode(ids) + + + def current_to_torque(self, currents_mA: List[float]|float, motor_idx: int = None) -> List[float]|float: """ - Calculate the new position of the motors based on the initial position and relative position in pulses. + Estimate torque (N·mm) from the measured currents (mA) using the polynomial + T(I) fitted for the model of the given motor index. + + The fit is quadratic in N·m (I in Amperes); the result is converted to N·mm. + Returns 0 for currents below the no-load threshold. Args: - init_pos: list of initial pulse values for each motor. - rel_pos: list of relative pulse values for each motor. + current_mA: signed current in milliamps (as returned by getCurrent_mA). + motor_idx: index into the active motor list (used to look up the motor model). Returns: - A list of new pulse values for each motor. + Estimated torque(s) in N·mm (always >= 0). """ - new_pos = [] - for i in range(len(init_pos)): - new_pos.append(init_pos[i] + rel_pos[i]) - return new_pos + if self._torque_polys is not None: + if motor_idx is None and isinstance(currents_mA, list): + polys = [self._torque_polys.get(cfg.id) for cfg in self._motor_configs] + torques_Nm = [float(np.polyval(poly, currents_mA[i]/1000)) if poly is not None else -1 for i, poly in enumerate(polys)] + return [max(0.0, torque_Nm * 1000000.0) for torque_Nm in torques_Nm] + elif motor_idx is not None: + if isinstance(currents_mA, list): + cfg = self._motor_configs[motor_idx] + poly = self._torque_polys.get(cfg.id) + if motor_idx >= len(currents_mA): + raise ValueError(f"motor_idx {motor_idx} is out of range for the currents_mA list of length {len(currents_mA)}.") + torque_Nm = float(np.polyval(poly, currents_mA[motor_idx]/1000)) + return max(0.0, torque_Nm * 1000000.0) + elif isinstance(currents_mA, (int, float)): + cfg = self._motor_configs[motor_idx] + poly = self._torque_polys.get(cfg.id) + torque_Nm = float(np.polyval(poly, currents_mA/1000)) + return max(0.0, torque_Nm * 1000000.0) + else: + raise ValueError("Invalid input: currents_mA should be a list if motor_idx is None, or a single value if motor_idx is specified.") + else: + raise ValueError("No torque points or curve estimation was givent at initialization.") + #################### + #### PROPERTIES #### + #################### + + + #### Read and Write properties #### + + @property + def torque(self) -> list: + """Get the current torque status of the motors.""" + with self._lock: + return self._mg.isTorqueEnable() + + @torque.setter + def torque(self, enable: bool): + """Enable or disable torque for the motors.""" + with self._lock: + if enable: + self._mg.enableTorque() + else: + self._mg.disableTorque() + + + @property + def goal_angles(self) -> list: + """Get the last commanded angles of the motors in radians.""" + return self.pulseToRad(self._goal_positions) + @property def angles(self) -> list: """Get the current angles of the motors in radians.""" with self._lock: - return self.pulseToRad(self._mg.getCurrentPosition()) + return self.pulseToRad(self._mg.getPresentPosition()) @angles.setter def angles(self, angles: list): """Set the goal angles of the motors in radians.""" with self._lock: - self._goal_position = angles - self._mg.setGoalPosition([int(self._pulse_center - self._rad_to_pulse * a) for a in angles]) - + self._goal_positions = angles + self._mg.setGoalPosition([ + int(cfg.pulse_center - cfg.rad_to_pulse * a) + for a, cfg in zip(angles, self._motor_configs) + ]) @property - def goal_velocity(self) -> list: - """Get the current velocity (rev/min) of the motors.""" - return self._goal_velocity + def goal_positions(self) -> list: + """Get the last commanded positions of the motors in pulses.""" + return self._goal_positions + + @property + def positions(self) -> list: + """Get the current positions of the motors in pulses.""" + with self._lock: + return self._mg.getPresentPosition() + + @positions.setter + def positions(self, positions: list): + """Set the goal positions of the motors in pulses.""" + with self._lock: + self._goal_positions = self.pulseToRad(positions) + self._mg.setGoalPosition(positions) + - @goal_velocity.setter - def goal_velocity(self, velocities: list): - """Set the goal velocity (rev/min) of the motors.""" - self._goal_velocity = velocities + @property + def goal_velocities(self) -> list: + """Get the last commanded velocity (rev/min) for each motor.""" + return self._goal_velocities + + @goal_velocities.setter + def goal_velocities(self, velocities: list): + """Set the goal velocity (rev/min) for each motor.""" + self._goal_velocities = velocities with self._lock: self._mg.setGoalVelocity(velocities) + @property + def goal_pwms(self) -> list: + """Get the last commanded PWM for each motor.""" + return self._goal_pwms + + @property + def pwms(self) -> list: + """Get the current PWM of the motors.""" + with self._lock: + return self._mg.getPresentPWM() + + @pwms.setter + def pwms(self, pwms: list): + """Set the goal PWM for each motor.""" + self._goal_pwms = pwms + with self._lock: + self._mg.setGoalPWM(pwms) + @property - def max_velocity(self)-> list: - """Get the current velocity (rev/min) profile of the motors.""" - return self._max_vel + def max_velocity(self) -> list: + """Get the maximum velocity profile (rev/min) for each motor.""" + return [cfg.max_vel for cfg in self._motor_configs] @max_velocity.setter def max_velocity(self, max_vel: list): - """Set the maximum velocities (rev/min) in position mode. - Arguments: + """ + Set the maximum velocity profile (rev/min) in position mode, per motor. + + Args: max_vel: list of maximum velocities for each motor in rev/min. """ - self._max_vel = max_vel + for cfg, v in zip(self._motor_configs, max_vel): + cfg.max_vel = v with self._lock: self._mg.setVelocityProfile(max_vel) @@ -325,10 +630,7 @@ def position_p_gain(self) -> list: @position_p_gain.setter def position_p_gain(self, p_gains: list): - """Set the position P gains of the motors. - Arguments: - p_gains: list of position P gains for each motor. - """ + """Set the position P gains of the motors.""" with self._lock: self._mg.setPositionPGain(p_gains) @@ -341,10 +643,7 @@ def position_i_gain(self) -> list: @position_i_gain.setter def position_i_gain(self, i_gains: list): - """Set the position I gains of the motors. - Arguments: - i_gains: list of position I gains for each motor. - """ + """Set the position I gains of the motors.""" with self._lock: self._mg.setPositionIGain(i_gains) @@ -357,69 +656,96 @@ def position_d_gain(self) -> list: @position_d_gain.setter def position_d_gain(self, d_gains: list): - """Set the position D gains of the motors. - Arguments: - d_gains: list of position D gains for each motor. - """ + """Set the position D gains of the motors.""" with self._lock: self._mg.setPositionDGain(d_gains) + @property + def velocity_profile(self) -> list: + """Get the velocity profile (rev/min) of the motors.""" + with self._lock: + return self._mg.getVelocityProfile() + + @velocity_profile.setter + def velocity_profile(self, profile: list): + """Set the velocity profile (rev/min) of the motors.""" + with self._lock: + self._mg.setVelocityProfile(profile) + + @property + def currents(self) -> list: + """Get the current (mA) of the motors.""" + with self._lock: + raw_currents = self._mg.getPresentCurrent() + currents = [] + for raw, cfg in zip(raw_currents, self._motor_configs): + if cfg.current_unit is not None: + currents.append(raw * cfg.current_unit) + else: + currents.append(raw) + return currents + + @currents.setter + def currents(self, currents: list): + """Set the current (mA) of the motors.""" + with self._lock: + self._mg.setPresentCurrent(currents) + #### Read-only properties #### + + @property + def motor_configs(self) -> List[MotorConfig]: + """Get the list of per-motor configurations.""" + return self._motor_configs + @property def is_connected(self) -> bool: """Check if the motors are connected.""" with self._lock: return self._mg.isConnected - @property def device_name(self) -> str: - """Get the name of the device.""" + """Get the name of the connected device port.""" with self._lock: return self._mg.deviceName - @property def device_index(self) -> int: - """Get the index of the device in the list of Robot Motors Devices from Robot MotorsAPI""" + """Get the index of the device in the list of available motor devices.""" return self._device_index - @property def moving(self) -> list: """Check if the motors are moving.""" with self._lock: return self._mg.isMoving() - @property def moving_status(self) -> list: - """Get the moving status of the motors. - Returns: - A Byte encoding different informations on the moving status like whether the desired position has been reached or not, if the profile is in progress or not, the kind of Profile used. + """Get the moving status byte of the motors. - See [here](https://emanual.robotis.com/docs/en/dxl/x/xc330-t288/#moving-status) for more details.""" + See https://emanual.robotis.com/docs/en/dxl/x/xc330-t288/#moving-status for details. + """ with self._lock: return self._mg.getMovingStatus() - @property def velocity(self) -> list: """Get the current velocity (rev/min) of the motors.""" with self._lock: - return self._mg.getCurrentVelocity() - + return self._mg.getPresentVelocity() @property - def velocity_trajectory(self)-> list: + def velocity_trajectory(self) -> list: """Get the velocity (rev/min) trajectory of the motors.""" with self._lock: return self._mg.getVelocityTrajectory() - @property - def position_trajectory(self)-> list: + def position_trajectory(self) -> list: """Get the position (pulse) trajectory of the motors.""" with self._lock: return self._mg.getPositionTrajectory() + \ No newline at end of file diff --git a/examples/motors_example.py b/examples/motors_example.py index a4535b6..806eb5e 100644 --- a/examples/motors_example.py +++ b/examples/motors_example.py @@ -8,24 +8,13 @@ sys.path.append(os.path.dirname(os.path.realpath(__file__))+'/..') from dynamixelmotorsapi import DynamixelMotors -from dynamixelmotorsapi._dynamixelmotorsparameters import DXL_IDs from dynamixelmotorsapi._logging_config import logger -class MyDynamixelMotors(DynamixelMotors): - _length_to_rad = 1.0 / 20.0 # 1/radius of the pulley - _rad_to_pulse = 4096 / (2 * pi) # the resolution of the Dynamixel xm430 w210 - _pulse_center= 2048 - _max_vel = 1000 # *0.01 rev/min - - def __init__(self): - super().__init__() # Check if all parameters have been set - - def main(robot_motors: DynamixelMotors, loops=1): - - initial_pos_pulse = [0] * len(DXL_IDs) - robot_motors.max_velocity = [1000] * len(DXL_IDs) + motors_count = robot_motors._motor_configs + initial_pos_pulse = [0] * len(motors_count) + robot_motors.max_velocity = [1000] * len(motors_count) logger.info(f"Initial position in rad: {initial_pos_pulse}") robot_motors.angles = initial_pos_pulse time.sleep(1) @@ -33,7 +22,7 @@ def main(robot_motors: DynamixelMotors, loops=1): for i in range(loops): - new_pos = [((2*3.14)*((i+1)%8)/8)] * len(DXL_IDs) + new_pos = [((2*3.14)*((i+1)%8)/8)] * len(motors_count) print("-"*20) logger.info(f"new_pos {new_pos}") try: @@ -54,11 +43,43 @@ def main(robot_motors: DynamixelMotors, loops=1): try: logger.info("Starting DynamixelMotors API test...") logger.info("Opening and configuring Robot Motors API...") + + motors_description = [ + { + "id": 0, + "model": "XM430-W210", + "pulley_radius": 20, # radius of the pulley in mm + "pulse_center": 2048, + "max_vel": 1000, + "baud_rate": 1000000, + "torque_points": [ + [0.406, 1.392], + [0.496, 4.424], + [0.586, 7.76], + [0.671, 10.489], + [0.761, 13.521], + [0.851, 16.856], + [0.941, 19.888], + [1.031, 23.224] + ] + }, + { + "id": [1, 2, 3], + "model": ["XM430-W210"]*3, + "pulley_radius": [20]*3, # radius of the pulley in mm + "pulse_center": [2048]*3, + "max_vel": [1000]*3, + "baud_rate": [1000000]*3 + } + ] - robot_motors = MyDynamixelMotors() - - if robot_motors.open(): - + robot_motors = DynamixelMotors.from_dicts(motors_description) + robot_motors.printConfig() + + logger.info(f"Estimated torques for currents [3, 2, 3, 2] mA: {robot_motors.current_to_torque([3, 2, 3, 2])}") # estimate of torque for each motor based on current in mA + logger.info(f"Estimated torque for motor 0 with 3 mA current: {robot_motors.current_to_torque(3, motor_idx=0)}") # estimate of torque for motor 0 based on current in mA + + if robot_motors.open(): robot_motors.printStatus() logger.info("Robot motors opened and configured.") diff --git a/examples/motors_multiturn.py b/examples/motors_multiturn.py index b5c05d1..a1da8bb 100644 --- a/examples/motors_multiturn.py +++ b/examples/motors_multiturn.py @@ -11,16 +11,6 @@ from dynamixelmotorsapi import DynamixelMotors from dynamixelmotorsapi._logging_config import logger -class MyDynamixelMotors(DynamixelMotors): - _length_to_rad = 1.0 / 20.0 # 1/radius of the pulley - _rad_to_pulse = 4096 / (2 * pi) # the resolution of the Dynamixel xm430 w210 - _pulse_center= 2048 - _max_vel = 1000 # *0.01 rev/min - - def __init__(self): - super().__init__() # Check if all parameters have been set - - def main(robot_motors: DynamixelMotors, loops=1): initial_pos_pulse = [0] * 4 @@ -57,8 +47,19 @@ def main(robot_motors: DynamixelMotors, loops=1): try: logger.info("Starting DynamixelMotors API test...") logger.info("Opening and configuring DynamixelMotors API...") + + motors_description = { + "id": [0, 1, 2, 3], + "model": "XM430-W210", + "pulley_radius": [20]*4, # radius of the pulley in mm + "pulse_center": [2048]*4, + "max_vel": [1000, 500, 600, 300], + "baud_rate": 1000000 + } - robot_motors = MyDynamixelMotors() + robot_motors = DynamixelMotors.from_dict(motors_description) + + robot_motors.printConfig() if robot_motors.open(multi_turn=True): diff --git a/extract_torque_curve.py b/extract_torque_curve.py new file mode 100644 index 0000000..482b004 --- /dev/null +++ b/extract_torque_curve.py @@ -0,0 +1,303 @@ +""" +chart_digitizer.py +------------------ +Automatically detects axis ranges from a chart image using OCR, +then extracts sample points from a target-coloured curve. + +Dependencies: + pip install opencv-python pytesseract pillow numpy + sudo apt install tesseract-ocr # Ubuntu + brew install tesseract # macOS +""" + +import re +import cv2 +import numpy as np +import easyocr + + +def detect_plot_area(img: np.ndarray) -> dict: + """ + Find the pixel bounding box of the plot area using column/row + intensity profiles (robust against dense grid lines). + + Returns dict with keys: x_left, x_right, y_top, y_bottom. + """ + h, w = img.shape[:2] + gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + inv = 255 - gray # dark pixels → high score + + col_sum = inv.sum(axis=0).astype(float) # profile along X + row_sum = inv.sum(axis=1).astype(float) # profile along Y + + x_left = int(np.argmax(col_sum)) + x_right = int(np.argmax(col_sum[:x_left])) + int(w * 0.7) + y_top = int(np.argmax(row_sum[:int(h * 0.2)])) + y_bottom= int(np.argmax(row_sum[int(h * 0.7):])) + int(h * 0.7) + + return dict(x_left=x_left, x_right=x_right, + y_top=y_top, y_bottom=y_bottom) + + +def _ocr_axis(img: np.ndarray, regions: dict=None, scale: int = 2) -> list[tuple]: + """ + Run Tesseract on the full image (upscaled for accuracy). + Returns list of dicts: {text, cx, cy, conf} + where cx/cy are centre coordinates in *original* image pixels. + """ + up = cv2.resize(img[:, regions["x_left"]:], None, fx=scale, fy=scale, + interpolation=cv2.INTER_CUBIC) + gray = cv2.cvtColor(up, cv2.COLOR_BGR2GRAY) + _, thresh = cv2.threshold(gray, 0, 255, + cv2.THRESH_BINARY_INV + cv2.THRESH_OTSU) + + # cv2.imshow("plot", thresh) + # cv2.waitKey(0) + + reader = easyocr.Reader(['en']) # this needs to run only once to load the model into memory + + # Extract X axis (Torque) + x_strip = thresh[regions["y_bottom"]*scale:, :] + dataX = reader.readtext(x_strip) + + for i, item in enumerate(dataX): + transformed = tuple([[ [int(regions["x_left"]+x/scale), int(regions["y_bottom"]+y/scale)] for x,y in item[0] ], item[1], item[2]]) + dataX[i] = transformed + + + # Extract Y axis (Current) + y_strip = thresh[regions["y_top"]*scale:regions["y_bottom"]*scale, (regions["x_right"]-regions["x_left"])*scale:] + dataY = reader.readtext(y_strip, rotation_info=[270]) + + for i, item in enumerate(dataY): + # transform back into original image + transformed = tuple([[ [int(regions["x_right"]+x/scale), int(regions["y_top"]+y/scale)] for x,y in item[0]], item[1], item[2]]) + dataY[i] = transformed + + return dataX, dataY + + +def _clean_numeric(text: str) -> float | None: + """ + Parse a tick label that may include OCR artefacts: + - leading dash from right-axis prefix e.g. "- 6.30" → 6.30 + - trailing dash (tick mark artefact) e.g. "63-" → 63 + - missing decimal point e.g. "150" → handled by caller + Returns float or None if not parseable. + """ + cleaned = re.sub(r"^-\s*", "", text) # strip leading dash + cleaned = re.sub(r"-$", "", cleaned) # strip trailing dash + cleaned = cleaned.strip() + try: + return float(cleaned) + except ValueError: + return None + + +def extract_axis_ticks(img: np.ndarray, plot: dict) -> dict: + """ + Classify every OCR'd number into one of four axes by its pixel position + relative to the plot area, then return: + + { + "x": [(px_x, value), ...], sorted by px_x + "y_right": [(px_y, value), ...], right axis (Current) + } + + X-axis: OCR sometimes drops the decimal point for values like 1.50 → "150". + We correct these by dividing by 100 when the value is implausibly large + (> 10) for an axis whose other ticks are all < 10. + """ + tokensX, tokensY = _ocr_axis(img, plot) + x_right = plot["x_right"] + y_bottom = plot["y_bottom"] + + x_ticks, y_ticks = [], [] + + for tok in tokensX+tokensY: + val = _clean_numeric(tok[1]) + if val is None: + continue + bbox = tok[0] + + cx, cy = int(bbox[0][0]+(bbox[1][0]-bbox[0][0])/2), int(bbox[1][1]+(bbox[2][1]-bbox[1][1])/2) + + if cy > y_bottom + 5: # below plot → X axis + x_ticks.append((cx, val)) + elif cx > x_right + 5: # right of plot → Y-right + y_ticks.append((cy, val)) + + # OCR sometimes drops the decimal point for values like 1.50 → "150". + # We correct these by dividing by 100 when the value is implausibly large + # (> 10) for an axis whose other ticks are all < 10. + small_x = [v for _, v in x_ticks if v <= 10] + if small_x: + threshold = max(small_x) * 10 + x_ticks = [(px, v / 100 if v > threshold else v) + for px, v in x_ticks] + + # First of x_ticks and last of y_ticks should be 0 + x_ticks[0] = (x_ticks[0][0], "0.0") + y_ticks[-1] = (y_ticks[-1][0], "0.0") + + return { + "x": sorted(x_ticks, key=lambda t: t[0]), + "y": sorted(y_ticks, key=lambda t: t[0]), + } + + + +def calibrate_axis(ticks: list[tuple]) -> dict: + """ + Fit a linear mapping value = a * pixel + b through the tick positions. + Requires at least 2 ticks. + + Returns: + { + "min": float, + "max": float, + "px_to_value": callable(px) → float, + "value_to_px": callable(val) → float, + } + """ + if len(ticks) < 2: + raise ValueError(f"Need ≥2 ticks to calibrate axis, got {len(ticks)}") + + pixels = np.array([t[0] for t in ticks], dtype=float) + values = np.array([t[1] for t in ticks], dtype=float) + a, b = np.polyfit(pixels, values, 1) + + return { + "min": float(values.min()), + "max": float(values.max()), + "px_to_value": lambda px: float(a * px + b), + "value_to_px": lambda val: int((val - b) / a), + } + + +def extract_curve_points( + img: np.ndarray, + plot: dict, + x_axis: dict, + y_axis: dict, + hsv_ranges: list[tuple[np.ndarray, np.ndarray]], + n_samples: int = 20, +) -> list[tuple[float, float]]: + """ + Detect a curve by colour, sample one pixel per column inside the plot area, + convert pixel coords to data coords using pre-calibrated axes. + + hsv_ranges: list of (lower, upper) HSV bounds — use multiple pairs for + colours that wrap around the hue wheel (e.g. pink/magenta). + n_samples: number of evenly-spaced points to return (None = all columns). + + Returns list of (x_value, y_value) tuples. + """ + hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) + mask = np.zeros(hsv.shape[:2], dtype=np.uint8) + for lo, hi in hsv_ranges: + mask |= cv2.inRange(hsv, lo, hi) + + # Clean up noise + k = np.ones((3, 3), np.uint8) + mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, k) + + x_left = plot["x_left"] + x_right = plot["x_right"] + y_top = plot["y_top"] + y_bottom = plot["y_bottom"] + + raw_points = [] + for px_x in range(x_left, x_right + 1): + ys = np.where(mask[:, px_x] > 0)[0] + ys = ys[(ys >= y_top) & (ys <= y_bottom)] + if len(ys) == 0: + continue + px_y = int(np.median(ys)) + x_val = x_axis["px_to_value"](px_x) + y_val = y_axis["px_to_value"](px_y) + raw_points.append((round(x_val, 3), round(y_val, 3))) + + if n_samples is None or len(raw_points) <= n_samples: + return raw_points + + idx = np.linspace(0, len(raw_points) - 1, n_samples, dtype=int) + return [raw_points[i] for i in idx] + + +def extract_torque_current_points(img_path: str): + """ + Main function to extract curve points from a chart image. + Steps: + 1. Detect plot area + 2. OCR tick labels and classify by axis + 3. Fit linear calibration for each axis + 4. Extract curve points by colour and convert to data coords + + Arguments: + img_path: path to the chart image file + Returns: + points: list of (x_value, y_value) tuples + x_axis: dict with keys "min", "max", "px_to_value", "value_to_px" + y_axis: dict with keys "min", "max", "px_to_value", "value_to_px" + """ + if img_path.startswith("http"): + from urllib.request import urlopen + req = urlopen(img_path) + arr = np.asarray(bytearray(req.read()), dtype=np.uint8) + img = cv2.imdecode(arr, -1) + else: + img = cv2.imread(img_path) + if img is None: + raise FileNotFoundError(f"Could not load {img_path}") + + # Step 1 – locate the plot area + plot = detect_plot_area(img) + print("Plot area (pixels):", plot) + + # Step 2 – OCR all tick labels and classify them per axis + ticks = extract_axis_ticks(img, plot) + print("\nDetected ticks:") + for name, t in ticks.items(): + print(f" {name}: {t}") + + # Step 3 – fit linear calibration for each axis + x_torque = calibrate_axis(ticks["x"]) + y_current = calibrate_axis(ticks["y"]) + + print(f"\nTorque axis: {x_torque['min']} - {x_torque['max']}") + print(f"Current axis: {y_current['min']} - {y_current['max']}") + + # Step 4 – extract the pink/magenta (Current) curve + # Pink/magenta wraps around both ends of the HSV hue wheel + pink_ranges = [ + (np.array([140, 80, 80]), np.array([180, 255, 255])), + (np.array([0, 80, 80]), np.array([10, 255, 255])), + ] + + points = extract_curve_points( + img, plot, + x_axis = x_torque, + y_axis = y_current, + hsv_ranges = pink_ranges, + n_samples = 20, + ) + + return points, x_torque, y_current + + +# ── Main ────────────────────────────────────────────────────────────────────── + +if __name__ == "__main__": + IMG_PATH = "chart.png" + img = cv2.imread(IMG_PATH) + + points, x_axis, y_axis = extract_points(IMG_PATH) + + print("\nCurrent curve sample points (Torque N·m → Current A):") + for torque, current in points: + print(f" Torque={torque:.2f} N·m → Current={current:.2f} A") + cv2.circle(img, (x_axis["value_to_px"](torque), y_axis["value_to_px"](current)), 2, (0, 255, 0), -1) + + cv2.imshow("plot", img) + cv2.waitKey(0) \ No newline at end of file diff --git a/pyproject.toml b/pyproject.toml index 373a27f..5717ac0 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -9,8 +9,14 @@ requires-python = ">=3.10" readme = "README.md" dependencies = [ "pyserial", - "dynamixel_sdk", - "numpy==1.26.4", + "dynamixel_sdk", + "numpy==1.26.4", + "requests", + "beautifulsoup4>=4.7.0", + "opencv-python>=4.11.0.86", + "pillow>=12.1.1", + "easyocr>=1.7.2", + "opencv-contrib-python>=4.11.0.86", ] description = "An API for controlling the motors of a group of Dynamixel motors." keywords = ["robotics", "api", "dynamixel"] @@ -21,6 +27,8 @@ authors = [ [project.urls] Repository = "https://github.com/SofaComplianceRobotics/DynamixelMotors.API" +[tool.setuptools.package-data] +dynamixelmotorsapi = ["*dynamixel_configs.json"] ####### # Configuration for pydoc-markdown diff --git a/scraper.py b/scraper.py new file mode 100644 index 0000000..860b49a --- /dev/null +++ b/scraper.py @@ -0,0 +1,438 @@ +""" +scrape_dynamixel.py +------------------- +Scrapes all Dynamixel motor pages from emanual.robotis.com and outputs +a JSON file with register addresses, sizes, and specs for each model. + +Usage: + pip install requests beautifulsoup4 + python scrape_dynamixel.py + +Output: + dynamixel_configs.json +""" + +import re +import json +import time +import logging +from dataclasses import dataclass, asdict, field, make_dataclass, fields +from typing import Optional + +import requests +from bs4 import BeautifulSoup + + +from extract_torque_curve import extract_torque_current_points + +logging.basicConfig( + level=logging.INFO, + format="%(asctime)s %(levelname)-7s %(message)s", + datefmt="%H:%M:%S", +) +log = logging.getLogger(__name__) + +BASE_URL = "https://emanual.robotis.com" +INDEX_URL = f"{BASE_URL}/docs/en/dxl/" + +SESSION = requests.Session() +SESSION.headers.update({ + "User-Agent": "Mozilla/5.0 (compatible; DynamixelScraper/1.0)" +}) + +# ── Utils ────────────────────────────────────────────────────────────────────── +def snakify(strToSnakeify: str): + return re.sub(r'\W+', '_', strToSnakeify).strip('_').lower() # works because we know that Dynamixel data names don't have non-ASCII characters + +# ── Data model ───────────────────────────────────────────────────────────────── +from dynamixelmotorsapi._dynamixelmotorsconfigs import ModelConfig +# We extend the base ModelConfig with an 'errors' field to capture any issues during scraping. +MotorConfig = make_dataclass( + "ModelConfig", + [(f.name, Optional[f.type], field(default=None)) for f in fields(ModelConfig)] + + [("errors", list, field(default_factory=list))] +) + +# ── Register name → field mapping ───────────────────────────────────────────── +# +# Each entry: (data_name_to_match, addr_field, len_field_or_None) +# Matching is case-insensitive, first-match wins. +# More specific entries must come before more generic ones. + +REGISTER_MAP = [ + # Baud rate + ("baud rate", "addr_baud_rate", "len_baud_rate", "initial_baud_rate"), + + # Torque / mode + ("torque enable", "addr_torque_enable", "len_torque_enable", "initial_torque_enable"), + ("operating mode", "addr_operating_mode", "len_operating_mode", "initial_operating_mode"), + + # Position + ("goal position", "addr_goal_position", "len_goal_position", "initial_goal_position"), + ("present position", "addr_present_position", "len_present_position", "initial_present_position"), + ("position trajectory", "addr_position_trajectory", "len_position_trajectory", "initial_position_trajectory"), + ("position p gain", "addr_position_p_gain", "len_position_p_gain", "initial_position_p_gain"), + ("position i gain", "addr_position_i_gain", "len_position_i_gain", "initial_position_i_gain"), + ("position d gain", "addr_position_d_gain", "len_position_d_gain", "initial_position_d_gain"), + ("p gain", "addr_position_p_gain", "len_position_p_gain", "initial_position_p_gain"), # some models only have i, p and d gain for position e.g: mx-106/ + ("i gain", "addr_position_i_gain", "len_position_i_gain", "initial_position_i_gain"), + ("d gain", "addr_position_d_gain", "len_position_d_gain", "initial_position_d_gain"), + ("min position limit", "addr_min_position", "len_min_position", "initial_min_position_limit"), + ("max position limit", "addr_max_position", "len_max_position", "initial_max_position_llimit"), + + # Current + ("present current", "addr_present_current", "len_present_current", "initial_present_current"), + + # PWM + ("goal pwm", "addr_goal_pwm", "len_goal_pwm", "initial_goal_pwm"), + ("present pwm", "addr_present_pwm", "len_present_pwm", "initial_present_pwm"), + ("pwm limit", "addr_pwm_limit", "len_pwm_limit", "initial_pwm_limit"), + + # Velocity + ("goal velocity", "addr_goal_velocity", "len_goal_velocity", "initial_goal_velocity"), + ("present velocity", "addr_present_velocity", "len_present_velocity", "initial_present_velocity"), + ("velocity trajectory", "addr_velocity_trajectory", "len_velocity_trajectory", "initial_velocity_trajectory"), + ("profile velocity", "addr_velocity_profile", "len_velocity_profile", "initial_velocity_profile"), + ("velocity p gain", "addr_velocity_p_gain", "len_velocity_p_gain", "initial_velocity_p_gain"), + ("velocity i gain", "addr_velocity_i_gain", "len_velocity_i_gain", "initial_velocity_i_gain"), + ("goal acceleration", "addr_velocity_profile", "len_velocity_profile", "initial_velocity_profile"), # PRO series uses same field for acceleration + + # Moving + ("moving status", "addr_moving_status", "len_moving_status", "initial_moving_status"), + ("moving", "addr_moving", "len_moving", "initial_moving"), +] + + + +# ── Series detection ─────────────────────────────────────────────────────────── + +def detect_series(url: str) -> str: + u = url.lower() + if "/dxl/y/" in u: return "Y_SERIES" + if "/dxl/p/" in u: return "P_SERIES" + if "/dxl/x/" in u: return "X_SERIES" + if "/dxl/mx/" in u: return "MX_SERIES" + if "/dxl/ax/" in u: return "AX_SERIES" + if "/dxl/rx/" in u: return "RX_SERIES" + if "/dxl/ex/" in u: return "EX_SERIES" + if "/dxl/dx/" in u: return "DX_SERIES" + if "/dxl/pro/" in u: + # URLs ending in 'a/' or 'ra/' are the (A) advanced firmware + return "PRO_A_SERIES" if re.search(r"[/-]r?a/?$", u) else "PRO_SERIES" + return "UNKNOWN" + + +# ── Page fetching ────────────────────────────────────────────────────────────── + +def fetch(url: str, retries: int = 3) -> Optional[BeautifulSoup]: + for attempt in range(retries): + try: + resp = SESSION.get(url, timeout=15) + resp.raise_for_status() + soup = BeautifulSoup(resp.content, "html.parser") + # save soup into a file for debugging + with open("debug_soup.html", "w", encoding="utf-8") as f: + f.write(str(soup)) + return soup + except requests.RequestException as e: + wait = 2 ** attempt + log.warning(f" Fetch error ({e}), retrying in {wait}s…") + time.sleep(wait) + return None + + +# ── Index scraping ───────────────────────────────────────────────────────────── + +# URL path segments that are NOT individual motor pages +_SKIP_PATHS = { + "/docs/en/dxl/", + "/docs/en/dxl/protocol1/", + "/docs/en/dxl/protocol2/", + "/docs/en/dxl/dxl-quick-start-guide/", + "/docs/en/dxl/x/", + "/docs/en/dxl/mx/", + "/docs/en/dxl/p/", + "/docs/en/dxl/pro/", + "/docs/en/dxl/y/", +} + +def get_motor_urls() -> list[tuple[str, str]]: + """ + Return a list of (model_name, absolute_url) for every motor listed + in the sidebar of the DYNAMIXEL index page, restricted to /docs/en/dxl/. + """ + soup = fetch(INDEX_URL) + if not soup: + raise RuntimeError("Could not fetch index page") + + seen = set() + motors = [] + + # The sidebar