class Yumi(Baserobot):
def __init__(self):
self.robot = YuMiRobot()
def get_robot(self):
return self.robot
def get_hand(self, hand):
assert hand == 'right' or 'left'
if hand == 'right':
return self.robot.right
else:
return self.robot.left
def set_speed(self, hand, speed=100):
assert 0 <= speed <= 500, "Speed is out of 0 ~ 500 range"
assert hand == 'left' or 'right' or 'all', "Must define a hand"
if hand == 'all':
self.get_hand('right').set_speed(self.robot.get_v(speed))
self.get_hand('left').set_speed(self.robot.get_v(speed))
else:
self.get_hand(hand).set_speed(self.robot.get_v(speed))
def get_pose(self, hand):
"""
:param hand: 'right' (right_arm) or 'left' (left_arm)
:return: end-effector pose in 4 by 4 matrix
"""
return self.get_hand(hand).get_pose(raw_res=False)
def get_state(self, hand):
return self.get_hand(hand).get_state(raw_res=False)
def go_center(self, hand):
return self.get_hand(hand).goto_pose(yc.board_center, relative=True)
def go_pickup_home(self, hand):
if hand == 'right':
self.get_hand(hand).goto_state(yc.right_pickup_home)
elif hand == 'left':
self.get_hand(hand).goto_state(yc.left_pickup_home)
# return self.get_hand(hand).goto_pose(T_rightH_yumi.as_frames('yumi', 'world'), relative=True)
def go_threading_home(self, hand):
if hand == 'right':
self.get_hand('right').goto_state(yc.right_threading_home)
self.move_delta('right', trans=[-0.08, 0.006, 0], rotation=None)
elif hand == 'left':
self.get_hand('left').goto_state(yc.left_threading_home)
self.move_delta('left',
trans=[-0.035, -0.04, 0.004],
rotation=None)
def move_delta(self, hand, trans, rotation=None):
self.get_hand(hand).goto_pose_delta(trans, rotation)
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
def main():
global home_left
global tool_cesar_cal
rospy.init_node('tcp_tf', anonymous=True)
br = tf2_ros.TransformBroadcaster()
y = YuMiRobot(arm_type='remote')
listener = tf.TransformListener()
y.left.set_tool(tool_cesar_cal)
rate = rospy.Rate(10)
move(y)
y.left.open_gripper(no_wait=False, wait_for_res=True)
pose_state = y.left.get_pose(raw_res=False)
y.left.close_gripper(no_wait=False, wait_for_res=True)
state_robot = False
temp_flag = False
previous_trans = [0.0, 0.0, 0.0]
while (not rospy.is_shutdown()):
print("flag:{}".format(temp_flag))
try:
(trans, rot) = listener.lookupTransform('/world', '/pose_object',
rospy.Time(0))
if not trans == None and not rot == None and not previous_trans == trans:
pose_state.translation = trans
temp_flag = y.left.is_pose_reachable(pose_state)
move(y)
previous_trans = trans
print("It is feasible to reach the given pose {}".format(
temp_flag))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
if temp_flag:
state_robot = y.left.goto_pose(pose_state,
linear=True,
relative=False,
wait_for_res=True)
y.left.open_gripper(no_wait=False, wait_for_res=True)
pose_state.translation[2] = 0.065
state_robot = y.left.goto_pose(pose_state,
linear=True,
relative=False,
wait_for_res=True)
y.left.close_gripper(force=20, no_wait=False, wait_for_res=True)
move(y)
y.left.goto_state(YuMiState(home_industrial_obj),
wait_for_res=False)
y.left.open_gripper(no_wait=False, wait_for_res=True)
temp_flag = False
print('moving!!!')
rate.sleep()
def __init__(self):
self.robot = YuMiRobot(arm_type='remote')
self.left_arm = self.robot.left
self.left_arm.calibrate_gripper()
self.right_arm = self.robot.right
self.right_arm.calibrate_gripper()
self._create_services()
rospy.loginfo("[ServiceBridge] Node is up!")
self.current_tool_l = 'gripper'
self.current_tool_r = 'gripper'
self.tools = {
'gripper': [0, 0, 136, 0, 0, 0, 1],
'suction': [63.5, 18.5, 37.5, 0, 0, 0, 1],
'calibration': [0, 0, 0, 0, 0, 0, 1]
}
self.arms_wait_joint = {'left': [-1.53013010964, -2.31535371672, 0.57962382732,
1.8151423679999998, 1.8772760875199999, 0.037873643639999996, 0.36808992828],
'right': [1.5475834016399999, -2.3253020931599995, 0.58520888076,
4.45861797432, 1.8971728404, -0.04206243372, -0.34033919399999996]}
def main():
global home_left
rospy.init_node('tcp_tf', anonymous=True)
br = tf2_ros.TransformBroadcaster()
# starting the robot interface
y = YuMiRobot(arm_type='remote')
global tool_cesar_cal
y.left.set_tool(tool_cesar_cal)
rate = rospy.Rate(10)
# state = y.left.get_state(raw_res=False)
# print(state)
#home_left=YuMiState(home_left)
#y.left.goto_state(home_left,wait_for_res=False)
# pose = y.left.get_pose(raw_res=False)
# print(pose.translation)
# print(pose.rotation)
# time.sleep(3)
# state1=[-134.07, -50.55, -4.95, -64.58, -58.61, 95.96, 134.53]
# state1=YuMiState(state1)
# y.left.goto_state(state1,wait_for_res=False)
#exit(0)
while (True):
pose = y.left.get_pose(raw_res=False)
print('translation {}'.format(pose.translation))
print('quaternion {}'.format(pose.quaternion))
print('rotation matrix \n{}'.format(pose.rotation))
print(pose)
print('moving!!!')
pose.translation += 0.05
y.left.goto_pose_linear_path(pose,
wait_for_res=True,
traj_len=10,
eef_delta=0.01,
jump_thresh=0.0)
#pose.quaternion= [-0.0640144 -0.702158 -0.07501688 0.70515868]
#move(y)
#pose_to_tf(br,pose.translation,pose.quaternion)
# we should expect to go through the loop 10 times per second
rate.sleep()
target_pt_world = cb_points_world[target_ind[0]]
# create robot pose relative to target point
R_gripper_world = np.array([[1.0, 0, 0],
[0, -1.0, 0],
[0, 0, -1.0]])
t_gripper_world = np.array([target_pt_world.x + config['gripper_offset_x'],
target_pt_world.y + config['gripper_offset_y'],
target_pt_world.z + config['gripper_offset_z']])
T_gripper_world = RigidTransform(rotation=R_gripper_world,
translation=t_gripper_world,
from_frame='gripper',
to_frame='cb')
logging.info('Moving robot to point x=%f, y=%f, z=%f' %(t_gripper_world[0], t_gripper_world[1], t_gripper_world[2]))
# move robot to pose
y = YuMiRobot(tcp=YMC.TCP_SUCTION_STIFF)
y.reset_home()
time.sleep(1)
T_lift = RigidTransform(translation=(0,0,0.05), from_frame='cb', to_frame='cb')
T_gripper_world_lift = T_lift * T_gripper_world
y.right.goto_pose(T_gripper_world_lift)
y.right.goto_pose(T_gripper_world)
# wait for human measurement
yesno = raw_input('Take measurement. Hit [ENTER] when done')
y.right.goto_pose(T_gripper_world_lift)
y.reset_home()
y.stop()
choose_object() # 3
choose_catching_direction() # 3
choose_giving_position() # 2
write_log()
catch()
give()
y.reset_home()
y.open_grippers()
if __name__ == '__main__':
######################################################################
global time_diff
y = YuMiRobot()
y.set_z('z50')
# 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)
######################################################################
time_str = "2020-11-10 16:33:00"
timestamp = time.mktime(
datetime.strptime(time_str, "%Y-%m-%d %H:%M:%S").timetuple())
def register_ensenso(config):
# set parameters
average = True
add_to_buffer = True
clear_buffer = False
decode = True
grid_spacing = -1
# read parameters
num_patterns = config["num_patterns"]
max_tries = config["max_tries"]
# load tf pattern to world
T_pattern_world = RigidTransform.load(config["pattern_tf"])
# initialize sensor
sensor_frame = config["sensor_frame"]
sensor_config = {"frame": sensor_frame}
logging.info("Creating sensor")
sensor = RgbdSensorFactory.sensor("ensenso", sensor_config)
# initialize node
rospy.init_node("register_ensenso", anonymous=True)
rospy.wait_for_service("/%s/collect_pattern" % (sensor_frame))
rospy.wait_for_service("/%s/estimate_pattern_pose" % (sensor_frame))
# start sensor
print("Starting sensor")
sensor.start()
time.sleep(1)
print("Sensor initialized")
# perform registration
try:
print("Collecting patterns")
num_detected = 0
i = 0
while num_detected < num_patterns and i < max_tries:
collect_pattern = rospy.ServiceProxy(
"/%s/collect_pattern" % (sensor_frame), CollectPattern)
resp = collect_pattern(add_to_buffer, clear_buffer, decode,
grid_spacing)
if resp.success:
print("Detected pattern %d" % (num_detected))
num_detected += 1
i += 1
if i == max_tries:
raise ValueError("Failed to detect calibration pattern!")
print("Estimating pattern pose")
estimate_pattern_pose = rospy.ServiceProxy(
"/%s/estimate_pattern_pose" % (sensor_frame), EstimatePatternPose)
resp = estimate_pattern_pose(average)
q_pattern_camera = np.array([
resp.pose.orientation.w,
resp.pose.orientation.x,
resp.pose.orientation.y,
resp.pose.orientation.z,
])
t_pattern_camera = np.array(
[resp.pose.position.x, resp.pose.position.y, resp.pose.position.z])
T_pattern_camera = RigidTransform(
rotation=q_pattern_camera,
translation=t_pattern_camera,
from_frame="pattern",
to_frame=sensor_frame,
)
except rospy.ServiceException as e:
print("Service call failed: %s" % (str(e)))
# compute final transformation
T_camera_world = T_pattern_world * T_pattern_camera.inverse()
# save final transformation
output_dir = os.path.join(config["calib_dir"], sensor_frame)
if not os.path.exists(output_dir):
os.makedirs(output_dir)
pose_filename = os.path.join(output_dir, "%s_to_world.tf" % (sensor_frame))
T_camera_world.save(pose_filename)
intr_filename = os.path.join(output_dir, "%s.intr" % (sensor_frame))
sensor.ir_intrinsics.save(intr_filename)
# log final transformation
print("Final Result for sensor %s" % (sensor_frame))
print("Rotation: ")
print(T_camera_world.rotation)
print("Quaternion: ")
print(T_camera_world.quaternion)
print("Translation: ")
print(T_camera_world.translation)
# stop sensor
sensor.stop()
# move robot to calib pattern center
if config["use_robot"]:
# create robot pose relative to target point
target_pt_camera = Point(T_pattern_camera.translation, sensor_frame)
target_pt_world = T_camera_world * target_pt_camera
R_gripper_world = np.array([[1.0, 0, 0], [0, -1.0, 0], [0, 0, -1.0]])
t_gripper_world = np.array([
target_pt_world.x + config["gripper_offset_x"],
target_pt_world.y + config["gripper_offset_y"],
target_pt_world.z + config["gripper_offset_z"],
])
T_gripper_world = RigidTransform(
rotation=R_gripper_world,
translation=t_gripper_world,
from_frame="gripper",
to_frame="world",
)
# move robot to pose
y = YuMiRobot(tcp=YMC.TCP_SUCTION_STIFF)
y.reset_home()
time.sleep(1)
T_lift = RigidTransform(translation=(0, 0, 0.05),
from_frame="world",
to_frame="world")
T_gripper_world_lift = T_lift * T_gripper_world
y.right.goto_pose(T_gripper_world_lift)
y.right.goto_pose(T_gripper_world)
# wait for human measurement
keyboard_input("Take measurement. Hit [ENTER] when done")
y.right.goto_pose(T_gripper_world_lift)
y.reset_home()
y.stop()
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')
def main():
global home_left
global tool_cesar_cal
rospy.init_node('tcp_tf', anonymous=True)
br = tf2_ros.TransformBroadcaster()
y = YuMiRobot(arm_type='remote')
listener = tf.TransformListener()
y.left.set_tool(tool_cesar_cal)
rate = rospy.Rate(10)
move(y)
y.left.open_gripper(no_wait=False, wait_for_res=True)
pose_state = y.left.get_pose(raw_res=False)
y.left.close_gripper(no_wait=False, wait_for_res=True)
state_robot=False
temp_flag=False
previous_trans=[0.0 , 0.0, 0.0]
#trick
tmp=pose_state.quaternion
#print('tmp',tmp)
orientation_list = [tmp[1],tmp[2],tmp[3],tmp[0]]
(roll_default, pitch_default, yaw_default)=tf.transformations.euler_from_quaternion(orientation_list)
#print("euler_angles:{}".format([math.degrees(roll_default),math.degrees(pitch_default),math.degrees(yaw_default)]))
while (not rospy.is_shutdown()):
print("flag:{}".format(temp_flag))
try:
(trans,rot) = listener.lookupTransform('/world','/pose_object', rospy.Time(0))
from tf.transformations import euler_from_quaternion, quaternion_from_euler
# ########### TEST#####################
# print('pose_state.quaternion:{}'.format(pose_state.quaternion))
# print('pose_state.rotation:{}'.format(pose_state.rotation))
# tmp=pose_state.quaternion
# print('tmp',tmp)
# orientation_list = [tmp[1],tmp[2],tmp[3],tmp[0]]
# (roll_default, pitch_default, yaw_default)=tf.transformations.euler_from_quaternion(orientation_list)
# print("euler_angles:{}".format([math.degrees(roll_default),math.degrees(pitch_default),math.degrees(yaw_default)]))
roll_, pitch_, yaw_=tf.transformations.euler_from_quaternion(rot)
#print('cesar:{}{}{}'.format(roll_, pitch_, yaw_))
# euler_angles=[math.degrees(roll_),math.degrees(pitch_),math.degrees(yaw_)]
# print('euler_angles:{}'.format(euler_angles))
print('---New computation about quaternions---')
q_orig = tf.transformations.quaternion_from_euler(roll_default,pitch_default,yaw_default)
q_rot = quaternion_from_euler(0, 0, yaw_)
q_new = quaternion_multiply(q_rot, q_orig)
q_new/=np.linalg.norm(q_new)
r_new = quaternion_matrix(q_new)
# print('r_new:',r_new)
# # print(type(r_new))
# # print(r_new[:-1,:-1])
# # pose_state.quaternion[0]=q_new[3]
# # pose_state.quaternion[1]=q_new[0]
# # pose_state.quaternion[2]=q_new[1]
# # pose_state.quaternion[3]=q_new[2]
# pose_state.translation=trans
# pose_state.rotation=r_new[:-1,:-1]
# state_robot=y.left.goto_pose(pose_state, linear=True, relative=False, wait_for_res=True)
# print('state_robot:{}'.format(state_robot))
# exit(0)
################END TEST########################
if not trans==None and not rot==None and not previous_trans==trans:
pose_state.translation=trans
pose_state.rotation=r_new[:-1,:-1]
temp_flag=y.left.is_pose_reachable(pose_state)
move(y)
previous_trans=trans
print("It is feasible to reach the given pose {}".format(temp_flag))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
if temp_flag:
state_robot=y.left.goto_pose(pose_state, linear=True, relative=False, wait_for_res=True)
y.left.open_gripper(no_wait=False, wait_for_res=True)
pose_state.translation[2]=0.065
state_robot=y.left.goto_pose(pose_state, linear=True, relative=False, wait_for_res=True)
y.left.close_gripper(force=20,no_wait=False, wait_for_res=True)
move(y)
y.left.goto_state(YuMiState(home_industrial_obj),wait_for_res=False)
y.left.open_gripper(no_wait=False, wait_for_res=True)
temp_flag=False
print('moving!!!')
rate.sleep()
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()
def __init__(self):
self.robot = YuMiRobot()
from pathlib import Path
import random
import write_log
import robot_action
import speed_optimization
if __name__ == '__main__':
######################################################################
global time_diff
timeServer = 'time.windows.com'
c = ntplib.NTPClient()
response = c.request(timeServer, version=3)
time_diff = response.offset
######################################################################
y = YuMiRobot()
y.set_z('z50')
####################################################
train_num = input("Enter number for traning : ")
f = open(write_log.file_name, 'r')
lines = f.readlines()
f.close()
count = 0
for line in lines:
if 'catching' in line:
count += 1
while (count < train_num):
class ServiceBridge:
def __init__(self):
self.robot = YuMiRobot(arm_type='remote')
self.left_arm = self.robot.left
self.left_arm.calibrate_gripper()
self.right_arm = self.robot.right
self.right_arm.calibrate_gripper()
self._create_services()
rospy.loginfo("[ServiceBridge] Node is up!")
self.current_tool_l = 'gripper'
self.current_tool_r = 'gripper'
self.tools = {
'gripper': [0, 0, 136, 0, 0, 0, 1],
'suction': [63.5, 18.5, 37.5, 0, 0, 0, 1],
'calibration': [0, 0, 0, 0, 0, 0, 1]
}
self.arms_wait_joint = {'left': [-1.53013010964, -2.31535371672, 0.57962382732,
1.8151423679999998, 1.8772760875199999, 0.037873643639999996, 0.36808992828],
'right': [1.5475834016399999, -2.3253020931599995, 0.58520888076,
4.45861797432, 1.8971728404, -0.04206243372, -0.34033919399999996]}
def _create_services(self):
rospy.Service("~/goto_pose", GotoPose, self.goto_pose_cb)
rospy.Service("~/goto_pose_plan", GotoPose, self.goto_pose_plan_cb)
rospy.Service("~/goto_pose_sync", GotoPoseSync, self.goto_pose_sync_cb)
rospy.Service("~/goto_joints", GotoJoint, self.goto_joints_cb)
rospy.Service("~/close_gripper", YumipyTrigger, self.close_gripper_cb)
rospy.Service("~/open_gripper", YumipyTrigger, self.open_gripper_cb)
rospy.Service("~/move_gripper", MoveGripper, self.move_gripper_cb)
rospy.Service("~/get_gripper_width", YumipyTrigger, self.get_gripper_width)
#rospy.Service("/set_speed", , self.set_speed_cb)
rospy.Service("~/set_tool", SetTool, self.set_tool_cb)
rospy.Service("~/set_zone", SetZ, self.set_zone_cb)
rospy.Service("~/reset_home", YumipyTrigger, self.reset_home_cb)
rospy.Service("~/calibrate_gripper", YumipyTrigger, self.calibrate_gripper_cb)
rospy.Service("~/turn_on_suction", YumipyTrigger, self.turn_on_suction_cb)
rospy.Service("~/turn_off_suction", YumipyTrigger, self.turn_off_suction_cb)
rospy.Service("~/goto_wait_pose", Trigger, self.goto_wait_pose_cb)
rospy.Service("~/goto_wait_joint", YumipyTrigger, self.goto_wait_joint_cb)
def goto_pose_cb(self, req):
response = GotoPoseResponse()
response.success = False
if (req.arm != 'left' and req.arm != 'right') or len(req.position) != 3 or len(req.quat) != 4:
return response
pose = ''
for index, p in enumerate(req.position):
pose += str(p) + ' '
for q in req.quat:
pose += str(q) + ' '
pose = message_to_pose(pose, 'tool')
if req.arm == 'left':
ret = self.left_arm.goto_pose(pose, wait_for_res=req.wait_for_res)
else:
ret = self.right_arm.goto_pose(pose, wait_for_res=req.wait_for_res)
if ret is not None:
response.success = True
return response
def goto_pose_plan_cb(self, req):
response = GotoPoseResponse()
response.success = False
if (req.arm != 'left' and req.arm != 'right') or len(req.position) != 3 or len(req.quat) != 4:
return response
pose = ''
for index, p in enumerate(req.position):
pose += str(p) + ' '
for q in req.quat:
pose += str(q) + ' '
pose = message_to_pose(pose, 'tool')
if req.arm == 'left':
ret = self.left_arm.goto_pose_shortest_path(pose, wait_for_res=req.wait_for_res, tool=self.current_tool_l)
else:
ret = self.right_arm.goto_pose_shortest_path(pose, wait_for_res=req.wait_for_res, tool=self.current_tool_r)
if ret is not None:
response.success = True
return response
def goto_pose_sync_cb(self, req):
response = GotoPoseResponse()
response.success = False
if len(req.position_left) != 3 or len(req.quat_left) != 4 or len(req.position_right) != 3 \
or len(req.quat_right) != 4:
rospy.logerr('one of the position or quat info is incorrect')
return response
pose_left = ''
for p in req.position_left:
pose_left += str(p) + ' '
for q in req.quat_left:
pose_left += str(q) + ' '
pose_left = message_to_pose(pose_left, 'tool')
pose_right = ''
for p in req.position_right:
pose_right += str(p) + ' '
for q in req.quat_right:
pose_right += str(q) + ' '
pose_right = message_to_pose(pose_right, 'tool')
ret = self.robot.goto_pose_sync(pose_left, pose_right)
response.success = True
return response
def goto_joints_cb(self, req):
response = GotoJointResponse()
response.success = False
if (req.arm != 'left' and req.arm != 'right') or len(req.joint) != 7:
return response
joints = ''
for j in req.joint:
joints += str(j) + ' '
state = message_to_state(joints)
if req.arm == 'left':
self.left_arm.goto_state(state)
else:
self.right_arm.goto_state(state)
response.success = True
return response
def close_gripper_cb(self, req):
response = YumipyTriggerResponse()
response.success = True
if req.arm == 'left':
self.left_arm.close_gripper()
elif req.arm == 'right':
self.right_arm.close_gripper()
else:
rospy.logerr("[Close Gripper] No arm named {}".format(req.arm))
response.success = False
response.message = "No arm named {}".format(req.arm)
return response
response.message = 'Successfully close {} gripper'.format(req.arm)
return response
def open_gripper_cb(self, req):
response = YumipyTriggerResponse()
response.success = True
if req.arm == 'left':
self.left_arm.open_gripper()
elif req.arm == 'right':
self.right_arm.open_gripper()
else:
rospy.logerr("[Open Gripper] No arm named {}".format(req.arm))
response.success = False
response.message = "No arm named {}".format(req.arm)
return response
response.message = 'Successfully open {} gripper'.format(req.arm)
return response
def move_gripper_cb(self, req):
response = MoveGripperResponse()
response.success = False
arm = None
if req.arm == 'left':
arm = self.left_arm
elif req.arm == 'right':
arm = self.right_arm
if arm is not None:
if req.width <= 0:
arm.close_gripper()
elif req.width >= 0.025:
arm.open_gripper()
else:
arm.move_gripper(req.width)
response.success = True
return response
# def set_speed_cb(self, req):
# # TODO write
#
def set_zone_cb(self, req):
zone_value = req.z
self.robot.set_z(zone_value)
return SetZResponse()
def set_tool_cb(self, req):
if req.arm == 'left':
arm = self.left_arm
self.current_tool_l = req.tool
if req.arm == 'right':
arm = self.right_arm
self.current_tool_r = req.tool
elif req.arm != 'right' and req.arm != 'left':
rospy.logerr("[Set Tool] No arm named {}".format(req.arm))
tcp = self.tools[req.tool]
pose = ''
for p in tcp:
pose += str(p) + ' '
pose_msg = message_to_pose(pose, 'tool')
arm.set_tool(pose=pose_msg)
return SetToolResponse()
def get_gripper_width(self, req):
response = YumipyTriggerResponse()
response.success = True
if req.arm == 'left':
width = self.left_arm.get_gripper_width()
elif req.arm == 'right':
width = self.right_arm.get_gripper_width()
else:
rospy.logerr("[Get Gripper Width] No arm named {}".format(req.arm))
response.success = False
response.message = "No arm named {}".format(req.arm)
return response
response.message = str(width)
return response
def reset_home_cb(self, req):
response = YumipyTriggerResponse()
response.success = True
if req.arm == 'left':
pose = '364.3 291.6 123.39 0.06213 0.86746 -0.10842 0.48156'
pose = message_to_pose(pose, 'tool')
ret = self.left_arm.goto_pose_shortest_path(pose, wait_for_res=True, tool='gripper')
self.left_arm.reset_home()
elif req.arm == 'right':
pose = '381 -314.7 136.97 0.05760 -0.84288 -0.11252 -0.52304'
pose = message_to_pose(pose, 'tool')
ret = self.right_arm.goto_pose_shortest_path(pose, wait_for_res=True, tool='gripper')
self.right_arm.reset_home()
elif req.arm == 'all':
# pose = '364.3 291.6 123.39 0.06213 0.86746 -0.10842 0.48156'
# pose = message_to_pose(pose, 'tool')
ret = self.left_arm.goto_pose_shortest_path(
message_to_pose('364.3 291.6 123.39 0.06213 0.86746 -0.10842 0.48156', 'tool'),
wait_for_res=True, tool='gripper')
self.left_arm.reset_home()
# pose = '381 -314.7 136.97 0.05760 -0.84288 -0.11252 -0.52304'
# pose = message_to_pose(pose, 'tool')
ret = self.right_arm.goto_pose_shortest_path(
message_to_pose('381 -314.7 136.97 0.05760 -0.84288 -0.11252 -0.52304', 'tool'),
wait_for_res=True, tool='gripper')
self.right_arm.reset_home()
else:
rospy.logerr("[Reset Home] No arm named {}".format(req.arm))
response.success = False
response.message = "No arm named {}".format(req.arm)
return response
response.message = 'Successfully reset {} arm to home'.format(req.arm)
return response
def calibrate_gripper_cb(self, req):
response = YumipyTriggerResponse()
response.success = True
if req.arm == 'left':
self.left_arm.calibrate_gripper()
elif req.arm == 'right':
self.right_arm.calibrate_gripper()
else:
rospy.logerr("[Reset Home] No arm named {}".format(req.arm))
response.success = False
response.message = "No arm named {}".format(req.arm)
return response
response.message = 'Successfully calibrate {} gripper'.format(req.arm)
return response
def turn_on_suction_cb(self, req):
response = YumipyTriggerResponse()
response.success = True
if req.arm == 'left':
self.left_arm.turn_on_suction()
elif req.arm == 'right':
self.right_arm.turn_on_suction()
else:
rospy.logerr("[Turn On Suction] No arm named {}".format(req.arm))
response.success = False
response.message = "No arm named {}".format(req.arm)
return response
response.message = 'Successfully Turn on {} suction'.format(req.arm)
return response
def turn_off_suction_cb(self, req):
response = YumipyTriggerResponse()
response.success = True
if req.arm == 'left':
self.left_arm.turn_off_suction()
elif req.arm == 'right':
self.right_arm.turn_off_suction()
else:
rospy.logerr("[Turn Off Suction] No arm named {}".format(req.arm))
response.success = False
response.message = "No arm named {}".format(req.arm)
return response
response.message = 'Successfully Turn off {} suction'.format(req.arm)
return response
def goto_wait_pose_cb(self, req):
arms_quat = {'left': [0, 0, -0.9238795, 0.3826834], 'right': [0, 0, 0.9238795, 0.3826834]}
arms_wait_pose = {'left': [356, 200, 100], 'right': [356, -200, 100]}
pose = ''
for p in arms_wait_pose['left']:
pose += str(p) + ' '
for q in arms_quat['left']:
pose += str(q) + ' '
pose = message_to_pose(pose, 'tool')
self.left_arm.goto_pose_shortest_path(pose, tool=self.current_tool_l)
pose = ''
for p in arms_wait_pose['right']:
pose += str(p) + ' '
for q in arms_quat['right']:
pose += str(q) + ' '
pose = message_to_pose(pose, 'tool')
self.right_arm.goto_pose_shortest_path(pose, tool=self.current_tool_r)
return TriggerResponse()
def goto_wait_joint_cb(self, req):
if req.arm == 'left':
left_joint = self.arms_wait_joint['left']
state = ""
for j in left_joint:
state += str(j*radian2degree) + ' '
state = message_to_state(state)
self.left_arm.goto_state(state)
elif req.arm == 'right':
right_joint = self.arms_wait_joint['right']
state = ""
for j in right_joint:
state += str(j * radian2degree) + ' '
state = message_to_state(state)
self.right_arm.goto_state(state)
return YumipyTriggerResponse()
def setup_yumi_ethernet():
YuMiServerTest.yumi = YuMiRobot(include_right=False,
log_state_histories=True,
log_pose_histories=True)
def register_ensenso(config):
# set parameters
average = True
add_to_buffer = True
clear_buffer = False
decode = True
grid_spacing = -1
# read parameters
num_patterns = config['num_patterns']
max_tries = config['max_tries']
# load tf pattern to world
T_pattern_world = RigidTransform.load(config['pattern_tf'])
# initialize sensor
sensor_frame = config['sensor_frame']
sensor = EnsensoSensor(sensor_frame)
# initialize node
rospy.init_node('register_ensenso', anonymous=True)
rospy.wait_for_service('/%s/collect_pattern' %(sensor_frame))
rospy.wait_for_service('/%s/estimate_pattern_pose' %(sensor_frame))
# start sensor
print('Starting sensor')
sensor.start()
time.sleep(1)
print('Sensor initialized')
# perform registration
try:
print('Collecting patterns')
num_detected = 0
i = 0
while num_detected < num_patterns and i < max_tries:
collect_pattern = rospy.ServiceProxy('/%s/collect_pattern' %(sensor_frame), CollectPattern)
resp = collect_pattern(add_to_buffer,
clear_buffer,
decode,
grid_spacing)
if resp.success:
print('Detected pattern %d' %(num_detected))
num_detected += 1
i += 1
if i == max_tries:
raise ValueError('Failed to detect calibration pattern!')
print('Estimating pattern pose')
estimate_pattern_pose = rospy.ServiceProxy('/%s/estimate_pattern_pose' %(sensor_frame), EstimatePatternPose)
resp = estimate_pattern_pose(average)
q_pattern_camera = np.array([resp.pose.orientation.w,
resp.pose.orientation.x,
resp.pose.orientation.y,
resp.pose.orientation.z])
t_pattern_camera = np.array([resp.pose.position.x,
resp.pose.position.y,
resp.pose.position.z])
T_pattern_camera = RigidTransform(rotation=q_pattern_camera,
translation=t_pattern_camera,
from_frame='pattern',
to_frame=sensor_frame)
except rospy.ServiceException as e:
print('Service call failed: %s' %(str(e)))
# compute final transformation
T_camera_world = T_pattern_world * T_pattern_camera.inverse()
# save final transformation
output_dir = os.path.join(config['calib_dir'], sensor_frame)
if not os.path.exists(output_dir):
os.makedirs(output_dir)
pose_filename = os.path.join(output_dir, '%s_to_world.tf' %(sensor_frame))
T_camera_world.save(pose_filename)
intr_filename = os.path.join(output_dir, '%s.intr' %(sensor_frame))
sensor.ir_intrinsics.save(intr_filename)
# log final transformation
print('Final Result for sensor %s' %(sensor_frame))
print('Rotation: ')
print(T_camera_world.rotation)
print('Quaternion: ')
print(T_camera_world.quaternion)
print('Translation: ')
print(T_camera_world.translation)
# stop sensor
sensor.stop()
# move robot to calib pattern center
if config['use_robot']:
# create robot pose relative to target point
target_pt_camera = Point(T_pattern_camera.translation, sensor_frame)
target_pt_world = T_camera_world * target_pt_camera
R_gripper_world = np.array([[1.0, 0, 0],
[0, -1.0, 0],
[0, 0, -1.0]])
t_gripper_world = np.array([target_pt_world.x + config['gripper_offset_x'],
target_pt_world.y + config['gripper_offset_y'],
target_pt_world.z + config['gripper_offset_z']])
T_gripper_world = RigidTransform(rotation=R_gripper_world,
translation=t_gripper_world,
from_frame='gripper',
to_frame='world')
# move robot to pose
y = YuMiRobot(tcp=YMC.TCP_SUCTION_STIFF)
y.reset_home()
time.sleep(1)
T_lift = RigidTransform(translation=(0,0,0.05), from_frame='world', to_frame='world')
T_gripper_world_lift = T_lift * T_gripper_world
y.right.goto_pose(T_gripper_world_lift)
y.right.goto_pose(T_gripper_world)
# wait for human measurement
yesno = raw_input('Take measurement. Hit [ENTER] when done')
y.right.goto_pose(T_gripper_world_lift)
y.reset_home()
y.stop()
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 main():
global home_left
rospy.init_node('tcp_tf', anonymous=True)
br = tf2_ros.TransformBroadcaster()
y = YuMiRobot(arm_type='remote')
global tool_cesar_cal
y.left.set_tool(tool_cesar_cal)
y.right.set_tool(tool_cesar_cal)
state_left = y.left.get_state(raw_res=False)
state_right = y.right.get_state(raw_res=False)
print('state_left: {}'.format(state_left))
#print('state_right: {}'.format(state_right))
exit(0)
y.left.close_gripper(no_wait=False, wait_for_res=True)
y.right.close_gripper(no_wait=False, wait_for_res=True)
y.left.open_gripper(no_wait=False, wait_for_res=True)
y.right.open_gripper(no_wait=False, wait_for_res=True)
exit(0)
#bridge position---------------------
state_left_b = [-90.8, -59.74, 17.98, 112.48, 87.42, -61.76, 66.21]
state_right_b = [92.48, -55.08, 15.49, -111.06, 82.59, 55.73, -70.21]
y.left.goto_state(YuMiState(state_left_b), wait_for_res=False)
y.right.goto_state(YuMiState(state_right_b), wait_for_res=False)
#home position-----------------------
state_left_h = [-59.49, -77.18, 16.56, 112.06, 63.68, -112.18, 66.82]
state_right_h = [59.78, -72.42, 21.53, -112.84, 63.46, -55.3, -67.8]
y.left.goto_state(YuMiState(state_left_h), wait_for_res=False)
y.right.goto_state(YuMiState(state_right_h), wait_for_res=False)
exit(0)
rate = rospy.Rate(10)
while (True):
pose = y.left.get_pose(raw_res=False)
print('pose: {}'.format(pose))
state = y.left.get_state(raw_res=False)
print("state: {}".format(state))
print('moving!!!')
# pose.translation=[0.48, 0.10, 0.04]
# #pose.translation[1] += 0.03
# #print(y.left.is_pose_reachable(pose))
# state_robot_1=y.left.goto_pose(pose, linear=True, relative=False, wait_for_res=True)
# print('state_robot {}'.format(state_robot_1))
# if state_robot_1:
# #y.left.open_gripper(no_wait=False, wait_for_res=True)
# time.sleep(3)
# print('done...')
# pose.translation=[0.45, 0.05, 0.03]
# #print(y.left.is_pose_reachable(pose))
# state_robot_2=y.left.goto_pose(pose, linear=True, relative=False, wait_for_res=True)
# #y.left.close_gripper(no_wait=False, wait_for_res=True)
# time.sleep(3)
# if(state_robot_2):
# print('done...')
# pose.translation=[0.50, 0.10, 0.03]
# #print(y.left.is_pose_reachable(pose))
# state_robot_3=y.left.goto_pose(pose, linear=True, relative=False, wait_for_res=True)
# time.sleep(3)
# continue
# else:
# pose.translation[0]=3
# move(y)
rate.sleep()
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("-c", "--control_mode", type=int, default=3)
parser.add_argument("-i",
"--input_h5_file",
type=str,
default='./h5_data/inference.h5')
parser.add_argument("-g", "--group_name", type=str, default='group1')
parser.add_argument("-v", "--speed", type=float, default=150)
parser.add_argument("-d", "--delta_time", type=float, default=0.2)
args = parser.parse_args()
# Set control mode
control_mode = args.control_mode
# Create instance of yumi robot
yumi_robot = YuMiRobot()
# Test communication
state = yumi_robot.left.get_state()
# Read trajectory data from h5
f = h5py.File(args.input_h5_file, 'r')
l_joint_angles = f[args.group_name + '/l_joint_angle_2'][:]
r_joint_angles = f[args.group_name + '/r_joint_angle_2'][:]
l_glove_angles = f[args.group_name + '/l_glove_angle_2'][:]
r_glove_angles = f[args.group_name + '/r_glove_angle_2'][:]
# Init HAND controller & control thread
left_hand_controller = InspireHand('COM12', 115200)
right_hand_controller = InspireHand('COM13', 115200)
left_hand_controller.setspeed(1000, 1000, 1000, 1000, 1000, 1000)
right_hand_controller.setspeed(1000, 1000, 1000, 1000, 1000, 1000)
left_hand_control_thread = HandControlThread(0, left_hand_controller,
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()
elif (job == 2):
write_log.write_log(5, speed, 1, catching_dir, pos, time_diff)
catch(catching_dir, pos)
write_log.write_log(6, speed, 1, catching_dir, pos, time_diff)
give(catching_dir, pos)
y.reset_home()
y.right.open_gripper()
write_log.write_log(2, None, None, None, None, time_diff)
time.sleep(5)
if __name__ == '__main__':
######################################################################
global time_diff
y = YuMiRobot()
y.set_z('z50')
# 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)
######################################################################
timeServer = 'time.windows.com'
c = ntplib.NTPClient()
response = c.request(timeServer, version=3)
from yumipy import YuMiRobot
if __name__ == '__main__':
y = YuMiRobot()
y.calibrate_grippers()
y.open_grippers()
y.stop()
def main():
global home_left
global tool_cesar_cal
rospy.init_node('tcp_tf', anonymous=True)
br = tf2_ros.TransformBroadcaster()
y = YuMiRobot(arm_type='remote')
listener = tf.TransformListener()
y.left.set_tool(tool_cesar_cal)
y.right.set_tool(tool_cesar_cal)
rate = rospy.Rate(10)
move(y)
y.left.close_gripper(no_wait=False, wait_for_res=True)
y.right.close_gripper(no_wait=False, wait_for_res=True)
state_robot = False
temp_flag_left = False
temp_flag_right = False
temp_flag = False
previous_trans = [0.0, 0.0, 0.0]
pose_state_left = y.left.get_pose(raw_res=False)
pose_state_right = y.right.get_pose(raw_res=False)
y.left.open_gripper(no_wait=False, wait_for_res=True)
y.right.open_gripper(no_wait=False, wait_for_res=True)
#exit(0)
while (not rospy.is_shutdown()):
print("temp_flag_left:{}".format(temp_flag_left))
print("temp_flag_right:{}".format(temp_flag_right))
try:
(trans, rot) = listener.lookupTransform('/world', '/pose_object',
rospy.Time(0))
print('rot:{}'.format(rot))
print('trans:{}'.format(trans))
#roll_, pitch_, yaw_=tf.transformations.euler_from_quaternion(rot)
#print(yaw_)
if not trans == None and not rot == None and not previous_trans == trans:
if trans[1] < -0.01:
print('right {}'.format(trans[1]))
pose_state_right.translation = trans
temp_flag_right = True
temp_flag_left = False
else:
if trans[1] > 0.01:
print('left {}'.format(trans[1]))
pose_state_left.translation = trans
temp_flag_right = False
temp_flag_left = True
move(y)
previous_trans = trans
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
if temp_flag_left:
y.left.goto_pose(pose_state_left,
linear=True,
relative=False,
wait_for_res=True)
pose_state_left.translation[2] = 0.065
#pose_state_right.translation[1]+=0.065
y.left.goto_pose(pose_state_left,
linear=True,
relative=False,
wait_for_res=True)
y.left.close_gripper(force=20, no_wait=False, wait_for_res=True)
move(y)
y.left.goto_state(YuMiState(state_left_h), wait_for_res=False)
y.left.open_gripper(no_wait=False, wait_for_res=True)
temp_flag_left = False
else:
if temp_flag_right:
y.right.goto_pose(pose_state_right,
linear=True,
relative=False,
wait_for_res=True)
pose_state_right.translation[2] = 0.065
#pose_state_right.translation[1]+=0.065
y.right.goto_pose(pose_state_right,
linear=True,
relative=False,
wait_for_res=True)
y.right.close_gripper(force=20,
no_wait=False,
wait_for_res=True)
move(y)
y.right.goto_state(YuMiState(state_right_h),
wait_for_res=False)
y.right.open_gripper(no_wait=False, wait_for_res=True)
temp_flag_right = False
print('moving!!!')
rate.sleep()
"""
Helper script to move YuMi back to home pose
Author: Jeff Mahler, Jacky Liang
"""
import argparse
from yumipy import YuMiRobot
from yumipy import YuMiConstants as YMC
if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument('-l', '--left', type=str, default='True')
parser.add_argument('-r', '--right', type=str, default='True')
args = parser.parse_args()
def _is_true(arg):
return arg.lower() in ['y', 'yes', 'true', '1', 't']
y = YuMiRobot(include_right=_is_true(args.right),
include_left=_is_true(args.left))
y.set_z('fine')
y.reset_home()
y.open_grippers()
y.stop()
for j in range(num_pts_y):
y = -float(grid_height) / 2 + grid_center_y + float(j * grid_height) / num_pts_y
# form robot pose
R_robot_world = np.array([[1, 0, 0],
[0, 0, 1],
[0, -1, 0]])
t_robot_world = np.array([x, y, gripper_height])
T_robot_world = RigidTransform(rotation=R_robot_world,
translation=t_robot_world,
from_frame='gripper',
to_frame='world')
robot_poses.append(T_robot_world)
# start robot
y = YuMiRobot(tcp=YMC.TCP_SUCTION_STIFF)
y.set_z('fine')
y.reset_home()
global clicked_pt
# iteratively go to poses
robot_points_camera = []
for robot_pose in robot_poses:
# reset clicked pt
clicked_pt = None
# move to pose
y.right.goto_pose(robot_pose, wait_for_res=True)
# capture image
color_im, depth_im, _ = sensor.frames()
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)
"""
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,
"""
Script to test YuMi movements within a bin
Author: Jeff Mahler
"""
import IPython
import numpy as np
import os
import sys
from yumipy import YuMiRobot
from yumipy import YuMiConstants as YMC
if __name__ == '__main__':
robot = YuMiRobot()
#robot.left.goto_pose(YMC.L_PREGRASP_POSE)
robot.left.goto_state(YMC.L_KINEMATIC_AVOIDANCE_STATE)
#robot.left.goto_pose(YMC.L_KINEMATIC_AVOIDANCE_POSE)
robot.left.goto_pose_delta([0, 0, 0], rotation=[0, 0, 90])
T_orig_gripper_world = robot.left.get_pose()
T_gripper_world = robot.left.get_pose()
delta_t = YMC.L_BIN_LOWER_LEFT.translation - T_gripper_world.translation
robot.left.goto_pose_delta(delta_t)
T_gripper_world = robot.left.get_pose()
delta_t = YMC.L_BIN_UPPER_LEFT.translation - T_gripper_world.translation
robot.left.goto_pose_delta(delta_t)
T_gripper_world = robot.left.get_pose()
from yumipy import YuMiRobot, YuMiCommException
import IPython
if __name__ == '__main__':
y = YuMiRobot()
print 'Initial ping'
pong = y.right.ping()
print 'Pong: {0}'.format(pong)
_ = raw_input("Please shut off YuMi's server. Hit [ENTER] When done.")
try:
pong = y.right.ping()
except YuMiCommException, e:
print "Got exception {0}".format(e)
_ = raw_input("Please turn on YuMi's server. Hit [ENTER] when done.")
print 'Resting'
y.reset()
print 'Test ping'
pong = y.right.ping()
print 'Pong: {0}'.format(pong)
IPython.embed()
exit(0)
"""
Helper script to move YuMi back to home pose
Author: Jeff Mahler, Jacky Liang
"""
import argparse
from yumipy import YuMiRobot
from yumipy import YuMiConstants as YMC
if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument('-l', '--left', type=str, default='True')
parser.add_argument('-r', '--right', type=str, default='True')
args = parser.parse_args()
def _is_true(arg):
return arg.lower() in ['y', 'yes', 'true', '1', 't']
y = YuMiRobot(include_right=_is_true(args.right),
include_left=_is_true(args.left))
y.set_z('fine')
y.left.goto_state(YMC.L_KINEMATIC_AVOIDANCE_STATE)
T_cur = y.left.get_pose()
delta_t = YMC.L_BIN_PREGRASP_POSE.translation - T_cur.translation
#delta_t = [0, 0, 0.1]
y.left.goto_pose_delta(delta_t)
#y.left.open_gripper()
y.stop()
