def init_pos():
sss = simple_script_server()
sss.move("arm", [[
-0.0002934322105607734, -0.38304632633953606, 6.931483707006691e-07,
0.8526320037121202, 5.69952198326007e-07, -0.47039657856235184,
-0.00029228225570943067
]])
def init_pos():
sss = simple_script_server()
sss.move("arm_right", "home")
sss.move("arm_right", [[
-0.039928546971938594, 0.7617065276699337, 0.01562043859727158,
-1.7979678396373568, 0.013899422367821046, 1.0327319085987252,
0.021045294231647915
]])
def init_pos():
sss = simple_script_server()
# sss.move("arm_right", [[-0.039928546971938594, 0.7617065276699337, 0.01562043859727158, -1.7979678396373568, 0.013899422367821046, 1.0327319085987252, 0.021045294231647915]])
sss.move("arm_left", [
[
-0.17566952192977148, 0.6230441162870415, 0.27476319388433623,
-1.1163222472275676, 0.3166667900974902, 0.5333301416131739,
-0.32441227738211875
],
])
def init_pos():
sss = simple_script_server()
# sss.move("arm_right", [[-0.039928546971938594, 0.7617065276699337, 0.01562043859727158, -1.7979678396373568, 0.013899422367821046, 1.0327319085987252, 0.021045294231647915]])
sss.move("arm_left", [
[
2.274847163975995, -0.08568661652370757, -0.8273207226405264,
-1.9404110013940103, 1.306315205401929, 1.5627416614617742,
-1.1199725164070955
],
])
def init_pos():
# Trick to move base back to odom_combined
switch_controller = rospy.ServiceProxy('/base/controller_manager/switch_controller', SwitchController)
print(switch_controller(None, ['odometry_controller', ], 1)) # switch off
time.sleep(1.0)
print(switch_controller(['odometry_controller', ], None, 1)) # switch on
time.sleep(1.0)
sss = simple_script_server()
sss.move("arm_left", "side") # for better pics
sss.move("arm_right", [[0.6849513492283021, 0.9811503738420306, -0.05053975117653131, -1.4680375957626568, -0.0824580145043452, 0.4964406318714998, -0.5817382519875354]])
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()
if __name__ == '__main__':
rospy.init_node('test_move_around_torus')
action_name = rospy.get_namespace() + 'cartesian_trajectory_action'
client = actionlib.SimpleActionClient(action_name,
CartesianControllerAction)
rospy.logwarn("Waiting for ActionServer: %s", action_name)
client.wait_for_server()
rospy.logwarn("...done")
init_dyn_recfg()
sss = simple_script_server()
sss.move("arm_right", "home")
move_around_torus()
def init_pos():
sss = simple_script_server()
sss.move("arm_right", "home")
sss.move("arm_right", "home")
def init_pos():
sss = simple_script_server()
sss.move("arm", [[-0.00032004093963244884, -0.7064191894021441, -1.577532922958369e-06, 1.4183686971944311, 1.2084352562169443e-05, -0.6913530502577565, -0.0002663056533762642]])
