def make_joint_traj(xyzs, quats, manip, ref_frame, targ_frame, filter_options = 0): "do ik and then fill in the points where ik failed" n = len(xyzs) assert len(quats) == n robot = manip.GetRobot() joint_inds = manip.GetArmJoints() robot.SetActiveDOFs(joint_inds) orig_joint = robot.GetActiveDOFValues() joints = [] inds = [] for i in xrange(0,n): mat4 = conv.trans_rot_to_hmat(xyzs[i], quats[i]) joint = PR2.cart_to_joint(manip, mat4, ref_frame, targ_frame, filter_options) if joint is not None: joints.append(joint) inds.append(i) robot.SetActiveDOFValues(joint) robot.SetActiveDOFValues(orig_joint) rospy.loginfo("found ik soln for %i of %i points",len(inds), n) if len(inds) > 2: joints2 = mu.interp2d(np.arange(n), inds, joints) return joints2, inds else: return np.zeros((n, len(joints))), []
def get_base_positions_bimanual_resampled(rars, joints): n_orig = len(rars) rars_ds = rars[::10] n_ds = len(rars_ds) base_xys_ds = get_base_positions_bimanual(rars_ds, joints) base_xys = mu.interp2d(np.linspace(0,1,n_orig), np.linspace(0,1,n_ds), base_xys_ds) return base_xys