diff --git a/dynamixel_general_hw/.media/sample1_rviz.png b/dynamixel_general_hw/.media/sample1_rviz.png new file mode 100644 index 00000000..8e2f1e21 Binary files /dev/null and b/dynamixel_general_hw/.media/sample1_rviz.png differ diff --git a/dynamixel_general_hw/.media/sample1_rviz_move.png b/dynamixel_general_hw/.media/sample1_rviz_move.png new file mode 100644 index 00000000..d0c9a0f6 Binary files /dev/null and b/dynamixel_general_hw/.media/sample1_rviz_move.png differ diff --git a/dynamixel_general_hw/.media/sample2_rviz.png b/dynamixel_general_hw/.media/sample2_rviz.png new file mode 100644 index 00000000..3ff35787 Binary files /dev/null and b/dynamixel_general_hw/.media/sample2_rviz.png differ diff --git a/dynamixel_general_hw/.media/sample3_rviz.png b/dynamixel_general_hw/.media/sample3_rviz.png new file mode 100644 index 00000000..65e34a0e Binary files /dev/null and b/dynamixel_general_hw/.media/sample3_rviz.png differ diff --git a/dynamixel_general_hw/.media/sample3_rviz_move.png b/dynamixel_general_hw/.media/sample3_rviz_move.png new file mode 100644 index 00000000..f4be3b4f Binary files /dev/null and b/dynamixel_general_hw/.media/sample3_rviz_move.png differ diff --git a/dynamixel_general_hw/CMakeLists.txt b/dynamixel_general_hw/CMakeLists.txt new file mode 100644 index 00000000..18cb50da --- /dev/null +++ b/dynamixel_general_hw/CMakeLists.txt @@ -0,0 +1,74 @@ +cmake_minimum_required(VERSION 3.0.2) +project(dynamixel_general_hw) + +find_package(catkin REQUIRED COMPONENTS + controller_manager + dynamixel_workbench_msgs + dynamixel_workbench_toolbox + hardware_interface + joint_limits_interface + message_generation + roscpp + std_msgs + transmission_interface +) + +add_message_files( + FILES + DynamixelState.msg + DynamixelStateList.msg +) + +generate_messages( + DEPENDENCIES + std_msgs +) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES dynamixel_general_hw + CATKIN_DEPENDS + controller_manager + dynamixel_workbench_msgs + dynamixel_workbench_toolbox + hardware_interface + joint_limits_interface + message_runtime + roscpp + std_msgs + transmission_interface +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +add_library(dynamixel_general_hw src/dynamixel_general_hw.cpp) +add_dependencies(dynamixel_general_hw ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(dynamixel_general_hw ${catkin_LIBRARIES}) + +add_executable(dynamixel_general_control_node src/dynamixel_general_control_node.cpp) +add_dependencies(dynamixel_general_control_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(dynamixel_general_control_node dynamixel_general_hw ${catkin_LIBRARIES}) + +install(TARGETS dynamixel_general_control_node + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(TARGETS dynamixel_general_hw + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +install(DIRECTORY config launch urdf + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + USE_SOURCE_PERMISSIONS +) diff --git a/dynamixel_general_hw/README.md b/dynamixel_general_hw/README.md new file mode 100644 index 00000000..23f500c9 --- /dev/null +++ b/dynamixel_general_hw/README.md @@ -0,0 +1,355 @@ +# dynamixel_general_hw + +General ros_control layer for Dynamixel actuators. With this layer, you can load [ros_control](http://wiki.ros.org/ros_control) controllers (e.g., [joint_trajectory_controller](http://wiki.ros.org/joint_trajectory_controller)) and make the actuators follow commands from those controllers. + +## Samples + +### Sample 1: simplest situation + +This sample assumes that one bare Dynamixel actuator whose ID and baud rate are `1` and `57600` is connected via the port `/dev/ttyUSB0`. +You can change the baud rate and the port via roslaunch arguments. + +```bash +roslaunch dynamixel_general_hw sample1.launch port_name:=/dev/ttyUSB0 baud_rate:=57600 +# If you face an error about Operating_Mode, please change the actuator mode to "Joint Mode" by yourself and execute the following: +# roslaunch dynamixel_general_hw sample1.launch port_name:=/dev/ttyUSB0 baud_rate:=57600 protocol_1_0:=true +``` + +You will see the following RViz window showing a virtual robot synchronized with the real actuator: +
+ +
+ +You can move the actuator by sending a command via `/sample_robot/position_joint_trajectory_controller/follow_joint_trajectory` action. +To try sending a command, +1. Type + ```bash + rostopic pub /sample_robot/position_joint_trajectory_controller/follow_joint_trajectory/goal + ``` + and enter `Tab` three times. +2. Rewrite `time_from_start` and `positions` of the auto-completed output as you want +3. Type `sample_joint` into the empty string following `joint_names` of the auto-completed output + +For example, if you send a command like: +```bash +rostopic pub /sample_robot/position_joint_trajectory_controller/follow_joint_trajectory/goal control_msgs/FollowJointTrajectoryActionGoal "header: + seq: 0 + stamp: + secs: 0 + nsecs: 0 + frame_id: '' +goal_id: + stamp: + secs: 0 + nsecs: 0 + id: '' +goal: + trajectory: + header: + seq: 0 + stamp: + secs: 0 + nsecs: 0 + frame_id: '' + joint_names: + - 'sample_joint' + points: + - positions: [0.78] + velocities: [0] + accelerations: [0] + effort: [0] + time_from_start: {secs: 10, nsecs: 0} + path_tolerance: + - {name: '', position: 0.0, velocity: 0.0, acceleration: 0.0} + goal_tolerance: + - {name: '', position: 0.0, velocity: 0.0, acceleration: 0.0} + goal_time_tolerance: {secs: 0, nsecs: 0}" +``` +you will get the following final state: +
+ +
+ +Note that this commanding method is only for checking on command line. +Please use [actionlib](http://wiki.ros.org/actionlib) when you write a code. + +#### Configuration files + +When you read the contents of `sample1.launch`, you will notice that there are three configuration files: + +- `$(find dynamixel_general_hw)/urdf/sample1.urdf` + + URDF of the virtual robot. + This describes the structure of that robot and the relationship between the joint of that robot and the real actuator. + Because [ros_control](http://wiki.ros.org/ros_control) controllers are joint-level, that relationship is required to control that actuator via those controllers. + +- `$(find dynamixel_general_hw)/config/sample1_2/dynamixel_info.yaml` + + Configuration file for the target Dynamixel actuator. + This describes the name of the target actuator and its initial configuration. + +- `$(find dynamixel_general_hw)/config/sample1_2/default_controllers.yaml` + + Configuration file for [ros_control](http://wiki.ros.org/ros_control) controllers. + +Please check those files to learn how you can configure dynamixel_general_hw. +Following samples also provide different examples of the configuration files. + +### Sample 2: with mechanical reduction and joint offset + +This sample assumes that one bare Dynamixel actuator whose ID and baud rate are `1` and `57600` is connected via the port `/dev/ttyUSB0`. +You can change the baud rate and the port via roslaunch arguments. + +```bash +roslaunch dynamixel_general_hw sample2.launch port_name:=/dev/ttyUSB0 baud_rate:=57600 +# If you face an error about Operating_Mode, please change the actuator mode to "Joint Mode" by yourself and execute the following: +# roslaunch dynamixel_general_hw sample2.launch port_name:=/dev/ttyUSB0 baud_rate:=57600 protocol_1_0:=true +``` + +You will see the following RViz window showing a virtual robot synchronized with the real actuator: +
+ +
+ +By launching after making the actuator position the same as Sample 1, you will notice that the joint position has an offset in comparison with Sample 1. +In addition, by sending some commands, you will notice there is a reduction between the real actuator and the virtual joint on RViz. + +### Sample 3: multiple actuators + +This sample assumes that two bare Dynamixel actuators whose IDs and baud rate are `1`, `2`, and `57600` are connected via the port `/dev/ttyUSB0`. +You can change the baud rate and the port via roslaunch arguments. + +```bash +roslaunch dynamixel_general_hw sample3.launch port_name:=/dev/ttyUSB0 baud_rate:=57600 +# If you face an error about Operating_Mode, please change the actuator mode to "Joint Mode" by yourself and execute the following: +# roslaunch dynamixel_general_hw sample3.launch port_name:=/dev/ttyUSB0 baud_rate:=57600 protocol_1_0:=true +``` + +You will see the following RViz window showing a virtual robot synchronized with the real actuators: +
+ +
+ +For example, if you send a command like: +```bash +rostopic pub /sample_robot/position_joint_trajectory_controller/follow_joint_trajectory/goal control_msgs/FollowJointTrajectoryActionGoal "header: + seq: 0 + stamp: + secs: 0 + nsecs: 0 + frame_id: '' +goal_id: + stamp: + secs: 0 + nsecs: 0 + id: '' +goal: + trajectory: + header: + seq: 0 + stamp: + secs: 0 + nsecs: 0 + frame_id: '' + joint_names: ['sample_pan_joint', 'sample_tilt_joint'] + points: + - positions: [-0.78, 0.78] + velocities: [] + accelerations: [] + effort: [] + time_from_start: {secs: 10, nsecs: 0} + path_tolerance: + - {name: '', position: 0.0, velocity: 0.0, acceleration: 0.0} + goal_tolerance: + - {name: '', position: 0.0, velocity: 0.0, acceleration: 0.0} + goal_time_tolerance: {secs: 0, nsecs: 0}" +``` +you will get the following final state: +
+ +
+ +### Sample 4: velocity control + +This sample assumes that one bare Dynamixel actuator whose ID and baud rate are `1` and `57600` is connected via the port `/dev/ttyUSB0`. +You can change the baud rate and the port via roslaunch arguments. + +```bash +roslaunch dynamixel_general_hw sample4.launch port_name:=/dev/ttyUSB0 baud_rate:=57600 +# If you face an error about Operating_Mode, please change the actuator mode to "Wheel Mode" by yourself and execute the following: +# roslaunch dynamixel_general_hw sample4.launch port_name:=/dev/ttyUSB0 baud_rate:=57600 protocol_1_0:=true +``` + +You can move the actuator by sending a command via `/sample_robot/joint_group_velocity_controller/command` topic. +For example, if you send a command like: +```bash +rostopic pub /sample_robot/joint_group_velocity_controller/command std_msgs/Float64MultiArray "layout: + dim: + - label: '' + size: 0 + stride: 0 + data_offset: 0 +data: +- 1" +``` +you will see the actuator rotates in 1 rad/s. +You can check the real velocity by `rostopic echo /sample_robot/joint_states`. +(Although those interfaces are joint-level, the joint equals the actuator in this sample.) + +### Sample 5: effort (torque) control + +This sample assumes that one bare Dynamixel actuator whose ID and baud rate are `1` and `57600` is connected via the port `/dev/ttyUSB0`. +You can change the baud rate and the port via roslaunch arguments. + +```bash +roslaunch dynamixel_general_hw sample5.launch port_name:=/dev/ttyUSB0 baud_rate:=57600 +# If you face an error about Operating_Mode or a warning that effort command to your actuator model is currently not supported, current dynamixel_general_hw does not support effort control of your actuator model. +# Your contribution is welcome +``` + +You can move the actuator by sending a command via `/sample_robot/joint_group_effort_controller/command` topic. +For example, if you send a command like: +```bash +rostopic pub /sample_robot/joint_group_effort_controller/command std_msgs/Float64MultiArray "layout: + dim: + - label: '' + size: 0 + stride: 0 + data_offset: 0 +data: +- 0.1" +``` +you will see the actuator outputs 0.1 Nm. +You can check the output effort by `rostopic echo /sample_robot/joint_states`. +(Although those interfaces are joint-level, the joint equals the actuator in this sample.) + +Note that this effort value is realistic only when `torque_constant` in `dynamixel_info.yaml` is well-configured. +If you want the actuator to output the exact effort you command, you may have to prepare your own `dynamixel_info.yaml` and pass it to `dynamixel_general_control.launch`. +In addition, if you command a big effort, the actuator vibrates because its velocity sometimes violates the velocity limit defined in URDF and the effort command is overwritten to zero. +If you want to avoid this vibration, you should prepare your own URDF having a well-configured velocity limit and pass it to `dynamixel_general_control.launch`. + +### Sample 6: position control with effort limit (called "Current-based Position Control Mode" in Dynamixel manual) + +This sample assumes that one bare Dynamixel actuator whose ID and baud rate are `1` and `57600` is connected via the port `/dev/ttyUSB0`. +You can change the baud rate and the port via roslaunch arguments. + +```bash +roslaunch dynamixel_general_hw sample6.launch port_name:=/dev/ttyUSB0 baud_rate:=57600 +# If you face an error about Operating_Mode or a warning that effort command to your actuator model is currently not supported, current dynamixel_general_hw does not support Current-based Position Control Mode of your actuator model. +# Your contribution is welcome +``` + +You can move the actuator by: +1. sending an effort command via `/sample_robot/joint_group_effort_controller/command` topic +2. sending a position command via `/sample_robot/position_joint_trajectory_controller/follow_joint_trajectory` action + +For example, if you send commands like: +```bash +rostopic pub /sample_robot/joint_group_effort_controller/command std_msgs/Float64MultiArray "layout: + dim: + - label: '' + size: 0 + stride: 0 + data_offset: 0 +data: +- 0.1" +``` +```bash +rostopic pub /sample_robot/position_joint_trajectory_controller/follow_joint_trajectory/goal control_msgs/FollowJointTrajectoryActionGoal "header: + seq: 0 + stamp: + secs: 0 + nsecs: 0 + frame_id: '' +goal_id: + stamp: + secs: 0 + nsecs: 0 + id: '' +goal: + trajectory: + header: + seq: 0 + stamp: + secs: 0 + nsecs: 0 + frame_id: '' + joint_names: + - 'sample_joint' + points: + - positions: [1.0] + velocities: [0] + accelerations: [0] + effort: [0] + time_from_start: {secs: 1, nsecs: 0} + path_tolerance: + - {name: '', position: 0.0, velocity: 0.0, acceleration: 0.0} + goal_tolerance: + - {name: '', position: 0.0, velocity: 0.0, acceleration: 0.0} + goal_time_tolerance: {secs: 0, nsecs: 0}" +``` +you will see the actuator moves to the commanded position (1.0 rad) with limited effort (<= 0.1 Nm). +You can check the real effort by `rostopic echo /sample_robot/joint_states`. +(Although those interfaces are joint-level, the joint equals the actuator in this sample.) + +## Launch files + +### dynamixel_general_control.launch + +A launch file to run the ros_control layer. Launch it when Dynamixel actuators are connected to your PC. + +#### Arguments + +Check them by `roslaunch dynamixel_general_hw dynamixel_general_control.launch --ros-args`: +``` +Required Arguments: + baud_rate: Baud rate of target Dynamixel actuators (e.g., '57600') + controllers_file: Configuration file for ros_control controllers (e.g., '/home/pazeshun/catkin_ws/src/dynamixel-workbench/dynamixel_general_hw/config/sample1_2/default_controllers.yaml') + controllers_to_start: Controllers started at launching (e.g., 'joint_state_controller position_joint_trajectory_controller') + dynamixel_info_file: Configuration file for target Dynamixel actuators (e.g., '/home/pazeshun/catkin_ws/src/dynamixel-workbench/dynamixel_general_hw/config/sample1_2/dynamixel_info.yaml') + namespace: Namespace of the nodes started by this launch file. Specifying your robot's name or the name of the part using target actuators is recommended (e.g., 'sample_robot') + port_name: Port connecting with target Dynamixel actuators (e.g., '/dev/ttyUSB0') + robot_description_args: Xacro args to parse robot_description_file + robot_description_file: URDF/Xacro of your robot (e.g., '/home/pazeshun/catkin_ws/src/dynamixel-workbench/dynamixel_general_hw/urdf/sample1.urdf'). If 'set_robot_description' is false, you can set a dummy string to this argument because this argument is not used +Optional Arguments: + calculate_effort (default "true"): Whether to calculate joint effort from Dynamixel actuator current/load + control_rate (default "20"): Target Hz of the control loop. The actual Hz will be the same as or lower than this. If the actual Hz is too low for you, try increasing baud rate and/or decreasing latency_timer. Cf. https://github.com/ROBOTIS-GIT/DynamixelSDK/blob/3ae73bf5179fbad2bd366f39a952ce549c10c58e/c%2B%2B/src/dynamixel_sdk/port_handler_linux.cpp#L33-L56. In addition, if the actual Hz is too lower than control_rate, reading errors may occur. See the description of write_read_interval below + joint_states_topic (default "joint_states"): Name of joint_states topic published and subscribed by the nodes started by this launch file. If this is a relative name, its global name becomes (namespace)/(joint_states_topic) + launch_robot_state_publisher (default "true"): Whether to start robot_state_publisher at launching + launch_rviz (default "false"): Whether to start RViz + publish_input_voltage (default "true"): Whether to publish Dynamixel actuator input voltage + publish_temperature (default "true"): Whether to publish Dynamixel actuator temperature + required (default "true"): Whether to kill entire roslaunch if control node dies + respawn (default "false"): Whether to restart control node automatically if it quits + robot_description_param (default "robot_description"): Name of robot_description parameter read by the nodes started by this launch file. If this is a relative name, its global name becomes (namespace)/(robot_description_param) + rvizconfig (default "/home/pazeshun/catkin_ws/src/dynamixel-workbench/dynamixel_general_hw/config/sample_robot.rviz"): Configuration file for RViz + set_robot_description (default "true"): Whether to set robot_description parameter at launching + write_read_interval (default "-1"): Minimal interval [sec] from writing Dynamixel to reading Dynamixel. Non-positive value means that interval will be entirely determined by control_rate and how long reading and writing take. You can increase this if you face reading error even when you decrease latency_timer as much as you can. Cf. https://forum.robotis.com/t/error-reading-position-value-after-write-position/6207 +``` + +#### Minimal publishing topics + +- `$(arg namespace)/dynamixel_general_control/dynamixel_state` (`dynamixel_general_hw/DynamixelStateList`) + + States of the connected actuators. + +#### Minimal subscribing topics + +- `$(arg namespace)/dynamixel_general_control/servo` (`std_msgs/Bool`) + + Servo ON/OFF signal. `true` means servo ON and `false` means servo OFF. + You can use this to implement an emergency stop function. + Servo OFF resets the other special states (i.e., `hold_position`). + +- `$(arg namespace)/dynamixel_general_control/hold_position` (`std_msgs/Bool`) + + When this is `true`, the actuators hold their current positions regardless of commanded positions. + You can also use this to implement an emergency stop function. + Note that if you use effort (torque) control or "Current-based Position Control Mode" on some actuators, they become torque OFF for a moment before holding their positions. + If this behavior is unacceptable to your application, avoid using this topic. + +#### Minimal services + +- `$(arg namespace)/dynamixel_general_control/dynamixel_command` (`dynamixel_workbench_msgs/DynamixelCommand`) + + Direct command to Dynamixel control table. This is the same as [/dynamixel_command service on dynamixel_workbench](https://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_workbench/#controllers). diff --git a/dynamixel_general_hw/config/sample1_2/default_controllers.yaml b/dynamixel_general_hw/config/sample1_2/default_controllers.yaml new file mode 100644 index 00000000..94590365 --- /dev/null +++ b/dynamixel_general_hw/config/sample1_2/default_controllers.yaml @@ -0,0 +1,8 @@ +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: $(arg control_rate) + +position_joint_trajectory_controller: + type: position_controllers/JointTrajectoryController + joints: + - sample_joint diff --git a/dynamixel_general_hw/config/sample1_2/dynamixel_info.yaml b/dynamixel_general_hw/config/sample1_2/dynamixel_info.yaml new file mode 100644 index 00000000..961c5f6a --- /dev/null +++ b/dynamixel_general_hw/config/sample1_2/dynamixel_info.yaml @@ -0,0 +1,19 @@ +# You can find control table of Dynamixel on emanual (http://emanual.robotis.com/#control-table). +# Control table item has to be set Camel_Case and not included whitespace. +# You are supposed to set at least Dynamixel ID. +# torque_constant is a special value which is not in the control table. +# This value is used for converting current ("Present Current" in [A]) or load ("Present Load" in dimensionless ratio) into torque [Nm] by the following equations: +# current * torque_constant = torque +# load * torque_constant = torque +# Note that ROBOTIS does not provide this value and you can set a value suitable for your application. +# Typical example value can be calculated from Stall Torque information. +# Current case: torque_constant = (Stall Torque [Nm]) / (Current at Stall Torque [A]) +# Load case: torque_constant = (Stall Torque [Nm]) +sample_motor: + ID: 1 + Return_Delay_Time: 0 + Operating_Mode: 3 # Position Control Mode + torque_constant: 1.15 +# Above torque_constant is an example value for XC330-T288-T: (Stall Torque [Nm]) / (Current at Stall Torque [A]) = 0.92 / 0.80 +# You do not have to change that value when you just try this sample because that value have little influence on main topics of this sample. +# But when you apply dynamixel_general_hw to your robot, you may have to change this value in your yaml depending on your situation (e.g., you use a different actuator, a different torque constant value is good for your operation range) diff --git a/dynamixel_general_hw/config/sample1_2/dynamixel_info_1_0.yaml b/dynamixel_general_hw/config/sample1_2/dynamixel_info_1_0.yaml new file mode 100644 index 00000000..14d6c129 --- /dev/null +++ b/dynamixel_general_hw/config/sample1_2/dynamixel_info_1_0.yaml @@ -0,0 +1,18 @@ +# You can find control table of Dynamixel on emanual (http://emanual.robotis.com/#control-table). +# Control table item has to be set Camel_Case and not included whitespace. +# You are supposed to set at least Dynamixel ID. +# torque_constant is a special value which is not in the control table. +# This value is used for converting current ("Present Current" in [A]) or load ("Present Load" in dimensionless ratio) into torque [Nm] by the following equations: +# current * torque_constant = torque +# load * torque_constant = torque +# Note that ROBOTIS does not provide this value and you can set a value suitable for your application. +# Typical example value can be calculated from Stall Torque information. +# Current case: torque_constant = (Stall Torque [Nm]) / (Current at Stall Torque [A]) +# Load case: torque_constant = (Stall Torque [Nm]) +sample_motor: + ID: 1 + Return_Delay_Time: 0 + torque_constant: 1.15 +# Above torque_constant is an example value for XC330-T288-T: (Stall Torque [Nm]) / (Current at Stall Torque [A]) = 0.92 / 0.80 +# You do not have to change that value when you just try this sample because that value have little influence on main topics of this sample. +# But when you apply dynamixel_general_hw to your robot, you may have to change this value in your yaml depending on your situation (e.g., you use a different actuator, a different torque constant value is good for your operation range) diff --git a/dynamixel_general_hw/config/sample3/default_controllers.yaml b/dynamixel_general_hw/config/sample3/default_controllers.yaml new file mode 100644 index 00000000..0b4f375d --- /dev/null +++ b/dynamixel_general_hw/config/sample3/default_controllers.yaml @@ -0,0 +1,9 @@ +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: $(arg control_rate) + +position_joint_trajectory_controller: + type: position_controllers/JointTrajectoryController + joints: + - sample_pan_joint + - sample_tilt_joint diff --git a/dynamixel_general_hw/config/sample3/dynamixel_info.yaml b/dynamixel_general_hw/config/sample3/dynamixel_info.yaml new file mode 100644 index 00000000..5ef88f87 --- /dev/null +++ b/dynamixel_general_hw/config/sample3/dynamixel_info.yaml @@ -0,0 +1,13 @@ +sample_pan_motor: + ID: 1 + Return_Delay_Time: 0 + Operating_Mode: 3 # Position Control Mode + torque_constant: 1.15 +sample_tilt_motor: + ID: 2 + Return_Delay_Time: 0 + Operating_Mode: 3 # Position Control Mode + torque_constant: 1.15 +# Above torque_constant is an example value for XC330-T288-T: (Stall Torque [Nm]) / (Current at Stall Torque [A]) = 0.92 / 0.80 +# You do not have to change that value when you just try this sample because that value have little influence on main topics of this sample. +# But when you apply dynamixel_general_hw to your robot, you may have to change this value in your yaml depending on your situation (e.g., you use a different actuator, a different torque constant value is good for your operation range) diff --git a/dynamixel_general_hw/config/sample3/dynamixel_info_1_0.yaml b/dynamixel_general_hw/config/sample3/dynamixel_info_1_0.yaml new file mode 100644 index 00000000..274a1e4e --- /dev/null +++ b/dynamixel_general_hw/config/sample3/dynamixel_info_1_0.yaml @@ -0,0 +1,11 @@ +sample_pan_motor: + ID: 1 + Return_Delay_Time: 0 + torque_constant: 1.15 +sample_tilt_motor: + ID: 2 + Return_Delay_Time: 0 + torque_constant: 1.15 +# Above torque_constant is an example value for XC330-T288-T: (Stall Torque [Nm]) / (Current at Stall Torque [A]) = 0.92 / 0.80 +# You do not have to change that value when you just try this sample because that value have little influence on main topics of this sample. +# But when you apply dynamixel_general_hw to your robot, you may have to change this value in your yaml depending on your situation (e.g., you use a different actuator, a different torque constant value is good for your operation range) diff --git a/dynamixel_general_hw/config/sample4/default_controllers.yaml b/dynamixel_general_hw/config/sample4/default_controllers.yaml new file mode 100644 index 00000000..c4d585ab --- /dev/null +++ b/dynamixel_general_hw/config/sample4/default_controllers.yaml @@ -0,0 +1,8 @@ +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: $(arg control_rate) + +joint_group_velocity_controller: + type: velocity_controllers/JointGroupVelocityController + joints: + - sample_joint diff --git a/dynamixel_general_hw/config/sample4/dynamixel_info.yaml b/dynamixel_general_hw/config/sample4/dynamixel_info.yaml new file mode 100644 index 00000000..a311a8ef --- /dev/null +++ b/dynamixel_general_hw/config/sample4/dynamixel_info.yaml @@ -0,0 +1,8 @@ +sample_motor: + ID: 1 + Return_Delay_Time: 0 + Operating_Mode: 1 # Velocity Control Mode + torque_constant: 1.15 +# Above torque_constant is an example value for XC330-T288-T: (Stall Torque [Nm]) / (Current at Stall Torque [A]) = 0.92 / 0.80 +# You do not have to change that value when you just try this sample because that value have little influence on main topics of this sample. +# But when you apply dynamixel_general_hw to your robot, you may have to change this value in your yaml depending on your situation (e.g., you use a different actuator, a different torque constant value is good for your operation range) diff --git a/dynamixel_general_hw/config/sample4/dynamixel_info_1_0.yaml b/dynamixel_general_hw/config/sample4/dynamixel_info_1_0.yaml new file mode 100644 index 00000000..9bb69d8d --- /dev/null +++ b/dynamixel_general_hw/config/sample4/dynamixel_info_1_0.yaml @@ -0,0 +1,7 @@ +sample_motor: + ID: 1 + Return_Delay_Time: 0 + torque_constant: 1.15 +# Above torque_constant is an example value for XC330-T288-T: (Stall Torque [Nm]) / (Current at Stall Torque [A]) = 0.92 / 0.80 +# You do not have to change that value when you just try this sample because that value have little influence on main topics of this sample. +# But when you apply dynamixel_general_hw to your robot, you may have to change this value in your yaml depending on your situation (e.g., you use a different actuator, a different torque constant value is good for your operation range) diff --git a/dynamixel_general_hw/config/sample5/default_controllers.yaml b/dynamixel_general_hw/config/sample5/default_controllers.yaml new file mode 100644 index 00000000..79c5b468 --- /dev/null +++ b/dynamixel_general_hw/config/sample5/default_controllers.yaml @@ -0,0 +1,8 @@ +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: $(arg control_rate) + +joint_group_effort_controller: + type: effort_controllers/JointGroupEffortController + joints: + - sample_joint diff --git a/dynamixel_general_hw/config/sample5/dynamixel_info.yaml b/dynamixel_general_hw/config/sample5/dynamixel_info.yaml new file mode 100644 index 00000000..56ce678e --- /dev/null +++ b/dynamixel_general_hw/config/sample5/dynamixel_info.yaml @@ -0,0 +1,8 @@ +sample_motor: + ID: 1 + Return_Delay_Time: 0 + Operating_Mode: 0 # Current Control Mode + torque_constant: 1.15 +# Above torque_constant is an example value for XC330-T288-T: (Stall Torque [Nm]) / (Current at Stall Torque [A]) = 0.92 / 0.80 +# You do not have to change that value when you just try this sample because that value have little influence on main topics of this sample. +# But when you apply dynamixel_general_hw to your robot, you may have to change this value in your yaml depending on your situation (e.g., you use a different actuator, a different torque constant value is good for your operation range) diff --git a/dynamixel_general_hw/config/sample6/default_controllers.yaml b/dynamixel_general_hw/config/sample6/default_controllers.yaml new file mode 100644 index 00000000..57e31963 --- /dev/null +++ b/dynamixel_general_hw/config/sample6/default_controllers.yaml @@ -0,0 +1,13 @@ +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: $(arg control_rate) + +position_joint_trajectory_controller: + type: position_controllers/JointTrajectoryController + joints: + - sample_joint + +joint_group_effort_controller: + type: effort_controllers/JointGroupEffortController + joints: + - sample_joint diff --git a/dynamixel_general_hw/config/sample6/dynamixel_info.yaml b/dynamixel_general_hw/config/sample6/dynamixel_info.yaml new file mode 100644 index 00000000..273827df --- /dev/null +++ b/dynamixel_general_hw/config/sample6/dynamixel_info.yaml @@ -0,0 +1,8 @@ +sample_motor: + ID: 1 + Return_Delay_Time: 0 + Operating_Mode: 5 # Current-based Position Control Mode + torque_constant: 1.15 +# Above torque_constant is an example value for XC330-T288-T: (Stall Torque [Nm]) / (Current at Stall Torque [A]) = 0.92 / 0.80 +# You do not have to change that value when you just try this sample because that value have little influence on main topics of this sample. +# But when you apply dynamixel_general_hw to your robot, you may have to change this value in your yaml depending on your situation (e.g., you use a different actuator, a different torque constant value is good for your operation range) diff --git a/dynamixel_general_hw/config/sample_robot.rviz b/dynamixel_general_hw/config/sample_robot.rviz new file mode 100644 index 00000000..9e3c86f0 --- /dev/null +++ b/dynamixel_general_hw/config/sample_robot.rviz @@ -0,0 +1,145 @@ +Panels: + - Class: rviz/Displays + Help Height: 138 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 330 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + SyncMode: 0 + SyncSource: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sample_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: /sample_robot/robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 4.338220119476318 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.570398211479187 + Target Frame: + Yaw: 0.8003979921340942 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001c400000244fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b1fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006f000002440000018400fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000013f00000244fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000006f000002440000013500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000065900000060fc0100000002fb0000000800540069006d0065010000000000000659000005cd00fffffffb0000000800540069006d006501000000000000045000000000000000000000033e0000024400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1625 + X: 60 + Y: 54 diff --git a/dynamixel_general_hw/include/dynamixel_general_hw/dynamixel_general_hw.h b/dynamixel_general_hw/include/dynamixel_general_hw/dynamixel_general_hw.h new file mode 100644 index 00000000..897941db --- /dev/null +++ b/dynamixel_general_hw/include/dynamixel_general_hw/dynamixel_general_hw.h @@ -0,0 +1,150 @@ +#ifndef DYNAMIXEL_GENERAL_HW_H +#define DYNAMIXEL_GENERAL_HW_H + +// C++ +#include +#include +#include + +// Dynamixel workbench +#include + +// ROS base +#include +#include +#include + +// ros_control +#include +#include +#include +#include +#include +#include + +// ROS msg and srv +#include +#include +#include + +// SYNC_WRITE_HANDLER +#define SYNC_WRITE_HANDLER_FOR_GOAL_POSITION 0 +#define SYNC_WRITE_HANDLER_FOR_GOAL_VELOCITY 1 +#define SYNC_WRITE_HANDLER_FOR_GOAL_CURRENT 2 + +// SYNC_READ_HANDLER(Only for Protocol 2.0) +#define SYNC_READ_HANDLER_FOR_PRESENT_INFO 0 + +namespace dynamixel_general_hw +{ + +typedef struct +{ + std::string item_name; + int32_t value; +} ItemValue; + +class DynamixelGeneralHw : public hardware_interface::RobotHW +{ +protected: + ros::NodeHandle nh_; + ros::NodeHandle pnh_; + + // Dynamixel workbench parameters + std::unique_ptr dxl_wb_; + std::map dynamixel_; + std::map control_items_; + std::vector> dynamixel_info_; + dynamixel_general_hw::DynamixelStateList dynamixel_state_list_; + uint16_t read_start_addr_; + uint16_t read_length_; + double write_read_interval_; + ros::Time last_write_tm_; + + // Transmission loader + transmission_interface::RobotTransmissions robot_transmissions_; + std::unique_ptr transmission_loader_; + + // Joint limits interface + std::vector jnt_names_; + joint_limits_interface::PositionJointSaturationInterface pos_jnt_sat_interface_; + joint_limits_interface::VelocityJointSaturationInterface vel_jnt_sat_interface_; + joint_limits_interface::EffortJointSaturationInterface eff_jnt_sat_interface_; + joint_limits_interface::PositionJointSoftLimitsInterface pos_jnt_soft_interface_; + joint_limits_interface::VelocityJointSoftLimitsInterface vel_jnt_soft_interface_; + joint_limits_interface::EffortJointSoftLimitsInterface eff_jnt_soft_interface_; + + // Actuator interface to transmission loader + hardware_interface::ActuatorStateInterface actr_state_interface_; + hardware_interface::PositionActuatorInterface pos_actr_interface_; + hardware_interface::VelocityActuatorInterface vel_actr_interface_; + hardware_interface::EffortActuatorInterface eff_actr_interface_; + + // Actuator raw data + std::vector actr_names_; + std::vector actr_curr_pos_; + std::vector actr_curr_vel_; + std::vector actr_curr_eff_; + std::vector actr_cmd_pos_; + std::vector actr_cmd_vel_; + std::vector actr_cmd_eff_; + + // Actuator parameters + std::map torque_consts_; + + // E-stop interface + ros::Subscriber servo_sub_; + bool is_servo_raw_; + bool is_servo_; + bool prev_is_servo_; + ros::Subscriber hold_pos_sub_; + bool is_hold_pos_raw_; + bool is_hold_pos_; + bool prev_is_hold_pos_; + std::map> normal_modes_; + + // Dynamixel-specific interfaces + ros::Publisher dynamixel_state_pub_; + ros::ServiceServer dynamixel_cmd_srv_; + + // For multi-threaded spinning + std::shared_ptr subscriber_spinner_; + ros::CallbackQueue subscriber_queue_; + std::mutex mtx_; + + bool is_calc_effort_; + bool is_pub_temp_; + bool is_pub_volt_; + bool is_current_ctrl_; + bool is_current_eq_load_; + +public: + DynamixelGeneralHw(); + + virtual bool checkForConflict(const std::list& info) const override; + + bool initWorkbench(const std::string port_name, const uint32_t baud_rate); + bool getDynamixelsInfo(XmlRpc::XmlRpcValue& dxl_info); + bool loadDynamixels(void); + bool initDynamixels(void); + bool initControlItems(void); + bool initSDKHandlers(void); + bool initRosInterface(void); + + void cleanup(void); + + void readDynamixelState(void); + void read(void); + void write(const ros::Time& time, const ros::Duration& period); + + bool isJntCmdIgnored(void); + + void servoCallback(const std_msgs::BoolConstPtr& msg); + void holdPosCallback(const std_msgs::BoolConstPtr& msg); + bool dynamixelCmdCallback(dynamixel_workbench_msgs::DynamixelCommand::Request& req, + dynamixel_workbench_msgs::DynamixelCommand::Response& res); +}; // end class DynamixelGeneralHw + +}; // end namespace dynamixel_general_hw + +#endif // DYNAMIXEL_GENERAL_HW_H diff --git a/dynamixel_general_hw/launch/dynamixel_general_control.launch b/dynamixel_general_hw/launch/dynamixel_general_control.launch new file mode 100644 index 00000000..01b839b6 --- /dev/null +++ b/dynamixel_general_hw/launch/dynamixel_general_control.launch @@ -0,0 +1,86 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + port_name: $(arg port_name) + baud_rate: $(arg baud_rate) + control_rate: $(arg control_rate) + calculate_effort: $(arg calculate_effort) + publish_temperature: $(arg publish_temperature) + publish_input_voltage: $(arg publish_input_voltage) + write_read_interval: $(arg write_read_interval) + + + + + + + + + + + + diff --git a/dynamixel_general_hw/launch/sample1.launch b/dynamixel_general_hw/launch/sample1.launch new file mode 100644 index 00000000..7def12e5 --- /dev/null +++ b/dynamixel_general_hw/launch/sample1.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/dynamixel_general_hw/launch/sample2.launch b/dynamixel_general_hw/launch/sample2.launch new file mode 100644 index 00000000..754426d8 --- /dev/null +++ b/dynamixel_general_hw/launch/sample2.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/dynamixel_general_hw/launch/sample3.launch b/dynamixel_general_hw/launch/sample3.launch new file mode 100644 index 00000000..a7481bf7 --- /dev/null +++ b/dynamixel_general_hw/launch/sample3.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/dynamixel_general_hw/launch/sample4.launch b/dynamixel_general_hw/launch/sample4.launch new file mode 100644 index 00000000..7e1c8212 --- /dev/null +++ b/dynamixel_general_hw/launch/sample4.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/dynamixel_general_hw/launch/sample5.launch b/dynamixel_general_hw/launch/sample5.launch new file mode 100644 index 00000000..ed4bc3fb --- /dev/null +++ b/dynamixel_general_hw/launch/sample5.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/dynamixel_general_hw/launch/sample6.launch b/dynamixel_general_hw/launch/sample6.launch new file mode 100644 index 00000000..1ba74674 --- /dev/null +++ b/dynamixel_general_hw/launch/sample6.launch @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/dynamixel_general_hw/msg/DynamixelState.msg b/dynamixel_general_hw/msg/DynamixelState.msg new file mode 100644 index 00000000..c7fbcfe6 --- /dev/null +++ b/dynamixel_general_hw/msg/DynamixelState.msg @@ -0,0 +1,10 @@ +# This message includes basic data of dynamixel + +string name # Name of dynamixel +uint8 id # ID of dynamixel + +int32 present_position # Present position (raw value in dynamixel RAM) +int32 present_velocity # Present velocity (raw value in dynamixel RAM) +int16 present_current # Present current/load (raw value in dynamixel RAM) +uint8 present_temperature # Present temperature (raw value in dynamixel RAM) +uint16 present_input_voltage # Present input voltage (raw value in dynamixel RAM) diff --git a/dynamixel_general_hw/msg/DynamixelStateList.msg b/dynamixel_general_hw/msg/DynamixelStateList.msg new file mode 100644 index 00000000..43cf6158 --- /dev/null +++ b/dynamixel_general_hw/msg/DynamixelStateList.msg @@ -0,0 +1,2 @@ +Header header +DynamixelState[] dynamixel_state diff --git a/dynamixel_general_hw/package.xml b/dynamixel_general_hw/package.xml new file mode 100644 index 00000000..b4b2af11 --- /dev/null +++ b/dynamixel_general_hw/package.xml @@ -0,0 +1,32 @@ + + + dynamixel_general_hw + 0.0.0 + General ros_control layer for Dynamixel actuators + + Shun Hasegawa + Shun Hasegawa + + BSD + + catkin + controller_manager + dynamixel_workbench_msgs + dynamixel_workbench_toolbox + hardware_interface + joint_limits_interface + roscpp + std_msgs + transmission_interface + message_generation + effort_controllers + joint_state_controller + joint_trajectory_controller + message_runtime + robot_state_publisher + velocity_controllers + xacro + + + + diff --git a/dynamixel_general_hw/src/dynamixel_general_control_node.cpp b/dynamixel_general_hw/src/dynamixel_general_control_node.cpp new file mode 100644 index 00000000..16ab085e --- /dev/null +++ b/dynamixel_general_hw/src/dynamixel_general_control_node.cpp @@ -0,0 +1,104 @@ +// ros_control +#include + +#include "dynamixel_general_hw/dynamixel_general_hw.h" + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "dynamixel_general_control_node"); + + std::string port_name; + int baud_rate; + XmlRpc::XmlRpcValue dxl_info; + int rate_hz; + + if (!(ros::param::get("~port_name", port_name) && ros::param::get("~baud_rate", baud_rate) && + ros::param::get("~dynamixel_info", dxl_info) && ros::param::get("~control_rate", rate_hz))) + { + ROS_ERROR("Couldn't get necessary parameters"); + return 0; + } + + dynamixel_general_hw::DynamixelGeneralHw dxl_hw; + + bool result = false; + + result = dxl_hw.initWorkbench(port_name, baud_rate); + if (result == false) + { + ROS_ERROR("Please check USB port name"); + return 0; + } + + result = dxl_hw.getDynamixelsInfo(dxl_info); + if (result == false) + { + ROS_ERROR("Please check dynamixel_info"); + return 0; + } + + result = dxl_hw.loadDynamixels(); + if (result == false) + { + ROS_ERROR("Please check Dynamixel ID or BaudRate"); + return 0; + } + + result = dxl_hw.initDynamixels(); + if (result == false) + { + ROS_ERROR("Please check control table (http://emanual.robotis.com/#control-table)"); + return 0; + } + + result = dxl_hw.initControlItems(); + if (result == false) + { + ROS_ERROR("Please check control items"); + return 0; + } + + result = dxl_hw.initSDKHandlers(); + if (result == false) + { + ROS_ERROR("Failed to set Dynamixel SDK Handler"); + return 0; + } + + result = dxl_hw.initRosInterface(); + if (result == false) + { + return 0; + } + + controller_manager::ControllerManager cm(&dxl_hw); + + // For non-realtime spinner thread + ros::AsyncSpinner spinner(1); + spinner.start(); + + // Control loop + ros::Rate rate(rate_hz); + ros::Time prev_time = ros::Time::now(); + bool prev_controller_ignored = false; + + while (ros::ok()) + { + const ros::Time now = ros::Time::now(); + const ros::Duration elapsed_time = now - prev_time; + + dxl_hw.read(); + const bool controller_ignored = dxl_hw.isJntCmdIgnored(); + const bool reset_controllers = (prev_controller_ignored && !controller_ignored); + cm.update(now, elapsed_time, reset_controllers); + dxl_hw.write(now, elapsed_time); + prev_time = now; + prev_controller_ignored = controller_ignored; + + rate.sleep(); + } + spinner.stop(); + dxl_hw.cleanup(); + + return 0; +} diff --git a/dynamixel_general_hw/src/dynamixel_general_hw.cpp b/dynamixel_general_hw/src/dynamixel_general_hw.cpp new file mode 100644 index 00000000..af2a06d9 --- /dev/null +++ b/dynamixel_general_hw/src/dynamixel_general_hw.cpp @@ -0,0 +1,1248 @@ +#include "dynamixel_general_hw/dynamixel_general_hw.h" + +namespace dynamixel_general_hw +{ + +DynamixelGeneralHw::DynamixelGeneralHw() + : pnh_("~") + , is_servo_raw_(true) + , is_servo_(true) + , prev_is_servo_(true) + , is_hold_pos_raw_(false) + , is_hold_pos_(false) + , prev_is_hold_pos_(false) + , is_current_ctrl_(true) + , is_current_eq_load_(false) + , last_write_tm_(0) +{ + is_calc_effort_ = pnh_.param("calculate_effort", true); + is_pub_temp_ = pnh_.param("publish_temperature", true); + is_pub_volt_ = pnh_.param("publish_input_voltage", true); + write_read_interval_ = pnh_.param("write_read_interval", -1); + + dxl_wb_.reset(new DynamixelWorkbench); +} + +// Override resource conflict check (https://github.com/ros-controls/ros_control/blob/0.20.0/hardware_interface/include/hardware_interface/robot_hw.h#L113-L146) +// to accept position and effort commands simultaneously for Current-based Position Control Mode of Dynamixel without creating new JointInterface. +// https://robotics.stackexchange.com/questions/65092/using-both-jointtrajectorycontroller-and-jointpositioncontroller-at-same-time-in/65093#65093 +bool DynamixelGeneralHw::checkForConflict(const std::list& info) const +{ + // Map from resource name to all controllers claiming it + std::map> resource_map; + + // Populate a map of all controllers claiming individual resources. + // We do this by iterating over every claimed resource of every hardware interface used by every controller + for (const auto& controller_info : info) + { + for (const auto& claimed_resource : controller_info.claimed_resources) + { + for (const auto& iface_resource : claimed_resource.resources) + { + hardware_interface::ControllerInfo unique_info; + unique_info.name = controller_info.name; + unique_info.type = controller_info.type; + unique_info.claimed_resources.push_back(hardware_interface::InterfaceResources()); + unique_info.claimed_resources[0].hardware_interface = claimed_resource.hardware_interface; + unique_info.claimed_resources[0].resources.insert(iface_resource); + resource_map[iface_resource].push_back(unique_info); + } + } + } + + // Enforce resource exclusivity policy: No resource can be claimed by more than one controller + bool in_conflict = false; + for (const auto& resource_name_and_claiming_controllers : resource_map) + { + if (resource_name_and_claiming_controllers.second.size() > 1) + { + bool prev_in_conflict = in_conflict; + + std::string controller_list; + for (const auto& controller : resource_name_and_claiming_controllers.second) + controller_list += controller.name + ", "; + ROS_WARN("Resource conflict on [%s]. Controllers = [%s]", resource_name_and_claiming_controllers.first.c_str(), controller_list.c_str()); + in_conflict = true; + + // Accept position and effort commands simultaneously for Current-based Position Control Mode of Dynamixel + // with common JointInterface (hardware_interface::PositionJointInterface/EffortJointInterface) + const auto& rnacc = resource_name_and_claiming_controllers; + if (rnacc.second.size() == 2 && + std::find_if(rnacc.second.begin(), rnacc.second.end(), + [](hardware_interface::ControllerInfo ci) { + return (ci.claimed_resources[0].hardware_interface == + "hardware_interface::PositionJointInterface"); + }) != rnacc.second.end() && + std::find_if(rnacc.second.begin(), rnacc.second.end(), + [](hardware_interface::ControllerInfo ci) { + return (ci.claimed_resources[0].hardware_interface == + "hardware_interface::EffortJointInterface"); + }) != rnacc.second.end()) + { + ROS_WARN_STREAM("However, this conflict is accepted because " + << rnacc.second.front().name << " uses " + << rnacc.second.front().claimed_resources[0].hardware_interface << " and " + << rnacc.second.back().name << " uses " + << rnacc.second.back().claimed_resources[0].hardware_interface + << ". Commands from both interfaces are used in Current-based Position Control Mode of " + "Dynamixel"); + in_conflict = prev_in_conflict; + } + } + } + + return in_conflict; +} + +bool DynamixelGeneralHw::initWorkbench(const std::string port_name, const uint32_t baud_rate) +{ + bool result = false; + const char* log; + + result = dxl_wb_->init(port_name.c_str(), baud_rate, &log); + if (result == false) + { + ROS_ERROR("%s", log); + } + + return result; +} + +bool DynamixelGeneralHw::getDynamixelsInfo(XmlRpc::XmlRpcValue& dxl_info) +{ + if (dxl_info.getType() != XmlRpc::XmlRpcValue::TypeStruct) + { + ROS_ERROR("dynamixel_info is not dictionary"); + return false; + } + for (std::map::iterator it_info = dxl_info.begin(); + it_info != dxl_info.end(); it_info++) + { + std::string name = it_info->first; + if (name.size() == 0) + { + continue; + } + + XmlRpc::XmlRpcValue& item = it_info->second; + for (std::map::iterator it_item = item.begin(); + it_item != item.end(); it_item++) + { + std::string item_name = it_item->first; + + if (item_name == "torque_constant") + { + torque_consts_[name] = it_item->second; + } + else + { + int32_t value = it_item->second; + + if (item_name == "ID") + { + dynamixel_[name] = value; + } + + ItemValue item_value = {item_name, value}; + std::pair info(name, item_value); + + dynamixel_info_.push_back(info); + } + } + } + + return true; +} + +bool DynamixelGeneralHw::loadDynamixels(void) +{ + bool result = false; + const char* log; + + for (auto const& dxl:dynamixel_) + { + uint16_t model_number = 0; + result = dxl_wb_->ping((uint8_t)dxl.second, &model_number, &log); + if (result == false) + { + ROS_ERROR("%s", log); + ROS_ERROR("Can't find Dynamixel ID '%d'", dxl.second); + return result; + } + else + { + ROS_INFO("Name : %s, ID : %d, Model Number : %d", dxl.first.c_str(), dxl.second, model_number); + } + } + + return result; +} + +bool DynamixelGeneralHw::initDynamixels(void) +{ + const char* log; + + for (auto const& dxl:dynamixel_) + { + dxl_wb_->torqueOff((uint8_t)dxl.second); + + for (auto const& info:dynamixel_info_) + { + if (dxl.first == info.first) + { + if (info.second.item_name != "ID" && info.second.item_name != "Baud_Rate") + { + bool result = dxl_wb_->itemWrite((uint8_t)dxl.second, info.second.item_name.c_str(), info.second.value, &log); + if (result == false) + { + ROS_ERROR("%s", log); + ROS_ERROR("Failed to write value[%d] on items[%s] to Dynamixel[Name : %s, ID : %d]", info.second.value, info.second.item_name.c_str(), dxl.first.c_str(), dxl.second); + return false; + } + } + } + } + + dxl_wb_->torqueOn((uint8_t)dxl.second); + } + + return true; +} + +bool DynamixelGeneralHw::initControlItems(void) +{ + bool result = false; + const char* log = NULL; + + auto it = dynamixel_.begin(); + + const ControlItem *goal_position = dxl_wb_->getItemInfo(it->second, "Goal_Position"); + if (goal_position == NULL) + { + return false; + } + + const ControlItem *goal_velocity = dxl_wb_->getItemInfo(it->second, "Goal_Velocity"); + if (goal_velocity == NULL) + { + goal_velocity = dxl_wb_->getItemInfo(it->second, "Moving_Speed"); + } + if (goal_velocity == NULL) + { + return false; + } + + const ControlItem *goal_current = dxl_wb_->getItemInfo(it->second, "Goal_Current"); + if (goal_current == NULL) + { + ROS_WARN("Effort command to %s is currently not supported", dxl_wb_->getModelName(it->second)); + is_current_ctrl_ = false; + } + + const ControlItem *present_position = dxl_wb_->getItemInfo(it->second, "Present_Position"); + if (present_position == NULL) + { + return false; + } + + const ControlItem *present_velocity = dxl_wb_->getItemInfo(it->second, "Present_Velocity"); + if (present_velocity == NULL) + { + present_velocity = dxl_wb_->getItemInfo(it->second, "Present_Speed"); + } + if (present_velocity == NULL) + { + return false; + } + + const ControlItem *present_current = dxl_wb_->getItemInfo(it->second, "Present_Current"); + if (present_current == NULL) + { + present_current = dxl_wb_->getItemInfo(it->second, "Present_Load"); + is_current_eq_load_ = true; + } + if (present_current == NULL) + { + is_current_eq_load_ = false; + return false; + } + + const ControlItem *present_temperature = dxl_wb_->getItemInfo(it->second, "Present_Temperature"); + if (present_temperature == NULL) + { + return false; + } + + const ControlItem *present_input_voltage = dxl_wb_->getItemInfo(it->second, "Present_Input_Voltage"); + if (present_input_voltage == NULL) + { + present_input_voltage = dxl_wb_->getItemInfo(it->second, "Present_Voltage"); + } + if (present_input_voltage == NULL) + { + return false; + } + + control_items_["Goal_Position"] = goal_position; + control_items_["Goal_Velocity"] = goal_velocity; + control_items_["Goal_Current"] = goal_current; + + control_items_["Present_Position"] = present_position; + control_items_["Present_Velocity"] = present_velocity; + control_items_["Present_Current"] = present_current; + control_items_["Present_Temperature"] = present_temperature; + control_items_["Present_Input_Voltage"] = present_input_voltage; + + return true; +} + +bool DynamixelGeneralHw::initSDKHandlers(void) +{ + bool result = false; + const char* log = NULL; + + auto it = dynamixel_.begin(); + + result = dxl_wb_->addSyncWriteHandler(control_items_["Goal_Position"]->address, control_items_["Goal_Position"]->data_length, &log); + if (result == false) + { + ROS_ERROR("%s", log); + return result; + } + else + { + ROS_INFO("%s", log); + } + + result = dxl_wb_->addSyncWriteHandler(control_items_["Goal_Velocity"]->address, control_items_["Goal_Velocity"]->data_length, &log); + if (result == false) + { + ROS_ERROR("%s", log); + return result; + } + else + { + ROS_INFO("%s", log); + } + + if (is_current_ctrl_) + { + result = dxl_wb_->addSyncWriteHandler(control_items_["Goal_Current"]->address, control_items_["Goal_Current"]->data_length, &log); + if (result == false) + { + ROS_ERROR("%s", log); + return result; + } + else + { + ROS_INFO("%s", log); + } + } + + std::vector target_items = {"Present_Position", "Present_Velocity", "Present_Current"}; + if (is_pub_temp_) + { + target_items.push_back("Present_Temperature"); + } + if (is_pub_volt_) + { + target_items.push_back("Present_Input_Voltage"); + } + std::vector target_addrs(target_items.size()); + std::transform(target_items.begin(), target_items.end(), target_addrs.begin(), + [this](std::string x) { return control_items_[x]->address; } ); + + std::vector::iterator min_addr = std::min_element(target_addrs.begin(), target_addrs.end()); + std::vector::iterator max_addr = std::max_element(target_addrs.begin(), target_addrs.end()); + int max_addr_idx = std::distance(target_addrs.begin(), max_addr); + read_start_addr_ = *min_addr; + read_length_ = (*max_addr - *min_addr) + control_items_[target_items[max_addr_idx]]->data_length; + + if (dxl_wb_->getProtocolVersion() == 2.0f) + { + result = dxl_wb_->addSyncReadHandler(read_start_addr_, read_length_, &log); + if (result == false) + { + ROS_ERROR("%s", log); + return result; + } + } + + return result; +} + +bool DynamixelGeneralHw::initRosInterface(void) +{ + // Register actuator interfaces to transmission loader + actr_names_.clear(); + for (const std::pair& dxl : dynamixel_) + { + actr_names_.push_back(dxl.first); + } + actr_curr_pos_.resize(actr_names_.size(), 0); + actr_curr_vel_.resize(actr_names_.size(), 0); + actr_curr_eff_.resize(actr_names_.size(), 0); + actr_cmd_pos_.resize(actr_names_.size(), 0); + actr_cmd_vel_.resize(actr_names_.size(), 0); + actr_cmd_eff_.resize(actr_names_.size(), 0); + for (int i = 0; i < actr_names_.size(); i++) + { + hardware_interface::ActuatorStateHandle state_handle(actr_names_[i], &actr_curr_pos_[i], &actr_curr_vel_[i], &actr_curr_eff_[i]); + actr_state_interface_.registerHandle(state_handle); + + hardware_interface::ActuatorHandle position_handle(state_handle, &actr_cmd_pos_[i]); + pos_actr_interface_.registerHandle(position_handle); + hardware_interface::ActuatorHandle velocity_handle(state_handle, &actr_cmd_vel_[i]); + vel_actr_interface_.registerHandle(velocity_handle); + hardware_interface::ActuatorHandle effort_handle(state_handle, &actr_cmd_eff_[i]); + eff_actr_interface_.registerHandle(effort_handle); + } + registerInterface(&actr_state_interface_); + registerInterface(&pos_actr_interface_); + registerInterface(&vel_actr_interface_); + registerInterface(&eff_actr_interface_); + + // Initialize transmission loader + try + { + transmission_loader_.reset(new transmission_interface::TransmissionInterfaceLoader(this, &robot_transmissions_)); + } + catch (const std::invalid_argument& ex) + { + ROS_ERROR_STREAM("Failed to create transmission interface loader. " << ex.what()); + return false; + } + catch (const pluginlib::LibraryLoadException& ex) + { + ROS_ERROR_STREAM("Failed to create transmission interface loader. " << ex.what()); + return false; + } + catch (...) + { + ROS_ERROR_STREAM("Failed to create transmission interface loader. "); + return false; + } + + // Load URDF from parameter + std::string urdf_string; + nh_.getParam("robot_description", urdf_string); + while (urdf_string.empty() && ros::ok()) + { + ROS_INFO_STREAM_THROTTLE(10, "Waiting for robot_description..."); + nh_.getParam("robot_description", urdf_string); + ros::Duration(0.1).sleep(); + } + if (ros::ok()) + { + ROS_INFO_STREAM("Got robot_description"); + } + else + { + return false; + } + + // Extract transmission infos from URDF + transmission_interface::TransmissionParser parser; + std::vector infos; + if (!parser.parse(urdf_string, infos)) + { + ROS_ERROR("Error parsing URDF"); + return false; + } + + // Load transmissions composed of target actuators + jnt_names_.clear(); + std::vector found_actrs; + for (const transmission_interface::TransmissionInfo& info : infos) + { + if (std::find(actr_names_.begin(), actr_names_.end(), info.actuators_[0].name_) != actr_names_.end()) + { + for (const transmission_interface::ActuatorInfo& actuator : info.actuators_) + { + if (std::find(actr_names_.begin(), actr_names_.end(), actuator.name_) == actr_names_.end()) + { + ROS_ERROR_STREAM("Error loading transmission: " << info.name_); + ROS_ERROR_STREAM("Cannot find " << actuator.name_ << " in target actuators"); + return false; + } + } + if (!transmission_loader_->load(info)) + { + ROS_ERROR_STREAM("Error loading transmission: " << info.name_); + return false; + } + ROS_INFO_STREAM("Loaded transmission: " << info.name_); + for (const transmission_interface::ActuatorInfo& actuator : info.actuators_) + { + found_actrs.push_back(actuator.name_); + } + for (const transmission_interface::JointInfo& joint : info.joints_) + { + jnt_names_.push_back(joint.name_); + } + } + } + for (const std::string& actr_name : actr_names_) + { + if (std::find(found_actrs.begin(), found_actrs.end(), actr_name) == found_actrs.end()) + { + ROS_ERROR_STREAM("Cannot find " << actr_name << " in loaded transmissions"); + return false; + } + } + + // Prepare joint limits interfaces + urdf::Model urdf_model; + if (!urdf_model.initString(urdf_string)) + { + ROS_ERROR("Error parsing URDF"); + return false; + } + hardware_interface::PositionJointInterface* pos_if = get(); + hardware_interface::VelocityJointInterface* vel_if = get(); + hardware_interface::EffortJointInterface* eff_if = get(); + for (const std::string& jnt_name : jnt_names_) + { + joint_limits_interface::JointLimits limits; + joint_limits_interface::SoftJointLimits soft_limits; + bool has_limits_urdf = getJointLimits(urdf_model.getJoint(jnt_name), limits); + bool has_limits_param = getJointLimits(jnt_name, nh_, limits); + if (has_limits_urdf || has_limits_param) + { + if (pos_if) + { + joint_limits_interface::PositionJointSaturationHandle pos_sat_handle(pos_if->getHandle(jnt_name), limits); + pos_jnt_sat_interface_.registerHandle(pos_sat_handle); + } + if (vel_if) + { + joint_limits_interface::VelocityJointSaturationHandle vel_sat_handle(vel_if->getHandle(jnt_name), limits); + vel_jnt_sat_interface_.registerHandle(vel_sat_handle); + } + if (eff_if) + { + joint_limits_interface::EffortJointSaturationHandle eff_sat_handle(eff_if->getHandle(jnt_name), limits); + eff_jnt_sat_interface_.registerHandle(eff_sat_handle); + } + } + has_limits_urdf = getSoftJointLimits(urdf_model.getJoint(jnt_name), soft_limits); + has_limits_param = getSoftJointLimits(jnt_name, nh_, soft_limits); + if (has_limits_urdf || has_limits_param) + { + if (pos_if) + { + joint_limits_interface::PositionJointSoftLimitsHandle pos_soft_handle(pos_if->getHandle(jnt_name), limits, + soft_limits); + pos_jnt_soft_interface_.registerHandle(pos_soft_handle); + } + if (vel_if) + { + joint_limits_interface::VelocityJointSoftLimitsHandle vel_soft_handle(vel_if->getHandle(jnt_name), limits, + soft_limits); + vel_jnt_soft_interface_.registerHandle(vel_soft_handle); + } + if (eff_if) + { + joint_limits_interface::EffortJointSoftLimitsHandle eff_soft_handle(eff_if->getHandle(jnt_name), limits, + soft_limits); + eff_jnt_soft_interface_.registerHandle(eff_soft_handle); + } + } + } + + // Initialize E-stop interface + servo_sub_ = pnh_.subscribe("servo", 1, &DynamixelGeneralHw::servoCallback, this); + hold_pos_sub_ = pnh_.subscribe("hold_position", 1, &DynamixelGeneralHw::holdPosCallback, this); + + // Initialize dynamixel-specific interfaces + dynamixel_state_pub_ = pnh_.advertise("dynamixel_state", 100); + dynamixel_cmd_srv_ = pnh_.advertiseService("dynamixel_command", &DynamixelGeneralHw::dynamixelCmdCallback, this); + + // Start spinning + nh_.setCallbackQueue(&subscriber_queue_); + subscriber_spinner_.reset(new ros::AsyncSpinner(1, &subscriber_queue_)); + subscriber_spinner_->start(); + + return true; +} + +void DynamixelGeneralHw::cleanup(void) +{ + subscriber_spinner_->stop(); +} + +void DynamixelGeneralHw::readDynamixelState(void) +{ + bool result = false; + const char* log = NULL; + + dynamixel_general_hw::DynamixelState dynamixel_state[dynamixel_.size()]; + dynamixel_state_list_.dynamixel_state.clear(); + + int32_t get_current[dynamixel_.size()]; + int32_t get_velocity[dynamixel_.size()]; + int32_t get_position[dynamixel_.size()]; + int32_t get_temperature[dynamixel_.size()]; + int32_t get_voltage[dynamixel_.size()]; + + uint8_t id_array[dynamixel_.size()]; + uint8_t id_cnt = 0; + + for (auto const& dxl:dynamixel_) + { + dynamixel_state[id_cnt].name = dxl.first; + dynamixel_state[id_cnt].id = (uint8_t)dxl.second; + + id_array[id_cnt++] = (uint8_t)dxl.second; + } + + if (write_read_interval_ > 0 && last_write_tm_ > ros::Time(0)) + { + // Sleep until write_read_interval_ is accomplished + const ros::Time now = ros::Time::now(); + if (now > last_write_tm_) + { + double sleep_dur = write_read_interval_ - (now - last_write_tm_).toSec(); + if (sleep_dur > 0) + { + ros::Duration(sleep_dur).sleep(); + } + } + } + if (dxl_wb_->getProtocolVersion() == 2.0f) + { + result = dxl_wb_->syncRead(SYNC_READ_HANDLER_FOR_PRESENT_INFO, + id_array, + dynamixel_.size(), + &log); + if (result == false) + { + ROS_ERROR("%s", log); + } + + result = dxl_wb_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_INFO, + id_array, + id_cnt, + control_items_["Present_Current"]->address, + control_items_["Present_Current"]->data_length, + get_current, + &log); + if (result == false) + { + ROS_ERROR("%s", log); + } + + result = dxl_wb_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_INFO, + id_array, + id_cnt, + control_items_["Present_Velocity"]->address, + control_items_["Present_Velocity"]->data_length, + get_velocity, + &log); + if (result == false) + { + ROS_ERROR("%s", log); + } + + result = dxl_wb_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_INFO, + id_array, + id_cnt, + control_items_["Present_Position"]->address, + control_items_["Present_Position"]->data_length, + get_position, + &log); + if (result == false) + { + ROS_ERROR("%s", log); + } + + if (is_pub_temp_) + { + result = dxl_wb_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_INFO, + id_array, + id_cnt, + control_items_["Present_Temperature"]->address, + control_items_["Present_Temperature"]->data_length, + get_temperature, + &log); + if (result == false) + { + ROS_ERROR("%s", log); + } + } + + if (is_pub_volt_) + { + result = dxl_wb_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_INFO, + id_array, + id_cnt, + control_items_["Present_Input_Voltage"]->address, + control_items_["Present_Input_Voltage"]->data_length, + get_voltage, + &log); + if (result == false) + { + ROS_ERROR("%s", log); + } + } + + for(uint8_t index = 0; index < id_cnt; index++) + { + dynamixel_state[index].present_current = get_current[index]; + dynamixel_state[index].present_velocity = get_velocity[index]; + dynamixel_state[index].present_position = get_position[index]; + if (is_pub_temp_) + { + dynamixel_state[index].present_temperature = get_temperature[index]; + } + if (is_pub_volt_) + { + dynamixel_state[index].present_input_voltage = get_voltage[index]; + } + + dynamixel_state_list_.dynamixel_state.push_back(dynamixel_state[index]); + } + } + else if(dxl_wb_->getProtocolVersion() == 1.0f) + { + uint32_t get_all_data[read_length_]; + uint8_t dxl_cnt = 0; + for (auto const& dxl:dynamixel_) + { + result = dxl_wb_->readRegister((uint8_t)dxl.second, + read_start_addr_, + read_length_, + get_all_data, + &log); + if (result == false) + { + ROS_ERROR("%s", log); + } + + uint16_t idx; + uint16_t len; + idx = control_items_["Present_Current"]->address - read_start_addr_; + len = control_items_["Present_Current"]->data_length; + if (len == 1) + { + dynamixel_state[dxl_cnt].present_current = get_all_data[idx]; + } + else if (len == 2) + { + dynamixel_state[dxl_cnt].present_current = DXL_MAKEWORD(get_all_data[idx], get_all_data[idx + 1]); + } + else if (len == 4) + { + dynamixel_state[dxl_cnt].present_current = + DXL_MAKEDWORD(DXL_MAKEWORD(get_all_data[idx], get_all_data[idx + 1]), + DXL_MAKEWORD(get_all_data[idx + 2], get_all_data[idx + 3])); + } + idx = control_items_["Present_Velocity"]->address - read_start_addr_; + len = control_items_["Present_Velocity"]->data_length; + if (len == 1) + { + dynamixel_state[dxl_cnt].present_velocity = get_all_data[idx]; + } + else if (len == 2) + { + dynamixel_state[dxl_cnt].present_velocity = DXL_MAKEWORD(get_all_data[idx], get_all_data[idx + 1]); + } + else if (len == 4) + { + dynamixel_state[dxl_cnt].present_velocity = + DXL_MAKEDWORD(DXL_MAKEWORD(get_all_data[idx], get_all_data[idx + 1]), + DXL_MAKEWORD(get_all_data[idx + 2], get_all_data[idx + 3])); + } + idx = control_items_["Present_Position"]->address - read_start_addr_; + len = control_items_["Present_Position"]->data_length; + if (len == 1) + { + dynamixel_state[dxl_cnt].present_position = get_all_data[idx]; + } + else if (len == 2) + { + dynamixel_state[dxl_cnt].present_position = DXL_MAKEWORD(get_all_data[idx], get_all_data[idx + 1]); + } + else if (len == 4) + { + dynamixel_state[dxl_cnt].present_position = + DXL_MAKEDWORD(DXL_MAKEWORD(get_all_data[idx], get_all_data[idx + 1]), + DXL_MAKEWORD(get_all_data[idx + 2], get_all_data[idx + 3])); + } + + if (is_pub_temp_) + { + idx = control_items_["Present_Temperature"]->address - read_start_addr_; + len = control_items_["Present_Temperature"]->data_length; + if (len == 1) + { + dynamixel_state[dxl_cnt].present_temperature = get_all_data[idx]; + } + else if (len == 2) + { + dynamixel_state[dxl_cnt].present_temperature = DXL_MAKEWORD(get_all_data[idx], get_all_data[idx + 1]); + } + else if (len == 4) + { + dynamixel_state[dxl_cnt].present_temperature = + DXL_MAKEDWORD(DXL_MAKEWORD(get_all_data[idx], get_all_data[idx + 1]), + DXL_MAKEWORD(get_all_data[idx + 2], get_all_data[idx + 3])); + } + } + + if (is_pub_volt_) + { + idx = control_items_["Present_Input_Voltage"]->address - read_start_addr_; + len = control_items_["Present_Input_Voltage"]->data_length; + if (len == 1) + { + dynamixel_state[dxl_cnt].present_input_voltage = get_all_data[idx]; + } + else if (len == 2) + { + dynamixel_state[dxl_cnt].present_input_voltage = DXL_MAKEWORD(get_all_data[idx], get_all_data[idx + 1]); + } + else if (len == 4) + { + dynamixel_state[dxl_cnt].present_input_voltage = + DXL_MAKEDWORD(DXL_MAKEWORD(get_all_data[idx], get_all_data[idx + 1]), + DXL_MAKEWORD(get_all_data[idx + 2], get_all_data[idx + 3])); + } + } + + dynamixel_state_list_.dynamixel_state.push_back(dynamixel_state[dxl_cnt]); + dxl_cnt++; + } + } + dynamixel_state_list_.header.stamp = ros::Time::now(); +} + +void DynamixelGeneralHw::read(void) +{ + std::lock_guard lock(mtx_); + + // Read current dynamixel state + readDynamixelState(); + dynamixel_state_pub_.publish(dynamixel_state_list_); + + // Convert dynamixel state to ros_control actuator state + uint8_t id_cnt = 0; + for (const std::pair& dxl : dynamixel_) + { + double torque_const = torque_consts_[dxl.first]; + if (is_calc_effort_ && torque_const > 0) + { + double current = 0; + if (is_current_eq_load_) + { + current = dxl_wb_->convertValue2Load((int16_t)dynamixel_state_list_.dynamixel_state[id_cnt].present_current); + current /= 100.0; // % -> dimensionless + } + else + { + current = dxl_wb_->convertValue2Current((int16_t)dynamixel_state_list_.dynamixel_state[id_cnt].present_current); + current /= 1000.0; // mA -> A + } + actr_curr_eff_[id_cnt] = torque_const * current; + } + + actr_curr_vel_[id_cnt] = dxl_wb_->convertValue2Velocity((uint8_t)dxl.second, (int32_t)dynamixel_state_list_.dynamixel_state[id_cnt].present_velocity); + actr_curr_pos_[id_cnt] = dxl_wb_->convertValue2Radian((uint8_t)dxl.second, (int32_t)dynamixel_state_list_.dynamixel_state[id_cnt].present_position); + + id_cnt++; + } + + // Propagate current actuator state to joints + if (robot_transmissions_.get()) + { + robot_transmissions_.get()->propagate(); + } + + // Update E-stop status + is_servo_ = is_servo_raw_; + is_hold_pos_ = is_hold_pos_raw_; +} + +void DynamixelGeneralHw::write(const ros::Time& time, const ros::Duration& period) +{ + std::lock_guard lock(mtx_); + + if (is_servo_) + { + // Servo off -> on + if (!prev_is_servo_) + { + for (const std::pair& dxl : dynamixel_) + { + dxl_wb_->torqueOn((uint8_t)dxl.second); + } + } + + if (robot_transmissions_.get()) + { + // Update & send actuator position commands only when is_hold_pos_ is false. + // This holds current positions of position-controlled actuators while is_hold_pos_ is true + if (!is_hold_pos_) + { + // Enforce joint position limits + pos_jnt_sat_interface_.enforceLimits(period); + pos_jnt_soft_interface_.enforceLimits(period); + + // Propagate joint position commands to actuators + robot_transmissions_.get()->propagate(); + + // Convert ros_control actuator position command to dynamixel command + std::vector pos_id_vec; + std::vector dxl_pos_vec; + uint8_t id_cnt = 0; + for (const std::pair& dxl : dynamixel_) + { + if (std::isnan(actr_cmd_pos_[id_cnt])) + { + ROS_DEBUG_STREAM_DELAYED_THROTTLE(10, "Skipping position command to " << dxl.first + << " because it is NaN. Its " + "controller may not work"); + } + else + { + pos_id_vec.push_back((uint8_t)dxl.second); + dxl_pos_vec.push_back(dxl_wb_->convertRadian2Value((uint8_t)dxl.second, actr_cmd_pos_[id_cnt])); + } + id_cnt++; + } + + // Write position command to dynamixel + if (pos_id_vec.size() > 0) + { + bool result = false; + const char* log = NULL; + uint8_t pos_id[pos_id_vec.size()]; + int32_t dxl_pos[dxl_pos_vec.size()]; + std::copy(pos_id_vec.begin(), pos_id_vec.end(), pos_id); + std::copy(dxl_pos_vec.begin(), dxl_pos_vec.end(), dxl_pos); + result = + dxl_wb_->syncWrite(SYNC_WRITE_HANDLER_FOR_GOAL_POSITION, pos_id, pos_id_vec.size(), dxl_pos, 1, &log); + if (result == false) + { + ROS_ERROR("%s", log); + } + last_write_tm_ = ros::Time::now(); + } + } + } + if (robot_transmissions_.get()) + { + // Update actuator velocity commands only when is_hold_pos_ is false. + // This keeps which actuator is valid (having non-NaN command) while is_hold_pos_ is true + if (!is_hold_pos_) + { + // Enforce joint velocity limits + vel_jnt_sat_interface_.enforceLimits(period); + vel_jnt_soft_interface_.enforceLimits(period); + + // Propagate joint velocity commands to actuators + robot_transmissions_.get()->propagate(); + } + + // Convert ros_control actuator velocity command to dynamixel command + std::vector vel_id_vec; + std::vector dxl_vel_vec; + uint8_t id_cnt = 0; + for (const std::pair& dxl : dynamixel_) + { + if (std::isnan(actr_cmd_vel_[id_cnt])) + { + ROS_DEBUG_STREAM_DELAYED_THROTTLE(10, "Skipping velocity command to " << dxl.first + << " because it is NaN. Its " + "controller may not work"); + } + else + { + if (is_hold_pos_) + { + // Forcibly set velocity commands of valid actuators to 0 when is_hold_pos_ is true. + // This holds current positions of velocity-controlled actuators while is_hold_pos_ is true + actr_cmd_vel_[id_cnt] = 0; + } + uint8_t id = (uint8_t)dxl.second; + vel_id_vec.push_back(id); + const char* model_name = NULL; + model_name = dxl_wb_->getModelName(id); + if (dxl_wb_->getProtocolVersion() == 2.0f && strcmp(model_name, "XL-320") != 0) + { + dxl_vel_vec.push_back(dxl_wb_->convertVelocity2Value((uint8_t)dxl.second, actr_cmd_vel_[id_cnt])); + } + else if ((dxl_wb_->getProtocolVersion() == 2.0f && strcmp(model_name, "XL-320") == 0) || + (dxl_wb_->getProtocolVersion() == 1.0f && (strncmp(model_name, "AX", strlen("AX")) == 0 || + strncmp(model_name, "RX", strlen("RX")) == 0 || + strncmp(model_name, "EX", strlen("EX")) == 0 || + strncmp(model_name, "MX", strlen("MX")) == 0))) + { + // In this case, convertVelocity2Value returns a value with the wrong sign, so we cannot use this method + const ModelInfo* model_info = NULL; + model_info = dxl_wb_->getModelInfo(id); + int32_t value = 0; + double velocity = actr_cmd_vel_[id_cnt]; + const float RPM2RADPERSEC = 0.104719755f; + if (velocity == 0) + { + value = 0; + } + else if (velocity < 0) + { + value = ((velocity * -1) / (model_info->rpm * RPM2RADPERSEC)) + 1023; + } + else if (velocity > 0) + { + value = (velocity / (model_info->rpm * RPM2RADPERSEC)); + } + dxl_vel_vec.push_back(value); + } + else + { + dxl_vel_vec.push_back(0); + } + } + id_cnt++; + } + + // Write velocity command to dynamixel + if (vel_id_vec.size() > 0) + { + bool result = false; + const char* log = NULL; + uint8_t vel_id[vel_id_vec.size()]; + int32_t dxl_vel[dxl_vel_vec.size()]; + std::copy(vel_id_vec.begin(), vel_id_vec.end(), vel_id); + std::copy(dxl_vel_vec.begin(), dxl_vel_vec.end(), dxl_vel); + result = + dxl_wb_->syncWrite(SYNC_WRITE_HANDLER_FOR_GOAL_VELOCITY, vel_id, vel_id_vec.size(), dxl_vel, 1, &log); + if (result == false) + { + ROS_ERROR("%s", log); + } + last_write_tm_ = ros::Time::now(); + } + } + if (is_calc_effort_ && is_current_ctrl_ && + robot_transmissions_.get()) + { + // Update actuator effort commands only when is_hold_pos_ is false. + // This keeps which actuator is valid (having non-NaN command) while is_hold_pos_ is true + if (!is_hold_pos_) + { + // Just after is_hold_pos_ becomes false, we must restore normal Operating Mode for normal execution + if (prev_is_hold_pos_) + { + for (const std::pair>& mode : normal_modes_) + { + dxl_wb_->torqueOff(dynamixel_[mode.first]); // We cannot change Operating Mode while torque on + bool result = true; + const char* log = NULL; + for (const std::pair& addr_val : mode.second) + { + if (!dxl_wb_->writeRegister(dynamixel_[mode.first], addr_val.first.c_str(), addr_val.second, &log)) + { + result = false; + break; + } + } + if (result == false) + { + ROS_ERROR("%s", log); + } + dxl_wb_->torqueOn(dynamixel_[mode.first]); + } + normal_modes_.clear(); + } + + // Enforce joint effort limits + eff_jnt_sat_interface_.enforceLimits(period); + eff_jnt_soft_interface_.enforceLimits(period); + + // Propagate joint effort commands to actuators + robot_transmissions_.get()->propagate(); + } + + // Convert ros_control actuator effort command to dynamixel command + std::vector eff_id_vec; + std::vector dxl_eff_vec; + uint8_t id_cnt = 0; + for (const std::pair& dxl : dynamixel_) + { + double torque_const = torque_consts_[dxl.first]; + if (torque_const > 0) + { + if (std::isnan(actr_cmd_eff_[id_cnt])) + { + ROS_DEBUG_STREAM_DELAYED_THROTTLE(10, "Skipping effort command to " << dxl.first + << " because it is NaN. Its " + "controller may not work"); + } + else + { + if (!is_hold_pos_) + { + eff_id_vec.push_back((uint8_t)dxl.second); + dxl_eff_vec.push_back( + dxl_wb_->convertCurrent2Value((uint8_t)dxl.second, (actr_cmd_eff_[id_cnt] / torque_const) * 1000)); + } + else + { + // Just after is_hold_pos_ becomes true, save normal Operating Mode of valid actuators and switch to Velocity Control. + // While is_hold_pos_ is true, leave their velocity commands initial values (zero). + // This holds current positions of actuators while is_hold_pos_ is true. + // We avoid keeping Current Control because it requires PID loop to hold current positions. + // We do not select Position Control because it resets Present Position within one full rotation. + // We do not select Extended Position Control because some actuators do not have this mode + if (!prev_is_hold_pos_) + { + bool result = false; + const char* log = NULL; + int32_t data[2]; + if (dxl_wb_->readRegister((uint8_t)dxl.second, "Operating_Mode", &data[0], &log)) + { + result = true; + normal_modes_[dxl.first]["Operating_Mode"] = data[0]; + } + else if (dxl_wb_->readRegister((uint8_t)dxl.second, "CW_Angle_Limit", &data[0], &log) && + dxl_wb_->readRegister((uint8_t)dxl.second, "CCW_Angle_Limit", &data[1], &log)) + { + // On some actuators, Operating Mode is represented by CW/CCW Angle Limit + result = true; + normal_modes_[dxl.first]["CW_Angle_Limit"] = data[0]; + normal_modes_[dxl.first]["CCW_Angle_Limit"] = data[1]; + } + if (result == false) + { + ROS_ERROR("%s", log); + } + dxl_wb_->torqueOff((uint8_t)dxl.second); // We cannot change Operating Mode while torque on + result = dxl_wb_->setVelocityControlMode((uint8_t)dxl.second, &log); + if (result == false) + { + ROS_ERROR("%s", log); + } + dxl_wb_->torqueOn((uint8_t)dxl.second); + last_write_tm_ = ros::Time::now(); + } + } + } + } + id_cnt++; + } + + // Write effort command to dynamixel + if (eff_id_vec.size() > 0) + { + bool result = false; + const char* log = NULL; + uint8_t eff_id[eff_id_vec.size()]; + int32_t dxl_eff[dxl_eff_vec.size()]; + std::copy(eff_id_vec.begin(), eff_id_vec.end(), eff_id); + std::copy(dxl_eff_vec.begin(), dxl_eff_vec.end(), dxl_eff); + result = + dxl_wb_->syncWrite(SYNC_WRITE_HANDLER_FOR_GOAL_CURRENT, eff_id, eff_id_vec.size(), dxl_eff, 1, &log); + if (result == false) + { + ROS_ERROR("%s", log); + } + last_write_tm_ = ros::Time::now(); + } + } + } + else + { + // Servo off + if (prev_is_servo_) + { + // Write servo off command to dynamixel only when previous state is servo on + // to prevent Hz from dropping in servo off + for (const std::pair& dxl : dynamixel_) + { + dxl_wb_->torqueOff((uint8_t)dxl.second); + } + last_write_tm_ = ros::Time::now(); + } + + // Servo off resets other special states + is_hold_pos_raw_ = false; + is_hold_pos_ = false; + if (normal_modes_.size() > 0) + { + // Restore normal Operating Mode + for (const std::pair>& mode : normal_modes_) + { + bool result = true; + const char* log = NULL; + for (const std::pair& addr_val : mode.second) + { + if (!dxl_wb_->writeRegister(dynamixel_[mode.first], addr_val.first.c_str(), addr_val.second, &log)) + { + result = false; + break; + } + } + if (result == false) + { + ROS_ERROR("%s", log); + } + last_write_tm_ = ros::Time::now(); + } + normal_modes_.clear(); + } + } + if (isJntCmdIgnored()) + { + // Reset joint limit interfaces to prevent runaway when going back to normal state + pos_jnt_sat_interface_.reset(); + pos_jnt_soft_interface_.reset(); + } + prev_is_servo_ = is_servo_; + prev_is_hold_pos_ = is_hold_pos_; +} + +bool DynamixelGeneralHw::isJntCmdIgnored(void) +{ + return (!is_servo_ || is_hold_pos_); +} + +void DynamixelGeneralHw::servoCallback(const std_msgs::BoolConstPtr& msg) +{ + std::lock_guard lock(mtx_); + + is_servo_raw_ = msg->data; +} + +void DynamixelGeneralHw::holdPosCallback(const std_msgs::BoolConstPtr& msg) +{ + std::lock_guard lock(mtx_); + + is_hold_pos_raw_ = msg->data; +} + +bool DynamixelGeneralHw::dynamixelCmdCallback(dynamixel_workbench_msgs::DynamixelCommand::Request& req, + dynamixel_workbench_msgs::DynamixelCommand::Response& res) +{ + std::lock_guard lock(mtx_); + + bool result = false; + const char* log; + + uint8_t id = req.id; + std::string item_name = req.addr_name; + int32_t value = req.value; + + result = dxl_wb_->itemWrite(id, item_name.c_str(), value, &log); + if (result == false) + { + ROS_ERROR("%s", log); + ROS_ERROR("Failed to write value[%d] on items[%s] to Dynamixel[ID : %d]", value, item_name.c_str(), id); + } + + res.comm_result = result; + + return true; +} + +}; // end namespace dynamixel_general_hw diff --git a/dynamixel_general_hw/urdf/sample1.urdf b/dynamixel_general_hw/urdf/sample1.urdf new file mode 100644 index 00000000..49f70880 --- /dev/null +++ b/dynamixel_general_hw/urdf/sample1.urdf @@ -0,0 +1,68 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + 0.0 + + + 1.0 + + + + diff --git a/dynamixel_general_hw/urdf/sample2.urdf b/dynamixel_general_hw/urdf/sample2.urdf new file mode 100644 index 00000000..cd5d8913 --- /dev/null +++ b/dynamixel_general_hw/urdf/sample2.urdf @@ -0,0 +1,58 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + 0.7853 + + + 2.0 + + + + diff --git a/dynamixel_general_hw/urdf/sample3.urdf b/dynamixel_general_hw/urdf/sample3.urdf new file mode 100644 index 00000000..624e43da --- /dev/null +++ b/dynamixel_general_hw/urdf/sample3.urdf @@ -0,0 +1,94 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + 0.0 + + + 1.0 + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + 0.0 + + + 1.0 + + + + diff --git a/dynamixel_general_hw/urdf/sample4.urdf b/dynamixel_general_hw/urdf/sample4.urdf new file mode 100644 index 00000000..f8700042 --- /dev/null +++ b/dynamixel_general_hw/urdf/sample4.urdf @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + 0.0 + + + 1.0 + + + + diff --git a/dynamixel_general_hw/urdf/sample5.urdf b/dynamixel_general_hw/urdf/sample5.urdf new file mode 100644 index 00000000..078389f3 --- /dev/null +++ b/dynamixel_general_hw/urdf/sample5.urdf @@ -0,0 +1,56 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + 0.0 + + + 1.0 + + + + diff --git a/dynamixel_general_hw/urdf/sample6.urdf b/dynamixel_general_hw/urdf/sample6.urdf new file mode 100644 index 00000000..702d55c9 --- /dev/null +++ b/dynamixel_general_hw/urdf/sample6.urdf @@ -0,0 +1,57 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + hardware_interface/EffortJointInterface + 0.0 + + + 1.0 + + + +