mock_servo.yaml

# Copyright (c) 2024-2025, Personal Robotics Laboratory
# License: BSD 3-Clause. See LICENSE.md file in root directory.

# 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
      monitored_planning_scene_topic: "monitored_planning_scene"

      ## 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: j2n6s200_end_effector  # 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.33  # 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:  200.0  # Start decelerating when the condition number hits this (close to singularity)
      hard_stop_singularity_threshold: 400.0 # Stop when the condition number hits this
      joint_limit_margin: 0.2 # 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.001 # Start decelerating when a self-collision is this far [m]
      scene_collision_proximity_threshold: 0.0075 # Start decelerating when a scene collision is this far [m]