def interp_hmats(newtimes, oldtimes, oldhmats):
oldposes = openravepy.poseFromMatrices(oldhmats)
newposes = np.empty((len(newtimes), 7))
newposes[:,4:7] = mu.interp2d(newtimes, oldtimes, oldposes[:,4:7])
newposes[:,0:4] = interp_quats(newtimes, oldtimes, oldposes[:,0:4])
newhmats = openravepy.matrixFromPoses(newposes)
return newhmats
def interp_hmats(newtimes, oldtimes, oldhmats):
oldposes = openravepy.poseFromMatrices(oldhmats)
newposes = np.empty((len(newtimes), 7))
newposes[:,4:7] = mu.interp2d(newtimes, oldtimes, oldposes[:,4:7])
newposes[:,0:4] = interp_quats(newtimes, oldtimes, oldposes[:,0:4])
newhmats = openravepy.matrixFromPoses(newposes)
return newhmats
def interp_hmats(newtimes, oldtimes, oldhmats):
oldposes = openravepy.poseFromMatrices(oldhmats)
for i in xrange(1, len(oldposes)):
qp = oldposes[i-1,0:4]
qc = oldposes[i,0:4]
if np.linalg.norm(qp + qc) < np.linalg.norm(qp-qc):
oldposes[i,0:4] = -qc
newposes = np.empty((len(newtimes), 7))
newposes[:,4:7] = mu.interp2d(newtimes, oldtimes, oldposes[:,4:7])
newposes[:,0:4] = interp_quats(newtimes, oldtimes, oldposes[:,0:4])
newhmats = openravepy.matrixFromPoses(newposes)
return newhmats
def pose_traj_request(robot, hmats):
n_steps = len(hmats)
request = {
"basic_info": {
"n_steps": n_steps,
"manip": "leftarm+rightarm+torso_lift_joint+base",
"start_fixed": False
},
"costs": [{
"type": "joint_vel",
"params": {
"coeffs": np.r_[np.ones(15), 10 * np.ones(3)].tolist()
}
}, {
"type": "collision",
"params": {
"coeffs": [.1],
"dist_pen": [0.01]
}
}],
"constraints": [],
"init_info": {
"type": "stationary"
}
}
poses = rave.poseFromMatrices(hmats)
xyzs = poses[:, 4:7]
quats = poses[:, 0:4]
for i in xrange(1, n_steps - 1):
print "waypoint xyz", xyzs[i]
waypoint_cnt = {
"type": "pose",
"name": "waypoint_pose",
"params": {
"xyz": list(xyzs[i]),
"wxyz": list(quats[i]),
"link": "l_gripper_tool_frame",
"pos_coeffs": [1, 1, 1],
"rot_coeffs": [1, 1, 1],
"timestep": i
}
}
request["constraints"].append(waypoint_cnt)
return request
def pose_traj_request(robot, hmats):
n_steps = len(hmats)
request = {
"basic_info" : {
"n_steps" : n_steps,
"manip" : "leftarm+rightarm+torso_lift_joint+base",
"start_fixed" : False
},
"costs" : [
{
"type" : "joint_vel",
"params": {"coeffs" : np.r_[np.ones(15), 10*np.ones(3)].tolist()}
},
{
"type" : "collision",
"params" : {"coeffs" : [.1],"dist_pen" : [0.01]}
}
],
"constraints" : [
],
"init_info" : {
"type" : "stationary"
}
}
poses = rave.poseFromMatrices(hmats)
xyzs = poses[:,4:7]
quats = poses[:,0:4]
for i in xrange(1, n_steps-1):
print "waypoint xyz", xyzs[i]
waypoint_cnt = {
"type" : "pose",
"name" : "waypoint_pose",
"params" : {
"xyz" : list(xyzs[i]),
"wxyz" : list(quats[i]),
"link" : "l_gripper_tool_frame",
"pos_coeffs" : [1,1,1],
"rot_coeffs" : [1,1,1],
"timestep" : i
}}
request["constraints"].append(waypoint_cnt)
return request
def do_traj_ik_graph_search_opt(pr2, lr, gripper_poses):
gripper_poses = [pose for i, pose in enumerate(gripper_poses) if i % 20 == 0]
graph_search_res = do_traj_ik_graph_search(pr2, lr, gripper_poses)
hmats = [juc.pose_to_hmat(pose) for pose in gripper_poses]
poses = rave.poseFromMatrices(hmats)
quats = poses[:,:4]
xyzs = poses[:,4:7]
manip_name = {"r":"rightarm", "l":"leftarm"}[lr]
request = get_ik_request(manip_name, "%s_gripper_tool_frame"%lr,
xyzs[-1], quats[-1], graph_search_res, xyzs, quats)
s = json.dumps(request)
print "REQUEST:", s
pr2.robot.SetDOFValues(graph_search_res[0], pr2.robot.GetManipulator(manip_name).GetArmIndices())
trajoptpy.SetInteractive(True);
prob = trajoptpy.ConstructProblem(s, pr2.env)
result = trajoptpy.OptimizeProblem(prob)
