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_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 get_base_positions_bimanual_resampled(rars, joints):
n_orig = len(rars)
rars_ds = rars[::10]
n_ds = len(rars_ds)
base_xys_ds = get_base_positions_bimanual(rars_ds, joints)
base_xys = mu.interp2d(np.linspace(0,1,n_orig), np.linspace(0,1,n_ds), base_xys_ds)
return base_xys
