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)
LOG.info("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 check_ik (lr, tfm):
"""
Checks if tfm is valid.
Uses openrave IK.
"""
global T_bf_hb, hydra_rel_tfm, env, robot, manips
# Initialize a bunch of parameters
if T_bf_hb is None or not hydra_rel_tfm:
ik_file = osp.join(feedback_dir, ik_feedback_name)
with open(ik_file,'r') as fh: data = cPickle.load(fh)
T_bf_hb = data['pr2']
hydra_rel_tfm['right'] = data['right']
hydra_rel_tfm['left'] = data['left']
if env is None:
env = opr.Environment()
env.Load('robots/pr2-beta-static.zae')
robot = env.GetRobots()[0]
manips = {'left':robot.GetManipulator('leftarm'),
'right':robot.GetManipulator('rightarm')}
l_vals = PR2.Arm.L_POSTURES['side']
robot.SetDOFValues(l_vals, manips['left'].GetArmIndices())
robot.SetDOFValues(PR2.mirror_arm_joints(l_vals), manips['left'].GetArmIndices())
# Maybe add table
ee_tfm = T_bf_hb.dot(tfm).dot(hydra_rel_tfm[lr]).dot(tfm_gtf_ee)
# account for height difference
ee_tfm[2,3] -= HEIGHT_DIFF
# ee_tfm[0,3] += X_DIFF
#z: 1.30
print lr, ee_tfm
#hack for peace of mind -- I want to be able to hear myself think while I do this
if ee_tfm[0,3] < 0.25:
return True
else:
print ee_tfm[0,3]
iksol = manips[lr].FindIKSolution(opr.IkParameterization(ee_tfm, opr.IkParameterizationType.Transform6D),
opr.IkFilterOptions.IgnoreEndEffectorEnvCollisions)
return iksol != None
