forked from MarkFzp/mobile-aloha
-
Notifications
You must be signed in to change notification settings - Fork 8
/
Copy pathsleep.py
executable file
·76 lines (62 loc) · 1.88 KB
/
sleep.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
74
75
76
#!/usr/bin/env python3
import argparse
from aloha.robot_utils import (
sleep_arms,
torque_on,
disable_gravity_compensation,
)
from interbotix_common_modules.common_robot.robot import (
create_interbotix_global_node,
robot_shutdown,
robot_startup,
)
from interbotix_xs_modules.xs_robot.arm import InterbotixManipulatorXS
def main():
argparser = argparse.ArgumentParser(
prog='sleep',
description='Sends arms to their sleep poses',
)
argparser.add_argument(
'-a', '--all',
help='If set, also sleeps leaders arms',
action='store_true',
default=False,
)
args = argparser.parse_args()
node = create_interbotix_global_node('aloha')
follower_bot_left = InterbotixManipulatorXS(
robot_model='vx300s',
robot_name='follower_left',
node=node,
iterative_update_fk=False,
)
follower_bot_right = InterbotixManipulatorXS(
robot_model='vx300s',
robot_name='follower_right',
node=node,
iterative_update_fk=False,
)
leader_bot_left = InterbotixManipulatorXS(
robot_model='wx250s',
robot_name='leader_left',
node=node,
iterative_update_fk=False,
)
leader_bot_right = InterbotixManipulatorXS(
robot_model='wx250s',
robot_name='leader_right',
node=node,
iterative_update_fk=False,
)
robot_startup(node)
disable_gravity_compensation(leader_bot_left)
disable_gravity_compensation(leader_bot_right)
all_bots = [follower_bot_left, follower_bot_right, leader_bot_left, leader_bot_right]
follower_bots = [follower_bot_left, follower_bot_right]
bots_to_sleep = all_bots if args.all else follower_bots
for bot in bots_to_sleep:
torque_on(bot)
sleep_arms(bots_to_sleep, home_first=True)
robot_shutdown(node)
if __name__ == '__main__':
main()