49 lines
1.6 KiB
Python
49 lines
1.6 KiB
Python
import math
|
|
from utils import RobotClient
|
|
|
|
HOST = "192.168.3.2"
|
|
PORT = 9090
|
|
|
|
config = {
|
|
"pre_grasp_position": {
|
|
'pos': [0.14726859839859208, 0.5302551637478977, 0.565792932407347],
|
|
'ori': [0.1305294928324298, -0.34660161091966263, 0.8293216816453197, 0.4183956538514503]
|
|
},
|
|
"grasp_position": {
|
|
'pos': [0.15928907070579643, 0.5270829242433969, 0.6510703359003733],
|
|
'ori': [0.15023706706737835, -0.32373703396446113, 0.8482916258752815, 0.39118342754871005]
|
|
},
|
|
"post_grasp_position": {
|
|
'pos': [0.08296640360052251, 0.5270826520836475, 0.4622718470958212],
|
|
'ori': [0.15024595915643826, -0.3237308590654784, 0.8482945564918141, 0.3911787674097941]
|
|
},
|
|
"pre_insert_position": {
|
|
'pos': [0.08296640360052251, 0.5270826520836475, 0.4622718470958212],
|
|
'ori': [0.15024595915643826, -0.3237308590654784, 0.8482945564918141, 0.3911787674097941]
|
|
},
|
|
"insert_position": {
|
|
"pos": [0.6, 0.0, 0.2],
|
|
"ori_offset": [10, -5, 0],
|
|
},
|
|
"post_insert_position": {
|
|
"pos": [0.6, 0.0, 0.2],
|
|
"ori_offset": [10, -5, 0],
|
|
},
|
|
}
|
|
|
|
def main():
|
|
robot = RobotClient(HOST, PORT)
|
|
#robot.set_joint_maxacc(3)
|
|
#robot.set_joint_maxvelc(3)
|
|
#robot.set_end_max_line_velc(0.6)
|
|
robot.set_end_max_line_acc(0.8)
|
|
robot.move_linear_cartesian(config["pre_grasp_position"])
|
|
# robot.move_to_pose(config["pre_grasp_position"])
|
|
# robot.move_to_pose(config["grasp_position"])
|
|
# robot.move_to_pose(config["post_grasp_position"])
|
|
robot.close()
|
|
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|