def retime_traj(robot, inds, traj, max_cart_vel=.02, max_finger_vel=.02, upsample_time=.1): """retime a trajectory so that it executes slowly enough for the simulation""" cart_traj = np.empty((len(traj), 6)) finger_traj = np.empty((len(traj), 2)) leftarm, rightarm = robot.GetManipulator("leftarm"), robot.GetManipulator( "rightarm") with robot: for i in range(len(traj)): robot.SetDOFValues(traj[i], inds) cart_traj[i, :3] = leftarm.GetTransform()[:3, 3] cart_traj[i, 3:] = rightarm.GetTransform()[:3, 3] finger_traj[i, :1] = robot.GetDOFValues( leftarm.GetGripperIndices()) finger_traj[i, 1:] = robot.GetDOFValues(rightarm.GetGripperIndices()) times = retiming.retime_with_vel_limits( np.c_[cart_traj, finger_traj], np.r_[np.repeat(max_cart_vel, 6), np.repeat(max_finger_vel, 2)]) times_up = np.linspace( 0, times[-1], times[-1] / upsample_time) if times[-1] > upsample_time else times traj_up = math_utils.interp2d(times_up, times, traj) return traj_up
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 = resampling.get_velocities(traj_up, times_up, .001) self.follow_timed_joint_trajectory(traj_up, vels, times_up)
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 = resampling.get_velocities(traj_up, times_up, .001) self.follow_timed_joint_trajectory(traj_up, vels, times_up)
def retime_traj(robot, inds, traj, max_cart_vel=.02, max_finger_vel=.02, upsample_time=.1): """retime a trajectory so that it executes slowly enough for the simulation""" cart_traj = np.empty((len(traj), 6)) finger_traj = np.empty((len(traj),2)) leftarm, rightarm = robot.GetManipulator("leftarm"), robot.GetManipulator("rightarm") with robot: for i in range(len(traj)): robot.SetDOFValues(traj[i], inds) cart_traj[i,:3] = leftarm.GetTransform()[:3,3] cart_traj[i,3:] = rightarm.GetTransform()[:3,3] finger_traj[i,:1] = robot.GetDOFValues(leftarm.GetGripperIndices()) finger_traj[i,1:] = robot.GetDOFValues(rightarm.GetGripperIndices()) times = retiming.retime_with_vel_limits(np.c_[cart_traj, finger_traj], np.r_[np.repeat(max_cart_vel, 6),np.repeat(max_finger_vel,2)]) times_up = np.linspace(0, times[-1], times[-1]/upsample_time) if times[-1] > upsample_time else times traj_up = math_utils.interp2d(times_up, times, traj) return traj_up
def follow_body_traj(pr2, bodypart2traj, wait=True, base_frame="/base_footprint", speed_factor=1): LOG.info("following trajectory with bodyparts %s", " ".join(bodypart2traj.keys())) name2part = { "lgrip": pr2.lgrip, "rgrip": pr2.rgrip, "larm": pr2.larm, "rarm": pr2.rarm, "base": pr2.base } for partname in bodypart2traj: if partname not in name2part: raise Exception("invalid part name %s" % partname) #### Go to initial positions ####### for (name, part) in name2part.items(): if name in bodypart2traj: part_traj = bodypart2traj[name] if name == "lgrip" or name == "rgrip": part.set_angle(np.squeeze(part_traj)[0]) elif name == "larm" or name == "rarm": part.goto_joint_positions(part_traj[0]) elif name == "base": part.goto_pose(part_traj[0], base_frame) pr2.join_all() #### Construct total trajectory so we can retime it ####### n_dof = 0 trajectories = [] vel_limits = [] acc_limits = [] bodypart2inds = {} 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) * speed_factor times = retiming.retime_with_vel_limits(trajectories, vel_limits) times_up = np.linspace(0, times[-1], int(np.ceil(times[-1] / .1))) traj_up = mu.interp2d(times_up, times, trajectories) #### Send all part trajectories ########### for (name, part) in name2part.items(): if name in bodypart2traj: part_traj = traj_up[:, bodypart2inds[name]] if name == "lgrip" or name == "rgrip": part.follow_timed_trajectory(times_up, part_traj.flatten()) elif name == "larm" or name == "rarm": vels = resampling.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, bodypart2traj, wait=True, base_frame = "/base_footprint", speed_factor=1): LOG.info("following trajectory with bodyparts %s", " ".join(bodypart2traj.keys())) name2part = {"lgrip":pr2.lgrip, "rgrip":pr2.rgrip, "larm":pr2.larm, "rarm":pr2.rarm, "base":pr2.base} for partname in bodypart2traj: if partname not in name2part: raise Exception("invalid part name %s"%partname) #### Go to initial positions ####### for (name, part) in name2part.items(): if name in bodypart2traj: part_traj = bodypart2traj[name] if name == "lgrip" or name == "rgrip": part.set_angle(np.squeeze(part_traj)[0]) elif name == "larm" or name == "rarm": part.goto_joint_positions(part_traj[0]) elif name == "base": part.goto_pose(part_traj[0], base_frame) pr2.join_all() #### Construct total trajectory so we can retime it ####### n_dof = 0 trajectories = [] vel_limits = [] acc_limits = [] bodypart2inds = {} 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)*speed_factor times = retiming.retime_with_vel_limits(trajectories, vel_limits) times_up = np.linspace(0, times[-1], int(np.ceil(times[-1]/.1))) traj_up = mu.interp2d(times_up, times, trajectories) #### Send all part trajectories ########### for (name, part) in name2part.items(): if name in bodypart2traj: part_traj = traj_up[:,bodypart2inds[name]] if name == "lgrip" or name == "rgrip": part.follow_timed_trajectory(times_up, part_traj.flatten()) elif name == "larm" or name == "rarm": vels = resampling.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