Python min_deriv_4dの例

コード例 #1

    def plan_trajectory(self):
        trajectories = []
        origin = np.array([0, 0, 2])
        self.waypoints = np.array(self.waypoints)
        for drone in range(self.waypoints.shape[2]):
            pos_wp = self.waypoints[:, :, drone] + origin
            yaw_wp = np.zeros((pos_wp.shape[0], 1))
            traj = tgen.min_deriv_4d(4,
                                     np.hstack([pos_wp, yaw_wp]),
                                     self.T,
                                     stop=False)
            trajectories.append(traj)

        traj_json = tgen.trajectories_to_json(trajectories)
        data = {}
        for key in traj_json.keys():
            data[key] = {
                'trajectory': traj_json[key],
                'T': self.T,
                'color': g.colors,
                'delay': [d[key] for d in g.delays]
            }
        data['repeat'] = 3
        assert len(trajectories) < 32
        return trajectories, data

コード例 #2

ファイル: P_formation.py プロジェクト: zp-yang/pybswarm

 def plan_trajectory(self, origin):
     trajectories = []
     waypoints = np.array(self.waypoints)
     assert len(waypoints) < 33
     for drone in range(waypoints.shape[2]):
         pos_wp = waypoints[:, :, drone] + origin
         yaw_wp = np.zeros((pos_wp.shape[0], 1))
         traj = tgen.min_deriv_4d(4, 
             np.hstack([pos_wp, yaw_wp]), self.T, stop=False)
         trajectories.append(traj)
     assert len(trajectories) < 32
     return trajectories

コード例 #3

ファイル: chacha.py プロジェクト: zp-yang/pybswarm

    c.left(2)
    c.right(3)
    c.crisscross(5)
    c.chacha(5)
    c.wait(1)

print('length of waypoitns', len(c.waypoints))
origin = np.array([1.5, 2, 2])
waypoints = np.array(c.waypoints)
T = c.T

trajectories = []
for drone in range(waypoints.shape[2]):
    pos_wp = waypoints[:, :, drone] + origin
    yaw_wp = np.zeros((pos_wp.shape[0], 1))
    traj = tgen.min_deriv_4d(4, np.hstack([pos_wp, yaw_wp]), T, stop=False)
    trajectories.append(traj)

tgen.plot_trajectories(trajectories)
tgen.trajectories_to_json(trajectories, 'scripts/data/chacha.json')

plt.show()

# %%
tgen.plot_trajectories_time_history(trajectories)

# %%
tgen.animate_trajectories('chacha.mp4', trajectories, fps=5)

#%%

コード例 #4

ファイル: block_land.py プロジェクト: zp-yang/pybswarm

    rgb['white'],
    rgb['blue'],
]

# delay, must be length of waypoints - 1
delays = [0, 0]

assert len(waypoints) < 33
trajectories = []
for drone in range(waypoints.shape[2]):
    print(waypoints[:, :, drone])
    pos_wp = waypoints[:, :, drone] + origin
    print(pos_wp)
    yaw_wp = np.zeros((pos_wp.shape[0], 1))
    traj = tgen.min_deriv_4d(4,
                             np.hstack([pos_wp, yaw_wp]),
                             time_vector,
                             stop=False)
    trajectories.append(traj)
assert len(trajectories) < 32
traj_json = tgen.trajectories_to_json(trajectories)
data = {}
for key in traj_json.keys():
    data[key] = {
        'trajectory': traj_json[key],
        'T': time_vector,
        'color': colors,
        'delay': delays
    }
# how many times to repeat trajectory
data['repeat'] = 1
assert len(trajectories) < 32