def fit_spline_to_tf_stream(strm, new_freq, deg=3):
"""
Interpolates a stream of transforms using splines, such that
there is no "NONE" when sampling the stream at NEW_FREQ frequency.
Returns a stream of transforms.
"""
tfs, ts = strm.get_data()
tstart = strm.get_start_time()
tmax = ts[-1]
ndt = 1./new_freq
new_ts = np.arange(tstart, tmax+ndt/4., ndt/2.)
## get data in xyzrpy format (6xn) matrix:
N = len(tfs)
tf_dat = np.empty((6, N))
tf_dat[0:3,0] = tfs[0][0:3,3]
tf_dat[3:6,0] = tfms.euler_from_matrix(tfs[0])
for i in xrange(1,N):
now_tf = tfs[i]
tf_dat[0:3,i] = now_tf[0:3,3]
prev_rpy = tf_dat[3:6,i-1]
now_rpy = tfms.euler_from_matrix(now_tf)
now_rpy = closer_angle(now_rpy, prev_rpy)
tf_dat[3:6,i] = now_rpy
blueprint("\t fitting spline to data (scipy) ..")
s = N*.001**2
(tck, _) = si.splprep(tf_dat, s=s, u=ts, k=deg)
blueprint("\t evaluating spline at new time-stamps ..")
interp_xyzrpys = np.r_[si.splev(new_ts, tck)].T
smooth_tfms = []
for xyzrpy in interp_xyzrpys:
tfm = tfms.euler_matrix(*xyzrpy[3:6])
tfm[0:3,3] = xyzrpy[0:3]
smooth_tfms.append(tfm)
return streamize(smooth_tfms, new_ts, new_freq, strm.favg, tstart)
def plan_follow_traj(
robot,
manip_name,
ee_link,
new_hmats,
old_traj,
rope_cloud=None,
rope_constraint_thresh=0.01,
end_pose_constraint=False,
):
n_steps = len(new_hmats)
assert old_traj.shape[0] == n_steps
assert old_traj.shape[1] == 7
arm_inds = robot.GetManipulator(manip_name).GetArmIndices()
ee_linkname = ee_link.GetName()
init_traj = old_traj.copy()
# init_traj[0] = robot.GetDOFValues(arm_inds)
end_pose = openravepy.poseFromMatrix(new_hmats[-1])
request = {
"basic_info": {"n_steps": n_steps, "manip": manip_name, "start_fixed": False},
"costs": [
{"type": "joint_vel", "params": {"coeffs": [n_steps / 5.0]}},
{"type": "collision", "params": {"coeffs": [1], "dist_pen": [0.01]}},
],
"constraints": [],
"init_info": {"type": "given_traj", "data": [x.tolist() for x in init_traj]},
}
request["costs"] += [
{"type": "collision", "name": "cont_coll", "params": {"coeffs": [50], "dist_pen": [0.05]}},
{"type": "collision", "name": "col", "params": {"continuous": False, "coeffs": [20], "dist_pen": [0.02]}},
]
# impose that the robot goes to final ee tfm at last ts
# the constraint works only when the arm is the 'grasp' arm; otherwise only cost is added
# if end_pose_constraint or not is_fake_motion(new_hmats, 0.1):
# hack to avoid missing grasp
if rope_cloud != None:
closest_point = find_closest_point(rope_cloud, end_pose[4:7])
dist = np.linalg.norm(end_pose[4:7] - closest_point)
if dist > rope_constraint_thresh and dist < 0.05:
end_pose[4:7] = closest_point
redprint("grasp hack is active, dist = %f" % dist)
else:
blueprint("grasp hack is inactive, dist = %f" % dist)
# raw_input()
request["constraints"] += [
{
"type": "pose",
"params": {
"xyz": end_pose[4:7].tolist(),
"wxyz": end_pose[0:4].tolist(),
"link": ee_linkname,
"pos_coeffs": [100, 100, 100],
"rot_coeffs": [10, 10, 10],
},
}
]
poses = [openravepy.poseFromMatrix(hmat) for hmat in new_hmats]
for (i_step, pose) in enumerate(poses):
request["costs"].append(
{
"type": "pose",
"params": {
"xyz": pose[4:7].tolist(),
"wxyz": pose[0:4].tolist(),
"link": ee_linkname,
"timestep": i_step,
"pos_coeffs": [100, 100, 100],
"rot_coeffs": [10, 10, 10],
},
}
)
s = json.dumps(request)
prob = trajoptpy.ConstructProblem(s, robot.GetEnv()) # create object that stores optimization problem
result = trajoptpy.OptimizeProblem(prob) # do optimization
traj = result.GetTraj()
saver = openravepy.RobotStateSaver(robot)
pos_errs = []
with saver:
for i_step in xrange(1, n_steps):
row = traj[i_step]
robot.SetDOFValues(row, arm_inds)
tf = ee_link.GetTransform()
pos = tf[:3, 3]
pos_err = np.linalg.norm(poses[i_step][4:7] - pos)
pos_errs.append(pos_err)
pos_errs = np.array(pos_errs)
print "planned trajectory for %s. max position error: %.3f. all position errors: %s" % (
manip_name,
pos_errs.max(),
pos_errs,
)
return traj