-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathAtoB.py
executable file
·73 lines (57 loc) · 2.16 KB
/
AtoB.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
from arm_modules_modifi.arm import InterbotixManipulatorXS
from board_2_pose import pose_H, pose_L, out_H, out_L
bot = InterbotixManipulatorXS('rx150', 'arm', 'gripper')
# Capture: False
def AtoB(squ_A, squ_B):
squ_A = squ_A.upper()
squ_B = squ_B.upper()
x1_H, y1_H, z1_H, pitch1_H = pose_H.get(squ_A)
*xy, z1_L, pitch1_L = pose_L.get(squ_A)
x2_H, y2_H, z2_H, pitch2_H = pose_H.get(squ_B)
*xy, z2_L, pitch2_L = pose_L.get(squ_B)
# Square 1
bot.arm.set_ee_pose_components(
x=x1_H*0.01, y=y1_H*0.01, z=z1_H, pitch=pitch1_H)
bot.arm.set_ee_cartesian_trajectory(
z=-(z1_H-z1_L), pitch=+ (pitch1_L - pitch1_H))
bot.gripper.close()
bot.arm.set_ee_cartesian_trajectory(
z=z1_H-z1_L, pitch=- (pitch1_L - pitch1_H))
# End Square 1
# Square 2
bot.arm.set_ee_pose_components(
x=x2_H*0.01, y=y2_H*0.01, z=z2_H, pitch=pitch2_H)
bot.arm.set_ee_cartesian_trajectory(
z=-(z2_H-z2_L), pitch=+ (pitch2_L - pitch2_H))
bot.gripper.open()
bot.arm.set_ee_cartesian_trajectory(
z=z2_H-z2_L, pitch=- (pitch2_L - pitch2_H))
# End Square 2
bot.arm.go_to_home_pose()
# bot.arm.got_to_rest_pose() (thay the home pose)
# Captrue: True
def AtoB_capture(squ_A, squ_B, grave):
squ_B = squ_B.upper()
x1_H, y1_H, z1_H, pitch1_H = pose_H.get(squ_B)
*xy, z1_L, pitch1_L = pose_L.get(squ_B)
x2_H, y2_H, z2_H, pitch2_H = out_H.get(grave)
*xy, z2_L, pitch2_L = out_L.get(grave)
# Square 1
bot.arm.set_ee_pose_components(
x=x1_H*0.01, y=y1_H*0.01, z=z1_H, pitch=pitch1_H)
bot.arm.set_ee_cartesian_trajectory(
z=-(z1_H-z1_L), pitch=+ (pitch1_L - pitch1_H))
bot.gripper.close()
bot.arm.set_ee_cartesian_trajectory(
z=z1_H-z1_L, pitch=- (pitch1_L - pitch1_H))
# End Square 1
# Square 2
bot.arm.set_ee_pose_components(
x=x2_H*0.01, y=y2_H*0.01, z=z2_H, pitch=pitch2_H)
bot.arm.set_ee_cartesian_trajectory(
z=-(z2_H-z2_L), pitch=+ (pitch2_L - pitch2_H))
bot.gripper.open()
bot.arm.set_ee_cartesian_trajectory(
z=z2_H-z2_L, pitch=- (pitch2_L - pitch2_H))
# End Square 2
AtoB(squ_A, squ_B)