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)
rospy.loginfo("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_trajectory(pr2, bodypart2traj):
"""
bodypart2traj is a dictionary with zero or more of the following fields: {l/r}_grab, {l/r}_gripper, {l/r_arm}
We'll follow all of these bodypart trajectories simultaneously
Also, if the trajectory involves grabbing with the gripper, and the grab fails, the trajectory will abort
"""
rospy.loginfo("following trajectory with bodyparts %s", " ".join(bodypart2traj.keys()))
trajectories = []
vel_limits = []
acc_limits = []
bodypart2inds = {}
n_dof = 0
name2part = {"l_gripper":pr2.lgrip,
"r_gripper":pr2.rgrip,
"l_arm":pr2.larm,
"r_arm":pr2.rarm}
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)
#print 'traj orig:'; print trajectories
#trajectories = np.r_[np.atleast_2d(pr2.get_joint_positions()), trajectories]
#print 'traj with first:'; print trajectories
for arm in ['l_arm', 'r_arm']:
if arm in bodypart2traj:
part_traj = trajectories[:,bodypart2inds[arm]]
part_traj[:,4] = np.unwrap(part_traj[:,4])
part_traj[:,6] = np.unwrap(part_traj[:,6])
#print 'traj after unwrap:'; print trajectories
vel_limits = np.array(vel_limits)
acc_limits = np.array(acc_limits)
times = retiming.retime_with_vel_limits(trajectories, vel_limits/2)
times_up = np.arange(0,times[-1],.1)
traj_up = interp2d(times_up, times, trajectories)
for (name, part) in name2part.items():
if name in bodypart2traj:
part_traj = traj_up[:,bodypart2inds[name]]
if name == "l_gripper" or name == "r_gripper":
part.follow_timed_trajectory(times_up, part_traj.flatten())
elif name == "l_arm" or name == "r_arm":
#print 'following traj for', name, part_traj
#print ' with velocities'
vels = kinematics_utils.get_velocities(part_traj, times_up, .001)
#print vels
part.follow_timed_joint_trajectory(part_traj, vels, times_up)
pr2.join_all()
return True
def get_base_positions_bimanual_resampled(rars, joints):
n_orig = len(rars)
rars_ds = rars[::10]
n_ds = len(rars_ds)
base_xys_ds = get_base_positions_bimanual(rars_ds, joints)
base_xys = mu.interp2d(np.linspace(0,1,n_orig), np.linspace(0,1,n_ds), base_xys_ds)
return base_xys
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 = ku.get_velocities(traj_up, times_up, .001)
self.follow_timed_joint_trajectory(traj_up, vels, times_up)
def get_base_positions_bimanual_resampled(rars, joints):
n_orig = len(rars)
rars_ds = rars[::10]
n_ds = len(rars_ds)
base_xys_ds = get_base_positions_bimanual(rars_ds, joints)
base_xys = mu.interp2d(np.linspace(0, 1, n_orig), np.linspace(0, 1, n_ds),
base_xys_ds)
return base_xys
def follow_body_traj2(pr2,
bodypart2traj,
times=None,
wait=True,
base_frame="/base_footprint"):
rospy.loginfo("following trajectory with bodyparts %s",
" ".join(bodypart2traj.keys()))
trajectories = []
vel_limits = []
acc_limits = []
bodypart2inds = {}
n_dof = 0
name2part = {
"l_gripper": pr2.lgrip,
"r_gripper": pr2.rgrip,
"l_arm": pr2.larm,
"r_arm": pr2.rarm,
"base": pr2.base
}
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)
acc_limits = np.array(acc_limits)
times = retiming.retime_with_vel_limits(trajectories, vel_limits)
times_up = np.arange(0, times[-1] + 1e-4, .1)
traj_up = mu.interp2d(times_up, times, trajectories)
for (name, part) in name2part.items():
if name in bodypart2traj:
part_traj = traj_up[:, bodypart2inds[name]]
if name == "l_gripper" or name == "r_gripper":
part.follow_timed_trajectory(times_up, part_traj.flatten())
elif name == "l_arm" or name == "r_arm":
vels = ku.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 make_joint_traj_by_graph_search(xyzs,
quats,
manip,
targ_frame,
downsample=1,
check_collisions=False):
assert (len(xyzs) == len(quats))
hmats = [
conv.trans_rot_to_hmat(xyz, quat) for xyz, quat in zip(xyzs, quats)
]
ds_hmats = hmats[::downsample]
orig_len, ds_len = len(hmats), len(ds_hmats)
if downsample != 1:
rospy.loginfo(
'Note: downsampled %s trajectory from %d points to %d points',
manip.GetName(), orig_len, ds_len)
link = manip.GetRobot().GetLink(targ_frame)
Tcur_w_link = link.GetTransform()
Tcur_w_ee = manip.GetEndEffectorTransform()
Tf_link_ee = np.linalg.solve(Tcur_w_link, Tcur_w_ee)
def ikfunc(hmat):
return manip.FindIKSolutions(hmat.dot(Tf_link_ee),
2 + 16 + (1 if check_collisions else 0))
def nodecost(joints):
robot = manip.GetRobot()
cost = 0
with robot:
robot.SetDOFValues(joints, manip.GetArmIndices())
cost = 100 * robot.GetEnv().CheckCollision(robot)
return cost
start_joints = manip.GetRobot().GetDOFValues(manip.GetArmIndices())
paths, costs, timesteps = traj_ik_graph_search.traj_cart2joint(
ds_hmats,
ikfunc,
start_joints=start_joints,
nodecost=nodecost if check_collisions else None)
rospy.loginfo("%s: %i of %i points feasible", manip.GetName(),
len(timesteps), ds_len)
i_best = np.argmin(costs)
rospy.loginfo("lowest cost of initial trajs: %f", costs[i_best])
path_init = unwrap_arm_traj(paths[i_best])
path_init_us = mu.interp2d(range(orig_len),
range(orig_len)[::downsample],
path_init) # un-downsample
assert len(path_init_us) == orig_len
return path_init_us, timesteps
def follow_body_traj2(pr2, bodypart2traj, times=None, wait=True, base_frame = "/base_footprint"):
rospy.loginfo("following trajectory with bodyparts %s", " ".join(bodypart2traj.keys()))
trajectories = []
vel_limits = []
acc_limits = []
bodypart2inds = {}
n_dof = 0
name2part = {"l_gripper":pr2.lgrip,
"r_gripper":pr2.rgrip,
"l_arm":pr2.larm,
"r_arm":pr2.rarm,
"torso":pr2.torso,
"base":pr2.base}
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)
acc_limits = np.array(acc_limits)
times = retiming.retime_with_vel_limits(trajectories, vel_limits)
times_up = np.arange(0,times[-1]+1e-4,.1)
traj_up = mu.interp2d(times_up, times, trajectories)
for (name, part) in name2part.items():
if name in bodypart2traj:
part_traj = traj_up[:,bodypart2inds[name]]
if name == "l_gripper" or name == "r_gripper":
part.follow_timed_trajectory(times_up, part_traj.flatten())
elif name == "l_arm" or name == "r_arm" or name == "torso":
vels = ku.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 execute(self, userdata):
Globals.pr2.update_rave()
base_offset = userdata.base_offset
STEPS = 10; TIME = 5.
xyas = mu.interp2d(np.linspace(0, 1, STEPS), [0, 1], [[0, 0, 0], base_offset])
rospy.loginfo('Following base trajectory %s', str(xyas))
ts = np.linspace(0, TIME, STEPS)
pub = rospy.Publisher("base_traj_controller/command", tm.JointTrajectory)
jt = tm.JointTrajectory()
jt.header.frame_id = "base_footprint"
for i in xrange(len(xyas)):
jtp = tm.JointTrajectoryPoint()
jtp.time_from_start = rospy.Duration(ts[i])
jtp.positions = xyas[i]
jt.points.append(jtp)
pub.publish(jt)
rospy.sleep(TIME*2)
ELOG.log('MoveBase', 'base_offset', base_offset)
return 'success'
def make_joint_traj_by_graph_search(xyzs, quats, manip, targ_frame, downsample=1, check_collisions=False):
assert(len(xyzs) == len(quats))
hmats = [conv.trans_rot_to_hmat(xyz, quat) for xyz, quat in zip(xyzs, quats)]
ds_hmats = hmats[::downsample]
orig_len, ds_len = len(hmats), len(ds_hmats)
if downsample != 1:
rospy.loginfo('Note: downsampled %s trajectory from %d points to %d points', manip.GetName(), orig_len, ds_len)
link = manip.GetRobot().GetLink(targ_frame)
Tcur_w_link = link.GetTransform()
Tcur_w_ee = manip.GetEndEffectorTransform()
Tf_link_ee = np.linalg.solve(Tcur_w_link, Tcur_w_ee)
def ikfunc(hmat):
return manip.FindIKSolutions(hmat.dot(Tf_link_ee), 2+16 + (1 if check_collisions else 0))
#return manip.FindIKSolutions(hmat.dot(Tf_link_ee), 2+16)
def nodecost(joints):
robot = manip.GetRobot()
cost = 0
with robot:
robot.SetDOFValues(joints, manip.GetArmIndices())
cost = 100*robot.GetEnv().CheckCollision(robot)
return cost
start_joints = manip.GetRobot().GetDOFValues(manip.GetArmIndices())
paths, costs, timesteps = traj_ik_graph_search.traj_cart2joint(
ds_hmats, ikfunc,
start_joints=start_joints
#nodecost=nodecost if check_collisions else None
)
rospy.loginfo("%s: %i of %i points feasible", manip.GetName(), len(timesteps), ds_len)
i_best = np.argmin(costs)
rospy.loginfo("lowest cost of initial trajs: %f", costs[i_best])
path_init = unwrap_arm_traj(paths[i_best])
path_init_us = mu.interp2d(range(orig_len), range(orig_len)[::downsample], path_init) # un-downsample
assert len(path_init_us) == orig_len
return path_init_us, timesteps
def make_joint_traj(xyzs, quats, joint_seeds, manip, ref_frame, targ_frame,
filter_options):
"""
do ik to make a joint trajectory
joint_seeds are the joint angles that this function will try to be close to
I put that in so you could try to stay near the demonstration joint angles
in retrospect, using that argument might be bad idea because it'll make the trajectory much more jerky in some situations
(TODO)
"""
n = len(xyzs)
assert len(quats) == n
robot = manip.GetRobot()
robot.SetActiveManipulator(manip.GetName())
joint_inds = manip.GetArmIndices()
orig_joint = robot.GetDOFValues(joint_inds)
joints = []
inds = []
for i in xrange(0, n):
mat4 = conv.trans_rot_to_hmat(xyzs[i], quats[i])
robot.SetDOFValues(joint_seeds[i], joint_inds)
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.SetDOFValues(joint, joint_inds)
robot.SetDOFValues(orig_joint, joint_inds)
rospy.loginfo("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, joint_seeds,manip, ref_frame, targ_frame,filter_options):
"""
do ik to make a joint trajectory
joint_seeds are the joint angles that this function will try to be close to
I put that in so you could try to stay near the demonstration joint angles
in retrospect, using that argument might be bad idea because it'll make the trajectory much more jerky in some situations
(TODO)
"""
n = len(xyzs)
assert len(quats) == n
robot = manip.GetRobot()
robot.SetActiveManipulator(manip.GetName())
joint_inds = manip.GetArmIndices()
orig_joint = robot.GetDOFValues(joint_inds)
joints = []
inds = []
for i in xrange(0,n):
mat4 = conv.trans_rot_to_hmat(xyzs[i], quats[i])
robot.SetDOFValues(joint_seeds[i], joint_inds)
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.SetDOFValues(joint, joint_inds)
robot.SetDOFValues(orig_joint, joint_inds)
rospy.loginfo("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)
rospy.loginfo("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_trajectory(pr2, bodypart2traj):
"""
bodypart2traj is a dictionary with zero or more of the following fields: {l/r}_grab, {l/r}_gripper, {l/r_arm}
We'll follow all of these bodypart trajectories simultaneously
Also, if the trajectory involves grabbing with the gripper, and the grab fails, the trajectory will abort
"""
rospy.loginfo("following trajectory with bodyparts %s",
" ".join(bodypart2traj.keys()))
trajectories = []
vel_limits = []
acc_limits = []
bodypart2inds = {}
n_dof = 0
name2part = {
"l_gripper": pr2.lgrip,
"r_gripper": pr2.rgrip,
"l_arm": pr2.larm,
"r_arm": pr2.rarm
}
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)
#print 'traj orig:'; print trajectories
#trajectories = np.r_[np.atleast_2d(pr2.get_joint_positions()), trajectories]
#print 'traj with first:'; print trajectories
for arm in ['l_arm', 'r_arm']:
if arm in bodypart2traj:
part_traj = trajectories[:, bodypart2inds[arm]]
part_traj[:, 4] = np.unwrap(part_traj[:, 4])
part_traj[:, 6] = np.unwrap(part_traj[:, 6])
#print 'traj after unwrap:'; print trajectories
vel_limits = np.array(vel_limits)
acc_limits = np.array(acc_limits)
times = retiming.retime_with_vel_limits(trajectories, vel_limits / 2)
times_up = np.arange(0, times[-1], .1)
traj_up = interp2d(times_up, times, trajectories)
for (name, part) in name2part.items():
if name in bodypart2traj:
part_traj = traj_up[:, bodypart2inds[name]]
if name == "l_gripper" or name == "r_gripper":
part.follow_timed_trajectory(times_up, part_traj.flatten())
elif name == "l_arm" or name == "r_arm":
#print 'following traj for', name, part_traj
#print ' with velocities'
vels = kinematics_utils.get_velocities(part_traj, times_up,
.001)
#print vels
part.follow_timed_joint_trajectory(part_traj, vels, times_up)
pr2.join_all()
return True
