forked from MarkFzp/mobile-aloha
-
Notifications
You must be signed in to change notification settings - Fork 10
/
Copy pathdual_side_teleop.py
executable file
·188 lines (165 loc) · 6.66 KB
/
dual_side_teleop.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
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
#!/usr/bin/env python3
import argparse
import signal
from functools import partial
from aloha.constants import (
DT_DURATION,
FOLLOWER_GRIPPER_JOINT_CLOSE,
LEADER2FOLLOWER_JOINT_FN,
LEADER_GRIPPER_CLOSE_THRESH,
LEADER_GRIPPER_JOINT_MID,
START_ARM_POSE,
)
from aloha.robot_utils import (
enable_gravity_compensation,
disable_gravity_compensation,
get_arm_gripper_positions,
move_arms,
move_grippers,
torque_off,
torque_on,
)
from interbotix_common_modules.common_robot.robot import (
create_interbotix_global_node,
get_interbotix_global_node,
robot_shutdown,
robot_startup,
)
from interbotix_xs_modules.xs_robot.arm import InterbotixManipulatorXS
from interbotix_xs_msgs.msg import JointSingleCommand
import rclpy
def opening_ceremony(
leader_bot_left: InterbotixManipulatorXS,
leader_bot_right: InterbotixManipulatorXS,
follower_bot_left: InterbotixManipulatorXS,
follower_bot_right: InterbotixManipulatorXS,
) -> None:
"""Move all 4 robots to a pose where it is easy to start demonstration."""
# reboot gripper motors, and set operating modes for all motors
follower_bot_left.core.robot_reboot_motors('single', 'gripper', True)
follower_bot_left.core.robot_set_operating_modes('group', 'arm', 'position')
follower_bot_left.core.robot_set_operating_modes('single', 'gripper', 'current_based_position')
leader_bot_left.core.robot_set_operating_modes('group', 'arm', 'position')
leader_bot_left.core.robot_set_operating_modes('single', 'gripper', 'position')
follower_bot_left.core.robot_set_motor_registers('single', 'gripper', 'current_limit', 300)
follower_bot_right.core.robot_reboot_motors('single', 'gripper', True)
follower_bot_right.core.robot_set_operating_modes('group', 'arm', 'position')
follower_bot_right.core.robot_set_operating_modes(
'single', 'gripper', 'current_based_position'
)
leader_bot_right.core.robot_set_operating_modes('group', 'arm', 'position')
leader_bot_right.core.robot_set_operating_modes('single', 'gripper', 'position')
follower_bot_right.core.robot_set_motor_registers('single', 'gripper', 'current_limit', 300)
torque_on(follower_bot_left)
torque_on(leader_bot_left)
torque_on(follower_bot_right)
torque_on(leader_bot_right)
# move arms to starting position
start_arm_qpos = START_ARM_POSE[:6]
move_arms(
[leader_bot_left, follower_bot_left, leader_bot_right, follower_bot_right],
[start_arm_qpos] * 4,
moving_time=4.0,
)
# move grippers to starting position
move_grippers(
[leader_bot_left, follower_bot_left, leader_bot_right, follower_bot_right],
[LEADER_GRIPPER_JOINT_MID, FOLLOWER_GRIPPER_JOINT_CLOSE] * 2,
moving_time=0.5
)
def press_to_start(
leader_bot_left: InterbotixManipulatorXS,
leader_bot_right: InterbotixManipulatorXS,
gravity_compensation: bool,
) -> None:
# press gripper to start teleop
# disable torque for only gripper joint of leader robot to allow user movement
leader_bot_left.core.robot_torque_enable('single', 'gripper', False)
leader_bot_right.core.robot_torque_enable('single', 'gripper', False)
print('Close the grippers to start')
pressed = False
while rclpy.ok() and not pressed:
pressed = (
(get_arm_gripper_positions(leader_bot_left) < LEADER_GRIPPER_CLOSE_THRESH) and
(get_arm_gripper_positions(leader_bot_right) < LEADER_GRIPPER_CLOSE_THRESH)
)
get_interbotix_global_node().get_clock().sleep_for(DT_DURATION)
if gravity_compensation:
enable_gravity_compensation(leader_bot_left)
enable_gravity_compensation(leader_bot_right)
else:
torque_off(leader_bot_left)
torque_off(leader_bot_right)
print('Started!')
def signal_handler(sig, frame, leader_bot_left, leader_bot_right):
print('You pressed Ctrl+C!')
disable_gravity_compensation(leader_bot_left)
disable_gravity_compensation(leader_bot_right)
exit(1)
def main(args: dict) -> None:
gravity_compensation = args.get('gravity_compensation', False)
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,
)
signal.signal(signal.SIGINT, partial(signal_handler, leader_bot_left=leader_bot_left, leader_bot_right=leader_bot_right))
robot_startup(node)
disable_gravity_compensation(leader_bot_left)
disable_gravity_compensation(leader_bot_right)
opening_ceremony(
leader_bot_left,
leader_bot_right,
follower_bot_left,
follower_bot_right,
)
press_to_start(leader_bot_left, leader_bot_right, gravity_compensation)
# Teleoperation loop
gripper_left_command = JointSingleCommand(name='gripper')
gripper_right_command = JointSingleCommand(name='gripper')
while rclpy.ok():
# sync joint positions
leader_left_state_joints = leader_bot_left.core.joint_states.position[:6]
leader_right_state_joints = leader_bot_right.core.joint_states.position[:6]
follower_bot_left.arm.set_joint_positions(leader_left_state_joints, blocking=False)
follower_bot_right.arm.set_joint_positions(leader_right_state_joints, blocking=False)
# sync gripper positions
gripper_left_command.cmd = LEADER2FOLLOWER_JOINT_FN(
leader_bot_left.core.joint_states.position[6]
)
gripper_right_command.cmd = LEADER2FOLLOWER_JOINT_FN(
leader_bot_right.core.joint_states.position[6]
)
follower_bot_left.gripper.core.pub_single.publish(gripper_left_command)
follower_bot_right.gripper.core.pub_single.publish(gripper_right_command)
# sleep DT
get_interbotix_global_node().get_clock().sleep_for(DT_DURATION)
robot_shutdown(node)
if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument(
'-g', '--gravity_compensation',
action='store_true',
help='If set, gravity compensation will be enabled for the leader robots when teleop starts.',
)
main(vars(parser.parse_args()))