85 lines
3.5 KiB
Markdown
85 lines
3.5 KiB
Markdown
# dynamixel-controller
|
|
This is a HardwareInterface for [`ros2_control`](https://github.com/ros-controls/ros2_control) for dynamixel actuators from [ROBOTIS](http://en.robotis.com/). Its mainly developed for and with the [XL](http://en.robotis.com/shop_en/list.php?ca_id=202030) series of actuators,
|
|
so other dynamixels may or may not work. Open up a issue when you encounter problems. Pull requests
|
|
are welcome too.
|
|
|
|
## Build
|
|
Make sure the [DynamixelSDK](https://github.com/ROBOTIS-GIT/DynamixelSDK) for ROS2 is installed.
|
|
Clone it in your workspace and checkout the branch ros2:
|
|
```
|
|
git clone https://github.com/ROBOTIS-GIT/DynamixelSDK.git
|
|
cd DynamixelSDK/
|
|
git checkout ros2
|
|
```
|
|
finally build with colcon
|
|
`colcon build`
|
|
|
|
## Configure Dynamixel motor parameters via URDF
|
|
|
|
Add `serial_port`, `baud_rate`, and `joint_ids` parameters to the ros2_control part of your `.urdf` / `.xarco` files.
|
|
|
|
```xml
|
|
<ros2_control name="my_robot" type="system">
|
|
<hardware>
|
|
<plugin>dynamixel_ros2_control/DynamixelHardwareInterface</plugin>
|
|
<param name="usb_port">/dev/ttyUSB0</param>
|
|
<param name="serial_port">/dev/ttyUSB0</param>
|
|
<param name="baud_rate">4000000</param>
|
|
</hardware>
|
|
<joint name="joint1">
|
|
<param name="id">10</param>
|
|
<command_interface name="position"/>
|
|
<!--command_interface name="velocity"/-->
|
|
<state_interface name="position"/>
|
|
<state_interface name="velocity"/>
|
|
<state_interface name="effort"/>
|
|
</joint>
|
|
...
|
|
```
|
|
## Run a test
|
|
You need the urdf of the robot with the ros2_control xml added.
|
|
After starting the control nodes and activating the Joint Trajectory Controller you can use ros2cli commands to test the actuators:
|
|
```
|
|
ros2 topic pub /joint_trajectory_controller/joint_trajectory trajectory_msgs/msg/JointTrajectory '{header: {stamp: {sec: '"$(date '+%s + 6' | bc)"'}}, joint_names: ["joint1"], points: [{positions: [0.0, ], velocities: [0, ], time_from_start: {sec: 0}}]}' --once
|
|
```
|
|
The example launch file expects the urdf in its robot description package and a robot bringup package with the `controller.yaml` (example for that is also in this package).
|
|
|
|
## USB parameter tuning
|
|
To increase the data rate you can decrease the usb latency of the serial contoller.
|
|
On linux you can do that with the help of sysfs:
|
|
```
|
|
sudo chmod o+w /sys/bus/usb-serial/devices/ttyUSB0/latency_timer
|
|
echo 1 > /sys/bus/usb-serial/devices/ttyUSB0/latency_timer
|
|
```
|
|
|
|
# USB Latency tuning
|
|
|
|
To increase the frequency the actuators can be read and be written to via usb you can decrease the usb latency. On Linux you can do that with the following command (if the dynamixel serial device is on `ttyUSB0`):
|
|
```
|
|
sudo chmod o+w /sys/bus/usb-serial/devices/ttyUSB0/latency_timer
|
|
echo 1 > /sys/bus/usb-serial/devices/ttyUSB0/latency_timer
|
|
```
|
|
|
|
This change however gets lost on reboot. You can create a systemd-service to automatically do this on reboot:
|
|
```
|
|
[Unit]
|
|
Description=lower usb latency timer
|
|
|
|
[Service]
|
|
Type=oneshot
|
|
User=root
|
|
ExecStart=/usr/bin/bash -c '/usr/bin/echo 1 > /sys/bus/usb-serial/devices/ttyUSB0/latency_timer'
|
|
|
|
[Install]
|
|
WantedBy=multi-user.target
|
|
```
|
|
(Filepath `/etc/systemd/system/usb-latency.service`)
|
|
and enable with:
|
|
```
|
|
sudo systemctl enable usb-latency.service --now
|
|
```
|
|
|
|
## Credits
|
|
This is partly based on the work of Youtalk (
|
|
[GitHub link](https://github.com/youtalk/dynamixel_control)), who made a dynamixel ros2_control driver.
|
|
I swapt out the dynamixel_workbench parts with my own implementation, that only uses the dynamixelSDK. |