diff --git a/ur10_description/config/gazebo_controllers.yaml b/ur10_description/config/gazebo_controllers.yaml index 36a73d3..23aa7f1 100644 --- a/ur10_description/config/gazebo_controllers.yaml +++ b/ur10_description/config/gazebo_controllers.yaml @@ -1,22 +1,69 @@ +# ur10: +# # Publish all joint states ----------------------------------- +# joint_state_controller: +# type: joint_state_controller/JointStateController +# publish_rate: 50 + controller_manager: ros__parameters: - update_rate: 100 # Hz + #update_rate: 100 # Hz + #210611 if NOT set, ur10 dangles down with gravity + #TODO test other values + #update_rate: 1000 # Hz =Gazebo joint_trajectory_controller: type: joint_trajectory_controller/JointTrajectoryController + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + publish_rate: 50 + write_op_modes: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint joint_state_controller: type: joint_state_controller/JointStateController + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + publish_rate: 50 + write_op_modes: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint joint_trajectory_controller: ros__parameters: joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + write_op_modes: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint interface_name: position state_publish_rate: 50.0 - action_monitor_rate: 20.0 \ No newline at end of file + #state_publish_rate: 25.0 + action_monitor_rate: 20.0 diff --git a/ur10_gazebo/launch/ur10_gazebo.launch.py b/ur10_gazebo/launch/ur10_gazebo.launch.py index 1ed640d..f77d982 100644 --- a/ur10_gazebo/launch/ur10_gazebo.launch.py +++ b/ur10_gazebo/launch/ur10_gazebo.launch.py @@ -84,39 +84,40 @@ def generate_launch_description(): arguments=['-topic', 'robot_description', '-entity', 'ur10'], output='screen') - - load_joint_state_controller = ExecuteProcess( - cmd=['ros2', 'control', 'load_start_controller', 'joint_state_controller'], - output='screen' - ) - - load_joint_trajectory_controller = ExecuteProcess( - cmd=['ros2', 'control', 'load_start_controller', 'joint_trajectory_controller'], - output='screen' - ) - - # Static TF - #static_tf = Node(package='tf2_ros', - # executable='static_transform_publisher', - # name='static_transform_publisher', - # output='log', - # arguments=['0.0', '0.0', '0.65', '0.0', '0.0', '0.0', 'world', 'ur_base']) + #210611 + # load_joint_state_controller = ExecuteProcess( + # cmd=['ros2', 'control', 'load_start_controller', 'joint_state_controller'], + # output='screen' + # ) + + # load_joint_trajectory_controller = ExecuteProcess( + # cmd=['ros2', 'control', 'load_start_controller', 'joint_trajectory_controller'], + # output='screen' + # ) + + #Static TF + static_tf = Node(package='tf2_ros', + executable='static_transform_publisher', + name='static_transform_publisher', + output='log', + arguments=['0.0', '0.0', '0.65', '0.0', '0.0', '0.0', 'world', 'ur_base']) return LaunchDescription([ - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=spawn_entity, - on_exit=[load_joint_state_controller], - ) - ), - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=load_joint_state_controller, - on_exit=[load_joint_trajectory_controller], - ) - ), + #210611 + # RegisterEventHandler( + # event_handler=OnProcessExit( + # target_action=spawn_entity, + # on_exit=[load_joint_state_controller], + # ) + # ), + # RegisterEventHandler( + # event_handler=OnProcessExit( + # target_action=load_joint_state_controller, + # on_exit=[load_joint_trajectory_controller], + # ) + # ), gazebo, node_robot_state_publisher, - #static_tf, + static_tf, spawn_entity ]) diff --git a/ur10_moveit_config/config/ur10_ros_controllers.yaml b/ur10_moveit_config/config/ur10_ros_controllers.yaml index fe92342..689d837 100644 --- a/ur10_moveit_config/config/ur10_ros_controllers.yaml +++ b/ur10_moveit_config/config/ur10_ros_controllers.yaml @@ -1,6 +1,7 @@ controller_manager: ros__parameters: - update_rate: 100 # Hz + #update_rate: 100 # Hz + #update_rate: 1000 # Hz =Gazebo ur10_arm_controller: type: joint_trajectory_controller/JointTrajectoryController @@ -8,7 +9,6 @@ controller_manager: joint_state_controller: type: joint_state_controller/JointStateController - ur10_arm_controller: ros__parameters: joints: