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)
