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,
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()
