56 lines
2 KiB
Python
56 lines
2 KiB
Python
import math
|
|
from utils import RobotClient
|
|
|
|
HOST = "192.168.3.2"
|
|
PORT = 9090
|
|
|
|
config = {
|
|
"pre_grasp_position": {
|
|
'pos': [0.11468976722774939, 0.49820734702047975, 0.232304304821942],
|
|
'ori': [0.03139065917169165, 0.007114594087226847, 0.7337638657217594, 0.6786417305388762]
|
|
},
|
|
"grasp_position": {
|
|
'pos': [0.10651987287035074, 0.4918349678759202, 0.12068363327771679],
|
|
'ori': [0.0461769536371005, 0.019808331049769666, 0.7271651527969424, 0.68462117958154]
|
|
},
|
|
"post_grasp_position": {
|
|
'pos': [0.10584392070929988, 0.4941985576613782, 0.1688168818859202],
|
|
'ori': [0.04549168126833617, 0.01984700639803424, 0.7237928131621344, 0.6882300246917887]
|
|
},
|
|
"pre_insert_position": {
|
|
'pos': [0.24027684741261485, 0.49126338168373945, 0.39655250532732794],
|
|
'ori': [0.1964966354829754, -0.025599877633873055, 0.6813871431453161, 0.7045887308673521]
|
|
},
|
|
"insert_position": {
|
|
'pos': [0.27300632591107804, 0.48041141969089585, 0.3049992290184565],
|
|
'ori': [0.17245191209515515, 6.719202049935031e-05, 0.7033203128819105, 0.689638217464526]
|
|
},
|
|
"post_insert_position": {
|
|
'pos': [0.2904632222294866, 0.4869030095129388, 0.1625443919495984],
|
|
'ori': [0.20018806753589566, 0.023937708375556675, 0.688796013797962, 0.6963560691988031]
|
|
},
|
|
}
|
|
|
|
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_to_pose_joint(config["pre_grasp_position"])
|
|
robot.move_to_pose_cart(config["grasp_position"])
|
|
robot.move_to_pose_joint(config["post_grasp_position"])
|
|
robot.move_to_pose_joint(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["insert_position"])
|
|
robot.move_to_pose_joint(config["pre_insert_position"])
|
|
|
|
|
|
robot.close()
|
|
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|