use JTC and acceleration filter

Signed-off-by: Paul Gesel <paulgesel@gmail.com>

src/picknik_ur_base_config/config/control/picknik_ur.ros2_control.yaml

@@ -14,7 +14,7 @@ controller_manager:
     robotiq_activation_controller:
       type: robotiq_controllers/RobotiqActivationController
     streaming_controller:
-      type: position_controllers/JointGroupPositionController
+      type: joint_trajectory_controller/JointTrajectoryController
     admittance_controller_open_door:
       type: admittance_controller/AdmittanceController
     joint_trajectory_controller_chained_open_door:
@@ -131,6 +131,32 @@ streaming_controller:
       - position
     state_interfaces:
       - position
+      - velocity
+    command_joints:
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+    state_publish_rate: 100.0
+    action_monitor_rate: 20.0
+    allow_partial_joints_goal: false
+    constraints:
+      stopped_velocity_tolerance: 0.0
+      goal_time: 0.0
+      shoulder_pan_joint:
+        goal: 0.05
+      shoulder_lift_joint:
+        goal: 0.05
+      elbow_joint:
+        goal: 0.05
+      wrist_1_joint:
+        goal: 0.05
+      wrist_2_joint:
+        goal: 0.05
+      wrist_3_joint:
+        goal: 0.05
 
 robotiq_gripper_controller:
   ros__parameters:

src/picknik_ur_base_config/config/moveit/ur_servo.yaml

@@ -18,24 +18,26 @@ scale:
 override_velocity_scaling_factor: 0.5
 
 ## Properties of outgoing commands
-publish_period: 0.002  # 1/Nominal publish rate [seconds]
+publish_period: 0.01  # 1/Nominal publish rate [seconds]
+max_expected_latency: 0.1 # delay between sending a command and the robot executing it [seconds]
 
 # What type of topic does your robot driver expect?
 # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory
-command_out_type: std_msgs/Float64MultiArray
+command_out_type: trajectory_msgs/JointTrajectory
 
 # What to publish? Can save some bandwidth as most robots only require positions or velocities
 publish_joint_positions: true
-publish_joint_velocities: false
+publish_joint_velocities: true
 publish_joint_accelerations: false
 
 ## Plugins for smoothing outgoing commands
 use_smoothing: true
-smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin"
-low_pass_filter_coeff: 1.5  # Larger --> trust the filtered data more, trust the measurements less.
+smoothing_filter_plugin_name: "online_signal_smoothing::AccelerationLimitedPlugin"
+acceleration_filter_update_rate: 0.01 # [seconds] Must match the publish_period parameter   
 
 ## MoveIt properties
 move_group_name: manipulator  # Often 'manipulator' or 'arm'
+acceleration_filter_move_group_name: manipulator  # Often 'manipulator' or 'arm'
 is_primary_planning_scene_monitor: false  # The MoveGroup node maintains the planning scene, so Servo needs to get its world info from there.
 
 ## Stopping behaviour
@@ -51,7 +53,7 @@ cartesian_command_in_topic: ~/delta_twist_cmds  # Topic for incoming Cartesian t
 joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands
 joint_topic: /joint_states
 status_topic: ~/status # Publish status to this topic
-command_out_topic: /streaming_controller/commands # Publish outgoing commands here
+command_out_topic: /streaming_controller/joint_trajectory # Publish outgoing commands here
 
 ## Collision checking for the entire robot body
 check_collisions: true # Check collisions?

src/picknik_ur_gazebo_config/config/control/picknik_ur5e.ros2_control.yaml

@@ -8,7 +8,7 @@ controller_manager:
     robotiq_gripper_controller:
       type: position_controllers/GripperActionController
     streaming_controller:
-      type: position_controllers/JointGroupPositionController
+      type: joint_trajectory_controller/JointTrajectoryController
 
 joint_trajectory_controller:
   ros__parameters:
@@ -63,6 +63,32 @@ streaming_controller:
       - position
     state_interfaces:
       - position
+      - velocity
+    command_joints:
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+    state_publish_rate: 100.0
+    action_monitor_rate: 20.0
+    allow_partial_joints_goal: false
+    constraints:
+      stopped_velocity_tolerance: 0.0
+      goal_time: 0.0
+      shoulder_pan_joint:
+        goal: 0.05
+      shoulder_lift_joint:
+        goal: 0.05
+      elbow_joint:
+        goal: 0.05
+      wrist_1_joint:
+        goal: 0.05
+      wrist_2_joint:
+        goal: 0.05
+      wrist_3_joint:
+        goal: 0.05
 
 robotiq_gripper_controller:
   ros__parameters:

src/picknik_ur_gazebo_config/config/moveit/ur_servo.yaml

@@ -18,24 +18,26 @@ scale:
 override_velocity_scaling_factor: 0.5
 
 ## Properties of outgoing commands
-publish_period: 0.005  # 1/Nominal publish rate [seconds]
+publish_period: 0.01  # 1/Nominal publish rate [seconds]
+max_expected_latency: 0.1 # delay between sending a command and the robot executing it [seconds]
 
 # What type of topic does your robot driver expect?
 # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory
-command_out_type: std_msgs/Float64MultiArray
+command_out_type: trajectory_msgs/JointTrajectory
 
 # What to publish? Can save some bandwidth as most robots only require positions or velocities
 publish_joint_positions: true
-publish_joint_velocities: false
+publish_joint_velocities: true
 publish_joint_accelerations: false
 
 ## Plugins for smoothing outgoing commands
 use_smoothing: true
-smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin"
-low_pass_filter_coeff: 4.0  # Larger --> trust the filtered data more, trust the measurements less.
+smoothing_filter_plugin_name: "online_signal_smoothing::AccelerationLimitedPlugin"
+acceleration_filter_update_rate: 0.01 # [seconds] Must match the publish_period parameter   
 
 ## MoveIt properties
 move_group_name: manipulator  # Often 'manipulator' or 'arm'
+acceleration_filter_move_group_name: manipulator  # Often 'manipulator' or 'arm'
 is_primary_planning_scene_monitor: false  # The MoveGroup node maintains the planning scene, so Servo needs to get its world info from there.
 
 ## Stopping behaviour
@@ -51,7 +53,7 @@ cartesian_command_in_topic: ~/delta_twist_cmds  # Topic for incoming Cartesian t
 joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands
 joint_topic: /joint_states
 status_topic: ~/status # Publish status to this topic
-command_out_topic: /robot_controllers/commands # Publish outgoing commands here
+command_out_topic: /streaming_controller/joint_trajectory # Publish outgoing commands here
 
 ## Collision checking for the entire robot body
 check_collisions: true # Check collisions?

src/picknik_ur_mock_hw_config/config/moveit/ur_servo.yaml

@@ -14,26 +14,28 @@ scale:
 override_velocity_scaling_factor: 0.5
 
 ## Properties of outgoing commands
-publish_period: 0.002  # 1/Nominal publish rate [seconds]
+publish_period: 0.01  # 1/Nominal publish rate [seconds]
+max_expected_latency: 0.1 # delay between sending a command and the robot executing it [seconds]
+
 low_latency_mode: false  # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored)
 
 # What type of topic does your robot driver expect?
 # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory
-command_out_type: std_msgs/Float64MultiArray
+command_out_type: trajectory_msgs/JointTrajectory
 
 # What to publish? Can save some bandwidth as most robots only require positions or velocities
 publish_joint_positions: true
-publish_joint_velocities: false
+publish_joint_velocities: true
 publish_joint_accelerations: false
 
 ## Plugins for smoothing outgoing commands
-smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin"
-
-## Incoming Joint State properties
-low_pass_filter_coeff: 1.5  # Larger --> trust the filtered data more, trust the measurements less.
+use_smoothing: true
+smoothing_filter_plugin_name: "online_signal_smoothing::AccelerationLimitedPlugin"
+acceleration_filter_update_rate: 0.01 # [seconds] Must match the publish_period parameter   
 
 ## MoveIt properties
 move_group_name: manipulator  # Often 'manipulator' or 'arm'
+acceleration_filter_move_group_name: manipulator  # Often 'manipulator' or 'arm'
 is_primary_planning_scene_monitor: false  # The MoveGroup node maintains the planning scene, so Servo needs to get its world info from there.
 
 ## Stopping behaviour
@@ -49,7 +51,7 @@ cartesian_command_in_topic: ~/delta_twist_cmds  # Topic for incoming Cartesian t
 joint_command_in_topic: ~/delta_joint_cmds  # Topic for incoming joint angle commands
 joint_topic: /joint_states
 status_topic: ~/status  # Publish status to this topic
-command_out_topic: /streaming_controller/commands  # Publish outgoing commands here
+command_out_topic: /streaming_controller/joint_trajectory # Publish outgoing commands here
 
 ## Collision checking for the entire robot body
 check_collisions: true  # Check collisions?