Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
17 changes: 17 additions & 0 deletions .github/workflows/ros-ci.yml
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,23 @@ jobs:
with:
required-ros-distributions: ${{ matrix.ros_distribution }}

- name: Check and Install ROS dependencies
shell: bash
run: |
set -e
source /opt/ros/${{ matrix.ros_distribution }}/setup.bash
echo "--- Updating rosdep definitions ---"
rosdep update
echo "--- Installing system dependencies for ROS 2 ${{ matrix.ros_distribution }} ---"
rosdep install --from-paths ros_ws/src --ignore-src -y -r --rosdistro ${{ matrix.ros_distribution }}
echo "--- Performing rosdep check for ROS 2 ${{ matrix.ros_distribution }} ---"
if rosdep check --from-paths ros_ws/src --ignore-src --rosdistro ${{ matrix.ros_distribution }}; then
echo "--- rosdep check passed ---"
else
echo "--- rosdep check failed: Missing system dependencies or unresolvable keys. ---"
exit 1
fi

- name: Build and Test
uses: ros-tooling/[email protected]
with:
Expand Down
6 changes: 6 additions & 0 deletions CHANGELOG.rst
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,12 @@
Changelog for package dynamixel_hardware_interface
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

1.4.8 (2025-06-23)
------------------
* Added new model for OMY to use virtual_dxl
* Added goal position synchronization before torque enable for OMY sync table feature
* Contributors: Woojin Wie

1.4.7 (2025-06-19)
------------------
* Added virtual_dxl and support for rcu
Expand Down
2 changes: 1 addition & 1 deletion package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>dynamixel_hardware_interface</name>
<version>1.4.7</version>
<version>1.4.8</version>
<description>
ROS 2 package providing a hardware interface for controlling Dynamixel motors via the ROS 2 control framework.
</description>
Expand Down
1 change: 1 addition & 0 deletions param/dxl_model/dynamixel.model
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,7 @@ Number Name
35074 rh_p12_rn.model
220 omy_hat.model
230 omy_end.model
231 omy_end_rh_p12_rn.model
536 sensorxel_joy.model
600 sensorxel_joy.model
602 ffw_g10_rcu.model
Expand Down
19 changes: 0 additions & 19 deletions param/dxl_model/omy_end.model
Original file line number Diff line number Diff line change
@@ -1,12 +1,3 @@
[type info]
name value
value_of_zero_radian_position 0
value_of_max_radian_position 740
value_of_min_radian_position -740
min_radian -1.099
max_radian 1.099
velocity_unit 0.114

[control table]
Address Size Data Name
0 2 Model Number
Expand Down Expand Up @@ -35,16 +26,6 @@ Address Size Data Name
60 2 SyncTable Write Size
378 2 Indirect Address
634 1 Indirect Data
72 1 Hardware Error Status
73 1 Moving
74 2 Present Current
76 4 Present Velocity
80 4 Present Position
84 4 Position Trajectory
200 1 Torque Enable
202 2 Goal Current
204 4 Goal Velocity
208 4 Goal Position
378 2 Indirect Address 1
634 1 Indirect Data 1
378 2 Indirect Address Write
Expand Down
21 changes: 21 additions & 0 deletions param/dxl_model/omy_end_rh_p12_rn.model
Original file line number Diff line number Diff line change
@@ -0,0 +1,21 @@
[type info]
name value
value_of_zero_radian_position 0
value_of_max_radian_position 740
value_of_min_radian_position -740
min_radian -1.099
max_radian 1.099
velocity_unit 0.114

[control table]
Address Size Data Name
72 1 Hardware Error Status
73 1 Moving
74 2 Present Current
76 4 Present Velocity
80 4 Present Position
84 4 Position Trajectory
200 1 Torque Enable
202 2 Goal Current
204 4 Goal Velocity
208 4 Goal Position
4 changes: 4 additions & 0 deletions param/dxl_model/omy_hat.model
Original file line number Diff line number Diff line change
Expand Up @@ -33,3 +33,7 @@ Address Size Data Name
560 1 SyncTable Read Data
688 1 SyncTable Write Data
816 1 Indirect Data
112 2 Indirect Address Write
816 1 Indirect Data Write
176 2 Indirect Address Read
848 1 Indirect Data Read
28 changes: 28 additions & 0 deletions src/dynamixel/dynamixel.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1834,6 +1834,20 @@ DxlError Dynamixel::SetDxlValueToSyncWrite()
int16_t goal_current = dxl_info_.ConvertEffortToCurrent(ID, data);
param_write_value[added_byte + 0] = DXL_LOBYTE(goal_current);
param_write_value[added_byte + 1] = DXL_HIBYTE(goal_current);
} else {
if (indirect_info_write_[comm_id].item_size.at(item_index) == 4) {
int32_t value = static_cast<int32_t>(data);
param_write_value[added_byte + 0] = DXL_LOBYTE(DXL_LOWORD(value));
param_write_value[added_byte + 1] = DXL_HIBYTE(DXL_LOWORD(value));
param_write_value[added_byte + 2] = DXL_LOBYTE(DXL_HIWORD(value));
param_write_value[added_byte + 3] = DXL_HIBYTE(DXL_HIWORD(value));
} else if (indirect_info_write_[comm_id].item_size.at(item_index) == 2) {
int16_t value = static_cast<int16_t>(data);
param_write_value[added_byte + 0] = DXL_LOBYTE(value);
param_write_value[added_byte + 1] = DXL_HIBYTE(value);
} else if (indirect_info_write_[comm_id].item_size.at(item_index) == 1) {
param_write_value[added_byte] = static_cast<uint8_t>(data);
}
}
Comment on lines +1837 to 1851
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

medium

Consider adding a default case to the if/else if block to handle unexpected item_size values. This can help prevent unexpected behavior if a new item size is introduced in the future.

Suggested change
} else {
if (indirect_info_write_[comm_id].item_size.at(item_index) == 4) {
int32_t value = static_cast<int32_t>(data);
param_write_value[added_byte + 0] = DXL_LOBYTE(DXL_LOWORD(value));
param_write_value[added_byte + 1] = DXL_HIBYTE(DXL_LOWORD(value));
param_write_value[added_byte + 2] = DXL_LOBYTE(DXL_HIWORD(value));
param_write_value[added_byte + 3] = DXL_HIBYTE(DXL_HIWORD(value));
} else if (indirect_info_write_[comm_id].item_size.at(item_index) == 2) {
int16_t value = static_cast<int16_t>(data);
param_write_value[added_byte + 0] = DXL_LOBYTE(value);
param_write_value[added_byte + 1] = DXL_HIBYTE(value);
} else if (indirect_info_write_[comm_id].item_size.at(item_index) == 1) {
param_write_value[added_byte] = static_cast<uint8_t>(data);
}
}
} else {
if (indirect_info_write_[comm_id].item_size.at(item_index) == 4) {
int32_t value = static_cast<int32_t>(data);
param_write_value[added_byte + 0] = DXL_LOBYTE(DXL_LOWORD(value));
param_write_value[added_byte + 1] = DXL_HIBYTE(DXL_LOWORD(value));
param_write_value[added_byte + 2] = DXL_LOBYTE(DXL_HIWORD(value));
param_write_value[added_byte + 3] = DXL_HIBYTE(DXL_HIWORD(value));
} else if (indirect_info_write_[comm_id].item_size.at(item_index) == 2) {
int16_t value = static_cast<int16_t>(data);
param_write_value[added_byte + 0] = DXL_LOBYTE(value);
param_write_value[added_byte + 1] = DXL_HIBYTE(value);
} else if (indirect_info_write_[comm_id].item_size.at(item_index) == 1) {
param_write_value[added_byte] = static_cast<uint8_t>(data);
} else {
// Handle unexpected item size
fprintf(stderr, "[ID:%03d] Unexpected item size: %d\n", ID, indirect_info_write_[comm_id].item_size.at(item_index));
}
}

added_byte += indirect_info_write_[comm_id].item_size.at(item_index);
}
Expand Down Expand Up @@ -2053,6 +2067,20 @@ DxlError Dynamixel::SetDxlValueToBulkWrite()
int16_t goal_current = dxl_info_.ConvertEffortToCurrent(ID, data);
param_write_value[added_byte + 0] = DXL_LOBYTE(goal_current);
param_write_value[added_byte + 1] = DXL_HIBYTE(goal_current);
} else {
if (indirect_info_write_[comm_id].item_size.at(item_index) == 4) {
int32_t value = static_cast<int32_t>(data);
param_write_value[added_byte + 0] = DXL_LOBYTE(DXL_LOWORD(value));
param_write_value[added_byte + 1] = DXL_HIBYTE(DXL_LOWORD(value));
param_write_value[added_byte + 2] = DXL_LOBYTE(DXL_HIWORD(value));
param_write_value[added_byte + 3] = DXL_HIBYTE(DXL_HIWORD(value));
} else if (indirect_info_write_[comm_id].item_size.at(item_index) == 2) {
int16_t value = static_cast<int16_t>(data);
param_write_value[added_byte + 0] = DXL_LOBYTE(value);
param_write_value[added_byte + 1] = DXL_HIBYTE(value);
} else if (indirect_info_write_[comm_id].item_size.at(item_index) == 1) {
param_write_value[added_byte] = static_cast<uint8_t>(data);
}
}
Comment on lines +2070 to 2084
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

medium

Consider adding a default case to the if/else if block to handle unexpected item_size values. This can help prevent unexpected behavior if a new item size is introduced in the future.

Suggested change
} else {
if (indirect_info_write_[comm_id].item_size.at(item_index) == 4) {
int32_t value = static_cast<int32_t>(data);
param_write_value[added_byte + 0] = DXL_LOBYTE(DXL_LOWORD(value));
param_write_value[added_byte + 1] = DXL_HIBYTE(DXL_LOWORD(value));
param_write_value[added_byte + 2] = DXL_LOBYTE(DXL_HIWORD(value));
param_write_value[added_byte + 3] = DXL_HIBYTE(DXL_HIWORD(value));
} else if (indirect_info_write_[comm_id].item_size.at(item_index) == 2) {
int16_t value = static_cast<int16_t>(data);
param_write_value[added_byte + 0] = DXL_LOBYTE(value);
param_write_value[added_byte + 1] = DXL_HIBYTE(value);
} else if (indirect_info_write_[comm_id].item_size.at(item_index) == 1) {
param_write_value[added_byte] = static_cast<uint8_t>(data);
}
}
} else {
if (indirect_info_write_[comm_id].item_size.at(item_index) == 4) {
int32_t value = static_cast<int32_t>(data);
param_write_value[added_byte + 0] = DXL_LOBYTE(DXL_LOWORD(value));
param_write_value[added_byte + 1] = DXL_HIBYTE(DXL_LOWORD(value));
param_write_value[added_byte + 2] = DXL_LOBYTE(DXL_HIWORD(value));
param_write_value[added_byte + 3] = DXL_HIBYTE(DXL_HIWORD(value));
} else if (indirect_info_write_[comm_id].item_size.at(item_index) == 2) {
int16_t value = static_cast<int16_t>(data);
param_write_value[added_byte + 0] = DXL_LOBYTE(value);
param_write_value[added_byte + 1] = DXL_HIBYTE(value);
} else if (indirect_info_write_[comm_id].item_size.at(item_index) == 1) {
param_write_value[added_byte] = static_cast<uint8_t>(data);
} else {
// Handle unexpected item size
fprintf(stderr, "[ID:%03d] Unexpected item size: %d\n", ID, indirect_info_write_[comm_id].item_size.at(item_index));
}
}

added_byte += indirect_info_write_[comm_id].item_size.at(item_index);
}
Expand Down
6 changes: 4 additions & 2 deletions src/dynamixel_hardware_interface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -497,9 +497,11 @@ hardware_interface::CallbackReturn DynamixelHardware::start()

CalcTransmissionToJoint();

// sync commands = states joint
SyncJointCommandWithStates();
std::this_thread::sleep_for(std::chrono::milliseconds(500));

CalcJointToTransmission();

dxl_comm_->WriteMultiDxlData();

// Enable torque only for Dynamixels that have torque enabled in their parameters
std::vector<uint8_t> torque_enabled_ids;
Expand Down
Loading