Effort/torque control with xc430-w150t #58
Description
Hey!
I have been using this package for position control/velocity control and everything has been working fine.
My urdf is:
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="my_ros2_control" params="name use_dummy device uncontrolled">
<ros2_control name="${name}" type="system">
<hardware>
<plugin>dynamixel_hardware_interface/DynamixelHardware</plugin>
<param name="port_name">${device}</param>
<param name="baud_rate">57600</param>
<param name="disable_torque_at_init">true</param>
<!--<param name="error_timeout_ms">100</param>-->
<param name="dynamixel_model_folder">/param/dxl_model</param>
<param name="number_of_joints">1</param>
<param name="number_of_transmissions">1</param>
<param name="transmission_to_joint_matrix">1</param>
<param name="joint_to_transmission_matrix">1</param>
<param name="dynamixel_state_pub_msg_name">dynamixel_hardware_interface/dxl_state</param>
<param name="get_dynamixel_data_srv_name">dynamixel_hardware_interface/get_dxl_data</param>
<param name="set_dynamixel_data_srv_name">dynamixel_hardware_interface/set_dxl_data</param>
<param name="reboot_dxl_srv_name">dynamixel_hardware_interface/reboot_dxl</param>
<param name="set_dxl_torque_srv_name">dynamixel_hardware_interface/set_dxl_torque</param>
<!-- This parameter is used for a prismatic joint that operates through a revolute mechanism.
If the joint does not require this configuration, this parameter can be removed. -->
<!--<param name="use_revolute_to_prismatic_gripper">1</param>-->
<!--<param name="revolute_to_prismatic_dxl">dxl</param>-->
<!--<param name="revolute_max">1.51</param>-->
<!--<param name="revolute_min">0.56</param>-->
<!--<param name="revolute_to_prismatic_joint">left_finger_joint</param>-->
<!--<param name="prismatic_max">0.019</param>-->
<!--<param name="prismatic_min">-0.01</param>-->
</hardware>
<joint name="left_finger_joint">
<param name="id">1</param>
<command_interface name="position"/>
<command_interface name="velocity"/>
<!--<command_interface name="effort"/>-->
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<gpio name="dxl1">
<param name="type">dxl</param>
<param name="ID">1</param>
<command_interface name="Goal Position"/>
<command_interface name="Goal Velocity"/>
<!--<command_interface name="Goal PWM"/>-->
<state_interface name="Present Position"/>
<state_interface name="Present Velocity"/>
<state_interface name="Present Load"/>
<param name="Position P Gain">0</param>
<param name="Position I Gain">0</param>
<param name="Position D Gain">0</param>
<param name="Velocity P Gain">0</param>
<param name="Velocity I Gain">0</param>
</gpio>
</ros2_control>
</xacro:macro>
</robot>if I launch a controller with a position or velocity controller defined by this file:
/**:
controller_manager:
ros__parameters:
update_rate: 50 # Hz
gripper_position_controller:
type: position_controllers/JointGroupPositionController
gripper_velocity_controller:
type: velocity_controllers/JointGroupVelocityController
gripper_effort_controller:
type: effort_controllers/JointGroupEffortController
gripper_position_controller:
ros__parameters:
joints:
- left_finger_joint
gripper_velocity_controller:
ros__parameters:
joints:
- left_finger_joint
gripper_effort_controller:
ros__parameters:
joints:
- left_finger_jointThen everything is fine. But as soon as I add the uncommented effort controllers, I get unexpected errors:
[ros2_control_node-1] transmission_to_joint_matrix_
[ros2_control_node-1] [0][0] 1.000000,
[ros2_control_node-1] joint_to_transmission_matrix_
[ros2_control_node-1] [0][0] 1.000000,
[ros2_control_node-1] [ERROR] [1750692552.509179181] [dynamixel_hardware_interface]: Failed to parse error_timeout_ms parameter: stod, using default value
[ros2_control_node-1] [INFO] [1750692552.509186799] [dynamixel_hardware_interface]: port_name /dev/ttyUSB1 / baudrate 57600
[ros2_control_node-1] Dynamixel Information File List.
[ros2_control_node-1] num: 350, name: xl320.model
[ros2_control_node-1] num: 1000, name: xh430_w350.model
[ros2_control_node-1] num: 1001, name: xd430_t350.model
[ros2_control_node-1] num: 1010, name: xh430_w210.model
[ros2_control_node-1] num: 1011, name: xd430_t210.model
[ros2_control_node-1] num: 1020, name: xm430_w350.model
[ros2_control_node-1] num: 1030, name: xm430_w210.model
[ros2_control_node-1] num: 1040, name: xh430_v350.model
[ros2_control_node-1] num: 1050, name: xh430_v210.model
[ros2_control_node-1] num: 1060, name: xl430_w250.model
[ros2_control_node-1] num: 1070, name: xc430_w150.model
[ros2_control_node-1] num: 1070, name: xc430_w150.model
[ros2_control_node-1] num: 1080, name: xc430_w240.model
[ros2_control_node-1] num: 1080, name: xc430_w240.model
[ros2_control_node-1] num: 1090, name: 2xl430_w250.model
[ros2_control_node-1] num: 1100, name: xh540_w270.model
[ros2_control_node-1] num: 1101, name: xd540_t270.model
[ros2_control_node-1] num: 1110, name: xh540_w150.model
[ros2_control_node-1] num: 1111, name: xd540_t150.model
[ros2_control_node-1] num: 1120, name: xm540_w270.model
[ros2_control_node-1] num: 1130, name: xm540_w150.model
[ros2_control_node-1] num: 1140, name: xh540_v270.model
[ros2_control_node-1] num: 1150, name: xh540_v150.model
[ros2_control_node-1] num: 1160, name: 2xc430_w250.model
[ros2_control_node-1] num: 1170, name: xw540_t260.model
[ros2_control_node-1] num: 1180, name: xw540_t140.model
[ros2_control_node-1] num: 1190, name: xl330_m077.model
[ros2_control_node-1] num: 1200, name: xl330_m288.model
[ros2_control_node-1] num: 1210, name: xc330_t181.model
[ros2_control_node-1] num: 1220, name: xc330_t288.model
[ros2_control_node-1] num: 1230, name: xc330_m181.model
[ros2_control_node-1] num: 1240, name: xc330_m288.model
[ros2_control_node-1] num: 1270, name: xw430_t333.model
[ros2_control_node-1] num: 1310, name: xw540_h260.model
[ros2_control_node-1] num: 2000, name: ph42_020_s300.model
[ros2_control_node-1] num: 4000, name: ym070_210_m001.model
[ros2_control_node-1] num: 4020, name: ym070_210_r051.model
[ros2_control_node-1] num: 4030, name: ym070_210_r099.model
[ros2_control_node-1] num: 4050, name: ym070_210_a099.model
[ros2_control_node-1] num: 4120, name: ym080_230_m001.model
[ros2_control_node-1] num: 4130, name: ym080_230_b001.model
[ros2_control_node-1] num: 4140, name: ym080_230_r051.model
[ros2_control_node-1] num: 4150, name: ym080_230_r099.model
[ros2_control_node-1] num: 4170, name: ym080_230_a099.model
[ros2_control_node-1] num: 35074, name: rh_p12_rn.model
[ros2_control_node-1] num: 220, name: omy_hat.model
[ros2_control_node-1] num: 230, name: omy_end.model
[ros2_control_node-1] num: 536, name: sensorxel_joy.model
[ros2_control_node-1] num: 600, name: sensorxel_joy.model
[ros2_control_node-1] [INFO] [1750692552.509283225] [dynamixel_hardware_interface]: $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
[ros2_control_node-1] [INFO] [1750692552.509285204] [dynamixel_hardware_interface]: $$$$$ Init Dxl Comm Port
[ros2_control_node-1] [INFO] [1750692552.509291581] [dynamixel_hardware_interface]: $$$$$ Init Items for type: controller
[ros2_control_node-1] Succeeded to open the port!
[ros2_control_node-1] Succeeded to change the [57600] baudrate!
[ros2_control_node-1] [ID:001] Request ping - Ping succeeded. Dynamixel model number : 1070
[ros2_control_node-1] [INFO] [1750692552.515528435] [dynamixel_hardware_interface]: Trying to connect to the communication port...
[ros2_control_node-1] [INFO] [1750692552.515540202] [dynamixel_hardware_interface]: $$$$$ Init Items for type: dxl
[ros2_control_node-1] [INFO] [1750692552.521432461] [dynamixel_hardware_interface]: [ID:1] item_name:Position D Gain data:0
[ros2_control_node-1] [INFO] [1750692552.527411634] [dynamixel_hardware_interface]: [ID:1] item_name:Position I Gain data:0
[ros2_control_node-1] [INFO] [1750692552.533434415] [dynamixel_hardware_interface]: [ID:1] item_name:Position P Gain data:0
[ros2_control_node-1] [INFO] [1750692552.539479785] [dynamixel_hardware_interface]: [ID:1] item_name:Velocity I Gain data:0
[ros2_control_node-1] [INFO] [1750692552.545411020] [dynamixel_hardware_interface]: [ID:1] item_name:Velocity P Gain data:0
[ros2_control_node-1] [INFO] [1750692552.545420699] [dynamixel_hardware_interface]: $$$$$ Init Items for type: sensor
[ros2_control_node-1] [INFO] [1750692552.545428459] [dynamixel_hardware_interface]: $$$$$ Init Dxl Read Items
[ros2_control_node-1] Dynamixel Read Type : sync read
[ros2_control_node-1] ID : 1,
[ros2_control_node-1] Read items : Present Position Present Velocity Present Load
[ros2_control_node-1] set fast sync read (indirect addr) : addr 634, size 10
[ros2_control_node-1] FastSyncRead handler set up successfully.
[ros2_control_node-1] Success to set SyncRead handler using indirect address
[ros2_control_node-1] [INFO] [1750692552.605469644] [dynamixel_hardware_interface]: $$$$$ Init Dxl Write Items
[ros2_control_node-1] Dynamixel Write Type : sync write
[ros2_control_node-1] ID : 1,
[ros2_control_node-1] Write items : Goal Position Goal Velocity
[ros2_control_node-1] set sync write (indirect addr) : addr 224, size 8
[ros2_control_node-1] Success to set SyncWrite handler using indirect address
[ros2_control_node-1] [INFO] [1750692552.655713064] [resource_manager]: Successful initialization of hardware 'aloha_gripper_ros2_control'
[ros2_control_node-1] terminate called after throwing an instance of 'std::runtime_error'
[ros2_control_node-1] what(): Wrong state or command interface configuration.
[ros2_control_node-1] missing state interfaces:
[ros2_control_node-1]
[ros2_control_node-1] missing command interfaces:
[ros2_control_node-1] ' dxl1/Goal PWM '
[ros2_control_node-1] Stack trace (most recent call last):
[ros2_control_node-1] #15 Object "", at 0xffffffffffffffff, in
[ros2_control_node-1] #14 Object "/opt/ros/humble/lib/controller_manager/ros2_control_node", at 0x56001ac7cee4, in
[ros2_control_node-1] #13 Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7fca8c017e3f, in __libc_start_main
[ros2_control_node-1] #12 Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7fca8c017d8f, in
[ros2_control_node-1] #11 Object "/opt/ros/humble/lib/controller_manager/ros2_control_node", at 0x56001ac7c3e3, in
[ros2_control_node-1] #10 Object "/opt/ros/humble/lib/libcontroller_manager.so", at 0x7fca8c72a74a, in controller_manager::ControllerManager::ControllerManager(std::shared_ptr<rclcpp::Executor>, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, rclcpp::NodeOptions const&)
[ros2_control_node-1] #9 Object "/opt/ros/humble/lib/libcontroller_manager.so", at 0x7fca8c7256df, in controller_manager::ControllerManager::init_resource_manager(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)
[ros2_control_node-1] #8 Object "/opt/ros/humble/lib/libhardware_interface.so", at 0x7fca8bee4334, in hardware_interface::ResourceManager::load_urdf(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool, bool)
[ros2_control_node-1] #7 Object "/opt/ros/humble/lib/libhardware_interface.so", at 0x7fca8bec0723, in
[ros2_control_node-1] #6 Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7fca8c2e54d7, in __cxa_throw
[ros2_control_node-1] #5 Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7fca8c2e5276, in std::terminate()
[ros2_control_node-1] #4 Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7fca8c2e520b, in
[ros2_control_node-1] #3 Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7fca8c2d9b9d, in
[ros2_control_node-1] #2 Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7fca8c0167f2, in abort
[ros2_control_node-1] #1 Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7fca8c030475, in raise
[ros2_control_node-1] #0 Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7fca8c0849fc, in pthread_kill
[ros2_control_node-1] Aborted (Signal sent by tkill() 33786 1000)
[spawner-2] [INFO] [1750692552.673427962] [gripper.spawner_gripper_velocity_controller]: waiting for service /gripper/controller_manager/list_controllers to become available...
[ERROR] [ros2_control_node-1]: process has died [pid 33786, exit code -6, cmd '/opt/ros/humble/lib/controller_manager/ros2_control_node --ros-args -r __ns:=/gripper --params-file /home/ros/ros2_ws/install/aloha4franka_bringup/share/aloha4franka_bringup/config/controllers.yaml --params-file /tmp/launch_params_vbg_du8s'].
[ERROR] [launch]: Caught exception in launch (see debug for traceback): expected a LaunchDescriptionEntity from event_handler, got '<launch.events.shutdown.Shutdown object at 0x7f87aeb197b0>'
[INFO] [spawner-2]: sending signal 'SIGINT' to process[spawner-2]
[INFO] [spawner-2]: process has finished cleanly [pid 33788]
I checked in the model file (https://github.com/ROBOTIS-GIT/dynamixel_hardware_interface/blob/main/param/dxl_model/xc430_w150.model) and the Goal PWM address seems to be correct. Any Idea why this is failing?
Cheers!
Daniel
Metadata
Metadata
Assignees
Type
Projects
Status