def follow_body_traj(pr2,
traj,
times=None,
r_arm=False,
l_arm=False,
r_gripper=False,
l_gripper=False,
head=False,
base=False,
base_frame="/odom_combined",
wait=True):
assert isinstance(pr2, PR2.PR2)
pr2.update_rave()
if r_arm:
pr2.rarm.goto_joint_positions(traj["r_arm"][0])
if l_arm:
pr2.larm.goto_joint_positions(traj["l_arm"][0])
if r_gripper:
pr2.rgrip.set_angle(traj["r_gripper"][0])
if l_gripper:
pr2.lgrip.set_angle(traj["l_gripper"][0])
if head:
raise NotImplementedError
if base:
print "warning: not going to initial base position (goto position not implemented)"
#pr2.base.goto_pose(traj["base"][0], base_frame)
pr2.join_all()
if times is None:
vel_limits = np.r_[pr2.rarm.vel_limits, pr2.larm.vel_limits, .02, .02,
pr2.head.vel_limits * vel_mult, [.2, .2, .2]]
acc_limits = np.r_[pr2.rarm.acc_limits, pr2.larm.acc_limits, np.inf,
np.inf, pr2.head.acc_limits, [1, 1, 1]]
#times, inds = retiming.retime(flatten_compound_dtype(traj), vel_limits, acc_limits)
times, inds = retiming.retime2(flatten_compound_dtype(traj),
vel_limits)
traj = traj[inds]
if r_arm:
pr2.rarm.follow_timed_joint_trajectory(
traj["r_arm"], ku.get_velocities(traj["r_arm"], times, .001),
times)
if l_arm:
pr2.larm.follow_timed_joint_trajectory(
traj["l_arm"], ku.get_velocities(traj["l_arm"], times, .001),
times)
if r_gripper:
pr2.rgrip.follow_timed_trajectory(times, traj["r_gripper"])
if l_gripper:
pr2.lgrip.follow_timed_trajectory(times, traj["l_gripper"])
if head:
pr2.head.follow_timed_trajectory(times, traj["head"])
if base:
pr2.base.follow_timed_trajectory(times,
traj["base"],
frame_id=base_frame)
if wait: pr2.join_all()
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_traj(pr2, traj, times=None, r_arm = False, l_arm = False, r_gripper = False, l_gripper = False, head = False, base = False,
base_frame = "/odom_combined", wait=True):
assert isinstance(pr2, PR2.PR2)
pr2.update_rave()
if r_arm:
pr2.rarm.goto_joint_positions(traj["r_arm"][0])
if l_arm:
pr2.larm.goto_joint_positions(traj["l_arm"][0])
if r_gripper:
pr2.rgrip.set_angle(traj["r_gripper"][0])
if l_gripper:
pr2.lgrip.set_angle(traj["l_gripper"][0])
if head:
raise NotImplementedError
if base:
print "warning: not going to initial base position (goto position not implemented)"
#pr2.base.goto_pose(traj["base"][0], base_frame)
pr2.join_all()
if times is None:
vel_limits = np.r_[pr2.rarm.vel_limits, pr2.larm.vel_limits, .02, .02, pr2.head.vel_limits*vel_mult, [.2,.2,.2]]
acc_limits = np.r_[pr2.rarm.acc_limits, pr2.larm.acc_limits, np.inf, np.inf, pr2.head.acc_limits, [1,1,1]]
#times, inds = retiming.retime(flatten_compound_dtype(traj), vel_limits, acc_limits)
times, inds = retiming.retime2(flatten_compound_dtype(traj), vel_limits)
traj = traj[inds]
if r_arm:
pr2.rarm.follow_timed_joint_trajectory(traj["r_arm"],ku.get_velocities(traj["r_arm"],times,.001),times)
if l_arm:
pr2.larm.follow_timed_joint_trajectory(traj["l_arm"],ku.get_velocities(traj["l_arm"],times,.001),times)
if r_gripper:
pr2.rgrip.follow_timed_trajectory(times, traj["r_gripper"])
if l_gripper:
pr2.lgrip.follow_timed_trajectory(times, traj["l_gripper"])
if head:
pr2.head.follow_timed_trajectory(times, traj["head"])
if base:
pr2.base.follow_timed_trajectory(times, traj["base"], frame_id = base_frame)
if wait: pr2.join_all()
def follow_body_traj(pr2, traj, times=None, r_arm = False, l_arm = False, r_gripper = False, l_gripper = False, head = False, base = False):
isinstance(pr2, PR2.PR2)
pr2.update_rave()
if r_arm:
pr2.rarm.goto_joint_positions(traj["r_arm"][0])
if l_arm:
pr2.larm.goto_joint_positions(traj["l_arm"][0])
if r_gripper:
pr2.rgrip.set_angle(traj["r_gripper"][0])
if l_gripper:
pr2.lgrip.set_angle(traj["l_gripper"][0])
if head or base:
raise NotImplementedError
pr2.join_all()
vel_mult = .2
if times is None:
vel_limits = np.r_[pr2.rarm.vel_limits*vel_mult, pr2.larm.vel_limits*vel_mult, .02, .02, pr2.head.vel_limits*vel_mult, [.2,.2,.2]]
acc_limits = np.r_[pr2.rarm.acc_limits, pr2.larm.acc_limits, np.inf, np.inf, pr2.head.acc_limits, [1,1,1]]
#times, inds = retiming.retime(flatten_compound_dtype(traj), vel_limits, acc_limits)
times, inds = retiming.retime2(flatten_compound_dtype(traj), vel_limits)
traj = traj[inds]
if r_arm:
pr2.rarm.follow_timed_joint_trajectory(traj["r_arm"],ku.get_velocities(traj["r_arm"],times,.001),times)
if l_arm:
pr2.larm.follow_timed_joint_trajectory(traj["l_arm"],ku.get_velocities(traj["l_arm"],times,.001),times)
if r_gripper:
pr2.rgrip.follow_timed_trajectory(times, traj["r_gripper"])
if l_gripper:
pr2.lgrip.follow_timed_trajectory(times, traj["l_gripper"])
if head:
pr2.head.follow_timed_trajectory(times, traj["head"])
if base:
pr2.base.follow_timed_trajectory(times, traj["base"], frame_id = "/map")
pr2.join_all()
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
pose_array = conversions.array_to_pose_array(asarray(traj["gripper_positions"]), 'base_link')
rviz.draw_curve(
pose_array,
id=0)
n_waypoints = 20
xyzquat = np.c_[traj["gripper_positions"],traj["gripper_orientations"]]
xyzquat_rs = kinematics_utils.unif_resample(xyzquat, n_waypoints, weights = np.ones(7), tol=.001)
times = np.linspace(0,10,n_waypoints)
pr2.torso.go_up()
pr2.join_all()
pr2.update_rave()
joint_positions,_ = trajectories.make_joint_traj(xyzquat_rs[:,0:3], xyzquat_rs[:,3:7], pr2.rarm.manip, 'base_link', 'r_wrist_roll_link', filter_options = 18)
joint_velocities = kinematics_utils.get_velocities(joint_positions, times, tol=.001)
pr2.rarm.follow_timed_joint_trajectory(joint_positions, joint_velocities, times)
#for xyzq in xyzquat_rs:
#xyz = xyzq[:3]
#quat = xyzq[3:]
#hmat = conversions.trans_rot_to_hmat(xyz,quat)
#try:
#pr2.rarm.goto_pose_matrix(hmat, 'base_link', 'r_wrist_roll_link')
#pr2.join_all()
#except IKFail:
#pass
pr2.join_all()
n_waypoints,
weights=np.ones(7),
tol=.001)
times = np.linspace(0, 10, n_waypoints)
pr2.torso.go_up()
pr2.join_all()
pr2.update_rave()
joint_positions, _ = trajectories.make_joint_traj(xyzquat_rs[:, 0:3],
xyzquat_rs[:, 3:7],
pr2.rarm.manip,
'base_link',
'r_wrist_roll_link',
filter_options=18)
joint_velocities = kinematics_utils.get_velocities(joint_positions,
times,
tol=.001)
pr2.rarm.follow_timed_joint_trajectory(joint_positions, joint_velocities,
times)
#for xyzq in xyzquat_rs:
#xyz = xyzq[:3]
#quat = xyzq[3:]
#hmat = conversions.trans_rot_to_hmat(xyz,quat)
#try:
#pr2.rarm.goto_pose_matrix(hmat, 'base_link', 'r_wrist_roll_link')
#pr2.join_all()
#except IKFail:
#pass
pr2.join_all()
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