def grab_rope(self, lr, seg_samples=5):
print colorize("TESTING FOR GRAB...", "magenta", True)
nodes, ctl_pts = self.rope.GetNodes(), self.rope.GetControlPoints()
graspable_inds = []
for i in xrange(len(ctl_pts)-1):
pts = math_utils.interp2d(np.linspace(0,1,seg_samples), [0,1], np.r_[np.reshape(ctl_pts[i], (1,3)), np.reshape(ctl_pts[i+1], (1,3))])
if np.any([in_grasp_region(self.robot, lr, pt) for pt in pts]):
graspable_inds.append(i)
print colorize('graspable inds for %s: %s' % (lr, str(graspable_inds)), "magenta", True)
if len(graspable_inds) == 0:
return False
robot_link = self.robot.GetLink("%s_gripper_l_finger_tip_link"%lr)
rope_links = self.rope.GetKinBody().GetLinks()
for i_node in graspable_inds:
for i_cnt in range(max(0, i_node-1), min(len(nodes), i_node+2)):
cnt = self.bt_env.AddConstraint({
"type": "generic6dof",
"params": {
"link_a": robot_link,
"link_b": rope_links[i_cnt],
"frame_in_a": np.linalg.inv(robot_link.GetTransform()).dot(rope_links[i_cnt].GetTransform()),
"frame_in_b": np.eye(4),
"use_linear_reference_frame_a": False,
"stop_erp": .8,
"stop_cfm": .1,
"disable_collision_between_linked_bodies": True,
}
})
self.constraints[lr].append(cnt)
return True
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 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 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 observe_cloud(self, upsample=0):
pts = self.rope.GetControlPoints()
if upsample == 0:
return pts
lengths = np.r_[0, self.rope.GetHalfHeights() * 2]
summed_lengths = np.cumsum(lengths)
assert len(lengths) == len(pts)
return math_utils.interp2d(np.linspace(0, summed_lengths[-1], upsample*len(pts)), summed_lengths, pts)
def bad_inds(x1, t1):
ibad = np.flatnonzero( (np.abs(mu.interp2d(l, l1, x1) - x) > tol).any(axis=1) )
jbad1 = np.flatnonzero((np.abs(x1[1:] - x1[:-1]) > max_change[None,:]).any(axis=1))
if len(ibad) == 0 and len(jbad1) == 0: return []
else:
lbad = l[ibad]
jbad = np.unique(np.searchsorted(l1, lbad)) - 1
jbad = np.union1d(jbad, jbad1)
return jbad
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 bad_inds(x1, t1):
ibad = np.flatnonzero(
(np.abs(mu.interp2d(l, l1, x1) - x) > tol).any(axis=1))
jbad1 = np.flatnonzero(
(np.abs(x1[1:] - x1[:-1]) > max_change[None, :]).any(axis=1))
if len(ibad) == 0 and len(jbad1) == 0: return []
else:
lbad = l[ibad]
jbad = np.unique(np.searchsorted(l1, lbad)) - 1
jbad = np.union1d(jbad, jbad1)
return jbad
def retime_traj(robot, inds, traj, base_hmats, max_cart_vel=.02, upsample_time=.1):
"""retime a trajectory so that it executes slowly enough for the simulation"""
cart_traj = np.empty((len(traj), 6))
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]
times = retiming.retime_with_vel_limits(cart_traj, np.repeat(max_cart_vel, 6))
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)
if base_hmats != None:
base_traj = [mat_to_base_pose(base_hmat) for base_hmat in base_hmats]
base_traj_up = math_utils.interp2d(times_up, times, base_traj)
base_hmats_up = [base_pose_to_mat(base_pose) for base_pose in base_traj_up]
else:
base_hmats_up = None
return traj_up, base_hmats_up
def get_traj_collisions(bodypart_traj, robot, n=100):
traj = []
for group in bodypart_traj:
traj.append(bodypart_traj[group])
traj_up = mu.interp2d(np.linspace(0, 1, n), np.linspace(0, 1, len(traj)), traj)
env = robot.GetEnv()
col_times = []
for (i, row) in enumerate(traj_up):
robot.SetActiveDOFValues(row)
col_now = env.CheckCollision(robot)
if col_now:
col_times.append(i)
return col_times
def adaptive_resample2(x, tol, max_change=-1, min_steps=3):
print type(x)
print x.shape
"""
resample original signal with a small number of waypoints so that the the sparsely sampled function,
when linearly interpolated, deviates from the original function by less than tol at every time
input:
x: 2D array in R^(t x k) where t is the number of timesteps
tol: tolerance. either a single scalar or a vector of length k
max_change: max change in the sparsely sampled signal at each timestep
min_steps: minimum number of timesteps in the new trajectory. (usually irrelevant)
output:
new_times, new_x
assuming that the old signal has times 0,1,2,...,len(x)-1
this gives the new times, and the new signal
"""
x = np.asarray(x)
assert x.ndim == 2
if np.isscalar(tol):
tol = np.ones(x.shape[1])*tol
else:
tol = np.asarray(tol)
assert tol.ndim == 1 and tol.shape[0] == x.shape[1]
times = np.arange(x.shape[0])
if max_change == -1:
max_change = np.ones(x.shape[1]) * np.inf
elif np.isscalar(max_change):
max_change = np.ones(x.shape[1]) * max_change
else:
max_change = np.asarray(max_change)
assert max_change.ndim == 1 and max_change.shape[0] == x.shape[1]
dl = mu.norms(x[1:] - x[:-1],1)
l = np.cumsum(np.r_[0,dl])
def bad_inds(x1, t1):
ibad = np.flatnonzero( (np.abs(mu.interp2d(l, l1, x1) - x) > tol).any(axis=1) )
jbad1 = np.flatnonzero((np.abs(x1[1:] - x1[:-1]) > max_change[None,:]).any(axis=1))
if len(ibad) == 0 and len(jbad1) == 0: return []
else:
lbad = l[ibad]
jbad = np.unique(np.searchsorted(l1, lbad)) - 1
jbad = np.union1d(jbad, jbad1)
return jbad
l1 = np.linspace(0,l[-1],min_steps)
for _ in xrange(20):
x1 = mu.interp2d(l1, l, x)
bi = bad_inds(x1, l1)
if len(bi) == 0:
return np.interp(l1, l, times), x1
else:
l1 = np.union1d(l1, (l1[bi] + l1[bi+1]) / 2 )
raise Exception("couldn't subdivide enough. something funny is going on. check your input data")
def interp_quats(newtimes, oldtimes, oldquats):
"should actually do slerp"
quats_unnormed = mu.interp2d(newtimes, oldtimes, oldquats)
return mu.normr(quats_unnormed)
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 adaptive_resample2(x, tol, max_change=-1, min_steps=3):
print type(x)
print x.shape
"""
resample original signal with a small number of waypoints so that the the sparsely sampled function,
when linearly interpolated, deviates from the original function by less than tol at every time
input:
x: 2D array in R^(t x k) where t is the number of timesteps
tol: tolerance. either a single scalar or a vector of length k
max_change: max change in the sparsely sampled signal at each timestep
min_steps: minimum number of timesteps in the new trajectory. (usually irrelevant)
output:
new_times, new_x
assuming that the old signal has times 0,1,2,...,len(x)-1
this gives the new times, and the new signal
"""
x = np.asarray(x)
assert x.ndim == 2
if np.isscalar(tol):
tol = np.ones(x.shape[1]) * tol
else:
tol = np.asarray(tol)
assert tol.ndim == 1 and tol.shape[0] == x.shape[1]
times = np.arange(x.shape[0])
if max_change == -1:
max_change = np.ones(x.shape[1]) * np.inf
elif np.isscalar(max_change):
max_change = np.ones(x.shape[1]) * max_change
else:
max_change = np.asarray(max_change)
assert max_change.ndim == 1 and max_change.shape[0] == x.shape[1]
dl = mu.norms(x[1:] - x[:-1], 1)
l = np.cumsum(np.r_[0, dl])
def bad_inds(x1, t1):
ibad = np.flatnonzero(
(np.abs(mu.interp2d(l, l1, x1) - x) > tol).any(axis=1))
jbad1 = np.flatnonzero(
(np.abs(x1[1:] - x1[:-1]) > max_change[None, :]).any(axis=1))
if len(ibad) == 0 and len(jbad1) == 0: return []
else:
lbad = l[ibad]
jbad = np.unique(np.searchsorted(l1, lbad)) - 1
jbad = np.union1d(jbad, jbad1)
return jbad
l1 = np.linspace(0, l[-1], min_steps)
for _ in xrange(20):
x1 = mu.interp2d(l1, l, x)
bi = bad_inds(x1, l1)
if len(bi) == 0:
return np.interp(l1, l, times), x1
else:
l1 = np.union1d(l1, (l1[bi] + l1[bi + 1]) / 2)
raise Exception(
"couldn't subdivide enough. something funny is going on. check your input data"
)
def interp_quats(newtimes, oldtimes, oldquats):
"should actually do slerp"
quats_unnormed = mu.interp2d(newtimes, oldtimes, oldquats)
return mu.normr(quats_unnormed)