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