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
