def retime_traj(robot,
inds,
traj,
max_cart_vel=.02,
max_finger_vel=.02,
upsample_time=.1):
"""retime a trajectory so that it executes slowly enough for the simulation"""
cart_traj = np.empty((len(traj), 6))
finger_traj = np.empty((len(traj), 2))
leftarm, rightarm = robot.GetManipulator("leftarm"), robot.GetManipulator(
"rightarm")
with robot:
for i in range(len(traj)):
robot.SetDOFValues(traj[i], inds)
cart_traj[i, :3] = leftarm.GetTransform()[:3, 3]
cart_traj[i, 3:] = rightarm.GetTransform()[:3, 3]
finger_traj[i, :1] = robot.GetDOFValues(
leftarm.GetGripperIndices())
finger_traj[i,
1:] = robot.GetDOFValues(rightarm.GetGripperIndices())
times = retime_with_vel_limits(
np.c_[cart_traj, finger_traj], np.r_[np.repeat(max_cart_vel, 6),
np.repeat(max_finger_vel, 2)])
times_up = np.linspace(
0, times[-1], times[-1] /
upsample_time) if times[-1] > upsample_time else times
traj_up = math_utils.interp2d(times_up, times, traj)
return traj_up
def traj_collisions(sim_env, full_traj, collision_dist_threshold, upsample=0):
"""
Returns the set of collisions.
manip = Manipulator or list of indices
"""
traj, dof_inds = full_traj
sim_util.unwrap_in_place(traj, dof_inds=dof_inds)
if upsample > 0:
traj_up = mu.interp2d(np.linspace(0,1,upsample), np.linspace(0,1,len(traj)), traj)
else:
traj_up = traj
cc = trajoptpy.GetCollisionChecker(sim_env.env)
with openravepy.RobotStateSaver(sim_env.robot):
sim_env.robot.SetActiveDOFs(dof_inds)
col_times = []
for (i,row) in enumerate(traj_up):
sim_env.robot.SetActiveDOFValues(row)
col_now = cc.BodyVsAll(sim_env.robot)
#with util.suppress_stdout():
# col_now2 = cc.PlotCollisionGeometry()
col_now = [cn for cn in col_now if cn.GetDistance() < collision_dist_threshold]
if col_now:
#print [cn.GetDistance() for cn in col_now]
col_times.append(i)
#print "trajopt.CollisionChecker: ", len(col_now)
#print col_now2
return col_times
def get_resampled_traj(self, timesteps_rs):
lr2arm_traj_rs = None if self.lr2arm_traj is None else {}
lr2finger_traj_rs = None if self.lr2finger_traj is None else {}
for (lr2traj_rs, self_lr2traj) in [(lr2arm_traj_rs, self.lr2arm_traj), (lr2finger_traj_rs, self.lr2finger_traj)]:
if self_lr2traj is None:
continue
for lr in self_lr2traj.keys():
lr2traj_rs[lr] = mu.interp2d(timesteps_rs, np.arange(len(self_lr2traj[lr])), self_lr2traj[lr])
if self.lr2ee_traj is None:
lr2ee_traj_rs = None
else:
lr2ee_traj_rs = {}
for lr in self.lr2ee_traj.keys():
lr2ee_traj_rs[lr] = np.asarray(resampling.interp_hmats(timesteps_rs, np.arange(len(self.lr2ee_traj[lr])), self.lr2ee_traj[lr]))
lr2open_finger_traj_rs = None if self.lr2open_finger_traj is None else {}
lr2close_finger_traj_rs = None if self.lr2close_finger_traj is None else {}
for (lr2oc_finger_traj_rs, self_lr2oc_finger_traj) in [(lr2open_finger_traj_rs, self.lr2open_finger_traj), (lr2close_finger_traj_rs, self.lr2close_finger_traj)]:
if self_lr2oc_finger_traj is None:
continue
for lr in self_lr2oc_finger_traj.keys():
self_oc_finger_traj = self_lr2oc_finger_traj[lr]
self_oc_inds = np.where(self_oc_finger_traj)[0]
oc_inds_rs = np.abs(timesteps_rs[:,None] - self_oc_inds[None,:]).argmin(axis=0)
oc_finger_traj_rs = np.zeros(len(timesteps_rs), dtype=bool)
oc_finger_traj_rs[oc_inds_rs] = True
lr2oc_finger_traj_rs[lr] = oc_finger_traj_rs
return AugmentedTrajectory(lr2arm_traj=lr2arm_traj_rs, lr2finger_traj=lr2finger_traj_rs, lr2ee_traj=lr2ee_traj_rs, lr2open_finger_traj=lr2open_finger_traj_rs, lr2close_finger_traj=lr2close_finger_traj_rs)
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 traj_collisions(sim_env, full_traj, collision_dist_threshold, upsample=0):
"""
Returns the set of collisions.
manip = Manipulator or list of indices
"""
traj, dof_inds = full_traj
sim_util.unwrap_in_place(traj, dof_inds=dof_inds)
if upsample > 0:
traj_up = mu.interp2d(np.linspace(0, 1, upsample),
np.linspace(0, 1, len(traj)), traj)
else:
traj_up = traj
cc = trajoptpy.GetCollisionChecker(sim_env.env)
with openravepy.RobotStateSaver(sim_env.robot):
sim_env.robot.SetActiveDOFs(dof_inds)
col_times = []
for (i, row) in enumerate(traj_up):
sim_env.robot.SetActiveDOFValues(row)
col_now = cc.BodyVsAll(sim_env.robot)
#with util.suppress_stdout():
# col_now2 = cc.PlotCollisionGeometry()
col_now = [
cn for cn in col_now
if cn.GetDistance() < collision_dist_threshold
]
if col_now:
#print [cn.GetDistance() for cn in col_now]
col_times.append(i)
#print "trajopt.CollisionChecker: ", len(col_now)
#print col_now2
return col_times
def observe_cloud(pts, radius, upsample=0, upsample_rad=1):
"""
If upsample > 0, the number of points along the rope's backbone is resampled to be upsample points
If upsample_rad > 1, the number of points perpendicular to the backbone points is resampled to be upsample_rad points, around the rope's cross-section
The total number of points is then: (upsample if upsample > 0 else len(self.rope.GetControlPoints())) * upsample_rad
"""
if upsample > 0:
lengths = np.r_[0, np.apply_along_axis(np.linalg.norm, 1, np.diff(pts, axis=0))]
summed_lengths = np.cumsum(lengths)
assert len(lengths) == len(pts)
pts = math_utils.interp2d(np.linspace(0, summed_lengths[-1], upsample), summed_lengths, pts)
if upsample_rad > 1:
# add points perpendicular to the points in pts around the rope's cross-section
vs = np.diff(pts, axis=0) # vectors between the current and next points
vs /= np.apply_along_axis(np.linalg.norm, 1, vs)[:,None]
perp_vs = np.c_[-vs[:,1], vs[:,0], np.zeros(vs.shape[0])] # perpendicular vectors between the current and next points in the xy-plane
perp_vs /= np.apply_along_axis(np.linalg.norm, 1, perp_vs)[:,None]
vs = np.r_[vs, vs[-1,:][None,:]] # define the vector of the last point to be the same as the second to last one
perp_vs = np.r_[perp_vs, perp_vs[-1,:][None,:]] # define the perpendicular vector of the last point to be the same as the second to last one
perp_pts = []
from openravepy import matrixFromAxisAngle
for theta in np.linspace(0, 2*np.pi, upsample_rad, endpoint=False): # uniformly around the cross-section circumference
for (center, rot_axis, perp_v) in zip(pts, vs, perp_vs):
rot = matrixFromAxisAngle(rot_axis, theta)[:3,:3]
perp_pts.append(center + rot.T.dot(radius * perp_v))
pts = np.array(perp_pts)
return pts
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 follow_joint_trajectory(self, traj):
traj = np.r_[np.atleast_2d(self.get_joint_positions()), traj]
for i in [2,4,6]:
traj[:,i] = np.unwrap(traj[:,i])
times = retiming.retime_with_vel_limits(traj, self.vel_limits)
times_up = np.arange(0,times[-1],.1)
traj_up = mu.interp2d(times_up, times, traj)
vels = resampling.get_velocities(traj_up, times_up, .001)
self.follow_timed_joint_trajectory(traj_up, vels, times_up)
def follow_joint_trajectory(self, traj):
traj = np.r_[np.atleast_2d(self.get_joint_positions()), traj]
for i in [2, 4, 6]:
traj[:, i] = np.unwrap(traj[:, i])
times = retiming.retime_with_vel_limits(traj, self.vel_limits)
times_up = np.arange(0, times[-1], .1)
traj_up = mu.interp2d(times_up, times, traj)
vels = resampling.get_velocities(traj_up, times_up, .001)
self.follow_timed_joint_trajectory(traj_up, vels, times_up)
def get_resampled_traj(self, timesteps_rs):
lr2arm_traj_rs = None if self.lr2arm_traj is None else {}
lr2finger_traj_rs = None if self.lr2finger_traj is None else {}
for (lr2traj_rs, self_lr2traj) in [(lr2arm_traj_rs, self.lr2arm_traj),
(lr2finger_traj_rs,
self.lr2finger_traj)]:
if self_lr2traj is None:
continue
for lr in self_lr2traj.keys():
lr2traj_rs[lr] = mu.interp2d(timesteps_rs,
np.arange(len(self_lr2traj[lr])),
self_lr2traj[lr])
if self.lr2ee_traj is None:
lr2ee_traj_rs = None
else:
lr2ee_traj_rs = {}
for lr in self.lr2ee_traj.keys():
lr2ee_traj_rs[lr] = np.asarray(
resampling.interp_hmats(
timesteps_rs, np.arange(len(self.lr2ee_traj[lr])),
self.lr2ee_traj[lr]))
lr2open_finger_traj_rs = None if self.lr2open_finger_traj is None else {}
lr2close_finger_traj_rs = None if self.lr2close_finger_traj is None else {}
for (lr2oc_finger_traj_rs, self_lr2oc_finger_traj) in [
(lr2open_finger_traj_rs, self.lr2open_finger_traj),
(lr2close_finger_traj_rs, self.lr2close_finger_traj)
]:
if self_lr2oc_finger_traj is None:
continue
for lr in self_lr2oc_finger_traj.keys():
self_oc_finger_traj = self_lr2oc_finger_traj[lr]
self_oc_inds = np.where(self_oc_finger_traj)[0]
oc_inds_rs = np.abs(timesteps_rs[:, None] -
self_oc_inds[None, :]).argmin(axis=0)
oc_finger_traj_rs = np.zeros(len(timesteps_rs), dtype=bool)
oc_finger_traj_rs[oc_inds_rs] = True
lr2oc_finger_traj_rs[lr] = oc_finger_traj_rs
return AugmentedTrajectory(
lr2arm_traj=lr2arm_traj_rs,
lr2finger_traj=lr2finger_traj_rs,
lr2ee_traj=lr2ee_traj_rs,
lr2open_finger_traj=lr2open_finger_traj_rs,
lr2close_finger_traj=lr2close_finger_traj_rs)
def retime_traj(robot, inds, traj, max_cart_vel=.02, max_finger_vel=.02, upsample_time=.1):
"""retime a trajectory so that it executes slowly enough for the simulation"""
cart_traj = np.empty((len(traj), 6))
finger_traj = np.empty((len(traj),2))
leftarm, rightarm = robot.GetManipulator("leftarm"), robot.GetManipulator("rightarm")
with robot:
for i in range(len(traj)):
robot.SetDOFValues(traj[i], inds)
cart_traj[i,:3] = leftarm.GetTransform()[:3,3]
cart_traj[i,3:] = rightarm.GetTransform()[:3,3]
finger_traj[i,:1] = robot.GetDOFValues(leftarm.GetGripperIndices())
finger_traj[i,1:] = robot.GetDOFValues(rightarm.GetGripperIndices())
times = retiming.retime_with_vel_limits(np.c_[cart_traj, finger_traj], np.r_[np.repeat(max_cart_vel, 6),np.repeat(max_finger_vel,2)])
times_up = np.linspace(0, times[-1], times[-1]/upsample_time) if times[-1] > upsample_time else times
traj_up = math_utils.interp2d(times_up, times, traj)
return traj_up
def observe_cloud(pts, radius, upsample=0, upsample_rad=1):
"""
If upsample > 0, the number of points along the rope's backbone is resampled to be upsample points
If upsample_rad > 1, the number of points perpendicular to the backbone points is resampled to be upsample_rad points, around the rope's cross-section
The total number of points is then: (upsample if upsample > 0 else len(self.rope.GetControlPoints())) * upsample_rad
"""
if upsample > 0:
lengths = np.r_[
0, np.apply_along_axis(np.linalg.norm, 1, np.diff(pts, axis=0))]
summed_lengths = np.cumsum(lengths)
assert len(lengths) == len(pts)
pts = math_utils.interp2d(np.linspace(0, summed_lengths[-1], upsample),
summed_lengths, pts)
if upsample_rad > 1:
# add points perpendicular to the points in pts around the rope's cross-section
vs = np.diff(pts,
axis=0) # vectors between the current and next points
vs /= np.apply_along_axis(np.linalg.norm, 1, vs)[:, None]
perp_vs = np.c_[
-vs[:, 1], vs[:, 0], np.zeros(
vs.shape[0]
)] # perpendicular vectors between the current and next points in the xy-plane
perp_vs /= np.apply_along_axis(np.linalg.norm, 1, perp_vs)[:, None]
vs = np.r_[vs, vs[-1, :][
None, :]] # define the vector of the last point to be the same as the second to last one
perp_vs = np.r_[perp_vs, perp_vs[-1, :][
None, :]] # define the perpendicular vector of the last point to be the same as the second to last one
perp_pts = []
from openravepy import matrixFromAxisAngle
for theta in np.linspace(
0, 2 * np.pi, upsample_rad, endpoint=False
): # uniformly around the cross-section circumference
for (center, rot_axis, perp_v) in zip(pts, vs, perp_vs):
rot = matrixFromAxisAngle(rot_axis, theta)[:3, :3]
perp_pts.append(center + rot.T.dot(radius * perp_v))
pts = np.array(perp_pts)
return pts
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 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
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
def create_rope(rope_poss, capsule_height=.02):
rope_pos_dists = np.linalg.norm(np.diff(rope_poss, axis=0), axis=1)
xp = np.r_[0, np.cumsum(rope_pos_dists/capsule_height)]
init_rope_nodes = mu.interp2d(np.arange(xp[-1]+1), xp, rope_poss)
rope_sim_obj = RopeSimulationObject("rope", init_rope_nodes)
return rope_sim_obj
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 create_rope(rope_poss, capsule_height=0.02):
rope_pos_dists = np.linalg.norm(np.diff(rope_poss, axis=0), axis=1)
xp = np.r_[0, np.cumsum(rope_pos_dists / capsule_height)]
init_rope_nodes = mu.interp2d(np.arange(xp[-1] + 1), xp, rope_poss)
rope_sim_obj = RopeSimulationObject("rope", init_rope_nodes)
return rope_sim_obj
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
