def show_ik_solns(part_name, manip, i, warped_demo):
frame = "%s_gripper_tool_frame"%part_name[0]
hmat = conv.trans_rot_to_hmat(warped_demo[frame]['position'][i], warped_demo[frame]['orientation'][i])
solns = traj_ik_graph_search.ik_for_link(hmat, manip, frame, return_all_solns=True)
# newrobots = []
print 'Displaying', len(solns), 'IK solutions for', part_name, 'at time', i
print solns
def show_ik_solns(part_name, manip, i, warped_demo):
frame = "%s_gripper_tool_frame" % part_name[0]
hmat = conv.trans_rot_to_hmat(warped_demo[frame]['position'][i],
warped_demo[frame]['orientation'][i])
solns = traj_ik_graph_search.ik_for_link(hmat,
manip,
frame,
return_all_solns=True)
# newrobots = []
print 'Displaying', len(solns), 'IK solutions for', part_name, 'at time', i
print solns
n_iter = 1000
rand_joints = []
for i in range(n_iter):
rand_joints.append(lower + numpy.random.rand(len(lower)) * (upper - lower))
rand_hmats = [
conv.trans_rot_to_hmat(
*traj_opt.joints2xyzquat(manip, 'l_gripper_tool_frame', j))
for j in rand_joints
]
start_time = time.time()
solns = []
for hmat in Bar().iter(rand_hmats):
s = numpy.array(
traj_ik_graph_search.ik_for_link(hmat,
manip,
'l_gripper_tool_frame',
return_all_solns=True))
if s.size == 0: continue
solns.append(s)
end_time = time.time()
print 'ik time: ', end_time - start_time, ' | ', (end_time -
start_time) / n_iter, 'each'
start_time = time.time()
for i in Bar().iter(range(len(solns) - 1)):
a, b, c = traj_ik_graph_search.build_graph_part(None, solns[i],
solns[i + 1])
end_time = time.time()
print 'pairwise dist time: ', end_time - start_time, ' | ', (
end_time - start_time) / n_iter, 'each'
robot=env.GetRobots()[0]
manip = robot.SetActiveManipulator('leftarm')
lower,upper = robot.GetDOFLimits(manip.GetArmIndices()) # get the limits of just the arm indices
ikmodel = databases.inversekinematics.InverseKinematicsModel(robot,iktype=IkParameterization.Type.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
n_iter = 1000
rand_joints = []
for i in range(n_iter):
rand_joints.append(lower+numpy.random.rand(len(lower))*(upper-lower))
rand_hmats = [conv.trans_rot_to_hmat(*traj_opt.joints2xyzquat(manip, 'l_gripper_tool_frame', j)) for j in rand_joints]
start_time = time.time()
solns = []
for hmat in Bar().iter(rand_hmats):
s = numpy.array(traj_ik_graph_search.ik_for_link(hmat, manip, 'l_gripper_tool_frame', return_all_solns=True))
if s.size == 0: continue
solns.append(s)
end_time = time.time()
print 'ik time: ', end_time-start_time, ' | ', (end_time-start_time)/n_iter, 'each'
start_time = time.time()
for i in Bar().iter(range(len(solns)-1)):
a, b, c = traj_ik_graph_search.build_graph_part(None, solns[i], solns[i+1])
end_time = time.time()
print 'pairwise dist time: ', end_time-start_time, ' | ', (end_time-start_time)/n_iter, 'each'
