###############################################
# Modify all parameters related to servoing here
###############################################
# adapt to dummy2 by Muzhxiaowen, check out the details on bilibili.com

use_gazebo: false # Whether the robot is started in a Gazebo simulation environment

## Properties of incoming commands
command_in_type: "unitless" # "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.4  # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands.
  rotational:  0.8 # 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: trajectory_msgs/JointTrajectory

# What to publish? Can save some bandwidth as most robots only require positions or velocities
publish_joint_positions: true
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: true

## MoveIt properties
move_group_name:  dummy2_arm  # Often 'manipulator' or 'arm'
planning_frame: base_link  # The MoveIt planning frame. Often 'base_link' or 'world'

## Other frames
ee_frame_name: J6_1 # 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
# 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: 4

## Configure handling of singularities and joint limits
lower_singularity_threshold:  170.0  # Start decelerating when the condition number hits this (close to singularity)
hard_stop_singularity_threshold: 3000.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: /dummy2_arm_controller/joint_trajectory # 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.002 # Start decelerating when a scene collision is this far [m]
