Updates for new MoveIt Servo (#98)

src/picknik_ur_base_config/config/moveit/ur_servo.yaml

@@ -19,7 +19,6 @@ override_velocity_scaling_factor: 0.5
 
 ## Properties of outgoing commands
 publish_period: 0.002  # 1/Nominal publish rate [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
@@ -31,9 +30,8 @@ publish_joint_velocities: false
 publish_joint_accelerations: false
 
 ## Plugins for smoothing outgoing commands
+use_smoothing: true
 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.
 
 ## MoveIt properties
@@ -42,7 +40,7 @@ planning_frame: base_link  # The MoveIt planning frame. Often 'base_link' or 'wo
 is_primary_planning_scene_monitor: false  # The MoveGroup node maintains the planning scene, so Servo needs to get its world info from there.
 
 ## Other frames
-ee_frame_name: manual_grasp_link  # The name of the end effector link, used to return the EE pose
+ee_frame: manual_grasp_link  # The name of the end effector link, used to return the EE pose
 robot_link_command_frame: base_link  # commands must be given in the frame of a robot link. Usually either the base or end effector
 
 ## Stopping behaviour

src/picknik_ur_base_config/objectives/teleoperate_joint_jog.xml

@@ -0,0 +1,8 @@
+<?xml version="1.0"?>
+<root BTCPP_format="4" main_tree_to_execute="Teleoperate Joint Jog">
+    <BehaviorTree ID="Teleoperate Joint Jog" _description="Teleoperate the robot in joint jog mode from the Web UI">
+        <Control ID="Sequence" name="root">
+            <Action ID="TeleoperateJointJog" controller_name="streaming_controller"/>
+        </Control>
+    </BehaviorTree>
+</root>

src/picknik_ur_base_config/objectives/teleoperate_twist.xml

@@ -0,0 +1,8 @@
+<?xml version="1.0"?>
+<root BTCPP_format="4" main_tree_to_execute="Teleoperate Twist">
+    <BehaviorTree ID="Teleoperate Twist" _description="Teleoperate the robot in Cartesian twist mode from the Web UI">
+        <Control ID="Sequence" name="root">
+            <Action ID="TeleoperateTwist" controller_name="streaming_controller"/>
+        </Control>
+    </BehaviorTree>
+</root>

src/picknik_ur_gazebo_config/config/config.yaml

@@ -76,7 +76,7 @@ moveit_params:
     path: "config/moveit/sensors_3d.yaml"
   servo:
     package: "picknik_ur_gazebo_config"
-    path: "config/moveit/ur5e_servo.yaml"
+    path: "config/moveit/ur_servo.yaml"
 
 octomap_manager:
   # Input point cloud topic name. The *output* point cloud topic published by

src/picknik_ur_gazebo_config/config/moveit/ur5e_servo.yaml → src/picknik_ur_gazebo_config/config/moveit/ur_servo.yaml

@@ -19,7 +19,6 @@ override_velocity_scaling_factor: 0.5
 
 ## Properties of outgoing commands
 publish_period: 0.005  # 1/Nominal publish rate [seconds]
-low_latency_mode: true  # 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
@@ -31,9 +30,8 @@ publish_joint_velocities: false
 publish_joint_accelerations: false
 
 ## Plugins for smoothing outgoing commands
+use_smoothing: true
 smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin"
-
-## Incoming Joint State properties
 low_pass_filter_coeff: 4.0  # Larger --> trust the filtered data more, trust the measurements less.
 
 ## MoveIt properties
@@ -42,11 +40,11 @@ planning_frame: base_link  # The MoveIt planning frame. Often 'base_link' or 'wo
 is_primary_planning_scene_monitor: false  # The MoveGroup node maintains the planning scene, so Servo needs to get its world info from there.
 
 ## Other frames
-ee_frame_name: manual_grasp_link  # The name of the end effector link, used to return the EE pose
+ee_frame: manual_grasp_link  # The name of the end effector link, used to return the EE pose
 robot_link_command_frame: base_link  # commands must be given in the frame of a robot link. Usually either the base or end effector
 
 ## Stopping behaviour
-incoming_command_timeout: 0.1  # Stop servoing if X seconds elapse without a new command
+incoming_command_timeout: 0.05  # Stop servoing if X seconds elapse without a new command
 
 ## Configure handling of singularities and joint limits
 lower_singularity_threshold: 30.0  # Start decelerating when the condition number hits this (close to singularity)

src/picknik_ur_mock_hw_config/config/moveit/ur_servo.yaml

@@ -38,7 +38,7 @@ planning_frame: base_link  # The MoveIt planning frame. Often 'base_link' or 'wo
 is_primary_planning_scene_monitor: false  # The MoveGroup node maintains the planning scene, so Servo needs to get its world info from there.
 
 ## Other frames
-ee_frame_name: manual_grasp_link  # The name of the end effector link, used to return the EE pose
+ee_frame: manual_grasp_link  # The name of the end effector link, used to return the EE pose
 robot_link_command_frame: base_link  # commands must be given in the frame of a robot link. Usually either the base or end effector
 
 ## Stopping behaviour