add joint velocity controller

src/lab_sim/config/config.yaml

@@ -80,6 +80,7 @@ ros2_control:
     - "servo_controller"
     - "joint_trajectory_admittance_controller"
     - "velocity_force_controller"
+    - "joint_velocity_controller"
   # Any controllers here will not be spawned by MoveIt Pro.
   # [Optional, default=[]]
   controllers_not_managed: []

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

@@ -15,6 +15,8 @@ controller_manager:
       type: joint_trajectory_admittance_controller/JointTrajectoryAdmittanceController
     velocity_force_controller:
       type: velocity_force_controller/VelocityForceController
+    joint_velocity_controller:
+      type: joint_velocity_controller/JointVelocityController
 
 
 joint_state_broadcaster:
@@ -278,3 +280,33 @@ velocity_force_controller:
     # command_joints: []
     # The type of command interface to use. Supported values are "position" and "velocity".
     command_interfaces: ["position"]
+
+joint_velocity_controller:
+  ros__parameters:
+    # Joint group to control.
+    planning_group_name: manipulator
+    # Maximum joint-space velocities.
+    max_joint_velocity:
+      - 1.0
+      - 1.0
+      - 1.0
+      - 1.0
+      - 1.0
+      - 1.0
+      - 1.0
+    # Maximum joint-space accelerations.
+    max_joint_acceleration:
+      - 2.0
+      - 2.0
+      - 2.0
+      - 2.0
+      - 2.0
+      - 2.0
+      - 2.0
+    command_interfaces: ["position"]
+    # Padding (in radians) to add to joint position limits as a safety margin.
+    joint_limit_position_tolerance: 0.02
+    # Timeout in seconds after which the controller will stop motion if no new commands are received.
+    command_timeout: 0.2
+    # Rate in Hz at which the controller will publish the state.
+    state_publish_rate: 20

src/lab_sim/config/moveit/joint_jog.yaml

@@ -0,0 +1,4 @@
+# Planning groups to use in JointJog, and their corresponding JVC controllers.
+# The number of elements in `planning_groups` and `controllers` must match.
+planning_groups: ['manipulator']
+controllers: ['joint_velocity_controller']

src/lab_sim/objectives/joint_jog_example.xml

@@ -0,0 +1,32 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Joint Jog Example">
+  <!--//////////-->
+  <BehaviorTree ID="Joint Jog Example" _description="" _favorite="false">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action
+        ID="SwitchController"
+        activate_asap="true"
+        automatic_deactivation="true"
+        controller_list_action_name="/controller_manager/list_controllers"
+        controller_switch_action_name="/controller_manager/switch_controller"
+        strictness="1"
+        timeout="-1.000000"
+        activate_controllers="joint_velocity_controller"
+      />
+      <Action
+        ID="JointJog"
+        controller_name="joint_velocity_controller"
+        link_padding="0.010000"
+        stop_safety_factor="1.500000"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Joint Jog Example">
+      <MetadataFields>
+        <Metadata runnable="true" />
+        <Metadata subcategory="Application - Advanced Examples" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
+</root>