def init_robot(config):
""" Initializes a robot """
robot = None
subscriber = None
initialized = False
while not initialized:
try:
robot = YuMiRobot(debug=config['robot_off'])
robot.set_v(config['control']['standard_velocity'])
robot.set_z(config['control']['standard_zoning'])
if config['control']['use_left']:
arm = robot.left
arm.goto_state(YMC.L_HOME_STATE)
home_pose = YMC.L_PREGRASP_POSE
else:
arm = robot.right
arm.goto_state(YMC.R_HOME_STATE)
home_pose = YMC.R_AWAY_STATE
subscriber = YuMiSubscriber()
subscriber.start()
initialized = True
except YuMiCommException as ymc:
if robot is not None:
robot.stop()
if subscriber is not None and subscriber._started:
subscriber.stop()
logging.error(str(ymc))
logging.error(
'Failed to initialize YuMi. Check the FlexPendant and connection to the YuMi.'
)
human_input = raw_input('Hit [ENTER] when YuMi is ready')
return robot, subscriber, arm, home_pose
"""
Helper script to move YuMi back to home pose
Author: Jeff Mahler
"""
from yumipy import YuMiRobot, YuMiState
from yumipy import YuMiConstants as YMC
if __name__ == '__main__':
y = YuMiRobot()
y.set_z('z50')
y.set_v(500)
y.left.close_gripper()
robot = y
arm = y.left
arm.goto_pose(YMC.L_PREGRASP_POSE, wait_for_res=True)
# shake test
radius = 0.1
angle = np.pi / 8
delta_T = RigidTransform(translation=[0, 0, -radius],
from_frame='gripper',
to_frame='gripper')
R_shake = np.array([[1, 0, 0], [0, np.cos(angle), -np.sin(angle)],
[0, np.sin(angle), np.cos(angle)]])
delta_T_up = RigidTransform(rotation=R_shake,
translation=[0, 0, radius],
from_frame='gripper',
to_frame='gripper')
delta_T_down = RigidTransform(rotation=R_shake.T,
time_str = "2020-11-10 16:33:00"
timestamp = time.mktime(
datetime.strptime(time_str, "%Y-%m-%d %H:%M:%S").timetuple())
# log = open('/home/urp/experiment/robotlog.txt', 'a')
timeServer = 'time.windows.com'
c = ntplib.NTPClient()
response = c.request(timeServer, version=3)
# log_str = "Time difference(Server time - Local time): %.4fs \n" %response.offset
# log.write(log_str)
# log.close()
time_diff = response.offset
######################################################################
# y.reset_home()
y.set_v(300)
# y.calibrate_grippers()
# y.open_grippers()
# while (time.time() < timestamp):
# continue
# main_work()
# y.right.goto_pose(RigidTransform(translation = [0.28, -0.1 , 0.07], rotation = right_side_rotation))
# y.right.goto_pose(RigidTransform(translation = [0.28, 0.04 , 0.07], rotation = right_side_rotation))
y.right.goto_pose(
RigidTransform(translation=[0.28, 0, 0.2],
rotation=right_top_rotation))
# y.right.goto_pose(RigidTransform(translation = [0.28, 0, 0.06], rotation = right_top_rotation))
def playback(args):
cfg = YamlConfig(args.config_path)
demo_name = args.demo_name
supervisor = args.supervisor
trial_num = args.trial_num
if cfg['mode'] not in ('poses', 'states'):
y.stop()
raise ValueError(
"Unknown playback mode! Only accepts 'poses' or 'joints'. Got {0}".
format(cfg['mode']))
# init robot
logging.info("Init robot.")
y = YuMiRobot()
y.set_v(cfg['v'])
y.set_z(cfg['z'])
# load demo data
demo_records = CSVModel.load(
os.path.join(cfg['data_path'], 'demo_records.csv'))
demo_record = demo_records.get_by_cols({
'demo_name': demo_name,
'trial_num': trial_num,
'supervisor': supervisor
})
trial_path = demo_record['trial_path']
demo_host_cfg = YamlConfig(os.path.join(trial_path, 'demo_config.yaml'))
# parse demo trajectory
# TODO: enforce fps
fps = demo_host_cfg['fps']
_, left_data = zip(
*load(os.path.join(trial_path, '{0}_left.jb'.format(cfg['mode']))))
_, right_data = zip(
*load(os.path.join(trial_path, '{0}_right.jb'.format(cfg['mode']))))
_, gripper_left_evs = zip(
*load(os.path.join(trial_path, 'grippers_evs_left.jb')))
_, gripper_right_evs = zip(
*load(os.path.join(trial_path, 'grippers_evs_right.jb')))
seqs = {
'left': Sequence([t[1] for t in left_data]),
'right': Sequence([t[1] for t in right_data]),
'gripper_left': Sequence(gripper_left_evs),
'gripper_right': Sequence(gripper_right_evs)
}
# subsampling
subsample_factor = cfg['subsample']
subsampled_seqs = {
'left':
seqs['left'].subsampler(subsample_factor),
'right':
seqs['right'].subsampler(subsample_factor),
'gripper_left':
seqs['gripper_left'].subsampler(subsample_factor,
retain_features=True),
'gripper_right':
seqs['gripper_right'].subsampler(subsample_factor,
retain_features=True)
}
# concating non-moving steps
if cfg['concat']:
concat_data = {
'left': [subsampled_seqs['left'].data[0]],
'right': [subsampled_seqs['right'].data[0]],
'gripper_left': [subsampled_seqs['gripper_left'].data[0]],
'gripper_right': [subsampled_seqs['gripper_right'].data[0]]
}
last_lp = concat_data['left'][0]
last_rp = concat_data['right'][0]
for t in range(
1, min([len(seq.data) for seq in subsampled_seqs.values()])):
lg_t = subsampled_seqs['gripper_left'].data[t]
rg_t = subsampled_seqs['gripper_right'].data[t]
lp_t = subsampled_seqs['left'].data[t]
rp_t = subsampled_seqs['right'].data[t]
if lg_t is not None or rg_t is not None or \
lp_t != last_lp or rp_t != last_rp:
concat_data['gripper_right'].append(rg_t)
concat_data['gripper_left'].append(lg_t)
concat_data['left'].append(lp_t)
concat_data['right'].append(rp_t)
last_lp = lp_t
last_rp = rp_t
concat_seqs = {
'left': Sequence(concat_data['left']),
'right': Sequence(concat_data['right']),
'gripper_left': Sequence(concat_data['gripper_left']),
'gripper_right': Sequence(concat_data['gripper_right']),
}
else:
concat_seqs = subsampled_seqs
N = min([len(seq.data) for seq in concat_seqs.values()])
# processing time steps where zoning should be set to fine
gripper_zoning = [None for _ in range(N)]
for t in range(N - 1):
if concat_seqs['gripper_left'].data[t] != None or \
concat_seqs['gripper_right'].data[t] != None:
if t == 0:
y.set_z('fine')
else:
gripper_zoning[t - 1] = 'fine'
gripper_zoning[t + 1] = cfg['z']
# perform setup motions
logging.info("Loading demo and performing setups.")
y.reset_home()
y.open_grippers()
demo_path = os.path.join(trial_path, '{0}.py'.format(demo_name))
demo_obj = DemoWrapper.load(demo_path, y)
demo_obj.setup()
# record torque and other debug data if needed
if cfg['record_torque']['use']:
ysub = YuMiSubscriber()
ysub.start()
data_torque_left = DataStreamRecorder('torques_left',
ysub.left.get_torque,
cache_path=cfg['cache_path'],
save_every=cfg['save_every'])
data_torque_right = DataStreamRecorder('torques_right',
ysub.right.get_torque,
cache_path=cfg['cache_path'],
save_every=cfg['save_every'])
syncer = DataStreamSyncer([data_torque_left, data_torque_right], fps)
syncer.start()
sleep(0.5)
syncer.pause()
syncer.flush()
syncer.resume(reset_time=True)
# perform trajectory
logging.info("Playing trajectory")
for t in range(N):
left_item = concat_seqs['left'].data[t]
right_item = concat_seqs['right'].data[t]
gripper_left_item = concat_seqs['gripper_left'].data[t]
gripper_right_item = concat_seqs['gripper_right'].data[t]
if cfg['mode'] == 'poses':
y.left.goto_pose(left_item, relative=True, wait_for_res=False)
y.right.goto_pose(right_item, relative=True, wait_for_res=True)
else:
y.left.goto_state(left_item, wait_for_res=False)
y.right.goto_state(right_item, wait_for_res=True)
if gripper_left_item != None and gripper_right_item != None:
getattr(y.left, gripper_left_item)(wait_for_res=False)
getattr(y.right, gripper_right_item)(wait_for_res=True)
elif gripper_left_item != None:
getattr(y.left, gripper_left_item)()
elif gripper_right_item != None:
getattr(y.right, gripper_right_item)()
z = gripper_zoning[t]
if z is not None:
logging.info("Setting zone to {0}".format(z))
y.set_z(z)
if cfg['record_torque']['use']:
syncer.pause()
torque_model = CSVModel.get_or_create(
os.path.join(cfg['data_path'], 'playback_torques_record.csv'),
[('supervisor', 'str'), ('demo_name', 'str'), ('trial_num', 'int'),
('playback_num', 'int'), ('playback_path', 'str')])
last_torque_record = torque_model.get_by_cols(
{
'demo_name': demo_name,
'trial_num': trial_num,
'supervisor': supervisor
},
direction=-1)
if last_torque_record == None:
playback_num = 1
else:
playback_num = last_torque_record['playback_num'] + 1
playback_path = os.path.join(trial_path, 'playback_torques',
str(playback_num))
if not os.path.exists(playback_path):
os.makedirs(playback_path)
data_torque_left.save_data(playback_path)
data_torque_right.save_data(playback_path)
torque_model.insert({
'supervisor': supervisor,
'demo_name': demo_name,
'trial_num': trial_num,
'playback_num': playback_num,
'playback_path': playback_path
})
basename = os.path.basename(args.config_path)
target_file_path = os.path.join(playback_path, basename)
shutil.copyfile(args.config_path, target_file_path)
ysub.stop()
syncer.stop()
# perform takedown motions
logging.info("Taking down..")
y.set_v(cfg['v'])
y.set_z(cfg['z'])
demo_obj.takedown()
y.reset_home()
y.open_grippers()
y.stop()
def run(arm, n, v, zone, output_path):
y = YuMiRobot(log_state_histories=True)
y.set_v(v)
y.set_z(zone)
y_arm = getattr(y, arm)
#collect data
t, m, d = move_times(y_arm.goto_state, targets, n)
#save data
np.savez(os.path.join(output_path, 'total_times'), t)
np.savez(os.path.join(output_path, 'motion_times'), m)
np.savez(os.path.join(output_path, 'diff_times'), d)
x_range = np.arange(len(t))
fig = plt.figure(figsize=(12, 8))
ax = fig.gca()
ax.plot(x_range, t, 'r-', label='Total times')
ax.plot(x_range, m, 'g-', label='Motion Times')
ax.plot(x_range, d, 'b-', label='Latency Times')
legend = ax.legend(loc='best', shadow=False)
frame = legend.get_frame()
for label in legend.get_texts():
label.set_fontsize('large')
for label in legend.get_lines():
label.set_linewidth(1.5)
ax.set_title("YuMi Command Times", fontsize=20)
ax.set_xlabel("Command", fontsize=14)
ax.set_ylabel("Seconds", fontsize=14)
fig.savefig(os.path.join(output_path, 'yumi_comm_times.pdf'), format='pdf')
#histograms for all 3 times
for data, name in ((t, 'total_times'), (m, 'motion_times'), (d,
'latencies')):
fig = plt.figure(figsize=(12, 8))
ax = fig.gca()
mean = np.mean(data)
std = np.std(data)
stats_str = 'mean: {:.3g}\nstd: {:.3g}'.format(mean, std)
props = dict(facecolor='white', alpha=0.5)
ax.set_title(
'Histogram of 2-way YuMi Communication Times: {0}'.format(name),
fontsize=20)
ax.set_xlabel('Seconds', fontsize=18)
ax.set_ylabel('Normalized Count', fontsize=18)
# place a text box in upper left in axes coords
ax.text(0.05,
0.95,
stats_str,
transform=ax.transAxes,
fontsize=14,
verticalalignment='top',
bbox=props)
h = plt.hist(data, normed=True, bins=30)
fig.savefig(os.path.join(output_path,
'hist_yumi_2_way_{0}.pdf'.format(name)),
format='pdf')
"""
Helper script to move YuMi back to home pose
Author: Jeff Mahler
"""
from autolab_core import RigidTransform
import numpy as np
from yumipy import YuMiRobot, YuMiState
from yumipy import YuMiConstants as YMC
if __name__ == '__main__':
y = YuMiRobot()
y.set_z('z50')
y.set_v(1000)
y.left.close_gripper()
robot = y
arm = y.left
arm.goto_pose(YMC.L_PREGRASP_POSE, wait_for_res=True)
# shake test
radius = 0.2
angle = np.pi / 64
delta_T = RigidTransform(translation=[0,0,radius], from_frame='gripper', to_frame='gripper')
R_shake = np.array([[1, 0, 0],
[0, np.cos(angle), -np.sin(angle)],
[0, np.sin(angle), np.cos(angle)]])
delta_T_up = RigidTransform(rotation=R_shake, translation=[0,0,-radius], from_frame='gripper', to_frame='gripper')
delta_T_down = RigidTransform(rotation=R_shake.T, translation=[0,0,-radius], from_frame='gripper', to_frame='gripper')
T_shake_up = YMC.L_PREGRASP_POSE.as_frames('gripper', 'world') * delta_T_up * delta_T
class _YuMiArmPoller(Process):
def __init__(self, pose_q, cmds_q, ret_q, z, v, arm_name):
Process.__init__(self)
self.pose_q = pose_q
self.cmds_q = cmds_q
self.ret_q = ret_q
self.z = z
self.v = v
self.arm_name = arm_name
self.forward_poses = False
self.filter = IdentityFilter()
self.filter_times = []
self.cmd_times = []
def run(self):
self.move_counter = 0
if self.arm_name == "left":
self.y = YuMiRobot(include_right=False)
self.arm = self.y.left
elif self.arm_name == "right":
self.y = YuMiRobot(include_left=False)
self.arm = self.y.right
self.y.set_v(self.v)
self.y.set_z(self.z)
while True:
try:
if not self.cmds_q.empty():
cmd = self.cmds_q.get()
if cmd[0] == 'forward':
self.forward_poses = cmd[1]
if cmd[1]:
self.filter.reset()
self.y.set_v(self.v)
self.y.set_z(self.z)
self.move_counter = 0
elif cmd[0] == 'stop':
break
elif cmd[0] == 'filter':
self.filter = cmd[1]
elif cmd[0] == 'count':
while self.ret_q.qsize() > 0:
self.ret_q.get_nowait()
self.ret_q.put(self.move_counter)
elif cmd[0] == 'method':
args = cmd[3]['args']
kwargs = cmd[3]['kwargs']
method_name = cmd[2]
if cmd[1] == 'both':
method = getattr(self.y, method_name)
elif cmd[1] == 'single':
method = getattr(self.arm, method_name)
retval = method(*args, **kwargs)
while self.ret_q.qsize() > 0:
self.ret_q.get_nowait()
if retval is not None:
self.ret_q.put(retval)
elif self.forward_poses and not self.pose_q.empty():
self.move_counter += 1
try:
# start = time()
pose = self.pose_q.get()
filtered_pose = self.filter.apply(pose)
# self.filter_times.append(time() - start)
# print '{} filter time is {}'.format(self.arm_name, np.mean(self.filter_times))
try:
# start = time()
res = self.arm.goto_pose(filtered_pose, relative=True)
# self.cmd_times.append(time() - start)
# print '{} cmd time is {}'.format(self.arm_name, np.mean(self.cmd_times))
except YuMiControlException:
logging.warn("Pose unreachable!")
except Empty:
pass
sleep(0.001)
except KeyboardInterrupt:
logging.debug("Shutting down {0} arm poller".format(self.arm_name))
self.y.stop()
def stop(self):
self.cmds_q.put(('stop',))
def set_forward(self, val):
self.cmds_q.put(('forward', val))
def send_cmd(self, packet):
self.cmds_q.put(packet)
def set_filter(self, motion_filter):
self.cmds_q.put(('filter', motion_filter))
import numpy as np
import time
from yumipy import YuMiRobot, YuMiState
from autolab_core import RigidTransform
if __name__ == '__main__':
num_attempts = 3
state = YuMiState([51.16, -99.4, 21.57, -107.19, 84.11, 94.61, -36.00])
robot = YuMiRobot(debug=False)
robot.set_v(50)
for i in range(num_attempts):
print 'Trying collision', i
robot.right.goto_state(state)
robot.right.goto_pose_delta([0, 0.236, 0])
robot.right.goto_pose_delta([0.053, 0, 0])
robot.right.goto_pose_delta([0, 0, -0.145])
time.sleep(3)
def playback(args):
cfg = YamlConfig(args.config_path)
demo_name = args.demo_name
supervisor = args.supervisor
trial_num = args.trial_num
if cfg['mode'] not in ('poses', 'states'):
y.stop()
raise ValueError("Unknown playback mode! Only accepts 'poses' or 'joints'. Got {0}".format(cfg['mode']))
# init robot
logging.info("Init robot.")
y = YuMiRobot()
y.set_v(cfg['v'])
y.set_z(cfg['z'])
# load demo data
demo_records = CSVModel.load(os.path.join(cfg['data_path'], 'demo_records.csv'))
demo_record = demo_records.get_by_cols({
'demo_name': demo_name,
'trial_num': trial_num,
'supervisor': supervisor
})
trial_path = demo_record['trial_path']
demo_host_cfg = YamlConfig(os.path.join(trial_path, 'demo_config.yaml'))
# parse demo trajectory
# TODO: enforce fps
fps = demo_host_cfg['fps']
_, left_data = zip(*load(os.path.join(trial_path, '{0}_left.jb'.format(cfg['mode']))))
_, right_data = zip(*load(os.path.join(trial_path, '{0}_right.jb'.format(cfg['mode']))))
_, gripper_left_evs = zip(*load(os.path.join(trial_path, 'grippers_evs_left.jb')))
_, gripper_right_evs = zip(*load(os.path.join(trial_path, 'grippers_evs_right.jb')))
seqs = {
'left': Sequence([t[1] for t in left_data]),
'right': Sequence([t[1] for t in right_data]),
'gripper_left': Sequence(gripper_left_evs),
'gripper_right': Sequence(gripper_right_evs)
}
# subsampling
subsample_factor = cfg['subsample']
subsampled_seqs = {
'left': seqs['left'].subsampler(subsample_factor),
'right': seqs['right'].subsampler(subsample_factor),
'gripper_left': seqs['gripper_left'].subsampler(subsample_factor, retain_features=True),
'gripper_right': seqs['gripper_right'].subsampler(subsample_factor, retain_features=True)
}
# concating non-moving steps
if cfg['concat']:
concat_data = {
'left': [subsampled_seqs['left'].data[0]],
'right': [subsampled_seqs['right'].data[0]],
'gripper_left': [subsampled_seqs['gripper_left'].data[0]],
'gripper_right': [subsampled_seqs['gripper_right'].data[0]]
}
last_lp = concat_data['left'][0]
last_rp = concat_data['right'][0]
for t in range(1, min([len(seq.data) for seq in subsampled_seqs.values()])):
lg_t = subsampled_seqs['gripper_left'].data[t]
rg_t = subsampled_seqs['gripper_right'].data[t]
lp_t = subsampled_seqs['left'].data[t]
rp_t = subsampled_seqs['right'].data[t]
if lg_t is not None or rg_t is not None or \
lp_t != last_lp or rp_t != last_rp:
concat_data['gripper_right'].append(rg_t)
concat_data['gripper_left'].append(lg_t)
concat_data['left'].append(lp_t)
concat_data['right'].append(rp_t)
last_lp = lp_t
last_rp = rp_t
concat_seqs = {
'left': Sequence(concat_data['left']),
'right': Sequence(concat_data['right']),
'gripper_left': Sequence(concat_data['gripper_left']),
'gripper_right': Sequence(concat_data['gripper_right']),
}
else:
concat_seqs = subsampled_seqs
N = min([len(seq.data) for seq in concat_seqs.values()])
# processing time steps where zoning should be set to fine
gripper_zoning = [None for _ in range(N)]
for t in range(N-1):
if concat_seqs['gripper_left'].data[t] != None or \
concat_seqs['gripper_right'].data[t] != None:
if t == 0:
y.set_z('fine')
else:
gripper_zoning[t-1] = 'fine'
gripper_zoning[t+1] = cfg['z']
# perform setup motions
logging.info("Loading demo and performing setups.")
y.reset_home()
y.open_grippers()
demo_path = os.path.join(trial_path, '{0}.py'.format(demo_name))
demo_obj = DemoWrapper.load(demo_path, y)
demo_obj.setup()
# record torque and other debug data if needed
if cfg['record_torque']['use']:
ysub = YuMiSubscriber()
ysub.start()
data_torque_left = DataStreamRecorder('torques_left', ysub.left.get_torque, cache_path=cfg['cache_path'], save_every=cfg['save_every'])
data_torque_right = DataStreamRecorder('torques_right', ysub.right.get_torque, cache_path=cfg['cache_path'], save_every=cfg['save_every'])
syncer = DataStreamSyncer([data_torque_left, data_torque_right], fps)
syncer.start()
sleep(0.5)
syncer.pause()
syncer.flush()
syncer.resume(reset_time=True)
# perform trajectory
logging.info("Playing trajectory")
for t in range(N):
left_item = concat_seqs['left'].data[t]
right_item = concat_seqs['right'].data[t]
gripper_left_item = concat_seqs['gripper_left'].data[t]
gripper_right_item = concat_seqs['gripper_right'].data[t]
if cfg['mode'] == 'poses':
y.left.goto_pose(left_item, relative=True, wait_for_res=False)
y.right.goto_pose(right_item, relative=True, wait_for_res=True)
else:
y.left.goto_state(left_item, wait_for_res=False)
y.right.goto_state(right_item, wait_for_res=True)
if gripper_left_item != None and gripper_right_item != None:
getattr(y.left, gripper_left_item)(wait_for_res=False)
getattr(y.right, gripper_right_item)(wait_for_res=True)
elif gripper_left_item != None:
getattr(y.left, gripper_left_item)()
elif gripper_right_item != None:
getattr(y.right, gripper_right_item)()
z = gripper_zoning[t]
if z is not None:
logging.info("Setting zone to {0}".format(z))
y.set_z(z)
if cfg['record_torque']['use']:
syncer.pause()
torque_model = CSVModel.get_or_create(
os.path.join(cfg['data_path'], 'playback_torques_record.csv'),
[
('supervisor', 'str'),
('demo_name', 'str'),
('trial_num', 'int'),
('playback_num', 'int'),
('playback_path', 'str')
]
)
last_torque_record = torque_model.get_by_cols({
'demo_name': demo_name,
'trial_num': trial_num,
'supervisor': supervisor
}, direction=-1)
if last_torque_record == None:
playback_num = 1
else:
playback_num = last_torque_record['playback_num'] + 1
playback_path = os.path.join(trial_path, 'playback_torques', str(playback_num))
if not os.path.exists(playback_path):
os.makedirs(playback_path)
data_torque_left.save_data(playback_path)
data_torque_right.save_data(playback_path)
torque_model.insert({
'supervisor': supervisor,
'demo_name': demo_name,
'trial_num': trial_num,
'playback_num': playback_num,
'playback_path': playback_path
})
basename = os.path.basename(args.config_path)
target_file_path = os.path.join(playback_path, basename)
shutil.copyfile(args.config_path, target_file_path)
ysub.stop()
syncer.stop()
# perform takedown motions
logging.info("Taking down..")
y.set_v(cfg['v'])
y.set_z(cfg['z'])
demo_obj.takedown()
y.reset_home()
y.open_grippers()
y.stop()
rotation=right_front_rotation))
# example of rigid transform: RigidTransform(translation = [x_pos(back to front), y_pos(right to left), z_pos], rotation=[w, x, y, z])
# To use rpy rotation, function rpy_to_wxyz(r, p, y) is implemented above
left_top_rotation = rpy_to_wxyz(-90, 0, 180)
left_front_rotation = rpy_to_wxyz(-90, 0, -90)
left_side_rotation = rpy_to_wxyz(0, 0, -90)
right_top_rotation = rpy_to_wxyz(90, 0, 180)
right_side_rotation = rpy_to_wxyz(0, 0, 90)
right_front_rotation = rpy_to_wxyz(90, 0, 90)
right_top_give_rotation = rpy_to_wxyz(90, 0, 210)
right_side_give_rotation = rpy_to_wxyz(0, 30, 90)
right_front_give_rotation = rpy_to_wxyz(90, 0, 120)
if __name__ == '__main__':
y = YuMiRobot()
y.set_z('z50')
y.set_v(450)
print('robot_action.py')
y.open_grippers()
y.reset_home()
plate_catch(y)
plate_drop(y)
y.reset_home()
y.open_grippers()
y.stop()
while (count < train_num):
catching_speed = random.randint(150, 1500)
giving_speed = random.randint(150, 1500)
catching_dir = random.randint(1, 3)
pos = random.randint(1, 3)
job = random.randint(1, 2)
if (job == 1 and pos == 3):
continue
count += 1
print("Train count : %d" % count)
if (job == 1):
y.set_v(catching_speed)
write_log.write_log(1, 0, catching_speed, catching_dir, pos,
time_diff)
robot_action.bottle_catch(y, catching_dir, pos)
else:
y.set_v(catching_speed)
write_log.write_log(1, 0, catching_speed, catching_dir, pos,
time_diff)
robot_action.bottle_catch(y, catching_dir, pos)
y.set_v(giving_speed)
write_log.write_log(2, 0, giving_speed, catching_dir, pos,
time_diff)
robot_action.bottle_give(y, catching_dir, pos)
time.sleep(1)
os.system('play -nq -t alsa synth {} sine {}'.format(1, 440))
"""
Helper script to move YuMi back to home pose
Author: Jeff Mahler
"""
from yumipy import YuMiRobot, YuMiState
from yumipy import YuMiConstants as YMC
if __name__ == '__main__':
y = YuMiRobot()
y.set_z('fine')
y.set_v(200)
y.left.close_gripper()
y.right.close_gripper()
state = YuMiState([69.42, -75.37, 20.21, -110.77, 91.29, -62.35, -43.74])
#y.left.goto_pose(YMC.L_HOME_POSE)
y.right.goto_state(state)
#y.stop()
#exit(0)
for i, tup in enumerate(YMC.BOX_CLOSE_SEQUENCE):
print i
arm, box_state = tup
if arm == 'L':
if isinstance(box_state, YuMiState):
y.left.goto_state(box_state)
elif isinstance(box_state, list):
y.left.goto_pose_delta(box_state)
else:
y.left.goto_pose(box_state)
