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 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 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 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 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