def adaptive_resample2(x, t = None, max_err = np.inf, max_dx = np.inf, max_dt = np.inf, normfunc = None):
"""
SLOW VERSION
"""
#return np.arange(0, len(x), 100)
LOG.info("resampling a path of length %i", len(x))
x = np.asarray(x)
if x.ndim == 1: x = x[:,None]
else: assert x.ndim == 2
if normfunc is None: normfunc = np.linalg.norm
if t is None: t = np.arange(len(x))
else: t = np.asarray(t)
assert(t.ndim == 1)
n = len(t)
assert len(x) == n
import networkx as nx
g = nx.DiGraph()
g.add_nodes_from(xrange(n))
for i_src in xrange(n):
for i_targ in xrange(i_src+1, n):
normdx = normfunc(x[i_targ] - x[i_src])
dt = t[i_targ] - t[i_src]
errs = lerp(x[i_src], x[i_targ], (t[i_src:i_targ+1] - t[i_src])/dt) - x[i_src:i_targ+1]
interp_err = errs.__abs__().max()#np.array([normfunc(err) for err in errs]).max()
if normdx > max_dx or dt > max_dt or interp_err > max_err: break
g.add_edge(i_src, i_targ)
resample_inds = nx.shortest_path(g, 0, n-1)
return resample_inds
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 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 adaptive_resample(x, t = None, max_err = np.inf, max_dx = np.inf, max_dt = np.inf, normfunc = None):
"""
SLOW VERSION
"""
#return np.arange(0, len(x), 100)
LOG.info("resampling a path of length %i", len(x))
x = np.asarray(x)
if x.ndim == 1: x = x[:,None]
else: assert x.ndim == 2
if normfunc is None: normfunc = np.linalg.norm
if t is None: t = np.arange(len(x))
else: t = np.asarray(t)
assert(t.ndim == 1)
n = len(t)
assert len(x) == n
import networkx as nx
g = nx.DiGraph()
g.add_nodes_from(xrange(n))
for i_src in xrange(n):
for i_targ in xrange(i_src+1, n):
normdx = normfunc(x[i_targ] - x[i_src])
dt = t[i_targ] - t[i_src]
errs = lerp(x[i_src], x[i_targ], (t[i_src:i_targ+1] - t[i_src])/dt) - x[i_src:i_targ+1]
interp_err = errs.__abs__().max()#np.array([normfunc(err) for err in errs]).max()
if normdx > max_dx or dt > max_dt or interp_err > max_err: break
g.add_edge(i_src, i_targ)
resample_inds = nx.shortest_path(g, 0, n-1)
return resample_inds
def dt_code_to_knot(dt_code):
def dt_code_to_knot_wrapper(q, x):
result = _dt_code_to_knot(x)
q.put(result)
q.close()
q = multiprocessing.Queue(1)
proc = multiprocessing.Process(target=dt_code_to_knot_wrapper, args=(q, dt_code))
proc.start()
TIMEOUT = 5
try:
result = q.get(True, TIMEOUT)
except:
LOG.warn("Timeout for knot identification exceeded, assuming no knot")
result = None
finally:
proc.terminate()
return result
def compute_dt_code(ctl_pts, plotting=False):
"""Takes rope control points (Nx3 array), closes the loop, and computes the Dowker-Thistlethwaite code for the knot.
The z-value for the points are used for determining over/undercrossings.
Follows procedure outlined here: http://katlas.math.toronto.edu/wiki/DT_(Dowker-Thistlethwaite)_Codes
"""
# First, close the loop by introducing extra points under the table and toward the robot (by subtracting z and x values)
# first_pt, last_pt = ctl_pts[0], ctl_pts[-1]
# flipped = False
# if first_pt[1] > last_pt[1]:
# first_pt, last_pt = last_pt, first_pt
# flipped = True
# min_z = ctl_pts[:,2].min()
# extra_first_pt, extra_last_pt = first_pt + [-.1, -.1, min_z-1], last_pt + [-.1, .1, min_z-1]
# if flipped:
# extra_pts = [extra_first_pt, extra_first_pt + [-1, 0, 0], extra_last_pt + [-1, 0, 0], extra_last_pt, last_pt]
# else:
# extra_pts = [extra_last_pt, extra_last_pt + [-1, 0, 0], extra_first_pt + [-1, 0, 0], extra_first_pt, first_pt]
# ctl_pts = np.append(ctl_pts, extra_pts, axis=0)
if plotting:
import trajoptpy, openravepy
env = openravepy.Environment()
viewer = trajoptpy.GetViewer(env)
handles = []
handles.append(env.plot3(ctl_pts, 5, [0, 0, 1]))
viewer.Idle()
# Upsampling loop: upsample until every segment has at most one crossing
need_upsample_ind = None
upsample_iters = 0
max_upsample_iters = 10
while True:
counter = 1
crossings = defaultdict(list)
# Walk along rope: for each segment, compute intersections with all other segments
for i in range(len(ctl_pts) - 1):
curr_seg = ctl_pts[i:i+2,:]
intersections, ts, us = intersect_segs(ctl_pts[:,:2], curr_seg[:,:2])
if len(intersections) == 0:
continue
if len(intersections) != 1:
LOG.debug('warning: more than one intersection for segment %d, now upsampling', i)
need_upsample_ind = i
break
# for each intersection, determine and record over/undercrossing
i_int = intersections[0]
if plotting:
handles.append(env.drawlinestrip(ctl_pts[i_int:i_int+2], 5, [1, 0, 0]))
int_point_rope = ctl_pts[i_int] + ts[i_int]*(ctl_pts[i_int+1] - ctl_pts[i_int])
int_point_curr_seg = curr_seg[0] + us[i_int]*(curr_seg[1] - curr_seg[0])
#assert np.allclose(int_point_rope[:2], int_point_curr_seg[:2])
above = int_point_curr_seg[2] > int_point_rope[2]
crossings[tuple(sorted((i, i_int)))].append(-counter if counter % 2 == 0 and above else counter)
counter += 1
if plotting: viewer.Idle()
# upsample if necessary
if need_upsample_ind is not None and upsample_iters < max_upsample_iters:
spacing = np.linspace(0, 1, len(ctl_pts))
new_spacing = np.insert(spacing, need_upsample_ind+1, (spacing[need_upsample_ind]+spacing[need_upsample_ind+1])/2.)
ctl_pts = math_utils.interp2d(new_spacing, spacing, ctl_pts)
upsample_iters += 1
need_upsample = None
continue
break
# Extract relevant part of crossing data to produce DT code
out = []
for pair in crossings.itervalues():
assert len(pair) == 2
odd = [p for p in pair if p % 2 == 1][0]
even = [p for p in pair if p % 2 == 0][0]
out.append((odd, even))
out.sort()
dt_code = [-o[1] for o in out]
return dt_code
def follow_body_traj(pr2, bodypart2traj, wait=True, base_frame = "/base_footprint", speed_factor=1):
LOG.info("following trajectory with bodyparts %s", " ".join(bodypart2traj.keys()))
name2part = {"lgrip":pr2.lgrip,
"rgrip":pr2.rgrip,
"larm":pr2.larm,
"rarm":pr2.rarm,
"base":pr2.base}
for partname in bodypart2traj:
if partname not in name2part:
raise Exception("invalid part name %s"%partname)
#### Go to initial positions #######
for (name, part) in name2part.items():
if name in bodypart2traj:
part_traj = bodypart2traj[name]
if name == "lgrip" or name == "rgrip":
part.set_angle(np.squeeze(part_traj)[0])
elif name == "larm" or name == "rarm":
part.goto_joint_positions(part_traj[0])
elif name == "base":
part.goto_pose(part_traj[0], base_frame)
pr2.join_all()
#### Construct total trajectory so we can retime it #######
n_dof = 0
trajectories = []
vel_limits = []
acc_limits = []
bodypart2inds = {}
for (name, part) in name2part.items():
if name in bodypart2traj:
traj = bodypart2traj[name]
if traj.ndim == 1: traj = traj.reshape(-1,1)
trajectories.append(traj)
vel_limits.extend(part.vel_limits)
acc_limits.extend(part.acc_limits)
bodypart2inds[name] = range(n_dof, n_dof+part.n_joints)
n_dof += part.n_joints
trajectories = np.concatenate(trajectories, 1)
vel_limits = np.array(vel_limits)*speed_factor
times = retiming.retime_with_vel_limits(trajectories, vel_limits)
times_up = np.linspace(0, times[-1], int(np.ceil(times[-1]/.1)))
traj_up = mu.interp2d(times_up, times, trajectories)
print traj_up.shape
#### Send all part trajectories ###########
for (name, part) in name2part.items():
if name in bodypart2traj:
part_traj = traj_up[:,bodypart2inds[name]]
if name == "lgrip" or name == "rgrip":
part.follow_timed_trajectory(times_up, part_traj.flatten())
elif name == "larm" or name == "rarm":
vels = resampling.get_velocities(part_traj, times_up, .001)
part.follow_timed_joint_trajectory(part_traj, vels, times_up)
elif name == "base":
part.follow_timed_trajectory(times_up, part_traj, base_frame)
if wait: pr2.join_all()
return True
def follow_body_traj(pr2, bodypart2traj, wait=True, base_frame = "/base_footprint", speed_factor=1):
LOG.info("following trajectory with bodyparts %s", " ".join(bodypart2traj.keys()))
name2part = {"lgrip":pr2.lgrip,
"rgrip":pr2.rgrip,
"larm":pr2.larm,
"rarm":pr2.rarm,
"base":pr2.base}
for partname in bodypart2traj:
if partname not in name2part:
raise Exception("invalid part name %s"%partname)
#### Go to initial positions #######
for (name, part) in name2part.items():
if name in bodypart2traj:
part_traj = bodypart2traj[name]
if name == "lgrip" or name == "rgrip":
part.set_angle(np.squeeze(part_traj)[0])
elif name == "larm" or name == "rarm":
part.goto_joint_positions(part_traj[0])
elif name == "base":
part.goto_pose(part_traj[0], base_frame)
pr2.join_all()
#### Construct total trajectory so we can retime it #######
n_dof = 0
trajectories = []
vel_limits = []
acc_limits = []
bodypart2inds = {}
for (name, part) in name2part.items():
if name in bodypart2traj:
traj = bodypart2traj[name]
if traj.ndim == 1: traj = traj.reshape(-1,1)
trajectories.append(traj)
vel_limits.extend(part.vel_limits)
acc_limits.extend(part.acc_limits)
bodypart2inds[name] = range(n_dof, n_dof+part.n_joints)
n_dof += part.n_joints
trajectories = np.concatenate(trajectories, 1)
vel_limits = np.array(vel_limits)*speed_factor
times = retiming.retime_with_vel_limits(trajectories, vel_limits)
times_up = np.linspace(0, times[-1], int(np.ceil(times[-1]/.1)))
traj_up = mu.interp2d(times_up, times, trajectories)
#### Send all part trajectories ###########
for (name, part) in name2part.items():
if name in bodypart2traj:
part_traj = traj_up[:,bodypart2inds[name]]
if name == "lgrip" or name == "rgrip":
part.follow_timed_trajectory(times_up, part_traj.flatten())
elif name == "larm" or name == "rarm":
vels = resampling.get_velocities(part_traj, times_up, .001)
part.follow_timed_joint_trajectory(part_traj, vels, times_up)
elif name == "base":
part.follow_timed_trajectory(times_up, part_traj, base_frame)
if wait: pr2.join_all()
return True