python** is as follows:
#!/usr/bin/python# -*coding: utf-8 -*import mathimport matplotlib.pyplot as pltimport numpy as npfrom celluloid import cameraclass vehicle: def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0, dt=0.1, l=3.0): self.steer = 0 self.x = x self.y = y self.yaw = yaw self.v = v self.dt = dt self.l = l wheelbase selfx_front = x + l * math.cos(yaw) self.y_front = y + l * math.sin(yaw) def update(self, a, delta, max_steer=np.pi): delta = np.clip(delta, -max_steer, max_steer) self.steer = delta self.x = self.x + self.v * math.cos(self.yaw) *self.dt self.y = self.y + self.v * math.sin(self.yaw) *self.dt self.yaw = self.yaw + self.v / self.l * math.tan(delta) *self.dt self.v = self.v + a * self.dt self.x_front = self.x + self.l * math.cos(self.yaw) self.y_front = self.y + self.l * math.sin(self.yaw)class vehicleinfo: # vehicle parameter l = 3.0 wheelbase w = 20 width lf = 38 Distance from the center of the rear axle to the front of the car lb = 08 Distance from the centre of the rear axle to the rear of the car Max Steer = 06 Maximum front wheel rotation angle tr = 05 wheel radius tw = 05 Wheel width wd = w wheel base length = lb + lf vehicle length def draw trailer(x, y, yaw, steer, ax, vehicle info=vehicleinfo, color='black'): vehicle_outline = np.array( [vehicle_info.lb, vehicle_info.lf, vehicle_info.lf, -vehicle_info.lb, -vehicle_info.lb], vehicle_info.w / 2, vehicle_info.w / 2, -vehicle_info.w / 2, -vehicle_info.w / 2, vehicle_info.w / 2]])wheel = np.array([[vehicle_info.tr, vehicle_info.tr, vehicle_info.tr, -vehicle_info.tr, -vehicle_info.tr], vehicle_info.tw / 2, vehicle_info.tw / 2, -vehicle_info.tw / 2, -vehicle_info.tw / 2, vehicle_info.tw / 2]])rr_wheel = wheel.copy() rl wheel = wheelcopy() left rear wheel fr wheel = wheelcopy() right front wheel fl wheel = wheelcopy() left front wheel rr wheel[1,:]= vehicle info.wd/2 rl_wheel[1,:]= vehicle_info.wd 2 steering wheel rotation rot1 = nparray([[np.cos(steer), np.sin(steer)],np.sin(steer), np.cos(steer)]] yaw rotation matrix rot2 = nparray([[np.cos(yaw), np.sin(yaw)],np.sin(yaw), np.cos(yaw)]]fr_wheel = np.dot(rot1, fr_wheel) fl_wheel = np.dot(rot1, fl_wheel) fr_wheel += np.array([[vehicle_info.l], vehicle_info.wd / 2]])fl_wheel += np.array([[vehicle_info.l], vehicle_info.wd / 2]])fr_wheel = np.dot(rot2, fr_wheel) fr_wheel[0, := x fr_wheel[1, := y fl_wheel = np.dot(rot2, fl_wheel) fl_wheel[0, := x fl_wheel[1, := y rr_wheel = np.dot(rot2, rr_wheel) rr_wheel[0, := x rr_wheel[1, := y rl_wheel = np.dot(rot2, rl_wheel) rl_wheel[0, := x rl_wheel[1, := y vehicle_outline = np.dot(rot2, vehicle_outline) vehicle_outline[0, := x vehicle_outline[1, := y ax.plot(fr_wheel[0, :fr_wheel[1, :color) ax.plot(rr_wheel[0, :rr_wheel[1, :color) ax.plot(fl_wheel[0, :fl_wheel[1, :color) ax.plot(rl_wheel[0, :rl_wheel[1, :color) ax.plot(vehicle_outline[0, :vehicle_outline[1, :color) # ax.axis('equal')if __name__ == "__main__": vehicle = vehicle(x=0.0, y=0.0, yaw=0, v=0.0, dt=0.1, l=vehicleinfo.l) vehicle.v = 1 trajectory_x = trajectory_y = fig = plt.figure() save ** with camera = camera(fig) for i in range(600): pltcla() plt.gca().set_aspect('equal', adjustable='box') vehicle.update(0, np.pi / 10) draw_trailer(vehicle.x, vehicle.y, vehicle.yaw, vehicle.steer, plt) trajectory_x.append(vehicle.x) trajectory_y.append(vehicle.y) plt.plot(trajectory_x, trajectory_y, 'blue') plt.xlim(-12, 12) plt.ylim(-2.5, 21) plt.pause(0.001) # camera.snap() # animation = camera.animate(interval=5) # animation.s**e('trajectory.gif')The result is as follows:
The article was first published***idoitnow If you like it, you can pay attention to it.