def testTimeSeriesConstantInput(self): kp = 0.2 ki = 0.02 kd = 0.1 p = PIDController(kp, 0.0, 0.0) i = PIDController(0.0, ki, 0.0) d = PIDController(0.0, 0.0, kd) t = 0.0 dt = 0.12 while t < 10.0: t += dt global_time.updateTime(t) desired = -0.5 measured = 2.0 p_command = p.update(desired, measured) i_command = i.update(desired, measured) d_command = d.update(desired, measured) pid_command = self.pid.update(desired, measured) self.assertAlmostEqual(p_command, kp * -2.5) # Because the input signal is constant, here the discrete-time # integral is exact self.assertAlmostEqual(i_command, ki * -2.5 * t) if (t == dt): # First run self.assertAlmostEqual(d_command, kd * -2.5 / dt) else: self.assertAlmostEqual(d_command, 0.0) self.assertAlmostEqual(pid_command, p_command + i_command + d_command)
def __init__(self): rospy.init_node('attitude_hold_controller') self.node_identifier = 2 self.input_rc = [1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000] self.sub_imu = rospy.Subscriber('/phx/imu', Imu, self.imuCallback) self.sub_attitude = rospy.Subscriber('/phx/fc/attitude', Attitude, self.attitudeCallback) self.autopilot_commands = rospy.Subscriber( '/phx/controller_commands', ControllerCmd, self.controllerCommandCallback) self.pub = rospy.Publisher('/phx/autopilot/input', AutoPilotCmd, queue_size=1) self.imu = Imu() self.freq = 100 # Hz self.r = rospy.Rate(self.freq) self.enabled = True self.currentPose = Attitude() self.yawController = PIDController(1500, 1, 0, 1, 0, 100, 0, 0) self.pitchController = PIDController(1500, 1, 0, 1, 0, 100, 0, 0) self.rollController = PIDController(1500, 1, 0, 1, 0, 100, 0, 0) self.controlCommand_pitch = 1500 self.controlCommand_roll = 1500 self.controlCommand_yaw = 1500
def __init__(self, global_plan_gps, global_plan_world_coord): self._turn_controller = PIDController(K_P=1.25, K_I=0.75, K_D=0.3, n=40, debug=False) self._speed_controller = PIDController(K_P=5.0, K_I=0.5, K_D=1.0, n=40) self._waypoint_planner = RoutePlanner(4.0, 50) self._command_planner = RoutePlanner(7.5, 25.0, 257) self._waypoint_planner.set_route(global_plan_gps, True) ds_ids = downsample_route(global_plan_world_coord, 50) global_plan_gps = [global_plan_gps[x] for x in ds_ids] self._command_planner.set_route(global_plan_gps, True) window_size = 50 self.speed_error_window = deque([0 for _ in range(window_size)], maxlen=window_size) self.angle_window = deque([0 for _ in range(window_size)], maxlen=window_size) self.max_speed_error = 0 self.min_speed_error = 0 self.max_angle = 0 self.min_angle = 0 self.error_hist = pd.DataFrame(data=[], columns=[ 'target_speed', 'speed_error', 'angle', ])
def __init__(self, factory): """Constructor. Args: factory (factory.FactoryCreate) """ # read file and get all paths self.img = lab11_image.VectorImage("lab11_img1.yaml") # init objects self.create = factory.create_create() self.time = factory.create_time_helper() self.penholder = factory.create_pen_holder() self.tracker = Tracker(factory.create_tracker, 1, sd_x=0.01, sd_y=0.01, sd_theta=0.01, rate=10) self.servo = factory.create_servo() self.odometry = Odometry() # alpha for tracker self.alpha_x = 0.3 self.alpha_y = self.alpha_x self.alpha_theta = 0.3 # init controllers self.pidTheta = PIDController(200, 5, 50, [-10, 10], [-200, 200], is_angle=True) self.pidDistance = PIDController(500, 0, 50, [0, 0], [-200, 200], is_angle=False) self.filter = ComplementaryFilter( self.odometry, None, self.time, 0.4, (self.alpha_x, self.alpha_y, self.alpha_theta)) # parameters self.base_speed = 100 # constant self.robot_marker_distance = 0.1906 # debug vars self.debug_mode = True self.odo = [] self.actual = [] self.xi = 0 self.yi = 0 self.init = True self.penholder_depth = -0.025
def __init__(self, factory): """Constructor. Args: factory (factory.FactoryCreate) """ self.create = factory.create_create() self.time = factory.create_time_helper() self.odometry = Odometry() self.pd_controller = PDController(500, 100, -75, 75) # self.pid_controller = PIDController(500, 100, 0, -75, 75, -50, 50) self.pid_controller = PIDController(500, 100, 10, -100, 100, -100, 100) self.pid_controller_dist = PIDController(500, 100, 10, -100, 100, -100, 100)
def __init__(self, factory): """Constructor. Args: factory (factory.FactoryCreate) """ self.create = factory.create_create() self.time = factory.create_time_helper() self.servo = factory.create_servo() self.sonar = factory.create_sonar() # Add the IP-address of your computer here if you run on the robot self.virtual_create = factory.create_virtual_create() self.map = lab8_map.Map("lab8_map.json") self.odometry = Odometry() self.pidTheta = PIDController(300, 5, 50, [-10, 10], [-200, 200], is_angle=True) # constants self.base_speed = 100 self.variance_sensor = 0.1 self.variance_distance = 0.01 self.variance_direction = 0.05 self.world_width = 3.0 # the x self.world_height = 3.0 # the y self.filter = ParticleFilter(self.virtual_create, self.variance_sensor, self.variance_distance, self.variance_direction, num_particles=100, world_width=self.world_width, world_height=self.world_height)
def __init__(self): rospy.init_node('altitude_hold_controller') self.input_rc = [1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000] self.node_identifier = 1 self.sub1 = rospy.Subscriber('/phx/fc/rc', RemoteControl, self.rcCallback) self.sub2 = rospy.Subscriber('/phx/marvicAltitude/altitude', Altitude, self.altitudeCallback) self.autopilot_commands = rospy.Subscriber( '/phx/controller_commands', ControllerCmd, self.controllerCommandCallback) # self.rc_pub = rospy.Publisher('/phx/rc_computer', RemoteControl, queue_size=1) self.altitude_pub = rospy.Publisher('/phx/autopilot/input', AutoPilotCmd, queue_size=1) setPoint_p = 1 self.enabled = True p = 1 i = 0 d = 4 setPoint_d = 0 i_stop = 1 controlCommand = 1500 self.altitudeController = PIDController(controlCommand, p, i, d, setPoint_p, i_stop, setPoint_d, 0) self.freq = 100 # Hz self.r = rospy.Rate(self.freq) self.previousAltitude = 0 self.firstCall = 1
def __init__(self): rospy.init_node('landing_controller') self.node_identifier = 3 self.input_rc = [1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000] self.sub_imu = rospy.Subscriber('/phx/imu', Imu, self.imuCallback) # self.autopilot_commands = rospy.Subscriber('/phx/controller_commands', ControllerCmd, self.controllerCommandCallback) self.sub = rospy.Subscriber('/phx/marvicAltitude/altitude', Altitude, self.altitudeCallback) self.pub = rospy.Publisher('/phx/rc_computer', RemoteControl, queue_size=1) # self.autopilot_commands = rospy.Subscriber('/phx/controller_commands', ControllerCmd, self.controllerCommandCallback) self.pub = rospy.Publisher('/phx/autopilot/input', AutoPilotCmd, queue_size=1) self.enabled = True self.p = 1 self.d = 5 self.setPoint_d = 9.81 self.setPoint = 1 self.freq = 100 # Hz self.r = rospy.Rate(self.freq) self.altitude_start = 1 self.altitude = 0.0 self.linear_acceleration_z = 0.0 self.controlCommand = 1500 self.landController = PIDController(1500, 1, 0, 5, 1, 0, 9.81, 0)
async def stage1(self, id): """Position the drone above the large central target, returns true if successful""" print("Docking stage 1") self.log.writeLiteral("Docking stage 1") debug_window=DebugWindow(1,self.target) failed_frames = 0 # Number of frames since we detected the target successful_frames = 0 # Number of frames within tolerance pid_controller = PIDController(self.dt) while True: start_millis = int(round(time.time() * 1000)) img = self.camera_simulator.updateCurrentImage(self.east, self.north, self.down * -1.0, self.yaw) errs = self.image_analyzer.process_image(img, 0, self.yaw) if errs is None: self.log.writeLiteral("No Errors") failed_frames = failed_frames + 1 if self.down * -1.0 - self.target.getAlt() > self.MAX_HEIGHT: # drone moves above max height, docking fails return False if failed_frames > 1 / self.dt: # haven't detected target in 1 second await self.drone.offboard.set_velocity_body( VelocityBodyYawspeed(0, 0, -0.2, 0)) await asyncio.sleep(self.dt) continue failed_frames = 0 x_err, y_err, alt_err, rot_err, tags_detected = errs debug_window.updateWindow(self.east, self.north, self.down * -1.0, self.yaw, tags_detected) if self.logging: self.log.write(self.east, self.north, self.down * -1.0, self.yaw, tags_detected,successful_frames,x_err,y_err,alt_err,rot_err) x_err, y_err, alt_err = self.offset_errors(x_err, y_err, alt_err, rot_err, id) east_velocity, north_velocity, down_velocity = pid_controller.get_velocities(x_err, y_err, alt_err, 0.4) # Checks if drone is aligned with central target if util.is_between_symm(alt_err, self.STAGE_1_TOLERANCE) and util.is_between_symm(x_err, self.STAGE_1_TOLERANCE) and util.is_between_symm(y_err, self.STAGE_1_TOLERANCE): successful_frames += 1 else: successful_frames = 0 # Ends loop if aligned for 1 second if successful_frames == 1 / self.dt: break await self.drone.offboard.set_velocity_ned( VelocityNedYaw(north_m_s=north_velocity, east_m_s=east_velocity, down_m_s=down_velocity, yaw_deg=rot_err)) # north, east, down (all m/s), yaw (degrees, north is 0, positive for clockwise) current_millis = int(round(time.time() * 1000)) if current_millis - start_millis < self.dt: await asyncio.sleep(self.dt - (current_millis - start_millis)) debug_window.destroyWindow() return True
def testTimeSeriesRampInput(self): kp = 0.2 ki = 0.02 kd = 0.1 p = PIDController(kp, 0.0, 0.0) i = PIDController(0.0, ki, 0.0) d = PIDController(0.0, 0.0, kd) t = 0.0 dt = 0.008 i_command_1 = 0.0 i_command_2 = 0.0 i_command_3 = 0.0 while t < 10.0: t += dt global_time.updateTime(t) desired = t measured = -2.0 * t p_command = p.update(desired, measured) i_command = i.update(desired, measured) d_command = d.update(desired, measured) pid_command = self.pid.update(desired, measured) self.assertAlmostEqual(p_command, kp * 3.0 * t) # Testing the integral here is hard because of the discrete-time # approximation. # 1st derivative is positive self.assertGreater(i_command, i_command_1) # 2nd derivative is positive i_diff = i_command - i_command_1 i_diff_1 = i_command_1 - i_command_2 self.assertGreater(i_diff, i_diff_1) # 3rd derivative is zero i_diff_2 = i_command_2 - i_command_3 self.assertAlmostEqual(i_diff - i_diff_1, i_diff_1 - i_diff_2, 5) i_command_3 = i_command_2 i_command_2 = i_command_1 i_command_1 = i_command self.assertAlmostEqual(d_command, kd * 3.0) self.assertAlmostEqual(pid_command, p_command + i_command + d_command)
distances = [(entity.position_of_center_of_gravity - position).norm() for entity in s.entities] if min(distances) > 5 * radius: s.add_entity( Planet(position_init=position, radius=radius, density=5 + np.random.rand() * 10, color=color, athmosphere_radius=athmosphere_radius, athmosphere_density=np.random.rand() * 10, athmosphere_color=athmosphere_color, max_wind=np.random.randint(2) * np.random.randint(-20, 20))) pid1 = PIDController(k_proportional=0.5, k_integral=0, k_derivative=0) pid2 = PIDController(k_proportional=0, k_integral=0, k_derivative=0) pid3 = PIDController(k_proportional=0, k_integral=0, k_derivative=0) pid4 = PIDController(k_proportional=0, k_integral=0, k_derivative=0) s.add_entity(rocket := Rocket( position_init=Vector(0, -3), orientation_init=np.pi, mass=100, max_thrust=250, max_thrust_thrusters=100, height=2, diameter=0.3, rel_height_pressure_center=0.2, throttle_fn=lambda: -1 * (s.joystick.get_axis(3) - 1) / 2, vector_fn=lambda: pid1.get(-rocket.velocity_angular, -s.joystick. get_axis(0)),
async def stage2(self, id): """Position the drone above the peripheral target and descend""" print("Docking stage 2") self.log.writeLiteral("Docking stage 2") debug_window = DebugWindow(2,self.target) pid_controller = PIDController(self.dt) successful_frames = 0 while True: img = self.camera_simulator.updateCurrentImage(self.east, self.north, self.down * -1.0, self.yaw,id) errs = self.image_analyzer.process_image(img, id, self.yaw) checked_frames = 0 docking_attempts = 0 # Waits for one second to detect target tag, ascends to find central target if fails while errs is None: checked_frames += 1 self.log.writeLiteral("No errors, checked frames: "+str(checked_frames)) await self.drone.offboard.set_velocity_body(VelocityBodyYawspeed(0, 0, -0.1, 0)) if checked_frames > 1 / self.dt: docking_attempts += 1 if docking_attempts > self.MAX_ATTEMPTS: print("Docking failed") self.log.writeLiteral("Docking failed") await self.safe_land() return img = self.camera_simulator.updateCurrentImage(self.east, self.north, self.down * -1.0, self.yaw,id) errs = self.image_analyzer.process_image(img,0,self.yaw) # Ascends until maximum height or until peripheral target detected while errs is None and self.down * -1.0 - self.target.getAlt() < self.MAX_HEIGHT_STAGE_2: await self.drone.offboard.set_velocity_body(VelocityBodyYawspeed(0, 0, -0.2, 0)) img = self.camera_simulator.updateCurrentImage(self.east, self.north, self.down * -1.0, self.yaw, id) errs = self.image_analyzer.process_image(img, id, self.yaw) if not errs is None: break # If peripheral tag not found, ascends until maximum height or until central target detected while errs is None and self.down * -1.0 - self.target.getAlt() < self.MAX_HEIGHT: await self.drone.offboard.set_velocity_body(VelocityBodyYawspeed(0, 0, -0.2, 0)) img = self.camera_simulator.updateCurrentImage(self.east, self.north, self.down * -1.0, self.yaw) errs = self.image_analyzer.process_image(img,0, self.yaw) # Re-attempts stage 1 if not errs is None: await self.stage1(id) img = self.camera_simulator.updateCurrentImage(self.east, self.north, self.down * -1.0, self.yaw, id) errs = self.image_analyzer.process_image(img, id, self.yaw) checked_frames = 0 else: # If the central target cannot be found at maximum height (maybe could have re-attempt stage 1 again, not sure) print("Docking failed") await self.safe_land() return await asyncio.sleep(self.dt) img = self.camera_simulator.updateCurrentImage(self.east, self.north, self.down * -1.0, self.yaw,id) errs = self.image_analyzer.process_image(img, id,self.yaw) x_err, y_err, alt_err, rot_err, tags_detected = errs alt_err = alt_err - .05 if self.logging: self.log.write(self.east, self.north, self.down * -1.0, self.yaw, tags_detected,successful_frames,x_err,y_err,alt_err,rot_err) # Checks if drone is aligned with docking if alt_err < self.STAGE_2_TOLERANCE * 2 and alt_err > -1 * self.STAGE_2_TOLERANCE and rot_err < 2.0 and rot_err > -2.0 and x_err > -1 * self.STAGE_2_TOLERANCE and x_err < 1 * self.STAGE_2_TOLERANCE and y_err > -1 * self.STAGE_2_TOLERANCE and y_err<self.STAGE_2_TOLERANCE: successful_frames += 1 else: successful_frames = 0 # Adjusts errors for distance to target to prevent overshoot # Adjusted errors asymptote to 1 as alt_err increases, goes to 0 as alt_err decreases OVERSHOOT_CONSTANT=.6 # use to adjust speed of descent, higher constant means faster, lower means less overshooting # x_err*=np.tanh(OVERSHOOT_CONSTANT*alt_err*alt_err) # y_err*=np.tanh(OVERSHOOT_CONSTANT*alt_err*alt_err) # rot_err*=np.tanh(OVERSHOOT_CONSTANT*alt_err*alt_err) # alt_err*=np.tanh(OVERSHOOT_CONSTANT*alt_err*alt_err) # Ends loop if aligned for 1 second if successful_frames == 1 / self.dt: break debug_window.updateWindow(self.east, self.north, self.down * -1.0, self.yaw, tags_detected) east_velocity, north_velocity, down_velocity = pid_controller.get_velocities(x_err, y_err, alt_err, 0.1) await self.drone.offboard.set_velocity_ned( VelocityNedYaw(north_m_s=north_velocity, east_m_s=east_velocity, down_m_s=down_velocity, yaw_deg=rot_err)) # north, east, down (all m/s), yaw (degrees, north is 0, positive for clockwise) await asyncio.sleep(self.dt)
def main(): if len(sys.argv) == 2: seed = int(sys.argv[1]) else: seed = random.randint(0, 100) # Render preferences matplotlib = 0 irrlicht = 1 # Chrono Simulation step size ch_step_size = 1e-2 # Matplotlib Simulation step size mat_step_size = 1e-2 # ------------ # Create track # ------------ reversed = random.randint(0, 1) track = RandomTrack() track.generateTrack(seed=seed, reversed=reversed) print('Using seed :: {}'.format(seed)) # -------------------- # Create controller(s) # -------------------- # Lateral controller (steering) lat_controller = PIDLateralController(track.center) lat_controller.SetGains(Kp=0.4, Ki=0, Kd=0.3) lat_controller.SetLookAheadDistance(dist=5) # Longitudinal controller (throttle and braking) long_controller = PIDLongitudinalController(track.center) long_controller.SetGains(Kp=0.4, Ki=0, Kd=0.45) long_controller.SetLookAheadDistance(dist=5) long_controller.SetTargetSpeed(speed=10.0) # PID controller (wraps both lateral and longitudinal controllers) controller = PIDController(lat_controller, long_controller) # ------------------------ # Create chrono components # ------------------------ setDataDirectory() # Create chrono system system = createChronoSystem() # Calculate initial position and initial rotation of vehicle initLoc, initRot = calcPose(track.center.points[0], track.center.points[1]) # Create chrono vehicle vehicle = ChronoVehicle(ch_step_size, system, controller, irrlicht=irrlicht, vehicle_type='json', initLoc=initLoc, initRot=initRot, vis_balls=True) # Create chrono terrain terrain = ChronoTerrain(ch_step_size, system, irrlicht=irrlicht, terrain_type='concrete') vehicle.SetTerrain(terrain) # Create chrono wrapper chrono_wrapper = ChronoWrapper(ch_step_size, system, track, vehicle, terrain, irrlicht=irrlicht, draw_barriers=True) if matplotlib: matplotlib_wrapper = MatplotlibWrapper(mat_step_size, vehicle, render_step_size=1.0 / 20) matplotlib_wrapper.plotTrack(track) ch_time = mat_time = 0 while True: # Update controllers if vehicle.vehicle.GetVehicleSpeed() < 7: lat_controller.SetGains(Kp=0.2, Ki=0, Kd=0.6) elif vehicle.vehicle.GetVehicleSpeed() < 8: lat_controller.SetGains(Kp=0.3, Ki=0, Kd=0.45) else: lat_controller.SetGains(Kp=0.4, Ki=0, Kd=0.3) # Update controller controller.Advance(ch_step_size, vehicle) if chrono_wrapper.Advance(ch_step_size) == -1: chrono_wrapper.Close() break if matplotlib and ch_time >= mat_time: if not matplotlib_wrapper.Advance(mat_step_size): print("Quit message received.") matplotlib_wrapper.close() break mat_time += mat_step_size ch_time += ch_step_size if ch_time > 50: break print("Exited") pass
tf = 30 # end time, (sec) time = np.linspace(t0, tf, N) dt = time[1] - time[0] # delta t, (sec) ################################################################################## # Core simulation code # Inital conditions (i.e., initial state vector) y = [0, 0] #y[0] = initial altitude, (m) #y[1] = initial speed, (m/s) # Initialize array to store values soln = np.zeros((len(time), len(y))) # Create instance of PID_Controller class pid = PIDController() # Set the Kp value of the controller pid.setKP(kp) # Set the Ki value of the controller pid.setKI(ki) # Set the Kd value of the controller pid.setKD(kd) # Set altitude target r = 10 # meters pid.setTarget(r) # Simulate quadrotor motion
def main(): global first, time if len(sys.argv) == 2: seed = int(sys.argv[1]) else: seed = random.randint(0, 100) # Render preferences matplotlib = 1 irrlicht = 1 # Chrono Simulation step size ch_step_size = 1e-2 # Matplotlib Simulation step size mat_step_size = 1e-2 # ------------ # Create track # ------------ reversed = random.randint(0, 1) track = RandomTrack(x_max=200, y_max=200, width=20) track.generateTrack(seed=seed, reversed=reversed) print('Using seed :: {}'.format(seed)) # ---------------- # Create obstacles # ---------------- # Change n to add more obstacles obstacles = RandomObstacleGenerator.generateObstacles( track.center, i_min=50, i_max=60, n=1, seed=seed * random.randint(0, 90), reversed=reversed, movement_rate=5, movement_distance=1) print(obstacles) # Is a python dictionary # To access: obstacles[<path_index>] = position_vector # -------------------- # Create controller(s) # -------------------- lat_controller = PIDLateralController(track, obstacles) lat_controller.SetGains(Kp=0.4, Ki=0, Kd=0.3) lat_controller.SetLookAheadDistance(lookDist=5) lat_controller.SetObstacleDistance(obsDist=50) long_controller = PIDLongitudinalController(track.center) long_controller.SetGains(Kp=0.4, Ki=0, Kd=0.45) long_controller.SetLookAheadDistance(dist=5) long_controller.SetTargetSpeed(speed=6.0) # PID controller (wraps both lateral and longitudinal controllers) controller = PIDController(lat_controller, long_controller) # ------------------------ # Create chrono components # ------------------------ setDataDirectory() # Create chrono system system = createChronoSystem() # Calculate initial position and initial rotation of vehicle initLoc, initRot = calcPose(track.center.points[0], track.center.points[1]) # Create chrono vehicle vehicle = ChronoVehicle(ch_step_size, system, controller, irrlicht=irrlicht, vehicle_type='json', initLoc=initLoc, initRot=initRot, vis_balls=True) # Create chrono terrain terrain = ChronoTerrain(ch_step_size, system, irrlicht=irrlicht, terrain_type='concrete') vehicle.SetTerrain(terrain) # Create chrono wrapper chrono_wrapper = ChronoWrapper(ch_step_size, system, track, vehicle, terrain, irrlicht=irrlicht, obstacles=obstacles, draw_barriers=True) if matplotlib: matplotlib_wrapper = MatplotlibWrapper(mat_step_size, vehicle, obstacles=obstacles, render_step_size=1.0 / 20) matplotlib_wrapper.plotTrack(track) ch_time = mat_time = 0 while True: # Update controller controller.Advance(ch_step_size, vehicle) if vehicle.vehicle.GetVehicleSpeed() < 7: lat_controller.SetGains(Kp=0.2, Ki=0, Kd=0.6) elif vehicle.vehicle.GetVehicleSpeed() < 8: lat_controller.SetGains(Kp=0.3, Ki=0, Kd=0.45) else: lat_controller.SetGains(Kp=0.4, Ki=0, Kd=0.3) if chrono_wrapper.Advance(ch_step_size) == -1: chrono_wrapper.Close() break if matplotlib and ch_time >= mat_time: if not matplotlib_wrapper.Advance(mat_step_size): print("Quit message received.") matplotlib_wrapper.close() break mat_time += mat_step_size ch_time += ch_step_size if ch_time > 50: break print("Exited") pass
import pyfly from pid_controller import PIDController from pyfly.pyfly import PyFly sim = PyFly("pyfly_config.json", "x8_param.mat") sim.seed(0) sim.reset(state={"roll": -0.5, "pitch": 0.15}) pid = PIDController(sim.dt) pid.set_reference(phi=0.2, theta=0, va=22) for step_i in range(500): phi = sim.state["roll"].value theta = sim.state["pitch"].value Va = sim.state["Va"].value omega = [sim.state["omega_p"].value, sim.state["omega_q"].value, sim.state["omega_r"].value] action = pid.get_action(phi, theta, Va, omega) success, step_info = sim.step(action) if not success: break sim.render(block=True)
def evaluate(const, params, ftype): print('Start evaluation exp: {} and {} and {}'.format( args.model_dir, args.data_type, args.ftype)) # Total running steps T = params['freq'] * params['simT'] # Target trajectory for evaluation if 'sine' in args.eval_type and 'Hz' in args.eval_type: target_traj = sin_target_traj(params['freq'], params['simT'], sine_type=args.eval_type) elif 'random_walk' in args.eval_type: target_traj = random_walk(T, args.eval_type) elif 'sine_freq_variation' == args.eval_type: freq_from = 0.5 freq_to = 10. sys_freq = params['freq'] simT = params['simT'] sine_type = 'sine_1Hz_10deg_0offset' target_traj = sin_freq_variation(freq_from, freq_to, sys_freq, simT, sine_type) elif 'sine_freq_variation_with_step' == args.eval_type: freq_from = 0.5 freq_to = 10. sys_freq = params['freq'] simT = params['simT'] sine_type = 'sine_1Hz_10deg_0offset' target_traj = sin_freq_variation_with_step(freq_from, freq_to, sys_freq, simT, sine_type) elif 'step' in args.eval_type: target_traj = step_target_traj(T, args.eval_type) else: raise Exception('I dont know your targets') # initiate values if 'step' in args.eval_type: t_OBS_vals = [ torch.FloatTensor([0]), torch.FloatTensor([0]), torch.FloatTensor([0]), torch.FloatTensor([0]) ] else: t_OBS_vals = [ torch.FloatTensor([target_traj[0]]), torch.FloatTensor([target_traj[1]]), torch.FloatTensor([target_traj[2]]), torch.FloatTensor([target_traj[3]]) ] f1_EST_vals = [torch.zeros(1)] F_EST = torch.zeros(1) # for plotting time_stamp = [] target_history = [] obs_history = [] est_history = [] f1_obs_history = [] f1_est_history = [] F_est_history = [] # Define PID controller Kp = params['Kp'] Kd = params['Kd'] * Kp Ki = params['Ki'] * Kp pid_cont = PIDController(p=Kp, i=Ki, d=Kd, del_t=const['del_t'] * 10) # Define models TODO for now we ignore f2. f1_OBS_fn = RealFriction(const) f1_EST_fn = Friction_EST(params['hdim']) if ftype == 2: state_dict_path = os.path.join(args.model_dir, args.data_type, 'MLP', 'f1_model') state_dict = torch.load(state_dict_path) f1_EST_fn.load_state_dict(state_dict) for t in range(4, T): # current target target = target_traj[t] # detach nodes for simplicity f1 = f1_EST_vals[-1].detach() # compute input force (F) at t-2 if t % 10 == 3: const['T_d'] = pid_cont.compute_torque(target, t_OBS_vals[-1]) F_EST = compute_input_force(const, t_OBS_vals[-1], f1) # compute frictions (f) at t-2 t_dot_OBS = (t_OBS_vals[-2] - t_OBS_vals[-3]) / const['del_t'] f1_OBS = f1_OBS_fn(t_OBS_vals[-2], t_OBS_vals[-3], F_EST) if ftype == 0: f1_EST = torch.zeros(1) elif ftype == 1: f1_EST = f1_OBS_fn(t_OBS_vals[-2], t_OBS_vals[-3], F_EST) elif ftype == 2: f1_EST = f1_EST_fn(torch.cat([t_OBS_vals[-2], t_dot_OBS, F_EST])) else: raise NotImplementedError() # compute theta_hat (t) at t t_OBS = compute_theta_hat(const, t_OBS_vals[-1], t_OBS_vals[-2], t_OBS_vals[-3], F_EST, f1_OBS) t_EST = compute_theta_hat(const, t_OBS_vals[-1], t_OBS_vals[-2], t_OBS_vals[-3], F_EST, f1_EST) # store values to containers t_OBS_vals[-4] = t_OBS_vals[-3] t_OBS_vals[-3] = t_OBS_vals[-2] t_OBS_vals[-2] = t_OBS_vals[-1] t_OBS_vals[-1] = t_OBS f1_EST_vals[-1] = f1_EST # store history for plotting time_stamp.append(float(t / params['freq'])) target_history.append(float(target.numpy())) obs_history.append(float(t_OBS.numpy())) est_history.append(float(t_EST.detach().numpy())) f1_obs_history.append(float(f1_OBS.detach().numpy())) f1_est_history.append(float(f1_EST.detach().numpy())) F_est_history.append(float(F_EST.detach().numpy())) # for debugging # if np.isnan(t_OBS.numpy()): # break # store values for post-visualization params_dir = os.path.join(args.model_dir, args.data_type) if ftype == 0: eval_log_dir = os.path.join(params_dir, 'f_est=0', 'evaluation', args.eval_type) elif ftype == 1: eval_log_dir = os.path.join(params_dir, 'oracle', 'evaluation', args.eval_type) elif ftype == 2: eval_log_dir = os.path.join(params_dir, 'MLP', 'evaluation', args.eval_type) if not os.path.exists(eval_log_dir): os.makedirs(eval_log_dir) store_logs(time_stamp, target_history, obs_history, est_history, f1_obs_history, f1_est_history, F_est_history, eval_log_dir) # visualize plot_theta(time_stamp, target_history, obs_history, est_history, eval_log_dir)
def main(): # --------------- # Create a system # System's handle the simulation settings system = wa.WASystem(args=args) # --------------------------- # Create a simple environment # Environment will create a track like path for the vehicle env_filename = wa.WASimpleEnvironment.EGP_ENV_MODEL_FILE environment = wa.WASimpleEnvironment(system, env_filename) # -------------------------------- # Create the vehicle inputs object # This is a shared object between controllers, visualizations and vehicles vehicle_inputs = wa.WAVehicleInputs() # ---------------- # Create a vehicle # Uses a premade json specification file that describes the properties of the vehicle init_pos = wa.WAVector([49.8, 132.9, 0.5]) veh_filename = wa.WALinearKinematicBicycle.GO_KART_MODEL_FILE vehicle = wa.WALinearKinematicBicycle(system, vehicle_inputs, veh_filename, init_pos=init_pos) # ------------- # Create a Path # Load data points from a csv file and interpolate a path filename = wa.get_wa_data_file("paths/sample_medium_loop.csv") points = wa.load_waypoints_from_csv(filename, delimiter=",") path = wa.WASplinePath(points, num_points=1000, is_closed=True) # ------------------ # Create n opponents opponents = [] opponent_vehicle_inputs_list = [] num_opponents = args.num_opponents for i in range(num_opponents): opponent_init_pos = wa.WAVector(points[i + 1]) opponent_vehicle_inputs = wa.WAVehicleInputs() opponent = wa.WALinearKinematicBicycle(system, opponent_vehicle_inputs, veh_filename, init_pos=opponent_init_pos) opponents.append(opponent) opponent_vehicle_inputs_list.append(opponent_vehicle_inputs) # ---------------------- # Create a visualization # Will use matplotlib for visualization visualization = wa.WAMatplotlibVisualization(system, vehicle, vehicle_inputs, environment=environment, plotter_type="multi", opponents=opponents) # ------------------- # Create a controller # Create a pid controller controllers = [PIDController(system, vehicle, vehicle_inputs, path)] for i in range(num_opponents): opponent_controller = PIDController(system, opponents[i], opponent_vehicle_inputs_list[i], path) controllers.append(opponent_controller) # -------------------------- # Create a simuation wrapper # Will be responsible for actually running the simulation sim_manager = wa.WASimulationManager(system, vehicle, visualization, *controllers, *opponents) # --------------- # Simulation loop step_size = system.step_size while sim_manager.is_ok(): time = system.time sim_manager.synchronize(time) sim_manager.advance(step_size)
def main(): if len(sys.argv) == 2: seed = int(sys.argv[1]) else: seed = random.randint(0, 100) # Render preferences matplotlib = 0 pyqtgraph = 1 irrlicht = 0 # Chrono simulation step size ch_step_size = 1e-2 # Matplotlib visualization step size mat_step_size = 1e-2 # PyQtGraph visualization step size pyqt_step_size = 1e-2 # ------------ # Create track # ------------ reversed = random.randint(0, 1) track = RandomTrack() track.generateTrack(seed=seed, reversed=reversed) print('Using seed :: {}'.format(seed)) # -------------------- # Create controller(s) # -------------------- # Lateral controller (steering) lat_controller = PIDLateralController(track.center) lat_controller.SetGains(Kp=0.4, Ki=0, Kd=0.25) lat_controller.SetLookAheadDistance(dist=5) # Longitudinal controller (throttle and braking) long_controller = PIDLongitudinalController() long_controller.SetGains(Kp=0.4, Ki=0, Kd=0) long_controller.SetTargetSpeed(speed=10.0) # PID controller (wraps both lateral and longitudinal controllers) controller = PIDController(lat_controller, long_controller) # ------------------------ # Create chrono components # ------------------------ setDataDirectory() # Create chrono system system = createChronoSystem() # Calculate initial position and initial rotation of vehicle initLoc, initRot = calcPose(track.center.points[0], track.center.points[1]) # Create chrono vehicle vehicle = ChronoVehicle(ch_step_size, system, controller, irrlicht=irrlicht, vehicle_type='json', initLoc=initLoc, initRot=initRot, vis_balls=True) # Create chrono terrain terrain = ChronoTerrain(ch_step_size, system, irrlicht=irrlicht, terrain_type='concrete') vehicle.SetTerrain(terrain) # Create chrono wrapper chrono_wrapper = ChronoWrapper(ch_step_size, system, track, vehicle, terrain, irrlicht=irrlicht, draw_barriers=True) if matplotlib: matplotlib_wrapper = MatplotlibWrapper(mat_step_size, vehicle, render_step_size=1.0 / 20) matplotlib_wrapper.plotTrack(track) elif pyqtgraph: pyqtgraph_wrapper = PyQtGraphWrapper(pyqt_step_size, vehicle, render_step_size=1.0 / 20) pyqtgraph_wrapper.plotTrack(track) pyqtgraph_wrapper.exec() ch_time = mat_time = pyqt_time = 0 updates = total_time = 0.0 while True: if chrono_wrapper.Advance(ch_step_size) == -1: chrono_wrapper.Close() break # Update controller controller.Advance(ch_step_size, vehicle) if matplotlib and ch_time >= mat_time: if not matplotlib_wrapper.Advance(mat_step_size): print("Quit message received.") matplotlib_wrapper.close() break mat_time += mat_step_size if pyqtgraph and ch_time >= pyqt_time: if not pyqtgraph_wrapper.Advance(pyqt_step_size): print("Quit message received.") pyqtgraph_wrapper.close() break pyqt_time += pyqt_step_size ch_time += ch_step_size if ch_time > 50: break updates += 1.0 # print('Update Summary:\n\tChrono Time :: {0:0.2f}\n\tTime per Update :: {1:0.5f}'.format(ch_time, total_time / updates)) print("Exited") pass
def setUp(self): resetTimeSourceForTestingPurposes(global_time) self.pid = PIDController(0.2, 0.02, 0.1)
class LimbController: def __init__(self, config_file="leg_model.conf", section="LimbController"): c = ConfigParser.ConfigParser() if not path.exists(path.abspath(config_file)): print 'Config file %s not found!' % config_file raise IOError c.read(config_file) def readFloatWithDefault(c, sec, name, val=0.0): try: return c.getfloat(sec, name) except ConfigParser.Error, mesg: print mesg print "Using default value %f for %s:%s" % (val, sec, name) return val # Default PID values self.kparray = array([ readFloatWithDefault(c, section, 'yaw_p'), readFloatWithDefault(c, section, 'hp_p'), readFloatWithDefault(c, section, 'kp_p') ]) self.kiarray = array([ readFloatWithDefault(c, section, 'yaw_i'), readFloatWithDefault(c, section, 'hp_i'), readFloatWithDefault(c, section, 'kp_i') ]) self.kdarray = array([ readFloatWithDefault(c, section, 'yaw_d'), readFloatWithDefault(c, section, 'hp_d'), readFloatWithDefault(c, section, 'kp_d') ]) self.kffarray = array([ readFloatWithDefault(c, section, 'yaw_ff'), readFloatWithDefault(c, section, 'hp_ff'), readFloatWithDefault(c, section, 'kp_ff') ]) self.kfaarray = array([ readFloatWithDefault(c, section, 'yaw_fa'), readFloatWithDefault(c, section, 'hp_fa'), readFloatWithDefault(c, section, 'kp_fa') ]) self.dearray = array([ readFloatWithDefault(c, section, 'yaw_de'), readFloatWithDefault(c, section, 'hp_de'), readFloatWithDefault(c, section, 'kp_de') ]) self.drarray = array([ readFloatWithDefault(c, section, 'yaw_dr'), readFloatWithDefault(c, section, 'hp_dr'), readFloatWithDefault(c, section, 'kp_dr') ]) self.proparray = array([ readFloatWithDefault(c, section, 'yaw_prop', 1.0), readFloatWithDefault(c, section, 'hp_prop', 1.0), readFloatWithDefault(c, section, 'kp_prop', 1.0) ]) self.length_rate_commands = [] self.pid_controllers = [ PIDController() for i in range(len(self.kparray)) ] self.updateGainConstants(self.kparray, self.kiarray, self.kdarray, self.kffarray, self.kfaarray) self.amount_of_joints = len(self.kp) self.desired_vel_array = array([0, 0, 0])
def main(): global first, time if len(sys.argv) == 2: seed = int(sys.argv[1]) else: seed = random.randint(0, 100) # Render preferences matplotlib = 0 irrlicht = 1 # Chrono Simulation step size ch_step_size = 1e-2 # Matplotlib Simulation step size mat_step_size = 1e-2 # ------------ # Create track # ------------ reversed = 1 # random.randint(0,1) track = RandomTrack(width=20) track.generateTrack(seed=seed, reversed=reversed) print('Using seed :: {}'.format(seed)) # -------------------- # Create controller(s) # -------------------- lat_controller = PIDLateralController(track.center) lat_controller.SetGains(Kp=0.4, Ki=0, Kd=0.25) lat_controller.SetLookAheadDistance(dist=5) long_controller = PIDLongitudinalController() long_controller.SetGains(Kp=0.4, Ki=0, Kd=0) long_controller.SetTargetSpeed(speed=10.0) # PID controller (wraps both lateral and longitudinal controllers) controller = PIDController(lat_controller, long_controller) # ------------------------ # Create chrono components # ------------------------ setDataDirectory() # Create chrono system system = createChronoSystem() # Calculate initial position and initial rotation of vehicle initLoc, initRot = calcPose(track.center.points[0], track.center.points[1]) # Create chrono vehicle vehicle = ChronoVehicle(ch_step_size, system, controller, irrlicht=irrlicht, vehicle_type='json', initLoc=initLoc, initRot=initRot, vis_balls=True) # Create chrono terrain terrain = ChronoTerrain(ch_step_size, system, irrlicht=irrlicht, terrain_type='concrete') vehicle.SetTerrain(terrain) # ------------------ # Create opponent(s) # ------------------ opponents = [] n = 1 for i in range(n): opponent_track = Track(track.center.waypoints, width=track.width / 2) opponent_track.generateTrack() if i % 3 == 0: opponent_path = opponent_track.left elif i % 3 == 1: opponent_path = opponent_track.right else: opponent_path = opponent_track.center opponent_lat_controller = PIDLateralController(opponent_path) opponent_lat_controller.SetGains(Kp=0.4, Ki=0, Kd=0.25) opponent_lat_controller.SetLookAheadDistance(dist=5) opponent_long_controller = PIDLongitudinalController() opponent_long_controller.SetGains(Kp=0.4, Ki=0, Kd=0) opponent_long_controller.SetTargetSpeed(speed=5.0) opponent_controller = PIDController(opponent_lat_controller, opponent_long_controller) opponent_initLoc, opponent_initRot = calcRandomPose( opponent_path, s_min=opponent_path.s[100 + i * 100], s_max=opponent_path.s[100 + i * 100]) opponent_vehicle = ChronoVehicle(ch_step_size, system, opponent_controller, irrlicht=irrlicht, vehicle_type='json', initLoc=opponent_initLoc, initRot=opponent_initRot) opponent_vehicle.SetTerrain(terrain) opponent = Opponent(opponent_vehicle, opponent_controller) opponents.append(opponent) controller.SetOpponents(opponents) # Create chrono wrapper chrono_wrapper = ChronoWrapper(ch_step_size, system, track, vehicle, terrain, irrlicht=irrlicht, opponents=opponents, draw_barriers=True) if matplotlib: matplotlib_wrapper = MatplotlibWrapper(mat_step_size, vehicle, opponents=opponents) matplotlib_wrapper.plotTrack(track) ch_time = mat_time = 0 while True: if chrono_wrapper.Advance(ch_step_size) == -1: chrono_wrapper.Close() break controller.Advance(ch_step_size, vehicle, perception_distance=30) for opponent in opponents: opponent.Update(ch_time, ch_step_size) if matplotlib and ch_time >= mat_time: if not matplotlib_wrapper.Advance(mat_step_size): print("Quit message received.") matplotlib_wrapper.close() break mat_time += mat_step_size ch_time += ch_step_size if ch_time > 50: break print("Exited") pass
def main(): # --------------- # Create a system # Systems describe simulation settings and can be used to # update dynamics system = wa.WAChronoSystem(args=args) # --------------------- # Create an environment # An environment handles external assets (trees, barriers, etc.) and terrain characteristics # Pre-made evGrand Prix (EGP) env file env_filename = wa.WAChronoEnvironment.EGP_ENV_MODEL_FILE environment = wa.WAChronoEnvironment(system, env_filename) # -------------------------------- # Create the vehicle inputs object # This is a shared object between controllers, visualizations and vehicles vehicle_inputs = wa.WAVehicleInputs() # ---------------- # Create a vehicle # Pre-made go kart veh file init_loc = wa.WAVector([49.8, 132.9, 0.5]) veh_filename = wa.WAChronoVehicle.GO_KART_MODEL_FILE vehicle = wa.WAChronoVehicle(system, vehicle_inputs, environment, veh_filename, init_loc=init_loc) # ------------- # Create a Path # Load data points from a csv file and interpolate a path filename = wa.get_wa_data_file("paths/sample_medium_loop.csv") points = wa.load_waypoints_from_csv(filename, delimiter=",") path = wa.WASplinePath(points, num_points=1000, is_closed=True) # ------------------ # Create n opponents opponents = [] opponent_vehicle_inputs_list = [] num_opponents = args.num_opponents for i in range(num_opponents): opponent_init_loc = wa.WAVector(points[i+1]) opponent_init_loc.z = 0.1 opponent_vehicle_inputs = wa.WAVehicleInputs() opponent = wa.WAChronoVehicle(system, opponent_vehicle_inputs, environment, veh_filename, init_loc=opponent_init_loc) opponents.append(opponent) opponent_vehicle_inputs_list.append(opponent_vehicle_inputs) # ---------------------- # Create a visualization # It's nice to visualize the "look ahead" points for the controller # Add two spheres/dots for that purpose position = wa.WAVector() size = wa.WAVector([0.1, 0.1, 0.1]) kwargs = {'position': position, 'size': size, 'body_type': 'sphere', 'updates': True} sentinel_sphere = environment.create_body(name='sentinel', color=wa.WAVector([1, 0, 0]), **kwargs) target_sphere = environment.create_body(name='target', color=wa.WAVector([0, 1, 0]), **kwargs) # Will use irrlicht or matplotlib for visualization visualizations = [] if args.irrlicht: irr = wa.WAChronoIrrlicht(system, vehicle, vehicle_inputs, environment=environment, opponents=opponents) visualizations.append(irr) if args.sensor: sens = wa.WAChronoSensorVisualization(system, vehicle, vehicle_inputs, environment=environment) visualizations.append(sens) if args.matplotlib: mat = wa.WAMatplotlibVisualization(system, vehicle, vehicle_inputs, environment=environment, plotter_type="multi", opponents=opponents) visualizations.append(mat) # ------------------- # Create a controller # Create a pid controller controller = PIDController(system, vehicle, vehicle_inputs, path) controller.get_long_controller().set_target_speed(9) controller.get_long_controller().set_gains(0.1, 0, 1e-2) controllers = [controller] for i in range(num_opponents): opponent_controller = PIDController(system, opponents[i], opponent_vehicle_inputs_list[i], path) opponent_controller.get_long_controller().set_target_speed(9) opponent_controller.get_long_controller().set_gains(0.1, 0, 1e-2) controllers.append(opponent_controller) # -------------------------- # Create a simuation wrapper # Will be responsible for actually running the simulation sim_manager = wa.WASimulationManager(system, environment, vehicle, *visualizations, *controllers, *opponents) # --------------- # Simulation loop step_size = system.step_size while sim_manager.is_ok(): time = system.time sim_manager.synchronize(time) sim_manager.advance(step_size) # Update the position of the spheres target_sphere.position = controller.get_target_pos() sentinel_sphere.position = controller.get_sentinel_pos()
time = np.linspace(t0, tf, N) dt = time[1] - time[0] # delta t, (sec) ################################################################################## # Core simulation code # Inital conditions (i.e., initial state vector) y = [0, 0] #y[0] = initial altitude, (m) #y[1] = initial speed, (m/s) # Initialize array to store values soln = np.zeros((len(time),len(y))) # Create instance of PID_Controller class and # initalize and set all the variables pid = PIDController(kp = kp, ki = ki, kd = kd, max_windup = 1e6, u_bounds = [0, umax], alpha = alpha) # Set altitude target r = 10 # meters pid.setTarget(r) # Simulate quadrotor motion j = 0 # dummy counter for t in time: # Evaluate state at next time point y = ydot(y,t,pid) # Store results soln[j,:] = y j += 1 ##################################################################################
def __init__( self, impedance, timeStep ): self.controller = PIDController( timeStep )
def main(): if len(sys.argv) == 2: seed = int(sys.argv[1]) else: seed = random.randint(0, 100) # Render preferences matplotlib = 0 irrlicht = 1 import matplotlib.pyplot as plt plt.figure() # Chrono simulation step size ch_step_size = 1e-2 # Matplotlib visualization step size mat_step_size = 1e-2 # ------------ # Create track # ------------ createRandomTrack = True track = None if createRandomTrack: reversed = random.randint(0, 1) track = RandomTrack(x_max=50, y_max=50) track.generateTrack(seed=seed, reversed=reversed) else: print('Using seed :: {}'.format(seed)) points = [ [49.8, 132.9], [60.3, 129.3], [75.6, 129.0], [87.9, 131.7], [96.9, 129.6], [111.0, 120.0], [115.2, 110.7], [120.6, 96.9], [127.8, 88.5], [135.9, 77.4], [135.9, 65.1], [133.2, 51.3], [128.4, 43.2], [119.7, 36.3], [105.0, 35.7], [90.0, 36.3], [82.5, 46.2], [82.5, 63.6], [83.4, 82.2], [77.1, 93.9], [61.2, 88.5], [55.5, 73.5], [57.9, 54.6], [66.6, 45.0], [75.9, 36.3], [79.2, 25.5], [78.0, 13.2], [65.1, 6.0], [50.7, 6.0], [36.6, 11.7], [29.1, 21.3], [24.0, 36.9], [24.0, 56.1], [29.1, 70.8], [24.9, 77.7], [13.5, 77.7], [6.3, 81.6], [5.7, 92.7], [6.3, 107.7], [8.7, 118.2], [15.3, 122.7], [24.3, 125.4], [31.2, 126.0], [40.8, 129.6], [49.8, 132.9], ] track = Track(points) track.generateTrack() segmentation = Segmentations(track) segmentation.create_segmentations() stable_path = ga_search(segmentation) stable_path.generate_final_path() plt.clf() stable_path.generate_final_path() path = stable_path.final_path path.update_vmax() path.update_profile() #path.plot_speed_profile() # plt.show() # path.v_max = v # plt.pause(1) # -------------------- # Create controller(s) # -------------------- # Lateral controller (steering) lat_controller = PIDLateralController(path) lat_controller.SetGains(Kp=1.9, Ki=0.000, Kd=0.40) lat_controller.SetLookAheadDistance(dist=5) # Longitudinal controller (throttle and braking) long_controller = PIDLongitudinalController(path) long_controller.SetGains(Kp=0.4, Ki=0, Kd=0) long_controller.SetLookAheadDistance(dist=path.ps[0] * 2) # PID controller (wraps both lateral and longitudinal controllers) controller = PIDController(lat_controller, long_controller) # ------------------------ # Create chrono components # ------------------------ setDataDirectory() # Create chrono system system = createChronoSystem() # Calculate initial position and initial rotation of vehicle initLoc, initRot = calcPose(path.points[0], path.points[1]) # Create chrono vehicle vehicle = ChronoVehicle(ch_step_size, system, controller, irrlicht=irrlicht, vehicle_type='json', initLoc=initLoc, initRot=initRot, vis_balls=True) # Create chrono terrain terrain = ChronoTerrain(ch_step_size, system, irrlicht=irrlicht, terrain_type='concrete') vehicle.SetTerrain(terrain) # Create chrono wrapper chrono_wrapper = ChronoWrapper(ch_step_size, system, track, vehicle, terrain, irrlicht=irrlicht, draw_barriers=True) if matplotlib: matplotlib_wrapper = MatplotlibWrapper(mat_step_size, vehicle, render_step_size=1.0 / 20) path.plot(color='-b', show=False) matplotlib_wrapper.plotTrack(track) ch_time = mat_time = 0 updates = total_time = 0.0 while True: # if vehicle.vehicle.GetVehicleSpeed() < 7: # lat_controller.SetGains(Kp=0.2, Ki=0, Kd=0.6) # elif vehicle.vehicle.GetVehicleSpeed() < 8: # lat_controller.SetGains(Kp=0.3, Ki=0, Kd=0.45) # else: # lat_controller.SetGains(Kp=0.4, Ki=0, Kd=0.3) if chrono_wrapper.Advance(ch_step_size) == -1: chrono_wrapper.Close() break # Update controller controller.Advance(ch_step_size, vehicle) if matplotlib and ch_time >= mat_time: if not matplotlib_wrapper.Advance(mat_step_size, save=False): print("Quit message received.") matplotlib_wrapper.close() break mat_time += mat_step_size ch_time += ch_step_size if ch_time > 300: break updates += 1.0 # print('Update Summary:\n\tChrono Time :: {0:0.2f}\n\tTime per Update :: {1:0.5f}'.format(ch_time, total_time / updates)) print("Exited") pass
def main(): # During some time steps... # 1. Compute theta_hat by f_TRUE (1) # 2. Compute theta_hat by f_EST (2) # 3. Compute loss between (1) and (2) # 4. Optimize parameters of f_EST print('start exp: {} and {}'.format(args.model_dir, args.data_type)) # Simulation parameters and intial values const['del_t'] = float(1 / args.freq) # sampling time for dynamics T = args.freq * args.simT # number of operation for dynamics # Target trajectory if 'sine' in args.data_type and 'Hz' in args.data_type: target_traj = sin_target_traj( args.freq, args.simT, sine_type=args.data_type) elif 'random_walk' in args.data_type: target_traj = random_walk(T, args.data_type) elif 'sine_freq_variation' == args.data_type: freq_from = 0.5 freq_to = 10. sys_freq = args.freq simT = args.simT sine_type = 'sine_1Hz_10deg_0offset' target_traj = sin_freq_variation(freq_from, freq_to, sys_freq, simT, sine_type) elif 'sine_freq_variation_with_step' == args.data_type: freq_from = 0.5 freq_to = 10. sys_freq = args.freq simT = args.simT sine_type = 'sine_1Hz_10deg_0offset' target_traj = sin_freq_variation_with_step(freq_from, freq_to, sys_freq, simT, sine_type) elif 'step' in args.data_type: target_traj = step_target_traj(T, args.data_type) else: raise Exception('I dont know your targets') # initiate values t_OBS_vals = [ torch.FloatTensor([target_traj[0]]), torch.FloatTensor([target_traj[1]]), torch.FloatTensor([target_traj[2]]), torch.FloatTensor([target_traj[3]]) ] f1_EST_vals = [torch.zeros(1)] F_EST = torch.zeros(1) # NOTE 0 if f_est=0, 1 if f_est is oracle, 2 if f_est is MLP friction_type = args.ftype # for plotting time_stamp = [] target_history = [] obs_history = [] est_history = [] f1_obs_history = [] f1_est_history = [] F_est_history = [] # Define PID controller Kp = args.Kp Kd = args.Kd * Kp Ki = args.Ki * Kp pid_cont = PIDController(p=Kp, i=Ki, d=Kd, del_t=const['del_t'] * 10) # Define models TODO for now we ignore f2. f1_OBS_fn = RealFriction(const) f1_EST_fn = Friction_EST(args.hdim) # Define loss_fn, optimizer optimizer = torch.optim.Adam(f1_EST_fn.parameters(), lr=args.lr) loss_fn = nn.MSELoss() for t in range(4, T): # current target target = target_traj[t] # detach nodes for simplicity f1 = f1_EST_vals[-1].detach() # compute input force (F) at t-2 if t % 10 == 3: const['T_d'] = pid_cont.compute_torque(target, t_OBS_vals[-1]) F_EST = compute_input_force(const, t_OBS_vals[-1], f1) # compute frictions (f) at t-2 t_dot_OBS = (t_OBS_vals[-2] - t_OBS_vals[-3]) / const['del_t'] f1_OBS = f1_OBS_fn(t_OBS_vals[-2], t_OBS_vals[-3], F_EST) if friction_type == 0: f1_EST = torch.zeros(1) elif friction_type == 1: f1_EST = f1_OBS_fn(t_OBS_vals[-2], t_OBS_vals[-3], F_EST) elif friction_type == 2: f1_EST = f1_EST_fn(torch.cat([t_OBS_vals[-2], t_dot_OBS, F_EST])) else: raise NotImplementedError() # compute theta_hat (t) at t t_OBS = compute_theta_hat(const, t_OBS_vals[-1], t_OBS_vals[-2], t_OBS_vals[-3], F_EST, f1_OBS) t_EST = compute_theta_hat(const, t_OBS_vals[-1], t_OBS_vals[-2], t_OBS_vals[-3], F_EST, f1_EST) # Optimization # print(t, t_OBS, f1_OBS, F_EST, t_dot_OBS, target) if friction_type == 2: loss = loss_fn(t_EST, t_OBS) optimizer.zero_grad() loss.backward() optimizer.step() # print('loss={} at t={}'.format(loss.item(), t)) # store values to containers t_OBS_vals[-4] = t_OBS_vals[-3] t_OBS_vals[-3] = t_OBS_vals[-2] t_OBS_vals[-2] = t_OBS_vals[-1] t_OBS_vals[-1] = t_OBS f1_EST_vals[-1] = f1_EST # store history for plotting time_stamp.append(float(t / args.freq)) target_history.append(float(target.numpy())) obs_history.append(float(t_OBS.numpy())) est_history.append(float(t_EST.detach().numpy())) f1_obs_history.append(float(f1_OBS.detach().numpy())) f1_est_history.append(float(f1_EST.detach().numpy())) F_est_history.append(float(F_EST.detach().numpy())) # for debugging if np.isnan(t_OBS.numpy()): break # store hyper-parameters and settings params_dir = os.path.join(args.model_dir, args.data_type) if not os.path.exists(params_dir): os.makedirs(params_dir) params = OrderedDict(vars(args)) const_ord = OrderedDict(cast_dict_to_float(const)) with open(os.path.join(params_dir, 'params.json'), 'w') as f: json.dump(params, f) with open(os.path.join(params_dir, 'const.json'), 'w') as f: json.dump(const_ord, f) # store values for post-visualization if friction_type == 0: training_log_dir = os.path.join(params_dir, 'f_est=0', 'training') elif friction_type == 1: training_log_dir = os.path.join(params_dir, 'oracle', 'training') elif friction_type == 2: training_log_dir = os.path.join(params_dir, 'MLP', 'training') if not os.path.exists(training_log_dir): os.makedirs(training_log_dir) store_logs(time_stamp, target_history, obs_history, est_history, f1_obs_history, f1_est_history, F_est_history, training_log_dir) # save trained model if friction_type == 2: model_name = os.path.join(params_dir, 'MLP', 'f1_model') torch.save(f1_EST_fn.state_dict(), model_name) # visualize plot_theta(time_stamp, target_history, obs_history, est_history, training_log_dir)
def main(): ymin, ymax = 0.0, 5.0 timestep = 0.03 kp, ki, kd = 0.5, 8.0, 0.001 pid = PIDController(kp, ki, kd, (ymax - ymin) / 2, timestep, min_output=0, max_output=1.0) plant = EmaFilter(alpha=0.7) init_plot_window(0, 1, 0, 1) queue_size = 100 t = deque(maxlen=queue_size) y1 = deque(maxlen=queue_size) y2 = deque(maxlen=queue_size) counter = 0 target = 0.0 t0 = time() while True: start = time() if counter % 100 == 0: target = np.random.randint(low=1, high=5) pid.setpoint = target / ymax # Normalize to lie inside [0, 1] # Simulation of measured input plant_value = plant.ema(pid.output * (ymax - ymin)) pid.update(plant_value / (ymax - ymin), time()) t.append(time() - t0) y1.append(target) y2.append(pid.output * (ymax - ymin)) if counter > 0: xmin, xmax = t[0], t[-1] # ymin, ymax = min(min(y1), min(y2)), max(max(y1), max(y2)) gr.clearws() gr.setwindow(xmin, xmax, ymin, ymax) # Target gr.setlinewidth(2) linecolor(0, 0, 1.0) gr.polyline(t, y1) # Controller value gr.setlinewidth(2) linecolor(1.0, 0, 0) gr.polyline(t, y2) gr.setlinewidth(1) linecolor(0, 0, 0) draw_axes(1.0, 5.0 / 10, xmin, ymin, x_major=2, y_major=2) gr.updatews() counter += 1 sleep(max(timestep - (time() - start), 0.0))
from simple_system import SimpleSystem from pid_controller import PIDController from signal_generator import PWMGenerator font_name = "Microsoft YaHei" matplotlib.rcParams['font.family']=font_name matplotlib.rcParams['axes.unicode_minus']=False # in case minus sign is shown as box t = [] y = [] x = [] t_step_len = 1e-2 sim_time = 5. system = SimpleSystem(t_step_len) controller = PIDController(200, 50, 500, 0.05, (-500,500)) pwm = PWMGenerator(t_step_len, 2, .5) ts = 0. desire_val = 0 while ts < sim_time: t += [ts] y += [system.observe()] x += [desire_val] observe = 0 sample_time = 50 for i in range(0,sample_time): observe += system.observe() observe /= float(sample_time) control = controller.update(observe, desire_val)