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.GetArmIndices()
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 make_joint_traj(xyzs, quats, joint_seeds, manip, ref_frame, targ_frame,
filter_options):
"""
do ik to make a joint trajectory
joint_seeds are the joint angles that this function will try to be close to
I put that in so you could try to stay near the demonstration joint angles
in retrospect, using that argument might be bad idea because it'll make the trajectory much more jerky in some situations
(TODO)
"""
n = len(xyzs)
assert len(quats) == n
robot = manip.GetRobot()
robot.SetActiveManipulator(manip.GetName())
joint_inds = manip.GetArmIndices()
orig_joint = robot.GetDOFValues(joint_inds)
joints = []
inds = []
for i in xrange(0, n):
mat4 = conv.trans_rot_to_hmat(xyzs[i], quats[i])
robot.SetDOFValues(joint_seeds[i], joint_inds)
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.SetDOFValues(joint, joint_inds)
robot.SetDOFValues(orig_joint, joint_inds)
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 make_joint_traj(xyzs, quats, joint_seeds,manip, ref_frame, targ_frame,filter_options):
"""
do ik to make a joint trajectory
joint_seeds are the joint angles that this function will try to be close to
I put that in so you could try to stay near the demonstration joint angles
in retrospect, using that argument might be bad idea because it'll make the trajectory much more jerky in some situations
(TODO)
"""
n = len(xyzs)
assert len(quats) == n
robot = manip.GetRobot()
robot.SetActiveManipulator(manip.GetName())
joint_inds = manip.GetArmIndices()
orig_joint = robot.GetDOFValues(joint_inds)
joints = []
inds = []
for i in xrange(0,n):
mat4 = conv.trans_rot_to_hmat(xyzs[i], quats[i])
robot.SetDOFValues(joint_seeds[i], joint_inds)
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.SetDOFValues(joint, joint_inds)
robot.SetDOFValues(orig_joint, joint_inds)
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 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.GetArmIndices()
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))), []
