def estimate_mechanism_kinematics(pull_dict, pr2_log):
if not pr2_log:
act_tl = at.joint_to_cartesian(pull_dict['actual'], pull_dict['arm'])
force_tl = pull_dict['force']
actual_cartesian, force_ts = at.account_segway_motion(act_tl,
force_tl, pull_dict['segway'])
cartesian_force_clean, _ = at.filter_trajectory_force(actual_cartesian,
pull_dict['force'])
pts_list = actual_cartesian.p_list
pts_2d, reject_idx = at.filter_cartesian_trajectory(cartesian_force_clean)
else:
# not performing force filtering for PR2 trajectories.
# might have to add that in later.
p_list, f_list = pull_dict['ee_list'], pull_dict['f_list']
p_list = p_list[::2]
f_list = f_list[::2]
pts = np.matrix(p_list).T
px = pts[0,:].A1
py = pts[1,:].A1
pts_2d = np.matrix(np.row_stack((px, py)))
#rad = 0.4
rad = 1.1
x_guess = pts_2d[0,0]
y_guess = pts_2d[1,0] - rad
rad_guess = rad
rad, cx, cy = at.fit_circle(rad_guess,x_guess,y_guess,pts_2d,
method='fmin_bfgs',verbose=False,
rad_fix=False)
#rad, cx, cy = rad_guess, x_guess, y_guess
return rad, cx, cy
def online_force_with_radius(pull_dict, pr2_log, radius_err = 0.,
with_prior = True):
if not pr2_log:
act_tl = at.joint_to_cartesian(pull_dict['actual'], pull_dict['arm'])
force_tl = pull_dict['force']
actual_cartesian, force_ts = at.account_segway_motion(act_tl,
force_tl, pull_dict['segway'])
p_list = actual_cartesian.p_list
f_list = force_ts.f_list
else:
p_list, f_list = pull_dict['ee_list'], pull_dict['f_list']
p_list = p_list[::2]
f_list = f_list[::2]
radius, _, _ = estimate_mechanism_kinematics(pull_dict, pr2_log)
radius += radius_err
print '_________________________________________________'
print 'using radius:', radius
print '_________________________________________________'
pts_list = []
ftan_list = []
config_list = []
for f,p in zip(f_list, p_list):
pts_list.append(p)
pts_2d = (np.matrix(pts_list).T)[0:2,:]
x_guess = pts_list[0][0]
y_guess = pts_list[0][1] - radius
rad_guess = radius
if with_prior:
rad, cx, cy = at.fit_circle_priors(rad_guess, x_guess,
y_guess, pts_2d, sigma_r = 0.2, sigma_xy = 0.2,
sigma_pts = 0.01, verbose=False)
else:
rad, cx, cy = at.fit_circle(rad_guess,x_guess,y_guess,pts_2d,
method='fmin_bfgs',verbose=False,
rad_fix=True)
print 'rad, cx, cy:', rad, cx, cy
p0 = p_list[0]
rad_vec_init = np.matrix((p0[0]-cx, p0[1]-cy)).T
rad_vec_init = rad_vec_init / np.linalg.norm(rad_vec_init)
rad_vec = np.array([p[0]-cx,p[1]-cy])
rad_vec = rad_vec/np.linalg.norm(rad_vec)
ang = np.arccos((rad_vec.T*rad_vec_init)[0,0])
config_list.append(ang)
tan_vec = (np.matrix([[0,-1],[1,0]]) * np.matrix(rad_vec).T).A1
f_vec = np.array([f[0],f[1]])
f_tan_mag = abs(np.dot(f_vec, tan_vec))
ftan_list.append(f_tan_mag)
return config_list, ftan_list
def calc_motion_all():
for i in range(len(ee_tl.p_list)):
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)
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]
vt,a = smc.segway_motion_repulse(curr_pos_tl,cx_tl,cy_tl,cy_ts,
start_pos_ts,eq_pt_tl,bndry)
print 'angle:',math.degrees(a)
def plot_single_point():
n_pts = 115
pts_ts = np.matrix(ee_ts.p_list[0:n_pts]).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)
print 'rad_opt,cx_ts,cy_ts:',rad_opt,cx_ts,cy_ts
c_ts = np.matrix([cx_ts,cy_ts,0.]).T
x,y,a = st.x_list[n_pts-1],st.y_list[n_pts-1],st.a_list[n_pts-1]
c_tl = smc.tlTts(c_ts,x,y,a)
cx_tl,cy_tl = c_tl[0,0],c_tl[1,0]
curr_pos_tl = np.matrix(ee_tl.p_list[n_pts-1]).T
eqpt_tl = np.matrix(eq_tl.p_list[n_pts-1]).T
plot_hook_translation(curr_pos_tl,cx_tl,cy_tl,cy_ts,start_pos_ts,
eqpt_tl,bndry,wrkspc_pts)
def circle_estimator(self):
self.run_fit_circle_thread = True
print 'Starting the circle estimating thread.'
while self.run_fit_circle_thread:
self.fit_circle_lock.acquire()
if len(self.cartesian_pts_list)==0:
self.fit_circle_lock.release()
continue
pts_2d = (np.matrix(self.cartesian_pts_list).T)[0:2,:]
self.fit_circle_lock.release()
rad = self.rad_guess
start_pos = self.firenze.FK('right_arm',self.pull_trajectory.q_list[0])
rad,cx,cy = at.fit_circle(rad,start_pos[0,0],start_pos[1,0]-rad,pts_2d,method='fmin_bfgs',verbose=False)
rad = ut.bound(rad,3.0,0.1)
self.fit_circle_lock.acquire()
self.cx = cx
self.cy = cy
# self.rad = rad
self.fit_circle_lock.release()
print 'Ended the circle estimating thread.'
def online_force_with_radius(pull_dict,
pr2_log,
radius_err=0.,
with_prior=True):
if not pr2_log:
act_tl = at.joint_to_cartesian(pull_dict['actual'], pull_dict['arm'])
force_tl = pull_dict['force']
actual_cartesian, force_ts = at.account_segway_motion(
act_tl, force_tl, pull_dict['segway'])
p_list = actual_cartesian.p_list
f_list = force_ts.f_list
else:
p_list, f_list = pull_dict['ee_list'], pull_dict['f_list']
p_list = p_list[::2]
f_list = f_list[::2]
radius, _, _ = estimate_mechanism_kinematics(pull_dict, pr2_log)
radius += radius_err
print '_________________________________________________'
print 'using radius:', radius
print '_________________________________________________'
pts_list = []
ftan_list = []
config_list = []
for f, p in zip(f_list, p_list):
pts_list.append(p)
pts_2d = (np.matrix(pts_list).T)[0:2, :]
x_guess = pts_list[0][0]
y_guess = pts_list[0][1] - radius
rad_guess = radius
if with_prior:
rad, cx, cy = at.fit_circle_priors(rad_guess,
x_guess,
y_guess,
pts_2d,
sigma_r=0.2,
sigma_xy=0.2,
sigma_pts=0.01,
verbose=False)
else:
rad, cx, cy = at.fit_circle(rad_guess,
x_guess,
y_guess,
pts_2d,
method='fmin_bfgs',
verbose=False,
rad_fix=True)
print 'rad, cx, cy:', rad, cx, cy
p0 = p_list[0]
rad_vec_init = np.matrix((p0[0] - cx, p0[1] - cy)).T
rad_vec_init = rad_vec_init / np.linalg.norm(rad_vec_init)
rad_vec = np.array([p[0] - cx, p[1] - cy])
rad_vec = rad_vec / np.linalg.norm(rad_vec)
ang = np.arccos((rad_vec.T * rad_vec_init)[0, 0])
config_list.append(ang)
tan_vec = (np.matrix([[0, -1], [1, 0]]) * np.matrix(rad_vec).T).A1
f_vec = np.array([f[0], f[1]])
f_tan_mag = abs(np.dot(f_vec, tan_vec))
ftan_list.append(f_tan_mag)
return config_list, ftan_list
