for i,p in enumerate(sturm_pts[1:]):
sturm_file.write(" ".join(map(str,p)))
sturm_file.write('\n')
sturm_file.write('\n')
sturm_file.close()
rad_guess = rad
rad, cx, cy = fit_circle(rad_guess,x_guess,y_guess,pts_2d,
method='fmin_bfgs',verbose=False)
c_ts = np.matrix([cx, cy, 0.]).T
start_angle = tr.angle_within_mod180(math.atan2(pts_2d[0,1]-cy,
pts_2d[0,0]-cx) - math.pi/2)
end_angle = tr.angle_within_mod180(math.atan2(pts_2d[-1,1]-cy,
pts_2d[-1,0]-cx) - math.pi/2)
mpu.plot_circle(cx, cy, rad, start_angle, end_angle,
label='Actual\_opt', color='r')
if opt.icra_presentation_plot:
mpu.set_figure_size(30,20)
rad = 1.0
x_guess = pts_2d[0,0]
y_guess = pts_2d[1,0] - rad
rad_guess = rad
rad, cx, cy = fit_circle(rad_guess,x_guess,y_guess,pts_2d,
method='fmin_bfgs',verbose=False)
print 'Estimated rad, cx, cy:', rad, cx, cy
start_angle = tr.angle_within_mod180(math.atan2(pts_2d[1,0]-cy,
pts_2d[0,0]-cx) - math.pi/2)
rad, cx, cy = fit_circle(rad_guess,
x_guess,
y_guess,
pts_2d,
method='fmin_bfgs',
verbose=False)
print 'rad, cx, cy:', rad, cx, cy
c_ts = np.matrix([cx, cy, 0.]).T
start_angle = tr.angle_within_mod180(
math.atan2(pts_2d[1, 0] - cy, pts_2d[0, 0] - cx) - math.pi / 2)
end_angle = tr.angle_within_mod180(
math.atan2(pts_2d[1, -1] - cy, pts_2d[0, -1] - cx) - math.pi / 2)
mpu.plot_circle(cx,
cy,
rad,
start_angle,
end_angle,
label='Actual\_opt',
color='r')
if opt.icra_presentation_plot:
mpu.set_figure_size(30, 20)
rad = 1.0
x_guess = pts_2d[0, 0]
y_guess = pts_2d[1, 0] - rad
rad_guess = rad
rad, cx, cy = fit_circle(rad_guess,
x_guess,
y_guess,
pts_2d,
robot_width,robot_length = 0.1,0.2
robot_x_list = [-robot_width/2,-robot_width/2,robot_width/2,robot_width/2,-robot_width/2]
robot_y_list = [-robot_length/2,robot_length/2,robot_length/2,-robot_length/2,-robot_length/2]
robot_mat = rot_mat*(np.matrix([robot_x_list,robot_y_list]) - translation_mat)
mpu.plot_yx(robot_mat[1,:].A1,robot_mat[0,:].A1,linewidth=2,scatter_size=0,
color='k',label='torso')
pts2d_actual = (np.matrix(actual_cartesian.p_list).T)[0:2]
pts2d_actual_t = rot_mat *(pts2d_actual - translation_mat)
mpu.plot_yx(pts2d_actual_t[1,:].A1,pts2d_actual_t[0,:].A1,scatter_size=20,label='FK')
end_pt = pts2d_actual_t[:,-1]
end_angle = tr.angle_within_mod180(math.atan2(end_pt[1,0],end_pt[0,0])-math.radians(90))
mpu.plot_circle(0,0,rad,0.,end_angle,label='Actual_opt',color='r')
mpu.plot_radii(0,0,rad,0.,end_angle,interval=math.radians(15),color='r')
pl.legend(loc='best')
pl.axis('equal')
if not(expt_plot):
str_parts = fname.split('.')
fig_name = str_parts[0]+'_robot_pose.png'
pl.savefig(fig_name)
pl.figure()
else:
pl.subplot(232)
pl.text(0.1,0.15,d['info'])
pl.text(0.1,0.10,'control: '+d['strategy'])
pl.text(0.1,0.05,'robot angle: %.2f'%math.degrees(angle))
sturm_file.write(" ".join(map(str,p)))
sturm_file.write('\n')
sturm_file.write('\n')
sturm_file.close()
rad_guess = rad
rad, cx, cy = fit_circle(rad_guess,x_guess,y_guess,pts_2d,
method='fmin_bfgs',verbose=False)
print 'rad, cx, cy:', rad, cx, cy
c_ts = np.matrix([cx, cy, 0.]).T
start_angle = tr.angle_within_mod180(math.atan2(pts_2d[1,0]-cy,
pts_2d[0,0]-cx) - math.pi/2)
end_angle = tr.angle_within_mod180(math.atan2(pts_2d[1,-1]-cy,
pts_2d[0,-1]-cx) - math.pi/2)
mpu.plot_circle(cx, cy, rad, start_angle, end_angle,
label='Actual\_opt', color='r')
if opt.icra_presentation_plot:
mpu.set_figure_size(30,20)
rad = 1.0
x_guess = pts_2d[0,0]
y_guess = pts_2d[1,0] - rad
rad_guess = rad
rad, cx, cy = fit_circle(rad_guess,x_guess,y_guess,pts_2d,
method='fmin_bfgs',verbose=False)
print 'Estimated rad, cx, cy:', rad, cx, cy
start_angle = tr.angle_within_mod180(math.atan2(pts_2d[1,0]-cy,
pts_2d[0,0]-cx) - math.pi/2)
def plot_pkl(pkl_nm):
pull_dict = ut.load_pickle(pkl_nm)
if 'pr2' in pkl_nm:
pr2_log = True
h_color = 'y'
else:
pr2_log = False
h_color = 'r'
t = load_ref_traj(pkl_nm)
if t !=None:
ref_mean, ref_std, ref_config, typ = t
mar.plot_reference_trajectory(ref_config, ref_mean, ref_std, typ, 'Hello')
ref_config = np.degrees(ref_config)
max_config = np.max(ref_config)
else:
typ = 'rotary'
max_config = 60.
if pr2_log:
o_ftan = pull_dict['ftan_list']
o_config = pull_dict['config_list']
else:
o_ftan = pull_dict['online_ftan']
o_config = pull_dict['online_ang']
h_config, h_ftan = force_trajectory_in_hindsight(pull_dict,
typ, pr2_log)
if typ == 'rotary':
if opt.prior:
r_config, r_ftan = online_force_with_radius(pull_dict,
pr2_log)
r_config = np.degrees(r_config)
o_config = np.degrees(o_config)
h_config = np.degrees(h_config)
o_ftan, o_config = truncate_to_config(o_ftan, o_config, max_config)
h_ftan, h_config = truncate_to_config(h_ftan, h_config, max_config)
if typ == 'rotary':
if opt.prior:
r_ftan, r_config = truncate_to_config(r_ftan, r_config, max_config)
bin_size = 1.
else:
bin_size = 0.01
#o_config, o_ftan = maa.bin(o_config, o_ftan, bin_size, np.mean, True)
#h_config, h_ftan = maa.bin(h_config, h_ftan, bin_size, np.mean, True)
# non_nans = ~np.isnan(h_ftan)
# h_ftan = np.array(h_ftan)[non_nans]
# h_config = np.array(h_config)[non_nans]
#
# non_nans = ~np.isnan(o_ftan)
# o_ftan = np.array(o_ftan)[non_nans]
# o_config = np.array(o_config)[non_nans]
# h_config = h_config[:-1]
# h_ftan = h_ftan[1:]
if not pr2_log:
m,c = get_cody_calibration()
o_ftan = (np.array(o_ftan) - c) / m
h_ftan = (np.array(h_ftan) - c) / m
pp.plot(o_config, o_ftan, 'bo-', ms=5, label='online')
pp.plot(h_config, h_ftan, h_color+'o-', ms=5, label='hindsight')
if typ == 'rotary':
if opt.prior:
r_config, r_ftan = maa.bin(r_config, r_ftan, bin_size, max, True)
pp.plot(r_config, r_ftan, 'go-', ms=5, label='online with priors')
pp.xlabel('Configuration')
pp.ylabel('Tangential Force')
if pr2_log:
pp.figure()
p_list, f_list = pull_dict['ee_list'], pull_dict['f_list']
p_list = p_list[::2]
f_list = f_list[::2]
x_l, y_l, z_l = zip(*p_list)
pp.plot(x_l, y_l)
r, cx, cy = estimate_mechanism_kinematics(pull_dict, pr2_log)
mpu.plot_circle(cx, cy, r, 0., math.pi/2,
label='Actual\_opt', color='r')
pp.axis('equal')
translation_mat = np.matrix([cx, cy]).T
robot_width, robot_length = 0.1, 0.2
robot_x_list = [-robot_width / 2, -robot_width / 2, robot_width / 2, robot_width / 2, -robot_width / 2]
robot_y_list = [-robot_length / 2, robot_length / 2, robot_length / 2, -robot_length / 2, -robot_length / 2]
robot_mat = rot_mat * (np.matrix([robot_x_list, robot_y_list]) - translation_mat)
mpu.plot_yx(robot_mat[1, :].A1, robot_mat[0, :].A1, linewidth=2, scatter_size=0, color="k", label="torso")
pts2d_actual = (np.matrix(actual_cartesian.p_list).T)[0:2]
pts2d_actual_t = rot_mat * (pts2d_actual - translation_mat)
mpu.plot_yx(pts2d_actual_t[1, :].A1, pts2d_actual_t[0, :].A1, scatter_size=20, label="FK")
end_pt = pts2d_actual_t[:, -1]
end_angle = tr.angle_within_mod180(math.atan2(end_pt[1, 0], end_pt[0, 0]) - math.radians(90))
mpu.plot_circle(0, 0, rad, 0.0, end_angle, label="Actual_opt", color="r")
mpu.plot_radii(0, 0, rad, 0.0, end_angle, interval=math.radians(15), color="r")
pl.legend(loc="best")
pl.axis("equal")
if not (expt_plot):
str_parts = fname.split(".")
fig_name = str_parts[0] + "_robot_pose.png"
pl.savefig(fig_name)
pl.figure()
else:
pl.subplot(232)
pl.text(0.1, 0.15, d["info"])
pl.text(0.1, 0.10, "control: " + d["strategy"])
pl.text(0.1, 0.05, "robot angle: %.2f" % math.degrees(angle))
def plot_pkl(pkl_nm):
pull_dict = ut.load_pickle(pkl_nm)
if 'pr2' in pkl_nm:
pr2_log = True
h_color = 'y'
else:
pr2_log = False
h_color = 'r'
t = load_ref_traj(pkl_nm)
if t != None:
ref_mean, ref_std, ref_config, typ = t
mar.plot_reference_trajectory(ref_config, ref_mean, ref_std, typ,
'Hello')
ref_config = np.degrees(ref_config)
max_config = np.max(ref_config)
else:
typ = 'rotary'
max_config = 60.
if pr2_log:
o_ftan = pull_dict['ftan_list']
o_config = pull_dict['config_list']
else:
o_ftan = pull_dict['online_ftan']
o_config = pull_dict['online_ang']
h_config, h_ftan = force_trajectory_in_hindsight(pull_dict, typ, pr2_log)
if typ == 'rotary':
if opt.prior:
r_config, r_ftan = online_force_with_radius(pull_dict, pr2_log)
r_config = np.degrees(r_config)
o_config = np.degrees(o_config)
h_config = np.degrees(h_config)
o_ftan, o_config = truncate_to_config(o_ftan, o_config, max_config)
h_ftan, h_config = truncate_to_config(h_ftan, h_config, max_config)
if typ == 'rotary':
if opt.prior:
r_ftan, r_config = truncate_to_config(r_ftan, r_config, max_config)
bin_size = 1.
else:
bin_size = 0.01
#o_config, o_ftan = maa.bin(o_config, o_ftan, bin_size, np.mean, True)
#h_config, h_ftan = maa.bin(h_config, h_ftan, bin_size, np.mean, True)
# non_nans = ~np.isnan(h_ftan)
# h_ftan = np.array(h_ftan)[non_nans]
# h_config = np.array(h_config)[non_nans]
#
# non_nans = ~np.isnan(o_ftan)
# o_ftan = np.array(o_ftan)[non_nans]
# o_config = np.array(o_config)[non_nans]
# h_config = h_config[:-1]
# h_ftan = h_ftan[1:]
if not pr2_log:
m, c = get_cody_calibration()
o_ftan = (np.array(o_ftan) - c) / m
h_ftan = (np.array(h_ftan) - c) / m
pp.plot(o_config, o_ftan, 'bo-', ms=5, label='online')
pp.plot(h_config, h_ftan, h_color + 'o-', ms=5, label='hindsight')
if typ == 'rotary':
if opt.prior:
r_config, r_ftan = maa.bin(r_config, r_ftan, bin_size, max, True)
pp.plot(r_config, r_ftan, 'go-', ms=5, label='online with priors')
pp.xlabel('Configuration')
pp.ylabel('Tangential Force')
if pr2_log:
pp.figure()
p_list, f_list = pull_dict['ee_list'], pull_dict['f_list']
p_list = p_list[::2]
f_list = f_list[::2]
x_l, y_l, z_l = zip(*p_list)
pp.plot(x_l, y_l)
r, cx, cy = estimate_mechanism_kinematics(pull_dict, pr2_log)
mpu.plot_circle(cx,
cy,
r,
0.,
math.pi / 2,
label='Actual\_opt',
color='r')
pp.axis('equal')
