74 lines
3.4 KiB
Python
74 lines
3.4 KiB
Python
import math
|
|
from utils import RobotClient
|
|
|
|
HOST = "192.168.3.2"
|
|
PORT = 9090
|
|
|
|
config = {
|
|
"init": {
|
|
'pos': [0.09810244822368663, 0.3313196269764149, 0.44544069305226175], 'ori': [0.8029422807647959, -0.4820363460296128, 0.034976080376991056, -0.3488571751707414]
|
|
},
|
|
"pre_grasp_position": {
|
|
'pos': [0.076875580075771, 0.4985114141223675, 0.2578368487288887], 'ori': [0.6837207910111557, -0.7294870127641278, -0.004201289362322976, -0.01889252008713971]
|
|
},
|
|
"grasp_position": {
|
|
'pos': [0.04022607947465449, 0.49870956679172035, 0.12855873351195465], 'ori': [0.7108673879318799, -0.6971760905452062, 0.04979916198276968, -0.07831410481402765]
|
|
},
|
|
"post_grasp_position": {
|
|
'pos': [0.04138766852425917, 0.49843996951036346, 0.17242170594713332], 'ori': [0.7105716687621472, -0.6972845331909826, 0.050898212328428484, -0.0793193249276688]
|
|
},
|
|
"pre_insert_position": {
|
|
'pos': [0.15531457163779136, 0.4887644805574635, 0.30996257822689316], 'ori': [0.7096793154895238, -0.6750311908027418, 0.12151729153083345, -0.1610022001985184]
|
|
},
|
|
"insert_position": {
|
|
'pos': [0.18819735499444024, 0.4933238342577449, 0.2803404963272801], 'ori': [0.7110943169204221, -0.6510710114660861, 0.06550934464000123, -0.2572157387036094]
|
|
},
|
|
"post_insert_position": {
|
|
'pos': [0.20150600619054684, 0.4986867571185973, 0.24683169991220366], 'ori': [0.6922110693388366, -0.6725304109660972, 0.0757396109325311, -0.2506196184386724]
|
|
},
|
|
"move_away_position": {
|
|
'pos': [0.06646318981938315, 0.28008362744706417, 0.3081809630089793], 'ori': [0.9557005454147172, 0.023550142689031436, 0.010835030563343097, -0.29319696517193]
|
|
},
|
|
"joint_move_away_position": {
|
|
'pos': [-0.03705798520615846, 0.2948740118379424, 0.1380593922055155], 'ori': [0.7044778798061612, -0.7037920747711072, 0.051086947698884985, -0.07601155259569223]
|
|
},
|
|
"final_moveaway": {
|
|
'pos': [0.06232913033349278, 0.28220623060555067, 0.1939445619236099], 'ori': [0.6689183667946061, -0.6855402199272477, 0.10599550652863488, -0.267110048524341]
|
|
},
|
|
"joint_speed_test": {
|
|
'pos': [-0.014667855657250158, 0.21178473592030767, 0.6099448870104303], 'ori': [0.8973159098098845, -0.2554666640875886, 0.044440682660961045, -0.35719177939399194]
|
|
},
|
|
}
|
|
|
|
def main():
|
|
robot = RobotClient(HOST, PORT)
|
|
robot.set_joint_maxacc(1)
|
|
robot.set_joint_maxvelc(1)
|
|
robot.set_end_max_line_velc(1)
|
|
robot.set_end_max_line_acc(1)
|
|
robot.set_end_max_angle_acc(1)
|
|
robot.set_end_max_angle_velc(1)
|
|
|
|
robot.move_to_pose_joint(config["final_moveaway"])
|
|
robot.move_to_pose_cart(config["pre_grasp_position"])
|
|
robot.move_to_pose_cart(config["grasp_position"])
|
|
robot.move_to_pose_cart(config["post_grasp_position"])
|
|
robot.move_to_pose_cart(config["pre_insert_position"])
|
|
robot.move_to_pose_cart(config["insert_position"])
|
|
robot.move_to_pose_cart(config["post_insert_position"])
|
|
robot.move_to_pose_cart(config["final_moveaway"])
|
|
|
|
# robot.move_to_pose_joint(config["final_moveaway"])
|
|
# robot.move_to_pose_joint(config["joint_speed_test"])
|
|
# robot.move_to_pose_joint(config["final_moveaway"])
|
|
# robot.move_to_pose_joint(config["joint_speed_test"])
|
|
# robot.move_to_pose_joint(config["final_moveaway"])
|
|
# robot.move_to_pose_joint(config["joint_speed_test"])
|
|
|
|
|
|
robot.close()
|
|
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|