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..a0eb01bf --- /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: using "Current-based Position Control Mode" of Dynamixel + +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"): Hz of the control loop + 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..c0634fec --- /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..8299a4fa --- /dev/null +++ b/dynamixel_general_hw/launch/sample6.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + 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..0d068721 --- /dev/null +++ b/dynamixel_general_hw/src/dynamixel_general_hw.cpp @@ -0,0 +1,1243 @@ +#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 + 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 + + + +