rcm_params.yml

action_server: 'h_rcm_vs/RCM_ActionServer'
control_client: 'PositionJointInterface_trajectory_controller/follow_joint_trajectory'
planning_group: 'arm_endoscope'

# task and remote center of motion gains - proportional, integral, differential
kpt: [0.04,0.04,0.04,0.04,0.04,0.04]
kit: [0.,0.,0.,0.,0.,0.]
kdt: [0.,0.,0.,0.,0.,0.]
kprcm: [100., 100., 100.]
kircm: [0., 0., 0.]
kdrcm: [0., 0., 0.]
lambda0: 0.7

# control interval, in velocity control: rate at which rcm goals are sent, else: sets interval to execute incremental motion
dt: 0.01

# velocity scaling
alpha: 0.1

# link names
link_pi: 'storz_endoscope_link_1'
link_pip1: 'storz_endoscope_link_cm'

# threshold 1, divergence check prior to execution
t1_td: 10.0
t1_p_trocar: 0.01

# threshold 2, convergence check after execution
t2_td: 0.001
t2_p_trocar: 0.001

# threshold scale
t_td_scale: [1.0, 1.0, 1.0, 0.1, 0.1, 0.1]

# max number of iterations
max_iter: 10

# exponential smoothing in [0, 1], dumping for pseudo inverse, RCM priority for nullspace optimization
exp_smooth: 1.0  # 1.0 means no smoothing is applied
dumping: 0.01
rcm_priority: false