コード例 #1
0
    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)
コード例 #2
0
    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)
コード例 #3
0
ファイル: reachability.py プロジェクト: rll/sushichallenge
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