def get_lin_interp_poses(start_pos, end_pos, n_steps):
xyz_start, quat_start = juc.hmat_to_trans_rot(start_pos)
xyz_end, quat_end = juc.hmat_to_trans_rot(end_pos)
xyzs = math_utils.linspace2d(xyz_start, xyz_end, n_steps)
quats = [rave.quatSlerp(quat_start, quat_end, t) for t in np.linspace(0, 1, n_steps)]
hmats = [rave.matrixFromPose(np.r_[quat[3], quat[:3], xyz]) for (xyz, quat) in zip(xyzs, quats)]
gripper_poses = np.array([juc.hmat_to_pose(hmat) for hmat in hmats])
return gripper_poses
def rotation_interpolation(r1, r2, t):
quat1 = rave.quatFromRotationMatrix(r1)
quat2 = rave.quatFromRotationMatrix(r2)
# print(r1)
# print(r2)
t = min(max(t, 0), 1)
quat_interpolate = rave.quatSlerp(quat1, quat2, t, True)
return rave.matrixFromQuat(quat_interpolate)[0:3, 0:3]
def interp_quats(newtimes, oldtimes, oldquats):
u_ioldtimes = np.interp(newtimes, oldtimes, range(len(oldtimes)))
newquats = np.empty((len(u_ioldtimes), oldquats.shape[1]))
for i, u_ioldtime in enumerate(u_ioldtimes):
u, ioldtime = np.modf(u_ioldtime)
if ioldtime+1 < oldquats.shape[0]:
newquats[i,:] = openravepy.quatSlerp(oldquats[ioldtime,:], oldquats[ioldtime+1,:], u)
else: # the last u is zero by definition
newquats[i,:] = oldquats[-1,:]
return newquats
def interp_quats(newtimes, oldtimes, oldquats):
u_ioldtimes = np.interp(newtimes, oldtimes, range(len(oldtimes)))
newquats = np.empty((len(u_ioldtimes), oldquats.shape[1]))
for i, u_ioldtime in enumerate(u_ioldtimes):
u, ioldtime = np.modf(u_ioldtime)
ioldtime = int(ioldtime)
if ioldtime+1 < oldquats.shape[0]:
# SH: adding a random "True" to quatSlerp function parameters,
# to fix weird binding error
newquats[i,:] = openravepy.quatSlerp(oldquats[ioldtime,:], oldquats[ioldtime+1,:], u, True)
else: # the last u is zero by definition
newquats[i,:] = oldquats[-1,:]
return newquats
def move_arm_straight_request(manip, n_steps, link_name, joints_start, joints_end, xyz_start, quat_start, xyz_end, quat_end):
'''
WARNING: start/end joints, xyz, and quats must all agree, otherwise the optimization problem will be infeasible!
'''
init_joints = mu.linspace2d(joints_start, joints_end, n_steps)
request = {
'basic_info': {
'n_steps': n_steps,
'manip': manip.GetName(),
'start_fixed': False, # take care of this with the init_joints constraint instead
},
'costs': [
{ 'type': 'joint_vel', 'params': { 'coeffs': [1] } },
{ 'type': 'collision', 'params': { 'coeffs': [1], 'dist_pen': [0.025] } },
],
'constraints': [
{
'type' : 'joint',
'name' : 'init_joints',
'params' : { 'vals': list(joints_start), 'timestep': 0 },
},
{
'type' : 'joint',
'name' : 'final_joints',
'params' : { 'vals': list(joints_end) },
},
],
'init_info': {
'type': 'given_traj',
'data': [j.tolist() for j in init_joints],
},
}
# add costs for deviation from a straight line in cartesian space
lin_xyzs = mu.linspace2d(xyz_start, xyz_end, n_steps)
lin_quats = [rave.quatSlerp(quat_start, quat_end, t) for t in np.linspace(0, 1, n_steps)]
for i in range(1, n_steps-1):
request['costs'].append({
'type': 'pose',
'params': {
'xyz': list(lin_xyzs[i]),
'wxyz': list(lin_quats[i]),
'pos_coeffs': [1, 1, 1],
'rot_coeffs': [1, 1, 1],
'link': link_name,
'timestep': i,
}
})
return request
def get_lin_interp_poses(start_pos, end_pos, n_steps):
xyz_start, quat_start = juc.hmat_to_trans_rot(start_pos)
xyz_end, quat_end = juc.hmat_to_trans_rot(end_pos)
xyzs = math_utils.linspace2d(xyz_start, xyz_end, n_steps)
quats = [
rave.quatSlerp(quat_start, quat_end, t)
for t in np.linspace(0, 1, n_steps)
]
hmats = [
rave.matrixFromPose(np.r_[quat[3], quat[:3], xyz])
for (xyz, quat) in zip(xyzs, quats)
]
gripper_poses = np.array([juc.hmat_to_pose(hmat) for hmat in hmats])
return gripper_poses
def move_arm_straight_request(manip, n_steps, link_name, joints_start,
joints_end, xyz_start, quat_start, xyz_end,
quat_end):
'''
WARNING: start/end joints, xyz, and quats must all agree, otherwise the optimization problem will be infeasible!
'''
init_joints = mu.linspace2d(joints_start, joints_end, n_steps)
request = {
'basic_info': {
'n_steps': n_steps,
'manip': manip.GetName(),
'start_fixed':
False, # take care of this with the init_joints constraint instead
},
'costs': [
{
'type': 'joint_vel',
'params': {
'coeffs': [1]
}
},
{
'type': 'collision',
'params': {
'coeffs': [1],
'dist_pen': [0.025]
}
},
],
'constraints': [
{
'type': 'joint',
'name': 'init_joints',
'params': {
'vals': list(joints_start),
'timestep': 0
},
},
{
'type': 'joint',
'name': 'final_joints',
'params': {
'vals': list(joints_end)
},
},
],
'init_info': {
'type': 'given_traj',
'data': [j.tolist() for j in init_joints],
},
}
# add costs for deviation from a straight line in cartesian space
lin_xyzs = mu.linspace2d(xyz_start, xyz_end, n_steps)
lin_quats = [
rave.quatSlerp(quat_start, quat_end, t)
for t in np.linspace(0, 1, n_steps)
]
for i in range(1, n_steps - 1):
request['costs'].append({
'type': 'pose',
'params': {
'xyz': list(lin_xyzs[i]),
'wxyz': list(lin_quats[i]),
'pos_coeffs': [1, 1, 1],
'rot_coeffs': [1, 1, 1],
'link': link_name,
'timestep': i,
}
})
return request
def quat_interpolate(quat1, quat2, t=.5):
return quatSlerp(quat1, quat2, t,
True) # TODO - not sure what the boolean argument is for
def move_arm_straight_request(manip, n_steps, link_name, xyz_start, xyz_end, quat_start, quat_end, start_joints):
manip_name = manip.GetName()
request = {
"basic_info": {"n_steps": n_steps, "manip": manip_name, "start_fixed": True},
"costs": [
{"type": "joint_vel", "params": {"coeffs": [1]}},
{"type": "collision", "params": {"coeffs": [10], "dist_pen": [0.025]}},
],
"constraints": [
{
"type": "pose",
"name": "final_pose",
"params": {
"pos_coeffs": [10, 10, 10],
"rot_coeffs": [10, 10, 10],
"xyz": list(xyz_end),
"wxyz": list(quat_end),
"link": link_name,
},
}
],
"init_info": {},
}
xyzs = mu.linspace2d(xyz_start, xyz_end, n_steps)
quats = [rave.quatSlerp(quat_start, quat_end, t) for t in np.linspace(0, 1, n_steps)]
hmats = [rave.matrixFromPose(np.r_[quat, xyz]) for (xyz, quat) in zip(xyzs, quats)]
def ikfunc(hmat):
return ku.ik_for_link(hmat, manip, link_name, return_all_solns=True)
def nodecost(joints):
robot = manip.GetRobot()
saver = rave.Robot.RobotStateSaver(robot)
robot.SetDOFValues(joints, manip.GetArmJoints())
return 100 * robot.GetEnv().CheckCollision(robot)
paths, costs, timesteps = ku.traj_cart2joint(hmats, ikfunc, start_joints=start_joints, nodecost=nodecost)
i_best = np.argmin(costs)
print "lowest cost of initial trajs:", costs[i_best]
if len(timesteps) < n_steps:
print "timesteps with soln: ", timesteps
print "linearly interpolating the rest"
path_init = mu.interp2d(np.arange(n_steps), timesteps, paths[i_best])
else:
print "all timesteps have an ik solution"
path_init = paths[i_best]
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": link_name,
# "pos_coeffs" : [10,10,10],
# "rot_coeffs" : [10,10,10],
"timestep": i,
},
}
request["constraints"].append(waypoint_cnt)
request["init_info"]["type"] = "given_traj"
request["init_info"]["data"] = [x.tolist() for x in path_init]
return request
def quat_interpolate(quat1, quat2, t=.5):
return quatSlerp(quat1, quat2, t, True)
def quat_interpolate(quat1, quat2, t=.5): return quatSlerp(quat1, quat2, t, True) # TODO - not sure what the boolean argument is for