Add joint_jog to all configs

src/factory_sim/config/config.yaml

@@ -80,6 +80,9 @@ moveit_params:
   pose_jog:
     package: "factory_sim"
     path: "config/moveit/pose_jog.yaml"
+  joint_jog:
+    package: "factory_sim"
+    path: "config/joint_jog.yaml"
   publish:
     planning_scene: True
     geometry_updates: True
@@ -112,6 +115,7 @@ ros2_control:
   controllers_inactive_at_startup:
     - "servo_controller"
     - "velocity_force_controller"
+    - "joint_velocity_controller"
   # Any controllers here will not be spawned by MoveIt Pro.
   # [Optional, default=[]]
   controllers_not_managed: []

src/factory_sim/config/control/picknik_fanuc.ros2_control.yaml

@@ -19,6 +19,8 @@ controller_manager:
       type: position_controllers/GripperActionController
     velocity_force_controller:
       type: velocity_force_controller/VelocityForceController
+    joint_velocity_controller:
+      type: joint_velocity_controller/JointVelocityController
 
 tool_attach_controller:
   ros__parameters:
@@ -152,3 +154,31 @@ velocity_force_controller:
     command_timeout: 0.2
     # Padding (in radians) to add to joint position limits as a safety margin.
     joint_limit_position_tolerance: 0.02
+
+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
+    # Maximum joint-space accelerations.
+    max_joint_acceleration:
+      - 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/factory_sim/config/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/grinding_sim/config/config.yaml

@@ -79,6 +79,9 @@ moveit_params:
   pose_jog:
     package: "grinding_sim"
     path: "config/moveit/pose_jog.yaml"
+  joint_jog:
+    package: "grinding_sim"
+    path: "config/joint_jog.yaml"
 
   publish:
     planning_scene: True
@@ -110,6 +113,7 @@ ros2_control:
   controllers_inactive_at_startup:
     - "servo_controller"
     - "velocity_force_controller"
+    - "joint_velocity_controller"
 
 # Configuration for loading behaviors and objectives.
 # [Required]

src/grinding_sim/config/control/ur20.ros2_control.yaml

@@ -9,6 +9,8 @@ controller_manager:
       type: joint_trajectory_controller/JointTrajectoryController
     velocity_force_controller:
       type: velocity_force_controller/VelocityForceController
+    joint_velocity_controller:
+      type: joint_velocity_controller/JointVelocityController
 
 
 joint_state_broadcaster:
@@ -168,3 +170,31 @@ velocity_force_controller:
     command_timeout: 0.2
     # Padding (in radians) to add to joint position limits as a safety margin.
     joint_limit_position_tolerance: 0.02
+
+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
+    # Maximum joint-space accelerations.
+    max_joint_acceleration:
+      - 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/grinding_sim/config/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/hangar_sim/config/config.yaml

@@ -50,6 +50,9 @@ moveit_params:
   pose_jog:
     package: "hangar_sim"
     path: "config/moveit/pose_jog.yaml"
+  joint_jog:
+    package: "hangar_sim"
+    path: "config/joint_jog.yaml"
 
 
 # Configuration for loading behaviors and objectives.
@@ -103,3 +106,5 @@ ros2_control:
     - "platform_velocity_controller_nav2"
     - "velocity_force_controller"
     - "arm_only_velocity_force_controller"
+    - "joint_velocity_controller"
+    - "arm_only_joint_velocity_controller"

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

@@ -19,6 +19,10 @@ controller_manager:
       type: velocity_force_controller/VelocityForceController
     arm_only_velocity_force_controller:
       type: velocity_force_controller/VelocityForceController
+    joint_velocity_controller:
+      type: joint_velocity_controller/JointVelocityController
+    arm_only_joint_velocity_controller:
+      type: joint_velocity_controller/JointVelocityController
 
 vacuum_gripper:
   ros__parameters:
@@ -487,3 +491,82 @@ arm_only_velocity_force_controller:
     command_timeout: 0.2
     # Padding (in radians) to add to joint position limits as a safety margin.
     joint_limit_position_tolerance: 0.02
+
+joint_velocity_controller:
+  ros__parameters:
+    # Joint group to control.
+    planning_group_name: manipulator
+    command_joints:
+      - platform_velocity_controller/linear_x_joint
+      - platform_velocity_controller/linear_y_joint
+      - platform_velocity_controller/rotational_yaw_joint
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+    # Maximum joint-space velocities.
+    max_joint_velocity:
+      - 1.0
+      - 1.0
+      - 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
+      - 2.0
+      - 2.0
+    command_interfaces: ["velocity"]
+    # 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
+
+arm_only_joint_velocity_controller:
+  ros__parameters:
+    # Joint group to control.
+    planning_group_name: arm_only
+    command_joints:
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+    # Maximum joint-space velocities.
+    max_joint_velocity:
+      - 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
+    command_interfaces: ["velocity"]
+    # 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/hangar_sim/config/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', 'arm_only']
+controllers: ['joint_velocity_controller', 'arm_only_joint_velocity_controller']

src/hangar_sim/objectives/request_teleoperation.xml

@@ -185,10 +185,41 @@
               </Decorator>
             </Control>
             <Control ID="Sequence" _while="teleop_mode == 1">
-              <Action
-                ID="TeleoperateJointJog"
-                controller_name="servo_controller"
-              />
+              <Decorator ID="Repeat" num_cycles="-1">
+                <Control ID="Sequence">
+                  <Action
+                    ID="SwitchController"
+                    activate_controllers="{controllers}"
+                    deactivate_controllers="platform_velocity_controller_nav2"
+                  />
+
+                  <Decorator ID="ForceSuccess">
+                    <Decorator ID="Repeat" num_cycles="-1">
+                      <Control
+                        ID="Parallel"
+                        success_count="1"
+                        failure_count="1"
+                      >
+                        <Decorator ID="KeepRunningUntilFailure">
+                          <Action
+                            ID="GetElementOfVector"
+                            vector_in="{controllers}"
+                            index="0"
+                            element="{first_controller}"
+                          />
+                        </Decorator>
+
+                        <Action
+                          ID="JointJog"
+                          controller_name="{first_controller}"
+                          link_padding="0.00000"
+                          stop_safety_factor="1.500000"
+                        />
+                      </Control>
+                    </Decorator>
+                  </Decorator>
+                </Control>
+              </Decorator>
             </Control>
           </Control>
         </Decorator>

src/lab_sim/config/config.yaml

@@ -32,6 +32,9 @@ moveit_params:
   pose_jog:
     package: "lab_sim"
     path: "config/moveit/pose_jog.yaml"
+  joint_jog:
+    package: "lab_sim"
+    path: "config/moveit/joint_jog.yaml"
 
 # Configuration for loading behaviors and objectives.
 # [Required]
@@ -84,6 +87,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/moveit_pro_franka_configs/franka_base_config/config/config.yaml

@@ -45,6 +45,9 @@ moveit_params:
   pose_jog:
     package: "franka_base_config"
     path: "config/moveit/pose_jog.yaml"
+  joint_jog:
+    package: "franka_base_config"
+    path: "config/moveit/joint_jog.yaml"
 
   publish:
     planning_scene: True

src/moveit_pro_franka_configs/franka_base_config/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: ['velocity_force_controller']

src/moveit_pro_franka_configs/franka_dual_arm_hw/config/config.yaml

@@ -77,6 +77,9 @@ moveit_params:
   pose_jog:
     package: "franka_dual_arm_sim"
     path: "config/moveit/pose_jog.yaml"
+  joint_jog:
+    package: "franka_dual_arm_hw"
+    path: "config/moveit/joint_jog.yaml"
 
   publish:
     planning_scene: True