def compute_workspace(z,plot=False,wrist_roll_angle=math.radians(0),subplotnum=None,title=''):
firenze = hr.M3HrlRobot(connect=False)
# hook_3dprint_angle = math.radians(20-2.54)
# rot_mat = tr.Rz(math.radians(-90.)-hook_3dprint_angle)*tr.Ry(math.radians(-90))
rot_mat = tr.Rz(wrist_roll_angle)*tr.Ry(math.radians(-90))
x_list,y_list = [],[]
for x in np.arange(0.15,0.65,0.02):
for y in np.arange(-0.05,-0.65,-0.02):
q = firenze.IK('right_arm',np.matrix([x,y,z]).T,rot_mat)
if q != None:
x_list.append(x)
y_list.append(y)
if len(x_list) > 2:
multipoint = sg.Point(x_list[0],y_list[0])
for x,y in zip(x_list[1:],y_list[1:]):
multipoint = multipoint.union(sg.Point(x,y))
hull = multipoint.convex_hull
if plot:
coords_seq = hull.boundary.coords
hull_x_list,hull_y_list = [],[]
for c in coords_seq:
hull_x_list.append(c[0])
hull_y_list.append(c[1])
mpu.plot_yx(y_list,x_list,linewidth=0,subplotnum=subplotnum,axis='equal',
plot_title=title)
mpu.plot_yx(hull_y_list,hull_x_list,linewidth=2,subplotnum=subplotnum,axis='equal')
return hull,len(x_list)
else:
return None,None
def plot_hook_translation(curr_pos_tl,cx_tl,cy_tl,cy_ts,
start_pos_ts,eq_pt_tl,bndry,wrkspc_pts):
vt,a = smc.segway_motion_repulse(curr_pos_tl,cx_tl,cy_tl,cy_ts,
start_pos_ts,eq_pt_tl,bndry,wrkspc_pts)
mpu.plot_yx(eq_pt_tl[1,:].A1, eq_pt_tl[0,:].A1, linewidth=2,
color='g', scatter_size=15, label='Eq Pt')
mpu.plot_yx(curr_pos_tl[1,:].A1, curr_pos_tl[0,:].A1, linewidth=0,
color='b', scatter_size = 15, label = 'FK')
mpu.plot_yx(bndry[1,:].A1, bndry[0,:].A1, linewidth=0, color='y',
scatter_size=8)
mpu.plot_yx([-0.2], [0.], linewidth=0, color='b', scatter_size=2)
bndry_dist_eq = smc.dist_from_boundary(eq_pt_tl, bndry, wrkspc_pts) # signed
bndry_dist_ee = smc.dist_from_boundary(curr_pos_tl, bndry, wrkspc_pts) # signed
if bndry_dist_ee < bndry_dist_eq:
p = curr_pos_tl
else:
p = eq_pt_tl
pts_close = smc.pts_within_dist(p[0:2,:],bndry,0.01,0.1)
mpu.plot_yx(pts_close[1,:].A1, pts_close[0,:].A1, linewidth=0,
color='r', scatter_size = 8)
nrm = np.linalg.norm(vt)
vt = vt/nrm
mpu.plot_quiver_yxv(p[1,:].A1, p[0,:].A1, vt, scale=12)
mpu.show()
def compute_workspace_z():
n_points_list,area_list,z_list = [],[],[]
#for z in np.arange(-0.1,-0.36,-0.02):
#for z in np.arange(-0.05,-0.35,-0.01):
for z in np.arange(-0.15,-0.155,-0.01):
pl.figure()
hull,n_points = compute_workspace(z,plot=True)
pl.title('z: %.2f'%(z))
pl.savefig('z_%.2f.png'%(z))
# hull,n_points = compute_workspace(z,plot=False)
if hull != None:
area_list.append(hull.area)
z_list.append(z)
n_points_list.append(n_points)
coords_seq = hull.boundary.coords
hull_x_list,hull_y_list = [],[]
for c in coords_seq:
hull_x_list.append(c[0])
hull_y_list.append(c[1])
pl.figure()
mpu.plot_yx(area_list,z_list,linewidth=2,label='area')
pl.savefig('hist_area.png')
pl.figure()
mpu.plot_yx(n_points_list,z_list,linewidth=2,color='g',label='n_points')
# pl.legend(loc='best')
pl.xlabel('Z coordinate (m)')
pl.ylabel('# points')
pl.savefig('hist_points.png')
def plot_workspace(pts, ha, z):
if pts.shape[1] == 0:
return
mpu.figure()
good_location = pts.mean(1)
mpu.plot_yx(pts[1, :].A1, pts[0, :].A1, label="ha:%.1f" % (math.degrees(ha)), axis="equal", linewidth=0)
mpu.plot_yx(good_location[1, :].A1, good_location[0, :].A1, axis="equal", linewidth=0, scatter_size=90, color="k")
mpu.savefig("z%.2f_ha%.1f.png" % (z, math.degrees(ha)))
def plot(combined_dict, savefig):
plot_trajectories(combined_dict)
# tup = ke.init_ros_node()
# mech_rot_list = compute_mech_rot_list(combined_dict, tup)
# combined_dict['mech_rot_list'] = mech_rot_list
# plot_trajectories(combined_dict)
cd = combined_dict
ft_mat = np.matrix(cd['ft_list']).T
force_mat = ft_mat[0:3, :]
mpu.plot_yx(ut.norm(force_mat).A1, cd['time_list'])
mpu.show()
def compare_tip_mechanism_trajectories(mech_mat, hand_mat):
# hand_proj, nrm_hand = project_points_plane(hand_mat)
# mech_proj, nrm_mech = project_points_plane(mech_mat)
hand_proj = hand_mat
mech_proj = mech_mat
kin_dict = ke.fit(hand_proj, tup)
print 'kin_dict from hook tip:', kin_dict
print 'measured radius:', cd['radius']
center_hand = np.matrix((kin_dict['cx'], kin_dict['cy'],
kin_dict['cz'])).T
kin_dict = ke.fit(mech_proj, tup)
print 'kin_dict from mechanism:', kin_dict
center_mech = np.matrix((kin_dict['cx'], kin_dict['cy'],
kin_dict['cz'])).T
# working with the projected coordinates.
directions_hand = hand_proj - center_hand
directions_hand = directions_hand / ut.norm(directions_hand)
directions_mech = mech_proj - center_mech
directions_mech = directions_mech / ut.norm(directions_mech)
start_normal = directions_mech[:,0]
print 'directions_mech[:,0]', directions_mech[:,0].A1
print 'directions_hand[:,0]', directions_hand[:,0].A1
ct = (start_normal.T * directions_hand).A1
st = ut.norm(np.matrix(np.cross(start_normal.A1, directions_hand.T.A)).T).A1
mech_angle = np.arctan2(st, ct).tolist()
#mech_angle = np.arccos(start_normal.T * directions_hand).A1.tolist()
mpu.plot_yx(np.degrees(mech_angle))
mpu.show()
# plot trajectory in 3D.
d3m.white_bg()
d3m.plot_points(hand_proj, color = (1.,0.,0.), mode='sphere',
scale_factor = 0.005)
d3m.plot_points(mech_proj[:,0:1], color = (0.,0.,0.), mode='sphere',
scale_factor = 0.01)
d3m.plot_points(mech_proj, color = (0.,0.,1.), mode='sphere',
scale_factor = 0.005)
d3m.plot(np.column_stack((mech_proj[:,-1],center_mech, mech_proj[:,0])),
color = (0.,0.,1.))
d3m.plot(np.column_stack((hand_proj[:,-1],center_hand, hand_proj[:,0])),
color = (1.,0.,0.))
d3m.show()
def plot_cartesian(traj, xaxis=None, yaxis=None, zaxis=None, color='b',label='_nolegend_',
linewidth=2, scatter_size=10, plot_velocity=False):
import arm_trajectories as at
#if traj.__class__ == at.JointTrajectory:
if isinstance(traj,at.JointTrajectory):
traj = joint_to_cartesian(traj)
pts = np.matrix(traj.p_list).T
label_list = ['X coord (m)', 'Y coord (m)', 'Z coord (m)']
x = pts[xaxis,:].A1.tolist()
y = pts[yaxis,:].A1.tolist()
if plot_velocity:
vels = np.matrix(traj.v_list).T
xvel = vels[xaxis,:].A1.tolist()
yvel = vels[yaxis,:].A1.tolist()
if zaxis == None:
mpu.plot_yx(y, x, color, linewidth, '-', scatter_size, label,
axis = 'equal', xlabel = label_list[xaxis],
ylabel = label_list[yaxis],)
if plot_velocity:
mpu.plot_quiver_yxv(y, x, np.matrix([xvel,yvel]),
width = 0.001, scale = 1.)
mpu.legend()
else:
from numpy import array
from enthought.mayavi.api import Engine
engine = Engine()
engine.start()
if len(engine.scenes) == 0:
engine.new_scene()
z = pts[zaxis,:].A1.tolist()
time_list = [t-traj.time_list[0] for t in traj.time_list]
mlab.plot3d(x,y,z,time_list,tube_radius=None,line_width=4)
mlab.axes()
mlab.xlabel(label_list[xaxis])
mlab.ylabel(label_list[yaxis])
mlab.zlabel(label_list[zaxis])
mlab.colorbar(title='Time')
# -------------------------------------------
axes = engine.scenes[0].children[0].children[0].children[1]
axes.axes.position = array([ 0., 0.])
axes.axes.label_format = '%-#6.2g'
axes.title_text_property.font_size=4
def plot_cartesian(traj, xaxis=None, yaxis=None, zaxis=None, color='b',label='_nolegend_',
linewidth=2, scatter_size=10, plot_velocity=False):
import matplotlib_util.util as mpu
import arm_trajectories as at
#if traj.__class__ == at.JointTrajectory:
if isinstance(traj,at.JointTrajectory):
traj = joint_to_cartesian(traj)
pts = np.matrix(traj.p_list).T
label_list = ['X coord (m)', 'Y coord (m)', 'Z coord (m)']
x = pts[xaxis,:].A1.tolist()
y = pts[yaxis,:].A1.tolist()
if plot_velocity:
vels = np.matrix(traj.v_list).T
xvel = vels[xaxis,:].A1.tolist()
yvel = vels[yaxis,:].A1.tolist()
if zaxis == None:
mpu.plot_yx(y, x, color, linewidth, '-', scatter_size, label,
axis = 'equal', xlabel = label_list[xaxis],
ylabel = label_list[yaxis],)
if plot_velocity:
mpu.plot_quiver_yxv(y, x, np.matrix([xvel,yvel]),
width = 0.001, scale = 1.)
mpu.legend()
else:
from numpy import array
from enthought.mayavi.api import Engine
engine = Engine()
engine.start()
if len(engine.scenes) == 0:
engine.new_scene()
z = pts[zaxis,:].A1.tolist()
time_list = [t-traj.time_list[0] for t in traj.time_list]
mlab.plot3d(x,y,z,time_list,tube_radius=None,line_width=4)
mlab.axes()
mlab.xlabel(label_list[xaxis])
mlab.ylabel(label_list[yaxis])
mlab.zlabel(label_list[zaxis])
mlab.colorbar(title='Time')
# -------------------------------------------
axes = engine.scenes[0].children[0].children[0].children[1]
axes.axes.position = array([ 0., 0.])
axes.axes.label_format = '%-#6.2g'
axes.title_text_property.font_size=4
def plot_workspace(pts, ha, z):
if pts.shape[1] == 0:
return
mpu.figure()
good_location = pts.mean(1)
mpu.plot_yx(pts[1, :].A1,
pts[0, :].A1,
label='ha:%.1f' % (math.degrees(ha)),
axis='equal',
linewidth=0)
mpu.plot_yx(good_location[1, :].A1,
good_location[0, :].A1,
axis='equal',
linewidth=0,
scatter_size=90,
color='k')
mpu.savefig('z%.2f_ha%.1f.png' % (z, math.degrees(ha)))
def angle_between_hooktip_mechanism_radial_vectors(mech_mat, hand_mat):
kin_dict = ke.fit(hand_mat, tup)
print 'kin_dict from hook tip:', kin_dict
print 'measured radius:', cd['radius']
center_hand = np.matrix((kin_dict['cx'], kin_dict['cy'],
kin_dict['cz'])).T
kin_dict = ke.fit(mech_mat, tup)
print 'kin_dict from mechanism:', kin_dict
center_mech = np.matrix((kin_dict['cx'], kin_dict['cy'],
kin_dict['cz'])).T
# working with the projected coordinates.
directions_hand = hand_mat - center_hand
directions_hand = directions_hand / ut.norm(directions_hand)
directions_mech = mech_mat - center_mech
directions_mech = directions_mech / ut.norm(directions_mech)
#import pdb; pdb.set_trace()
ang = np.degrees(np.arccos(np.sum(np.multiply(directions_mech, directions_hand), 0))).A1
mpu.plot_yx(ang, label = 'angle between hooktip-radial and mechanism radial')
mpu.legend()
mpu.show()
def check_time_sync(ft_time_list, mechanism_time_list, hand_time_list):
mpu.plot_yx(np.zeros(len(ft_time_list))+1, ft_time_list,
color = mpu.random_color(), label='ft\_time\_list',
axis=None, linewidth=0.5, scatter_size=10)
mpu.plot_yx(np.zeros(len(mechanism_time_list))+2, mechanism_time_list,
color = mpu.random_color(), label='mechanism\_time\_list',
axis=None, linewidth=0.5, scatter_size=10)
mpu.plot_yx(np.zeros(len(hand_time_list))+3, hand_time_list,
color = mpu.random_color(), label='hand\_time\_list',
axis=None, linewidth=0.5, scatter_size=10)
mpu.legend()
def plot_radial_tangential(mech_dict, savefig, fig_name=''):
radial_mech = mech_dict['force_rad_list']
tangential_mech = mech_dict['force_tan_list']
typ = mech_dict['mech_type']
mech_x = mech_dict['mechanism_x']
if typ == 'rotary':
mech_x_degrees = np.degrees(mech_x)
xlabel = 'angle (degrees)'
else:
mech_x_degrees = mech_x
xlabel = 'distance (meters)'
mpu.pl.clf()
mpu.plot_yx(radial_mech, mech_x_degrees, axis=None, label='radial force',
xlabel=xlabel, ylabel='N', color='r')
mpu.plot_yx(tangential_mech, mech_x_degrees, axis=None, label='tangential force',
xlabel=xlabel, ylabel='N', color='g')
mpu.legend()
if typ == 'rotary':
mpu.figure()
rad = mech_dict['radius']
torques_1 = rad * np.array(tangential_mech)
torques_3 = np.array(mech_dict['moment_tip_list']) + torques_1
mpu.plot_yx(torques_1, mech_x_degrees, axis=None,
label='torque from tangential',
xlabel=xlabel, ylabel='moment', color='r')
mpu.plot_yx(torques_3, mech_x_degrees, axis=None,
label='total torque',
xlabel=xlabel, ylabel='moment', color='y')
mpu.legend()
if savefig:
mpu.savefig(opt.dir+'/%s_force_components.png'%fig_name)
else:
mpu.show()
def visualize_boundary():
d = ut.load_pickle('../../pkls/workspace_dict_2009Sep03_010426.pkl')
z = -0.23
k = d.keys()
k_idx = np.argmin(np.abs(np.array(k) - z))
pts = d[k[k_idx]]
bpts = compute_boundary(pts)
cl_list = []
for pt in pts.T:
if close_to_boundary(pt.T, bpts, dist_thresh=0.05) == True:
cl_list.append(pt.A1.tolist())
clpts = np.matrix(cl_list).T
print 'clpts.shape:', clpts.shape
mpu.plot_yx(pts[1, :].A1, pts[0, :].A1, linewidth=0)
mpu.plot_yx(clpts[1, :].A1, clpts[0, :].A1, linewidth=0, color='r')
mpu.plot_yx(bpts[1, :].A1, bpts[0, :].A1, linewidth=0, color='y')
mpu.show()
def visualize_boundary():
d = ut.load_pickle('../../pkls/workspace_dict_2009Sep03_010426.pkl')
z = -0.23
k = d.keys()
k_idx = np.argmin(np.abs(np.array(k)-z))
pts = d[k[k_idx]]
bpts = compute_boundary(pts)
cl_list = []
for pt in pts.T:
if close_to_boundary(pt.T,bpts,dist_thresh=0.05)==True:
cl_list.append(pt.A1.tolist())
clpts = np.matrix(cl_list).T
print 'clpts.shape:', clpts.shape
mpu.plot_yx(pts[1,:].A1,pts[0,:].A1,linewidth=0)
mpu.plot_yx(clpts[1,:].A1,clpts[0,:].A1,linewidth=0,color='r')
mpu.plot_yx(bpts[1,:].A1,bpts[0,:].A1,linewidth=0,color='y')
mpu.show()
def plot_eq_pt_motion_tl():
vec_list = []
for i in range(len(ee_tl.p_list)):
# for i in range(5):
curr_pos_tl = np.matrix(ee_tl.p_list[i]).T
eq_pt_tl = np.matrix(eq_tl.p_list[i]).T
pts_ts = np.matrix(ee_ts.p_list[0:i+1]).T
pts_2d_ts = pts_ts[0:2,:]
# rad_opt,cx_ts,cy_ts = at.fit_circle(rad_guess,x_guess,y_guess,pts_2d_ts,
# method='fmin_bfgs',verbose=False)
rad_opt = 1.0
cx_ts,cy_ts = 0.5,-1.3
c_ts = np.matrix([cx_ts,cy_ts,0.]).T
x,y,a = st.x_list[i],st.y_list[i],st.a_list[i]
c_tl = smc.tlTts(c_ts,x,y,a)
cx_tl,cy_tl = c_tl[0,0],c_tl[1,0]
t0 = time.time()
vt,a = smc.segway_motion_repulse(curr_pos_tl,cx_tl,cy_tl,cy_ts,
start_pos_ts,eq_pt_tl,bndry,wrkspc_pts)
t1 = time.time()
# print 'time to segway_motion_repulse:',t1-t0
nrm = np.linalg.norm(vt)
# if nrm > 0.005:
vt = vt/nrm
vec_list.append(vt.A1.tolist())
v = np.matrix(vec_list).T
eq_pts = np.matrix(eq_tl.p_list).T
ee_pts = np.matrix(ee_tl.p_list).T
mpu.plot_yx(eq_pts[1,:].A1,eq_pts[0,:].A1,linewidth=1,color='g',label='eq')
mpu.plot_yx(ee_pts[1,:].A1,ee_pts[0,:].A1,linewidth=1,color='b',label='FK')
mpu.plot_yx(bndry[1,:].A1,bndry[0,:].A1,linewidth=0,color='y')
mpu.plot_quiver_yxv(eq_pts[1,:].A1,eq_pts[0,:].A1,v,scale=30)
mpu.legend()
mpu.show()
ll.sort()
key_list, l = zip(*ll)
if ha == 0:
label = "Hook Left"
elif abs(ha - math.pi / 2) < 0.01:
label = "Hook Down"
continue
else:
label = "Hook Up"
mpu.plot_yx(
key_list,
l,
axis=None,
label=label,
color=color_list[i],
xlabel="\# of points with IK soln",
ylabel="Height (m)",
scatter_size=8,
)
i += 1
max_idx = np.argmax(l)
good_height = key_list[max_idx]
print "good_height:", good_height
mpu.plot_yx(
[good_height],
[l[max_idx]],
axis=None,
color="r",
xlabel="\# of points with IK soln",
math.atan2(pts_2d[1, -1] - cy, pts_2d[0, -1] - cx) - math.pi / 2)
subsample_ratio = 1
pts_2d_s = pts_2d[:, ::subsample_ratio]
cep_force_clean, force_new = filter_trajectory_force(
eq_cartesian, d['force'])
cep_2d = np.matrix(cep_force_clean.p_list).T[0:2, reject_idx:]
# first draw the entire CEP and end effector trajectories
mpu.figure()
mpu.plot_yx(pts_2d_s[1, :].A1,
pts_2d_s[0, :].A1,
color='b',
label='\huge{End Effector Trajectory}',
axis='equal',
alpha=1.0,
scatter_size=7,
linewidth=0,
marker='x',
marker_edge_width=1.5)
cep_2d_s = cep_2d[:, ::subsample_ratio]
mpu.plot_yx(cep_2d_s[1, :].A1,
cep_2d_s[0, :].A1,
color='g',
label='\huge{Equilibrium Point Trajectory}',
axis='equal',
alpha=1.0,
scatter_size=10,
linewidth=0,
marker='+',
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)
subsample_ratio = 1
pts_2d_s = pts_2d[:,::subsample_ratio]
cep_force_clean, force_new = filter_trajectory_force(eq_cartesian,
d['force'])
cep_2d = np.matrix(cep_force_clean.p_list).T[0:2,reject_idx:]
# first draw the entire CEP and end effector trajectories
mpu.figure()
mpu.plot_yx(pts_2d_s[1,:].A1, pts_2d_s[0,:].A1, color='b',
label = 'FK', axis = 'equal', alpha = 1.0,
scatter_size=7, linewidth=0, marker='x',
marker_edge_width = 1.5)
cep_2d_s = cep_2d[:,::subsample_ratio]
mpu.plot_yx(cep_2d_s[1,:].A1, cep_2d_s[0,:].A1, color='g',
label = 'CEP', axis = 'equal', alpha = 1.0,
scatter_size=10, linewidth=0, marker='+',
marker_edge_width = 1.5)
mpu.plot_circle(cx, cy, rad, start_angle, end_angle,
label='Estimated Kinematics', color='r',
alpha=0.7)
mpu.plot_radii(cx, cy, rad, start_angle, end_angle,
interval=end_angle-start_angle, color='r',
alpha=0.7)
mpu.legend()
k = d.keys()
k_idx = np.argmin(np.abs(np.array(k)-z))
pts = d[k[k_idx]]
# visualize_boundary()
for kk in k:
pts = d[kk]
bpts = compute_boundary(pts)
cx,cy = 0.7,-0.6
rad = 0.4
# pts_in = point_contained(cx,cy,0.,rad,pts,
# start_angle=math.radians(140),
# end_angle=math.radians(190))
mpu.figure()
mpu.plot_yx(pts[1,:].A1,pts[0,:].A1,linewidth=0)
mpu.plot_yx(bpts[1,:].A1,bpts[0,:].A1,linewidth=0,color='y')
# mpu.plot_yx(pts_in[1,:].A1,pts_in[0,:].A1,linewidth=0,color='g')
# mpu.plot_yx([cy],[cx],linewidth=0,color='r')
mpu.show()
### apologies for the poor name. computes the translation and rotation
## of the torso frame that move the eq pt away from closest boundary
## and rotate such that local x axis is perp to mechanism
## returns 2x1 np matrix, angle
#def segway_motion_repulse(curr_pos_tl,cx_tl,cy_tl,cy_ts,start_pos_ts,
# eq_pt_tl,bndry):
# vec_bndry = vec_from_boundary(eq_pt_tl,bndry)
if expt_plot:
pl.subplot(231)
# transform points so that the mechanism is in a fixed position.
start_pt = actual_cartesian.p_list[0]
x_diff = start_pt[0] - cx
y_diff = start_pt[1] - cy
angle = math.atan2(y_diff, x_diff) - math.radians(90)
rot_mat = tr.Rz(angle)[0:2, 0:2]
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(".")
k = d.keys()
k_idx = np.argmin(np.abs(np.array(k) - z))
pts = d[k[k_idx]]
# visualize_boundary()
for kk in k:
pts = d[kk]
bpts = compute_boundary(pts)
cx, cy = 0.7, -0.6
rad = 0.4
# pts_in = point_contained(cx,cy,0.,rad,pts,
# start_angle=math.radians(140),
# end_angle=math.radians(190))
mpu.figure()
mpu.plot_yx(pts[1, :].A1, pts[0, :].A1, linewidth=0)
mpu.plot_yx(bpts[1, :].A1, bpts[0, :].A1, linewidth=0, color='y')
# mpu.plot_yx(pts_in[1,:].A1,pts_in[0,:].A1,linewidth=0,color='g')
# mpu.plot_yx([cy],[cx],linewidth=0,color='r')
mpu.show()
### apologies for the poor name. computes the translation and rotation
## of the torso frame that move the eq pt away from closest boundary
## and rotate such that local x axis is perp to mechanism
## returns 2x1 np matrix, angle
#def segway_motion_repulse(curr_pos_tl,cx_tl,cy_tl,cy_ts,start_pos_ts,
# eq_pt_tl,bndry):
# vec_bndry = vec_from_boundary(eq_pt_tl,bndry)
# dist_boundary = np.linalg.norm(vec_bndry)
# vec_bndry = vec_bndry/dist_boundary
#
def plot_radii(csv_data, color='#3366FF'):
keys = ['radius', 'type', 'name', 'repetition']
llists = expand(extract_keys(csv_data, keys), keys)
rad_list = np.array([float(r) for r in llists[0]]) / 100.0
types = llists[1]
names = np.array(llists[2])
all_types = set(types)
print 'Radii types', all_types
types_arr = np.array(types)
# np.where(types_arr == 'C')
cabinet_rad_list = rad_list[np.where(types_arr == 'C')[0]]
others_rad_list = rad_list[np.where(types_arr != 'C')[0]]
other_names = names[np.where(types_arr != 'C')[0]]
print '>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>'
print 'radii other names'
for i, n in enumerate(other_names):
print n, others_rad_list[i]
print '>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>'
rad_lists = [rad_list, cabinet_rad_list, others_rad_list]
titles = ['Radii of Rotary Mechanisms', 'Radii of Cabinets', 'Radii of Other Mechanisms']
bin_width = 0.05
max_radius = np.max(rad_list)
print 'MIN RADIUS', np.min(rad_list)
print 'MAX RADIUS', max_radius
mpu.set_figure_size(5.,5.)
for idx, radii in enumerate(rad_lists):
f = pb.figure()
f.set_facecolor('w')
bins = np.arange(0.-bin_width/2., max_radius+2*bin_width, bin_width)
hist, bin_edges = np.histogram(radii, bins)
h = mpu.plot_histogram(bin_edges[:-1]+bin_width/2., hist,
width=0.8*bin_width, xlabel='Radius (meters)',
ylabel='\# of mechanisms',
plot_title=titles[idx],
color=color, label='All')
pb.xlim(.1, 1.)
pb.ylim(0, 55)
mpu.legend(display_mode = 'less_space', handlelength=1.)
#-- different classes in different colors in the same histogram.
f = pb.figure()
f.set_facecolor('w')
bins = np.arange(0.-bin_width/2., max_radius+2*bin_width, bin_width)
hist, bin_edges = np.histogram(rad_lists[0], bins)
h = mpu.plot_histogram(bin_edges[:-1]+bin_width/2., hist,
width=0.8*bin_width, xlabel='Radius (meters)',
ylabel='\# of mechanisms',
plot_title=titles[1],
color='g', label='Cabinets')
hist, bin_edges = np.histogram(rad_lists[2], bins)
h = mpu.plot_histogram(bin_edges[:-1]+bin_width/2., hist,
width=0.8*bin_width, xlabel='Radius (meters)',
ylabel='\# of mechanisms',
plot_title='Cabinets and Other Mechanisms',
color='y', label='Other')
pb.xlim(.1, 1.)
pb.ylim(0, 55)
mpu.legend(display_mode = 'less_space', handlelength=1.)
color_list = ['g', 'b', 'r']
marker_list = ['s', '^', 'v']
label_list = ['All', 'Cabinets', 'Other']
scatter_size_list = [8, 5, 5]
mpu.set_figure_size(5.,5.)
mpu.figure()
for idx, radii in enumerate(rad_lists):
bins = np.arange(0.-bin_width/2., max_radius+2*bin_width, bin_width)
hist, bin_edges = np.histogram(radii, bins)
bin_midpoints = np.arange(0., max_radius+bin_width, bin_width)
mpu.plot_yx(hist, bin_midpoints, color = color_list[idx],
alpha = 0.6, marker = marker_list[idx],
scatter_size = scatter_size_list[idx], xlabel='Radius (meters)',
ylabel='\# of mechanisms', label = label_list[idx])
mpu.legend(display_mode = 'less_space')
pkl_name = glob.glob(opt.dir + '/combined_log*.pkl')[0]
mech_pkl_name = glob.glob(opt.dir + '/mechanism_info*.pkl')[0]
md = ut.load_pickle(mech_pkl_name)
cd = ut.load_pickle(pkl_name)
cd['hook_checker_number'] = md['checkerboard_number']
cd['radius'] = md['radius']
rad, tan, ang, typ = al.compute_mechanism_properties(cd)
rad, tan_b, ang, typ = al.compute_mechanism_properties(cd,
bias_ft=True)
#------------ plot spring scale and FT data -------------
spring_pkl = glob.glob(opt.dir+'/spring_scale*.pkl')[0]
spring_list = ut.load_pickle(spring_pkl)
mpu.plot_yx(spring_list, label='Spring Scale', color='b', axis=None)
mpu.plot_yx(rad, label='Measured Radial force (unbiased)', color='r',
xlabel='Reading number', ylabel='Force (N)', axis=None)
mpu.plot_yx(tan, label='Measured Tangential force (unbiased)', color='g',
xlabel='Reading number', ylabel='Force (N)', axis=None)
mpu.plot_yx(tan_b, label='Measured Tangential force (biased)', color='y',
xlabel='Reading number', ylabel='Force (N)',
axis=None, plot_title=opt.dir)
mpu.legend()
mpu.savefig(opt.dir.split('/')[0]+'.png')
mpu.show()
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)
subsample_ratio = 1
pts_2d_s = pts_2d[:,::subsample_ratio]
cep_force_clean, force_new = filter_trajectory_force(eq_cartesian,
d['force'])
cep_2d = np.matrix(cep_force_clean.p_list).T[0:2,reject_idx:]
# first draw the entire CEP and end effector trajectories
mpu.figure()
mpu.plot_yx(pts_2d_s[1,:].A1, pts_2d_s[0,:].A1, color='b',
label = 'FK', axis = 'equal', alpha = 1.0,
scatter_size=7, linewidth=0, marker='x',
marker_edge_width = 1.5)
cep_2d_s = cep_2d[:,::subsample_ratio]
mpu.plot_yx(cep_2d_s[1,:].A1, cep_2d_s[0,:].A1, color='g',
label = 'CEP', axis = 'equal', alpha = 1.0,
scatter_size=10, linewidth=0, marker='+',
marker_edge_width = 1.5)
mpu.plot_circle(cx, cy, rad, start_angle, end_angle,
label='Estimated Kinematics', color='r',
alpha=0.7)
mpu.plot_radii(cx, cy, rad, start_angle, end_angle,
interval=end_angle-start_angle, color='r',
alpha=0.7)
mpu.legend()
tup = ke.init_ros_node()
ma2 = compute_mech_angle_2(cd, tup, project_plane=False)
ma1 = compute_mech_angle_1(cd)
ma3 = compute_mech_angle_2(cd, tup, project_plane=True)
# ma4 = compute_mech_angle_1(cd, min_evec)
lab1 = 'orientation only'
lab2 = 'checker origin position + circle fit'
lab3 = 'checker origin position + PCA projection + circle fit'
# lab4 = 'PCA projection + orientation'
mpu.figure()
mpu.plot_yx(np.degrees(ma3), color='r', label=lab3,
linewidth = 1, scatter_size = 5)
mpu.plot_yx(np.degrees(ma2), color='b', label=lab2,
linewidth = 1, scatter_size = 5)
mpu.plot_yx(np.degrees(ma1), color='y', label=lab1,
linewidth = 1, scatter_size = 5)
mpu.legend()
vel3 = ma.compute_velocity(ma3, cd['time_list'], 1)
vel2 = ma.compute_velocity(ma2, cd['time_list'], 1)
vel1 = ma.compute_velocity(ma1, cd['time_list'], 1)
mpu.figure()
mpu.plot_yx(np.degrees(vel3), np.degrees(ma3), color='r',
label=lab3, linewidth = 1, scatter_size = 5)
mpu.plot_yx(np.degrees(vel2), np.degrees(ma2), color='b',
label=lab2, linewidth = 1, scatter_size = 5)
mpu.plot_yx(np.degrees(vel1), np.degrees(ma1), color='y',
#------------ compute radial and tangential forces ----------
pkl_name = glob.glob(opt.dir + '/combined_log*.pkl')[0]
mech_pkl_name = glob.glob(opt.dir + '/mechanism_info*.pkl')[0]
md = ut.load_pickle(mech_pkl_name)
cd = ut.load_pickle(pkl_name)
cd['hook_checker_number'] = md['checkerboard_number']
cd['radius'] = md['radius']
rad, tan, ang, typ = al.compute_mechanism_properties(cd)
rad, tan_b, ang, typ = al.compute_mechanism_properties(cd, bias_ft=True)
#------------ plot spring scale and FT data -------------
spring_pkl = glob.glob(opt.dir + '/spring_scale*.pkl')[0]
spring_list = ut.load_pickle(spring_pkl)
mpu.plot_yx(spring_list, label='Spring Scale', color='b', axis=None)
mpu.plot_yx(rad,
label='Measured Radial force (unbiased)',
color='r',
xlabel='Reading number',
ylabel='Force (N)',
axis=None)
mpu.plot_yx(tan,
label='Measured Tangential force (unbiased)',
color='g',
xlabel='Reading number',
ylabel='Force (N)',
axis=None)
mpu.plot_yx(tan_b,
if expt_plot:
pl.subplot(231)
# transform points so that the mechanism is in a fixed position.
start_pt = actual_cartesian.p_list[0]
x_diff = start_pt[0] - cx
y_diff = start_pt[1] - cy
angle = math.atan2(y_diff,x_diff) - math.radians(90)
rot_mat = tr.Rz(angle)[0:2,0:2]
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.,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('.')
def plot_radii(csv_data, color='#3366FF'):
keys = ['radius', 'type', 'name', 'repetition']
llists = expand(extract_keys(csv_data, keys), keys)
rad_list = np.array([float(r) for r in llists[0]]) / 100.0
types = llists[1]
names = np.array(llists[2])
all_types = set(types)
print 'Radii types', all_types
types_arr = np.array(types)
# np.where(types_arr == 'C')
cabinet_rad_list = rad_list[np.where(types_arr == 'C')[0]]
others_rad_list = rad_list[np.where(types_arr != 'C')[0]]
other_names = names[np.where(types_arr != 'C')[0]]
print '>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>'
print 'radii other names'
for i, n in enumerate(other_names):
print n, others_rad_list[i]
print '>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>'
rad_lists = [rad_list, cabinet_rad_list, others_rad_list]
titles = [
'Radii of Rotary Mechanisms', 'Radii of Cabinets',
'Radii of Other Mechanisms'
]
bin_width = 0.05
max_radius = np.max(rad_list)
print 'MIN RADIUS', np.min(rad_list)
print 'MAX RADIUS', max_radius
mpu.set_figure_size(5., 5.)
for idx, radii in enumerate(rad_lists):
f = pb.figure()
f.set_facecolor('w')
bins = np.arange(0. - bin_width / 2., max_radius + 2 * bin_width,
bin_width)
hist, bin_edges = np.histogram(radii, bins)
h = mpu.plot_histogram(bin_edges[:-1] + bin_width / 2.,
hist,
width=0.8 * bin_width,
xlabel='Radius (meters)',
ylabel='\# of mechanisms',
plot_title=titles[idx],
color=color,
label='All')
pb.xlim(.1, 1.)
pb.ylim(0, 55)
mpu.legend(display_mode='less_space', handlelength=1.)
#-- different classes in different colors in the same histogram.
f = pb.figure()
f.set_facecolor('w')
bins = np.arange(0. - bin_width / 2., max_radius + 2 * bin_width,
bin_width)
hist, bin_edges = np.histogram(rad_lists[0], bins)
h = mpu.plot_histogram(bin_edges[:-1] + bin_width / 2.,
hist,
width=0.8 * bin_width,
xlabel='Radius (meters)',
ylabel='\# of mechanisms',
plot_title=titles[1],
color='g',
label='Cabinets')
hist, bin_edges = np.histogram(rad_lists[2], bins)
h = mpu.plot_histogram(bin_edges[:-1] + bin_width / 2.,
hist,
width=0.8 * bin_width,
xlabel='Radius (meters)',
ylabel='\# of mechanisms',
plot_title='Cabinets and Other Mechanisms',
color='y',
label='Other')
pb.xlim(.1, 1.)
pb.ylim(0, 55)
mpu.legend(display_mode='less_space', handlelength=1.)
color_list = ['g', 'b', 'r']
marker_list = ['s', '^', 'v']
label_list = ['All', 'Cabinets', 'Other']
scatter_size_list = [8, 5, 5]
mpu.set_figure_size(5., 5.)
mpu.figure()
for idx, radii in enumerate(rad_lists):
bins = np.arange(0. - bin_width / 2., max_radius + 2 * bin_width,
bin_width)
hist, bin_edges = np.histogram(radii, bins)
bin_midpoints = np.arange(0., max_radius + bin_width, bin_width)
mpu.plot_yx(hist,
bin_midpoints,
color=color_list[idx],
alpha=0.6,
marker=marker_list[idx],
scatter_size=scatter_size_list[idx],
xlabel='Radius (meters)',
ylabel='\# of mechanisms',
label=label_list[idx])
mpu.legend(display_mode='less_space')
def split_forces_hooktip_test(hand_mat):
kin_dict = ke.fit(hand_mat, tup)
center_hand = np.matrix((kin_dict['cx'], kin_dict['cy'],
kin_dict['cz'])).T
print 'kin_dict:', kin_dict
# radial vectors.
radial_mat = hand_mat - center_hand
radial_mat = radial_mat / ut.norm(radial_mat)
# cannot use hook tip to compute mechanism angle because I
# don't have a way of knowing when the hook starts opening the
# mechanism. (Think hook makes contact with door, moves in
# freespace and then makes contact with the handle.)
#start_rad = radial_mat[:,0]
#ct = (start_rad.T * radial_mat).A1
#st = ut.norm(np.matrix(np.cross(start_rad.A1, radial_mat.T.A)).T).A1
#mech_angle_l = np.arctan2(st, ct).tolist()
_, nrm_hand = project_points_plane(hand_mat)
print 'nrm_hand:', nrm_hand.A1
f_cam_l = []
m_cam_l = []
m_base_l = []
frad_l = []
ftan_l = []
hook_num = cd['hook_checker_number']
print 'hook_num:', hook_num
for i, f in enumerate(force_mat.T):
f = f.T
m = moment_mat[:,i]
f_cam, m_cam, m_base = ft_to_camera_3(f, m, hook_rot_l[i], hook_num,
return_moment_cam = True)
f_cam_l.append(f_cam)
m_cam_l.append(abs((m_cam.T*nrm_hand)[0,0]))
m_base_l.append(abs((m_base.T*nrm_hand)[0,0]))
#m_base_l.append(np.linalg.norm(f))
tangential_vec = np.matrix(np.cross(radial_mat[:,i].A1, nrm_hand.A1)).T
ftan = (f_cam.T * tangential_vec)[0,0]
ftan_l.append(ftan)
#frad = np.linalg.norm(f_cam - tangential_vec * ftan)
frad = (f_cam.T*radial_mat[:,i])[0,0]
frad_l.append(abs(frad))
fig1 = mpu.figure()
mech_ang_deg = np.degrees(mech_angle_l)
mpu.plot_yx(ftan_l, mech_ang_deg, label='Tangential Force (hook tip)', color='b')
mpu.plot_yx(frad_l, mech_ang_deg, label='Radial Force (hook tip)', color='y')
mech_pkl_name = glob.glob(opt.dir + '/open_mechanism_trajectories_*.pkl')[0]
md = ut.load_pickle(mech_pkl_name)
radial_mech = md['force_rad_list']
tangential_mech = md['force_tan_list']
mech_x = np.degrees(md['mechanism_x'])
mpu.plot_yx(tangential_mech, mech_x, label='Tangential Force (mechanism checker)', color='g')
mpu.plot_yx(radial_mech, mech_x, label='Radial Force (mechanism checker)', color='r')
mpu.legend()
fig2 = mpu.figure()
mpu.plot_yx(m_cam_l, mech_ang_deg, label='\huge{$m_{axis}$}')
mpu.plot_yx(m_base_l, mech_ang_deg, label='\huge{$m^s$}',
color = 'r')
mpu.legend()
mpu.show()
ll = zip(key_list, l)
ll.sort()
key_list, l = zip(*ll)
if ha == 0:
label = 'Hook Left'
elif abs(ha - math.pi / 2) < 0.01:
label = 'Hook Down'
continue
else:
label = 'Hook Up'
mpu.plot_yx(key_list,
l,
axis=None,
label=label,
color=color_list[i],
xlabel='\# of points with IK soln',
ylabel='Height (m)',
scatter_size=8)
i += 1
max_idx = np.argmax(l)
good_height = key_list[max_idx]
print 'good_height:', good_height
mpu.plot_yx([good_height], [l[max_idx]],
axis=None,
color='r',
xlabel='\# of points with IK soln',
ylabel='Height (m)',
scatter_size=8)
d['height'] = good_height
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)
subsample_ratio = 1
pts_2d_s = pts_2d[:,::subsample_ratio]
cep_force_clean, force_new = filter_trajectory_force(eq_cartesian,
d['force'])
cep_2d = np.matrix(cep_force_clean.p_list).T[0:2,reject_idx:]
# first draw the entire CEP and end effector trajectories
mpu.figure()
mpu.plot_yx(pts_2d_s[1,:].A1, pts_2d_s[0,:].A1, color='b',
label = '\huge{End Effector Trajectory}', axis = 'equal', alpha = 1.0,
scatter_size=7, linewidth=0, marker='x',
marker_edge_width = 1.5)
cep_2d_s = cep_2d[:,::subsample_ratio]
mpu.plot_yx(cep_2d_s[1,:].A1, cep_2d_s[0,:].A1, color='g',
label = '\huge{Equilibrium Point Trajectory}', axis = 'equal', alpha = 1.0,
scatter_size=10, linewidth=0, marker='+',
marker_edge_width = 1.5)
mpu.plot_circle(cx, cy, rad, start_angle, end_angle,
label='\huge{Estimated Kinematics}', color='r',
alpha=0.7)
mpu.plot_radii(cx, cy, rad, start_angle, end_angle,
interval=end_angle-start_angle, color='r',
alpha=0.7)
mpu.legend()