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