Implement MoveIt Servo

README.md

@@ -3,7 +3,7 @@ ROS2 Hardware Interface and Description for the ADA Robot
 
 ## Dependencies
 1. Install the Kinova SDK for your robot by searching [here](https://www.kinovarobotics.com/resources?r=79301&s). PRL currently uses the [Gen2 SDK v1.5.1](https://drive.google.com/file/d/1UEQAow0XLcVcPCeQfHK9ERBihOCclkJ9/view). Note that although the latest version of that SDK is for Ubuntu 16.04, it still works on Ubuntu 22.04 (only for x86 systems, not ARM system).
-2. Install required ROS binaries. Note that some or all of these may already be installed. `sudo apt install ros-humble-ros2-control ros-humble-ros2-controllers ros-humble-kinematics-interface-kdl ros-humble-ament-cmake-clang-format ros-humble-rviz2 ros-humble-moveit ros-humble-moveit-ros-perception ros-humble-ackermann-msgs ros-humble-control-toolbox ros-humble-generate-parameter-library ros-humble-generate-parameter-library-py`
+2. Install required ROS binaries. Note that some or all of these may already be installed. `sudo apt install ros-humble-ros2-control ros-humble-ros2-controllers ros-humble-kinematics-interface-kdl ros-humble-ament-cmake-clang-format ros-humble-rviz2 ros-humble-moveit ros-humble-moveit-ros-perception ros-humble-ackermann-msgs ros-humble-control-toolbox ros-humble-generate-parameter-library ros-humble-generate-parameter-library-py ros-humble-moveit-ros-control-interface`
 3. Install required python packages: `python3 -m pip install trimesh`
 4. Configure and build your workspace:
     1. Git clone [this repo (ada_ros2)](https://github.com/personalrobotics/ada_ros2) and [`pr_ros_controllers` (branch: `main`)](https://github.com/personalrobotics/pr_ros_controllers) into your ROS2 workspace's `src` folder.
@@ -17,6 +17,31 @@ ROS2 Hardware Interface and Description for the ADA Robot
 ### Real
 1. Run `ros2 launch ada_moveit demo.launch.py` command from your ROS2 workspace. If running for feeding specifically (i.e., where the watchdog dying kills the controllers) run `ros2 launch ada_moveit demo_feeding.launch.py`. Make sure the watchdog is running before you launch this node.
 
+### MoveIt Servo
+
+MoveIt Servo allows [real-time arm servoing](https://moveit.picknik.ai/humble/doc/examples/realtime_servo/realtime_servo_tutorial.html) in cartesian space by sending twist commands to the end effector. 
+
+To use Servo with keyboard teleop:
+1. Launch the force-torque sensor:
+    1. Sim: `ros2 run ada_feeding dummy_ft_sensor.py`
+    2. Real: `ros2 run forque_sensor_hardware forque_sensor_hardware`
+2. Launch MoveIt:
+    1. Sim:`ros2 launch ada_moveit demo.launch.py sim:=mock`
+    2. Real: `ros2 launch ada_moveit demo.launch.py`
+3. Re-tare the F/T sensor: `ros2 service call /wireless_ft/set_bias std_srvs/srv/SetBool "{data: true}"`
+4. Enable MoveIt Servo:
+    1. Switch Controllers: `ros2 service call /controller_manager/switch_controller controller_manager_msgs/srv/SwitchController "{activate_controllers: [\"jaco_arm_servo_controller\"], deactivate_controllers: [\"jaco_arm_controller\"], start_controllers: [], stop_controllers: [], strictness: 0, start_asap: false, activate_asap: false, timeout: {sec: 0, nanosec: 0}}"`
+    2. Toggle Servo On: `ros2 service call /servo_node/start_servo std_srvs/srv/Trigger "{}"`
+5. Run the keyboard teleop script: `ros2 run ada_moveit ada_keyboard_teleop.py`
+6. Follow the on-screen instructions to teleoperate the robot.
+7. Toggle Servo Off: `ros2 service call /servo_node/stop_servo std_srvs/srv/Trigger "{}"`
+
+To create your own Servo client:
+1. Follow steps 1-4 above.
+2. Have your client publish Twist commands to `/servo_node/delta_twist_cmds`. Note the following:
+    1. For reliable cartesian control when sending angular velocities on the real robot and `lovelace`, ensure the angular velocity is <= 0.3 rad/s in magnitude. Greater angular velocities might change the end effector's position in addition to its orientation. We believe this is because of latencies with MoveIt Servo getting the robot's joint states via the joint state publisher.
+    2. Be sure to send 0-velocity Twist messages at the end to stop the robot.
+
 ## Camera Calibration
 
 See the [README for the `default` calibration](./ada_moveit/calib/default/README.md) for details about our extrinsics calibration methdology.

ada_moveit/CMakeLists.txt

@@ -2,11 +2,20 @@ cmake_minimum_required(VERSION 3.22)
 project(ada_moveit)
 
 find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
 
-ament_package()
+ament_python_install_package(${PROJECT_NAME})
+
+# Install Python executables
+install(PROGRAMS
+  scripts/ada_keyboard_teleop.py
+  DESTINATION lib/${PROJECT_NAME}
+)
 
 install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}
   PATTERN "setup_assistant.launch" EXCLUDE)
 install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
 install(DIRECTORY calib DESTINATION share/${PROJECT_NAME})
 install(FILES .setup_assistant DESTINATION share/${PROJECT_NAME})
+
+ament_package()

ada_moveit/config/mock_controllers.yaml

@@ -7,6 +7,10 @@ controller_manager:
       type: force_gate_controller/ForceGateController
 
 
+    jaco_arm_servo_controller:
+      type: force_gate_controller/ForceGatePositionController
+
+
     hand_controller:
       type: joint_trajectory_controller/JointTrajectoryController
 
@@ -50,6 +54,16 @@ jaco_arm_controller:
       goal: 0.02
       trajectory: 0.05
 
+jaco_arm_servo_controller:
+  ros__parameters:
+    joints:
+      - j2n6s200_joint_1
+      - j2n6s200_joint_2
+      - j2n6s200_joint_3
+      - j2n6s200_joint_4
+      - j2n6s200_joint_5
+      - j2n6s200_joint_6
+
 hand_controller:
   ros__parameters:
     joints:

ada_moveit/config/mock_servo.yaml

@@ -0,0 +1,75 @@
+# Adapted from https://github.com/ros-planning/moveit2/blob/humble/moveit_ros/moveit_servo/config/panda_simulated_config.yaml
+###############################################
+# Modify all parameters related to servoing here
+###############################################
+servo_node:
+  ros__parameters:
+    moveit_servo:
+      use_gazebo: false # Whether the robot is started in a Gazebo simulation environment
+
+      ## Properties of incoming commands
+      command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
+      scale:
+        # Scale parameters are only used if command_in_type=="unitless"
+        linear:  0.1  # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands.
+        rotational:  0.3 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands.
+        # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic.
+        joint: 0.5
+
+      # Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling)
+      # override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0]
+
+      ## Properties of outgoing commands
+      publish_period: 0.034  # 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
+      command_out_type: std_msgs/Float64MultiArray
+
+      # 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_accelerations: false
+
+      ## Plugins for smoothing outgoing commands
+      smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin"
+
+      # If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service,
+      # which other nodes can use as a source for information about the planning environment.
+      # NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node),
+      # then is_primary_planning_scene_monitor needs to be set to false.
+      is_primary_planning_scene_monitor: false
+
+      ## MoveIt properties
+      move_group_name:  jaco_arm  # Often 'manipulator' or 'arm'
+      planning_frame: j2n6s200_link_base  # The MoveIt planning frame. Often 'base_link' or 'world'
+
+      ## Other frames
+      ee_frame_name: forkTip  # The name of the end effector link, used to return the EE pose
+      robot_link_command_frame:  j2n6s200_link_base  # 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
+      # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish.
+      # Important because ROS may drop some messages and we need the robot to halt reliably.
+      num_outgoing_halt_msgs_to_publish: 0
+
+      ## Configure handling of singularities and joint limits
+      lower_singularity_threshold:  17.0  # Start decelerating when the condition number hits this (close to singularity)
+      hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this
+      joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
+      leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/ros-planning/moveit2/pull/620)
+
+      ## Topic names
+      cartesian_command_in_topic: ~/delta_twist_cmds  # Topic for incoming Cartesian twist commands
+      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: /jaco_arm_servo_controller/commands # Publish outgoing commands here
+
+      ## Collision checking for the entire robot body
+      check_collisions: true # Check collisions?
+      collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often.
+      self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m]
+      scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m]

ada_moveit/config/moveit_controllers.yaml

@@ -1,9 +1,10 @@
 # MoveIt uses this configuration for controller management
 
-moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
+moveit_controller_manager: moveit_ros_control_interface/Ros2ControlManager
 
 moveit_simple_controller_manager:
   controller_names:
+    - jaco_arm_servo_controller
     - jaco_arm_controller
     - hand_controller
 
@@ -18,6 +19,17 @@ moveit_simple_controller_manager:
       - j2n6s200_joint_4
       - j2n6s200_joint_5
       - j2n6s200_joint_6
+  jaco_arm_servo_controller:
+    type: ""
+    action_ns: commands
+    default: false
+    joints:
+      - j2n6s200_joint_1
+      - j2n6s200_joint_2
+      - j2n6s200_joint_3
+      - j2n6s200_joint_4
+      - j2n6s200_joint_5
+      - j2n6s200_joint_6
   hand_controller:
     type: FollowJointTrajectory
     action_ns: follow_joint_trajectory

ada_moveit/config/real_controllers.yaml

@@ -7,6 +7,10 @@ controller_manager:
       type: force_gate_controller/ForceGateController
 
 
+    jaco_arm_servo_controller:
+      type: force_gate_controller/ForceGateVelocityController
+
+
     hand_controller:
       type: joint_trajectory_controller/JointTrajectoryController
 
@@ -60,6 +64,19 @@ jaco_arm_controller:
       goal: 0.02
       trajectory: 0.05
 
+jaco_arm_servo_controller:
+  ros__parameters:
+    joints:
+      - j2n6s200_joint_1
+      - j2n6s200_joint_2
+      - j2n6s200_joint_3
+      - j2n6s200_joint_4
+      - j2n6s200_joint_5
+      - j2n6s200_joint_6
+    wrench_threshold:
+      topic: /wireless_ft/ftSensor1
+      fMag: 1.0
+
 hand_controller:
   ros__parameters:
     joints:

ada_moveit/config/real_servo.yaml

@@ -0,0 +1,75 @@
+# Adapted from https://github.com/ros-planning/moveit2/blob/humble/moveit_ros/moveit_servo/config/panda_simulated_config.yaml
+###############################################
+# Modify all parameters related to servoing here
+###############################################
+servo_node:
+  ros__parameters:
+    moveit_servo:
+      use_gazebo: false # Whether the robot is started in a Gazebo simulation environment
+
+      ## Properties of incoming commands
+      command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
+      scale:
+        # Scale parameters are only used if command_in_type=="unitless"
+        linear:  0.1  # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands.
+        rotational:  0.3 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands.
+        # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic.
+        joint: 0.5
+
+      # Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling)
+      # override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0]
+
+      ## Properties of outgoing commands
+      publish_period: 0.034  # 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
+      command_out_type: std_msgs/Float64MultiArray
+
+      # What to publish? Can save some bandwidth as most robots only require positions or velocities
+      publish_joint_positions: false
+      publish_joint_velocities: true
+      publish_joint_accelerations: false
+
+      ## Plugins for smoothing outgoing commands
+      smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin"
+
+      # If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service,
+      # which other nodes can use as a source for information about the planning environment.
+      # NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node),
+      # then is_primary_planning_scene_monitor needs to be set to false.
+      is_primary_planning_scene_monitor: false
+
+      ## MoveIt properties
+      move_group_name:  jaco_arm  # Often 'manipulator' or 'arm'
+      planning_frame: j2n6s200_link_base  # The MoveIt planning frame. Often 'base_link' or 'world'
+
+      ## Other frames
+      ee_frame_name: forkTip  # The name of the end effector link, used to return the EE pose
+      robot_link_command_frame:  j2n6s200_link_base  # 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
+      # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish.
+      # Important because ROS may drop some messages and we need the robot to halt reliably.
+      num_outgoing_halt_msgs_to_publish: 0
+
+      ## Configure handling of singularities and joint limits
+      lower_singularity_threshold:  17.0  # Start decelerating when the condition number hits this (close to singularity)
+      hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this
+      joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
+      leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/ros-planning/moveit2/pull/620)
+
+      ## Topic names
+      cartesian_command_in_topic: ~/delta_twist_cmds  # Topic for incoming Cartesian twist commands
+      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: /jaco_arm_servo_controller/commands # Publish outgoing commands here
+
+      ## Collision checking for the entire robot body
+      check_collisions: true # Check collisions?
+      collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often.
+      self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m]
+      scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m]

ada_moveit/launch/demo.launch.py

@@ -1,4 +1,5 @@
 import os
+import yaml
 from ament_index_python.packages import get_package_share_directory
 from moveit_configs_utils import MoveItConfigsBuilder
 from moveit_configs_utils.launches import generate_demo_launch
@@ -54,11 +55,19 @@ def generate_launch_description():
     )
     controllers_file = LaunchConfiguration("controllers_file")
 
+    servo_da = DeclareLaunchArgument(
+        "servo_file",
+        default_value=[sim, "_servo.yaml"],
+        description="MoveIt Servo YAML configuration in config folder",
+    )
+    servo_file = LaunchConfiguration("servo_file")
+
     # Copy from generate_demo_launch
     ld = LaunchDescription()
+    ld.add_action(calib_da)
     ld.add_action(sim_da)
     ld.add_action(ctrl_da)
-    ld.add_action(calib_da)
+    ld.add_action(servo_da)
 
     # Camera Calibration File
     ld.add_action(
@@ -127,6 +136,7 @@ def generate_launch_description():
         )
     )
 
+    # Launch the Move Group
     ld.add_action(
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(
@@ -172,6 +182,23 @@ def generate_launch_description():
         "robot_description": ParameterValue(robot_description_content, value_type=str)
     }
 
+    # Launch MoveIt Servo
+    servo_config = PathJoinSubstitution(
+        [str(moveit_config.package_path), "config", servo_file]
+    )
+    ld.add_action(Node(
+        package="moveit_servo",
+        executable="servo_node_main",
+        name="servo_node",
+        parameters=[
+            servo_config,
+            robot_description,
+            moveit_config.robot_description_semantic,
+            # moveit_config.robot_description_kinematics, # If set, use IK instead of the inverse jacobian
+        ],
+        output="screen",
+    ))
+
     robot_controllers = PathJoinSubstitution(
         [str(moveit_config.package_path), "config", controllers_file]
     )

ada_moveit/package.xml

@@ -17,11 +17,13 @@
   <author email="[email protected]">Ethan K. Gordon</author>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
 
   <exec_depend>moveit_ros_move_group</exec_depend>
   <exec_depend>moveit_kinematics</exec_depend>
   <exec_depend>moveit_planners</exec_depend>
   <exec_depend>moveit_simple_controller_manager</exec_depend>
+  <exec_depend>moveit_ros_controller_interface</exec_depend>
   <exec_depend>joint_state_publisher</exec_depend>
   <exec_depend>joint_state_publisher_gui</exec_depend>
   <exec_depend>tf2_ros</exec_depend>