def init_dyn_recfg():
cli = tcc.TwistControllerReconfigureClient()
cli.init()
cli.set_config_param(tcc.DAMP_METHOD, tcc.TwistController_MANIPULABILITY)
cli.set_config_param(tcc.LAMBDA_MAX, 0.1)
cli.set_config_param(tcc.W_THRESH, 0.05)
cli.set_config_param(tcc.SOLVER, tcc.TwistController_STACK_OF_TASKS)
cli.set_config_param(tcc.K_H, 1.0)
cli.set_config_param(tcc.CONSTR_CA, tcc.TwistController_CA)
cli.set_config_param(tcc.K_H_CA, -1.9)
cli.set_config_param(tcc.DAMP_CA, 0.000001)
cli.set_config_param(tcc.ACTIV_THRESH_CA, 0.1)
cli.set_config_param(tcc.CONSTR_JLA, tcc.TwistController_JLA)
cli.set_config_param(tcc.K_H_JLA, -1.0)
cli.set_config_param(tcc.DAMP_JLA, 0.00001)
cli.set_config_param(tcc.ACTIV_THRESH_JLA, 10.0)
cli.set_config_param(tcc.KIN_EXT, tcc.TwistController_NO_EXTENSION)
cli.set_config_param(tcc.KEEP_DIR, True)
cli.set_config_param(tcc.ENF_VEL_LIM, True)
cli.set_config_param(tcc.ENF_POS_LIM, True)
cli.update()
time.sleep(1.0)
cli.set_config_param(tcc.K_H_CA, -2.0)
cli.update()
cli.close()
def change_dyn_recfg_for_jla(solver_type):
cli = tcc.TwistControllerReconfigureClient()
cli.init()
cli.set_config_param(
tcc.CONSTR_CA,
tcc.TwistController_CA_OFF) # switch CA off, now only JLA interesting!
cli.set_config_param(tcc.SOLVER, solver_type)
cli.set_config_param(tcc.CONSTR_JLA, tcc.TwistController_JLA)
cli.set_config_param(tcc.K_H_JLA, -0.1)
cli.set_config_param(tcc.ACTIV_THRESH_JLA, 10.0)
cli.set_config_param(tcc.ACTIV_BUF_JLA, 300.0)
cli.set_config_param(tcc.CRIT_THRESH_JLA, 5.0)
cli.set_config_param(tcc.DAMP_JLA, 0.00001)
cli.set_config_param(tcc.KIN_EXT, tcc.TwistController_NO_EXTENSION)
cli.set_config_param(tcc.KEEP_DIR, True)
cli.set_config_param(tcc.ENF_VEL_LIM, True)
cli.set_config_param(tcc.ENF_POS_LIM, False)
cli.set_config_param(tcc.TOL,
0.01) # To show limits are violate -> no tolerance
cli.update()
time.sleep(0.5)
print(cli.get_configuration(5.0))
cli.close()
def init_dyn_recfg():
cli = tcc.TwistControllerReconfigureClient()
cli.init()
cli.set_config_param(tcc.CTRL_IF, tcc.TwistController_VELOCITY_INTERFACE)
cli.set_config_param(tcc.NUM_FILT, False)
cli.set_config_param(tcc.DAMP_METHOD, tcc.TwistController_MANIPULABILITY)
cli.set_config_param(tcc.LAMBDA_MAX, 0.1)
cli.set_config_param(tcc.W_THRESH, 0.05)
# cli.set_config_param(tcc.DAMP_METHOD, tcc.TwistController_LEAST_SINGULAR_VALUE)
# cli.set_config_param(tcc.LAMBDA_MAX, 0.2)
# cli.set_config_param(tcc.EPS_DAMP, 0.2)
cli.set_config_param(tcc.EPS_TRUNC, 0.001)
cli.set_config_param(tcc.PRIO_CA, 100)
cli.set_config_param(tcc.PRIO_JLA, 50)
cli.set_config_param(tcc.SOLVER, tcc.TwistController_STACK_OF_TASKS)
cli.set_config_param(tcc.K_H, 1.0)
cli.set_config_param(tcc.CONSTR_CA, tcc.TwistController_CA)
cli.set_config_param(tcc.K_H_CA, -2.0)
cli.set_config_param(tcc.ACTIV_THRESH_CA, 0.1)
cli.set_config_param(tcc.ACTIV_BUF_CA, 50.0)
cli.set_config_param(tcc.CRIT_THRESH_CA, 0.025)
cli.set_config_param(tcc.DAMP_CA, 0.000001)
cli.set_config_param(tcc.CONSTR_JLA, tcc.TwistController_JLA)
cli.set_config_param(tcc.K_H_JLA, -1.0)
cli.set_config_param(tcc.ACTIV_THRESH_JLA, 10.0)
cli.set_config_param(tcc.ACTIV_BUF_JLA, 300.0)
cli.set_config_param(tcc.CRIT_THRESH_JLA, 5.0)
cli.set_config_param(tcc.DAMP_JLA, 0.00001)
cli.set_config_param(tcc.KIN_EXT, tcc.TwistController_NO_EXTENSION)
cli.set_config_param(tcc.KEEP_DIR, True)
cli.set_config_param(tcc.ENF_VEL_LIM, True)
cli.set_config_param(tcc.ENF_POS_LIM, True)
cli.update()
cli.close()
cli = Client('frame_tracker')
ft_param = {
'cart_min_dist_threshold_lin': 0.2,
'cart_min_dist_threshold_rot': 0.2
}
cli.update_configuration(ft_param)
cli.close()
def init_dyn_recfg():
cli = tcc.TwistControllerReconfigureClient()
cli.init()
cli.set_config_param(tcc.CTRL_IF, tcc.TwistController_VELOCITY_INTERFACE)
cli.set_config_param(tcc.NUM_FILT, False)
cli.set_config_param(tcc.DAMP_METHOD, tcc.TwistController_CONSTANT)
cli.set_config_param(tcc.DAMP_FACT, 0.2)
cli.set_config_param(tcc.EPS_TRUNC, 0.001)
cli.set_config_param(tcc.PRIO_CA, 100)
cli.set_config_param(tcc.PRIO_JLA, 50)
cli.set_config_param(tcc.SOLVER, tcc.TwistController_STACK_OF_TASKS)
cli.set_config_param(tcc.K_H, 1.0)
cli.set_config_param(tcc.CONSTR_CA, tcc.TwistController_CA)
cli.set_config_param(tcc.K_H_CA, -1.0)
cli.set_config_param(tcc.ACTIV_THRESH_CA, 0.1)
cli.set_config_param(tcc.ACTIV_BUF_CA, 25.0)
cli.set_config_param(tcc.CRIT_THRESH_CA, 0.025)
cli.set_config_param(tcc.DAMP_CA, 0.000001)
cli.set_config_param(tcc.CONSTR_JLA, tcc.TwistController_JLA)
cli.set_config_param(tcc.K_H_JLA, -1.0)
cli.set_config_param(tcc.ACTIV_THRESH_JLA, 10.0)
cli.set_config_param(tcc.ACTIV_BUF_JLA, 300.0)
cli.set_config_param(tcc.CRIT_THRESH_JLA, 5.0)
cli.set_config_param(tcc.DAMP_JLA, 0.00001)
cli.set_config_param(tcc.KIN_EXT, tcc.TwistController_BASE_ACTIVE)
cli.set_config_param(tcc.EXT_RATIO, 0.05)
cli.set_config_param(tcc.KEEP_DIR, True)
cli.set_config_param(tcc.ENF_VEL_LIM, True)
cli.set_config_param(
tcc.ENF_POS_LIM,
False) # To show that joint pos limits are violated if possible.
cli.update()
cli.close()
cli = Client('frame_tracker')
ft_param = {
'cart_min_dist_threshold_lin': 5.0,
'cart_min_dist_threshold_rot': 5.0
}
cli.update_configuration(ft_param)
cli.close()
def init_dyn_recfg():
cli = tcc.TwistControllerReconfigureClient()
cli.init()
cli.set_config_param(tcc.NUM_FILT, False)
cli.set_config_param(tcc.DAMP_METHOD, tcc.TwistController_MANIPULABILITY)
cli.set_config_param(tcc.LAMBDA_MAX, 0.1)
cli.set_config_param(tcc.W_THRESH, 0.05)
cli.set_config_param(tcc.EPS_TRUNC, 0.001)
cli.set_config_param(tcc.PRIO_CA, 50)
cli.set_config_param(
tcc.PRIO_JLA,
100) # change priorities now CA constraint has highest prio
cli.set_config_param(tcc.SOLVER, tcc.TwistController_STACK_OF_TASKS)
cli.set_config_param(tcc.K_H, 1.0)
cli.set_config_param(tcc.CONSTR_CA, tcc.TwistController_CA)
cli.set_config_param(tcc.K_H_CA, -1.0)
cli.set_config_param(tcc.DAMP_CA, 0.000001)
cli.set_config_param(tcc.ACTIV_THRESH_CA, 0.1)
cli.set_config_param(tcc.ACTIV_BUF_CA, 50.0)
cli.set_config_param(tcc.CRIT_THRESH_CA, 0.03)
cli.set_config_param(tcc.CONSTR_JLA, tcc.TwistController_JLA)
cli.set_config_param(tcc.K_H_JLA, -1.0)
cli.set_config_param(tcc.DAMP_JLA, 0.000001)
cli.set_config_param(tcc.ACTIV_THRESH_JLA, 10.0)
cli.set_config_param(tcc.ACTIV_BUF_JLA, 300.0)
cli.set_config_param(tcc.CRIT_THRESH_JLA, 5.0)
cli.set_config_param(tcc.KIN_EXT, tcc.TwistController_NO_EXTENSION)
cli.set_config_param(tcc.KEEP_DIR, True)
cli.set_config_param(tcc.ENF_VEL_LIM, True)
cli.set_config_param(tcc.ENF_POS_LIM, True)
cli.update()
cli.close()
cli = Client('frame_tracker')
ft_param = {
'cart_min_dist_threshold_lin': 0.2,
'cart_min_dist_threshold_rot': 0.2
}
cli.update_configuration(ft_param)
cli.close()
def init_dyn_recfg():
cli = tcc.TwistControllerReconfigureClient()
cli.init()
cli.set_config_param(tcc.CTRL_IF, tcc.TwistController_VELOCITY_INTERFACE)
cli.set_config_param(tcc.DAMP_METHOD, tcc.TwistController_MANIPULABILITY)
cli.set_config_param(tcc.LAMBDA_MAX, 0.1)
cli.set_config_param(tcc.W_THRESH, 0.05)
cli.set_config_param(tcc.PRIO_CA, 100)
cli.set_config_param(tcc.PRIO_JLA, 50)
cli.set_config_param(tcc.SOLVER, tcc.TwistController_GPM)
cli.set_config_param(tcc.K_H, 1.0)
cli.set_config_param(tcc.CONSTR_CA, tcc.TwistController_CA)
cli.set_config_param(tcc.K_H_CA, -2.0)
cli.set_config_param(tcc.DAMP_CA, 0.000001)
cli.set_config_param(tcc.ACTIV_THRESH_CA, 0.1)
cli.set_config_param(tcc.CONSTR_JLA, tcc.TwistController_JLA)
cli.set_config_param(tcc.K_H_JLA, -1.0)
cli.set_config_param(tcc.DAMP_JLA, 0.00001)
cli.set_config_param(tcc.ACTIV_THRESH_JLA, 10.0)
cli.set_config_param(tcc.KIN_EXT, tcc.TwistController_NO_EXTENSION)
cli.set_config_param(tcc.KEEP_DIR, True)
cli.set_config_param(tcc.ENF_VEL_LIM, True)
cli.set_config_param(
tcc.ENF_POS_LIM,
False) # To show that joint pos limits are violated if possible.
cli.update()
cli.close()
cli = Client('frame_tracker')
ft_param = {
'cart_min_dist_threshold_lin': 0.2,
'cart_min_dist_threshold_rot': 0.2
}
cli.update_configuration(ft_param)
cli.close()
