def force_trajectory_in_hindsight(pull_dict, mechanism_type, pr2_log):
print '_________________________________________________'
print 'starting force_trajectory_in_hindsight'
print '_________________________________________________'
if not pr2_log:
## print pull_dict.keys() # Note problem!!
arm = pull_dict['arm']
## print 'arm:', arm
## act_tl = at.joint_to_cartesian(pull_dict['actual'], arm)
traj = at.JointTrajectory()
traj.q_list = pull_dict['actual_JT_q_list']
traj.qdot_list = pull_dict['actual_JT_qdot_list']
traj.qdotdot_list = pull_dict['actual_JT_qdotdot_list']
traj.time_list = pull_dict['actual_JT_time_list']
act_tl = at.joint_to_cartesian(traj, arm)
## force_tl = pull_dict['force']
force_tl = at.ForceTrajectory()
force_tl.time_list = pull_dict['force_FT_time_list']
force_tl.f_list = pull_dict['force_FT_f_list']
actual_cartesian, force_ts = act_tl, force_tl
#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]
#################################################################
if mechanism_type == 'rotary':
# Estimate mechanism kinematics parameters
r, cx, cy = estimate_mechanism_kinematics(pull_dict, pr2_log)
print 'rad, cx, cy:', r, cx, cy
# Get angle and tangential force lists (1xN, 1xN)
frad_list,ftan_list,_ = at.compute_radial_tangential_forces(f_list,
p_list,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) # unit initial c-ee vector
config_list = []
for p in p_list:
rvec = np.matrix((p[0]-cx, p[1]-cy)).T
rvec = rvec / np.linalg.norm(rvec)
ang = np.arccos((rvec.T*rad_vec_init)[0,0])
if np.isnan(ang):
ang = 0
config_list.append(ang)
else:
p0 = p_list[0]
ftan_list, config_list = [], []
for f, p in zip(f_list, p_list):
config_list.append(p0[0] - p[0])
ftan_list.append(abs(f[0]))
return config_list, ftan_list
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 estimate_mechanism_kinematics(pull_dict, pr2_log):
if not pr2_log:
## act_tl = at.joint_to_cartesian(pull_dict['actual'], pull_dict['arm'])
traj = at.JointTrajectory()
traj.q_list = pull_dict['actual_JT_q_list']
traj.qdot_list = pull_dict['actual_JT_qdot_list']
traj.qdotdot_list = pull_dict['actual_JT_qdotdot_list']
traj.time_list = pull_dict['actual_JT_time_list']
act_tl = at.joint_to_cartesian(traj, pull_dict['arm'])
## force_tl = pull_dict['force']
force_tl = at.ForceTrajectory()
force_tl.time_list = pull_dict['force_FT_time_list']
force_tl.f_list = pull_dict['force_FT_f_list']
actual_cartesian, force_ts = act_tl, force_tl
#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'])
cartesian_force_clean, _ = at.filter_trajectory_force(
actual_cartesian, force_tl)
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] # N x dim
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 force_trajectory_in_hindsight(pull_dict, mechanism_type, pr2_log):
print '_________________________________________________'
print 'starting force_trajectory_in_hindsight'
print '_________________________________________________'
if not pr2_log:
arm = pull_dict['arm']
act_tl = at.joint_to_cartesian(pull_dict['actual'], 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]
if mechanism_type == 'rotary':
r, cx, cy = estimate_mechanism_kinematics(pull_dict, pr2_log)
print 'rad, cx, cy:', r, cx, cy
frad_list,ftan_list,_ = at.compute_radial_tangential_forces(f_list,
p_list,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)
config_list = []
for p in p_list:
rvec = np.matrix((p[0]-cx, p[1]-cy)).T
rvec = rvec / np.linalg.norm(rvec)
ang = np.arccos((rvec.T*rad_vec_init)[0,0])
if np.isnan(ang):
ang = 0
config_list.append(ang)
else:
p0 = p_list[0]
ftan_list, config_list = [], []
for f, p in zip(f_list, p_list):
config_list.append(p0[0] - p[0])
ftan_list.append(abs(f[0]))
return config_list, ftan_list
def force_trajectory_in_hindsight(pull_dict, mechanism_type, pr2_log):
print '_________________________________________________'
print 'starting force_trajectory_in_hindsight'
print '_________________________________________________'
if not pr2_log:
arm = pull_dict['arm']
act_tl = at.joint_to_cartesian(pull_dict['actual'], 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]
if mechanism_type == 'rotary':
r, cx, cy = estimate_mechanism_kinematics(pull_dict, pr2_log)
print 'rad, cx, cy:', r, cx, cy
frad_list, ftan_list, _ = at.compute_radial_tangential_forces(
f_list, p_list, 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)
config_list = []
for p in p_list:
rvec = np.matrix((p[0] - cx, p[1] - cy)).T
rvec = rvec / np.linalg.norm(rvec)
ang = np.arccos((rvec.T * rad_vec_init)[0, 0])
if np.isnan(ang):
ang = 0
config_list.append(ang)
else:
p0 = p_list[0]
ftan_list, config_list = [], []
for f, p in zip(f_list, p_list):
config_list.append(p0[0] - p[0])
ftan_list.append(abs(f[0]))
return config_list, ftan_list
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)
argv = sys.argv
fname = argv[1]
d = ut.load_pickle(fname)
st = d['segway']
ee_tl = at.joint_to_cartesian(d['actual'])
ee_ts = at.account_segway_motion(ee_tl,st)
eq_tl = at.joint_to_cartesian(d['eq_pt'])
eq_ts = at.account_segway_motion(eq_tl,st)
bndry = d['bndry']
wrkspc_pts = d['wrkspc']
rad_guess = 1.0
start_pos_ts = np.matrix(ee_ts.p_list[0]).T
x_guess = start_pos_ts[0,0]
y_guess = start_pos_ts[1,0] - rad_guess
plot_single_point()
#calc_motion_all()
def force_trajectory_in_hindsight(pull_dict, mechanism_type, pr2_log):
print '_________________________________________________'
print 'starting force_trajectory_in_hindsight'
print '_________________________________________________'
if not pr2_log:
## print pull_dict.keys() # Note problem!!
arm = pull_dict['arm']
## print 'arm:', arm
## act_tl = at.joint_to_cartesian(pull_dict['actual'], arm)
traj = at.JointTrajectory()
traj.q_list = pull_dict['actual_JT_q_list']
traj.qdot_list = pull_dict['actual_JT_qdot_list']
traj.qdotdot_list = pull_dict['actual_JT_qdotdot_list']
traj.time_list = pull_dict['actual_JT_time_list']
act_tl = at.joint_to_cartesian(traj, arm)
## force_tl = pull_dict['force']
force_tl = at.ForceTrajectory()
force_tl.time_list = pull_dict['force_FT_time_list']
force_tl.f_list = pull_dict['force_FT_f_list']
actual_cartesian, force_ts = act_tl, force_tl
#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]
#################################################################
if mechanism_type == 'rotary':
# Estimate mechanism kinematics parameters
r, cx, cy = estimate_mechanism_kinematics(pull_dict, pr2_log)
print 'rad, cx, cy:', r, cx, cy
# Get angle and tangential force lists (1xN, 1xN)
frad_list, ftan_list, _ = at.compute_radial_tangential_forces(
f_list, p_list, 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) # unit initial c-ee vector
config_list = []
for p in p_list:
rvec = np.matrix((p[0] - cx, p[1] - cy)).T
rvec = rvec / np.linalg.norm(rvec)
ang = np.arccos((rvec.T * rad_vec_init)[0, 0])
if np.isnan(ang):
ang = 0
config_list.append(ang)
else:
p0 = p_list[0]
ftan_list, config_list = [], []
for f, p in zip(f_list, p_list):
config_list.append(p0[0] - p[0])
ftan_list.append(abs(f[0]))
return config_list, ftan_list
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