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