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)
"""
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('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)
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
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 __init__(self, params):
'''Initialize internal variables'''
PIDController.__init__(self,params)
# This variable should contain a list of vectors
# calculated from the relevant sensor readings.
# It is used by the supervisor to draw & debug
# the controller's behaviour
self.vectors = []
def restart(self):
"""Reset internal state"""
PIDController.restart(self)
# This vector points to the closest point of the wall
self.to_wall_vector = None
# This vector points along the wall
self.along_wall_vector = None
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 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, params):
"""Initialize internal variables"""
PIDController.__init__(self, params)
# These two angles are used by the supervisor
# to debug the controller's behaviour, and contain
# the headings as returned by the two subcontrollers.
self.goal_angle = 0
self.away_angle = 0
# Initial vectors for avoid-obstacles
self.vectors = []
def __init__(self, params):
"""Initialize internal variables"""
PIDController.__init__(self,params)
# These two angles are used by the supervisor
# to debug the controller's behaviour, and contain
# the headings as returned by the two subcontrollers.
self.goal_angle = 0
self.away_angle = 0
# Initial vectors for avoid-obstacles
self.vectors = []
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(250, 30, 1, -100, 100, -100, 100)
def set_parameters(self, params):
"""Set PID values, sensor poses, direction and distance.
The params structure is expected to have sensor poses in the robot's
reference frame as ``params.sensor_poses``, the direction of wall
following (either 'right' for clockwise or 'left' for anticlockwise)
as ``params.direction``, the desired distance to the wall
to maintain as ``params.distance``, and the maximum sensor reading
as ``params.ir_max``.
"""
PIDController.set_parameters(self,params)
self.poses = params.sensor_poses
self.direction = params.direction
self.distance = params.distance
self.sensor_max = params.ir_max
class PIDTeleoperator:
def __init__( self, impedance, timeStep ):
self.controller = PIDController( timeStep )
def SetRemoteSystem( self, impedance ):
pass
def SetLocalSystem( self, impedance ):
pass
def Process( self, localState, remoteState, remoteForce, timeDelay ):
self.controller.PreProcess( remoteState, timeDelay )
feedbackForce = self.controller.Process( localState, remoteForce )
remotePredictedState = self.controller.PostProcess()
return ( feedbackForce, remotePredictedState, remotePredictedState )
def set_parameters(self, params):
"""Set PID values and sensor poses.
The params structure is expected to have sensor poses in the robot's
reference frame as ``params.sensor_poses``.
"""
PIDController.set_parameters(self, params)
self.poses = params.sensor_poses
# Now we know the poses, it makes sense to also
# calculate the weights
self.weights = [(math.cos(p.theta) + 1.5) for p in self.poses]
# Normalizing weights
ws = sum(self.weights)
self.weights = [w / ws for w in self.weights]
def set_parameters(self, params):
"""Set PID values and sensor poses.
The params structure is expected to have sensor poses in the robot's
reference frame as ``params.sensor_poses``.
"""
PIDController.set_parameters(self,params)
self.poses = params.sensor_poses
# Now we know the poses, it makes sense to also
# calculate the weights
self.weights = [(math.cos(p.theta)+1.5) for p in self.poses]
# Normalizing weights
ws = sum(self.weights)
self.weights = [w/ws for w in self.weights]
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 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)
def servo_to_tag(self, pose_goal, goal_error=[0.03, 0.03, 0.1], initial_ar_pose=None):
lost_tag_thresh = 0.6 #0.4
# TODO REMOVE
# err_pub = rospy.Publisher("servo_err", Float32MultiArray)
# if False:
# self.test_move()
# return "aborted"
#######################
goal_ar_pose = homo_mat_from_2d(*pose_goal)
rate = 8.
kf_x = ServoKalmanFilter(delta_t=1./rate)
kf_y = ServoKalmanFilter(delta_t=1./rate)
kf_r = ServoKalmanFilter(delta_t=1./rate)
if initial_ar_pose is not None:
ar_err = homo_mat_to_2d(homo_mat_from_2d(*initial_ar_pose) * goal_ar_pose**-1)
kf_x.update(ar_err[0])
kf_y.update(ar_err[1])
kf_r.update(ar_err[2])
print "initial_ar_pose", initial_ar_pose
pid_x = PIDController(k_p=0.75, rate=rate, saturation=0.05)
pid_y = PIDController(k_p=0.75, rate=rate, saturation=0.05)
pid_r = PIDController(k_p=0.75, rate=rate, saturation=0.08)
r = rospy.Rate(rate)
self.preempt_requested = False
while True:
if rospy.is_shutdown():
self.base_pub.publish(Twist())
return 'aborted'
if self.preempt_requested:
self.preempt_requested = False
self.base_pub.publish(Twist())
return 'preempted'
goal_mkr = create_base_marker(goal_ar_pose, 0, (0., 1., 0.))
self.mkr_pub.publish(goal_mkr)
if self.cur_ar_pose is not None:
# make sure we have a new observation
new_obs = self.ar_pose_updated
self.ar_pose_updated = False
# find the error between the AR tag and goal pose
print "self.cur_ar_pose", self.cur_ar_pose
cur_ar_pose_2d = homo_mat_from_2d(*homo_mat_to_2d(self.cur_ar_pose))
print "cur_ar_pose_2d", cur_ar_pose_2d
ar_mkr = create_base_marker(cur_ar_pose_2d, 1, (1., 0., 0.))
self.mkr_pub.publish(ar_mkr)
ar_err = homo_mat_to_2d(cur_ar_pose_2d * goal_ar_pose**-1)
print "ar_err", ar_err
print "goal_ar_pose", goal_ar_pose
# filter this error using a Kalman filter
x_filt_err, x_filt_cov, x_unreli = kf_x.update(ar_err[0], new_obs=new_obs)
y_filt_err, y_filt_cov, y_unreli = kf_y.update(ar_err[1], new_obs=new_obs)
r_filt_err, r_filt_cov, r_unreli = kf_r.update(ar_err[2], new_obs=new_obs)
print ([x_unreli, y_unreli, r_unreli])
if np.any(np.array([x_unreli, y_unreli, r_unreli]) > [lost_tag_thresh*3]): #moved the bracket to the right to be around the 3
self.base_pub.publish(Twist())
print "Why is this here.....???"
return 'lost_tag'
print "Noise:", x_unreli, y_unreli, r_unreli
# TODO REMOVE
#ma = Float32MultiArray()
#ma.data = [x_filt_err[0,0], x_filt_err[1,0], ar_err[0],
# x_unreli, y_unreli, r_unreli]
#err_pub.publish(ma)
print "xerr"
print x_filt_err
print x_filt_cov
print "Cov", x_filt_cov[0,0], y_filt_cov[0,0], r_filt_cov[0,0]
x_ctrl = pid_x.update_state(x_filt_err[0,0])
y_ctrl = pid_y.update_state(y_filt_err[0,0])
r_ctrl = pid_r.update_state(r_filt_err[0,0])
base_twist = Twist()
base_twist.linear.x = x_ctrl
base_twist.linear.y = y_ctrl
base_twist.angular.z = r_ctrl
cur_filt_err = np.array([x_filt_err[0,0], y_filt_err[0,0], r_filt_err[0,0]])
print "err", ar_err
print "Err filt", cur_filt_err
print "Twist:", base_twist
if np.all(np.fabs(cur_filt_err) < goal_error):
self.base_pub.publish(Twist())
return 'succeeded'
self.base_pub.publish(base_twist)
r.sleep()
class PIDControllerTestCase(unittest.TestCase):
def setUp(self):
resetTimeSourceForTestingPurposes(global_time)
self.pid = PIDController(0.2, 0.02, 0.1)
def tearDown(self):
pass
def testUpdateNoop(self):
global_time.updateDelta(0.1)
self.assertEquals(0, self.pid.update(0, 0))
def testUpdateSmallPositiveError(self):
global_time.updateDelta(0.1)
self.assertTrue(0 < self.pid.update(.1, 0))
def testUpdateSmallNegativeError(self):
global_time.updateDelta(0.1)
self.assertTrue(0 > self.pid.update(-.1, 0))
def testUpdatePTerm(self):
self.pid.ki = 0
self.pid.kd = 0
global_time.updateDelta(0.1)
small = self.pid.update(.1, 0)
global_time.updateDelta(0.1)
large = self.pid.update(2, 0)
self.assertTrue(small < large)
def testUpdateDTerm(self):
self.pid.kp = 0
self.pid.ki = 0
global_time.updateDelta(0.1)
large = self.pid.update(.1, 0)
self.pid.prev_error = .2
global_time.updateDelta(0.1)
small = self.pid.update(.1, 0)
self.assertTrue(small < large)
def testUpdateITerm(self):
self.pid.kp = 0
self.pid.kd = 0
global_time.updateDelta(0.1)
small = self.pid.update(.1, 0)
global_time.updateDelta(0.1)
large = self.pid.update(.1, 0)
self.assertTrue(small < large)
def testNanGetsSanitized(self):
try:
global_time.updateDelta(0.1)
self.pid.update(float("nan"), 2)
self.assertTrue(False)
except ValueError as error:
self.assertTrue("cannot be NaN" in str(error))
def testSetPointCappedToAngleRange(self):
#commented out because now we throw an error rather
#than bounding any angle quietly
""""global_time.updateDelta(0.1)
first = self.pid.update(22, 0)
self.pid.prev_error = 0
self.pid.integral_error_accumulator = 0
second = self.pid.update(10, 0)
self.assertEquals(first, second)
self.pid.prev_error = 0
self.pid.integral_error_accumulator = 0
first = self.pid.update(-10, 0)
self.pid.prev_error = 0
self.pid.integral_error_accumulator = 0
global_time.updateDelta(0.1)
second = self.pid.update(-math.pi/2, 0)
self.assertEquals(first, second)"""
def testMaxMovementRateBounded(self):
self.pid.max_movement_rate = 100
try:
global_time.updateDelta(0.1)
self.pid.update(20, 2)
self.assertTrue(False)
except ValueError as error:
self.assertTrue("unsafe rate" in str(error))
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)
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, params):
"""Initialize internal variables"""
PIDController.__init__(self,params)
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))
class AutoPilot():
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 _get_angle_to(self, pos, theta, target):
R = np.array([
[np.cos(theta), -np.sin(theta)],
[np.sin(theta), np.cos(theta)],
])
aim = R.T.dot(target - pos)
angle = -np.degrees(np.arctan2(-aim[1], aim[0]))
angle = 0.0 if np.isnan(angle) else angle
return angle
def _get_control(self, target, far_target, position, speed, theta):
# Steering.
angle_unnorm = self._get_angle_to(position, theta, target)
angle = angle_unnorm / 90
# Acceleration.
angle_far_unnorm = self._get_angle_to(position, theta, far_target)
angle_far = angle_far_unnorm / 90
self.angle_window.append(angle)
self.max_angle = max(self.max_angle, abs(angle))
self.min_angle = -abs(self.max_angle)
debug(self.angle_window, self.max_angle, self.min_angle, 'angle')
steer_control = controlsteer()
steer_control.input['angle_target'] = angle
steer_control.compute()
steer = steer_control.output['steer']
speed_target_control = controltargetspeed()
speed_target_control.input['angle_far_target'] = angle_far
speed_target_control.input['angle_target'] = angle
speed_target_control.compute()
target_speed = speed_target_control.output['target_speed']
self.speed_error_window.append(target_speed - speed)
self.max_speed_error = max(self.max_speed_error,
abs(target_speed - speed))
self.min_speed_error = -abs(self.max_speed_error)
debug(self.angle_window, self.max_speed_error, self.min_speed_error,
'speed')
self.error_hist = self.error_hist.append(
{
'target_speed': target_speed,
'speed_error': target_speed - speed,
'angle': angle
},
ignore_index=True)
self.error_hist.to_csv('error_hist.csv')
throttle_control = controlthrottle()
throttle_control.input['desired_speed'] = target_speed
throttle_control.input['speed'] = speed
throttle_control.compute()
throttle = throttle_control.output['throttle']
steer = self._turn_controller.step(angle)
steer = np.clip(steer, -1.0, 1.0)
steer = round(steer, 3)
delta = np.clip(target_speed - speed, 0.0, 0.25)
throttle = self._speed_controller.step(delta)
throttle = np.clip(throttle, 0.0, 0.75)
brake = False
return steer, throttle, brake
def _get_position(self, observation):
gps = observation['gps']
gps = (gps - self._command_planner.mean) * self._command_planner.scale
return gps
def run_step(self, observation):
position = self._get_position(observation)
near_node, _ = self._waypoint_planner.run_step(position)
far_node, _ = self._command_planner.run_step(position)
print('--------------')
print(position)
print(near_node)
print(far_node)
print(observation['compass'])
steer, throttle, brake = self._get_control(near_node, far_node,
position,
observation['speed'],
observation['compass'])
control = carla.VehicleControl()
control.steer = steer + 1e-2 * np.random.randn()
control.throttle = throttle
control.brake = float(brake)
return control
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 setUp(self):
resetTimeSourceForTestingPurposes(global_time)
self.pid = PIDController(0.2, 0.02, 0.1)
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 __init__( self, impedance, timeStep ): self.controller = PIDController( timeStep )
class Run:
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.odometry.x = 1
self.odometry.y = 0.5
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.travel_dist = 0.25
self.min_dist_to_wall = self.travel_dist + 0.2
self.min_dist_to_localize = 0.2
self.min_theta_to_localize = math.pi / 4
self.n_threshold = math.log(0.09)
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,
input_map=self.map,
n_threshold=self.n_threshold)
def run(self):
self.create.start()
self.create.safe()
# request sensors
self.create.start_stream([
create2.Sensor.LeftEncoderCounts,
create2.Sensor.RightEncoderCounts,
])
isLocalized = False
angle_counter = 0
# This is an example on how to detect that a button was pressed in V-REP
while not isLocalized:
self.draw_particles()
# generate random num
command = random.choice([c for c in Command])
if command is Command.FORWARD:
if self.sonar.get_distance() < self.min_dist_to_wall:
continue
self.filter.move(0, self.travel_dist)
# 100 mm/s = 0.1 m/s
self.create.drive_direct(self.base_speed, self.base_speed)
self.sleep(abs(self.travel_dist / 0.1))
# stop
self.create.drive_direct(0, 0)
elif command is Command.TURN_LEFT:
# turn_angle = math.pi / 2 + self.odometry.theta
turn_angle = math.pi / 2 + angle_counter
turn_angle %= 2 * math.pi
angle_counter += math.pi / 2
angle_counter %= 2 * math.pi
self.filter.move(turn_angle, 0)
# turn left by 90 degree
self.go_to_angle(turn_angle)
elif command is Command.TURN_RIGHT:
# turn_angle = -math.pi / 2 + self.odometry.theta
turn_angle = -math.pi / 2 + angle_counter
turn_angle %= 2 * math.pi
angle_counter -= math.pi / 2
angle_counter %= 2 * math.pi
self.filter.move(turn_angle, 0)
# turn right by 90 degree
self.go_to_angle(turn_angle)
self.filter.sense(self.sonar.get_distance())
# check if localized
# distance between odometry and estimated positions
dist_position_to_goal = math.sqrt(
(self.odometry.x - self.filter.x)**2 +
(self.odometry.y - self.filter.y)**2)
diff_theta_to_goal = abs(self.odometry.theta - self.filter.theta)
isLocalized = dist_position_to_goal < self.min_dist_to_localize and diff_theta_to_goal < self.min_theta_to_localize
def go_to_angle(self, goal_theta):
while abs(
math.atan2(math.sin(goal_theta - self.odometry.theta),
math.cos(goal_theta - self.odometry.theta))) > 0.1:
# print("Go TO: " + str(math.degrees(goal_theta)) + " " + str(math.degrees(self.odometry.theta)))
output_theta = self.pidTheta.update(self.odometry.theta,
goal_theta, self.time.time())
self.create.drive_direct(int(+output_theta), int(-output_theta))
self.sleep(0.01)
self.create.drive_direct(0, 0)
def sleep(self, time_in_sec):
"""Sleeps for the specified amount of time while keeping odometry up-to-date
Args:
time_in_sec (float): time to sleep in seconds
"""
start = self.time.time()
while True:
state = self.create.update()
if state is not None:
self.odometry.update(state.leftEncoderCounts,
state.rightEncoderCounts)
# print("[{},{},{}]".format(self.odometry.x, self.odometry.y, math.degrees(self.odometry.theta)))
t = self.time.time()
if start + time_in_sec <= t:
break
def draw_particles(self):
data = []
# get position data from all particles
for particle in self.filter.particles:
data.extend([particle.x, particle.y, 0.1, particle.theta])
# paint all particles in simulation
self.virtual_create.set_point_cloud(data)
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)
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
##################################################################################
class Run:
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 sleep(self, time_in_sec):
"""Sleeps for the specified amount of time while keeping odometry up-to-date
Args:
time_in_sec (float): time to sleep in seconds
"""
start = self.time.time()
while True:
state = self.create.update()
if state is not None:
self.odometry.update(state.leftEncoderCounts,
state.rightEncoderCounts)
# print("[{},{},{}]".format(self.odometry.x, self.odometry.y, np.rad2deg(self.odometry.theta)))
t = self.time.time()
if start + time_in_sec <= t:
break
def run(self):
self.create.start()
self.create.safe()
# request sensors
self.create.start_stream([
create2.Sensor.LeftEncoderCounts,
create2.Sensor.RightEncoderCounts,
])
plt_time_arr = np.array([])
plt_angle_arr = np.array([])
plt_goal_angle_arr = np.array([])
goal_x = -0.5
goal_y = -0.5
goal_coor = np.array([goal_x, goal_y])
goal_angle = np.arctan2(goal_y, goal_x)
goal_angle %= 2 * np.pi
print("goal angle = %f" % np.rad2deg(goal_angle))
base_speed = 100
# timeout = abs(17 * (goal_angle / np.pi)) + 1
goal_r = 0.05
angle = self.odometry.theta
dist_to_goal = dist(goal_coor,
np.array([self.odometry.x, self.odometry.y]))
plt_time_arr = np.append(plt_time_arr, self.time.time())
plt_angle_arr = np.append(plt_angle_arr, angle)
plt_goal_angle_arr = np.append(plt_goal_angle_arr, goal_angle)
while abs(dist_to_goal) > goal_r:
goal_angle = np.arctan2(goal_y - self.odometry.y,
goal_x - self.odometry.x)
goal_angle %= 2 * np.pi
print("(%f, %f), dist to goal = %f\n" %
(self.odometry.x, self.odometry.y, dist_to_goal))
dist_to_goal = dist(goal_coor,
np.array([self.odometry.x, self.odometry.y]))
angle = self.odometry.theta
plt_angle_arr = np.append(plt_angle_arr, angle)
plt_goal_angle_arr = np.append(plt_goal_angle_arr, goal_angle)
# output = self.pd_controller.update(angle, goal_angle, self.time.time())
output = self.pid_controller.update(angle, goal_angle,
self.time.time())
output_dist = self.pid_controller_dist.update(
0, dist_to_goal, self.time.time())
self.create.drive_direct(int(base_speed + output),
int(base_speed - output))
# self.create.drive_direct(
# int((dist_to_goal * base_speed) + output),
# int((dist_to_goal * base_speed) - output)
# )
# self.create.drive_direct(
# int(output_dist + output),
# int(output_dist - output)
# )
self.sleep(0.01)
np.savetxt("timeOutput.csv", plt_time_arr, delimiter=",")
np.savetxt("angleOutput.csv", plt_angle_arr, delimiter=",")
np.savetxt("goalAngleOutput.csv", plt_goal_angle_arr, delimiter=",")
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():
# ---------------
# 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
class Run:
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 run(self):
self.create.start()
self.create.safe()
# request sensors
self.create.start_stream([
create2.Sensor.LeftEncoderCounts,
create2.Sensor.RightEncoderCounts,
])
self.penholder.set_color(0.0, 1.0, 0.0)
# generate spline points and line drawing order
splines, splines_color = PathFinder.get_spline_points(self.img.paths)
# in format [index, is_parallel, is_spline]
line_index_list = PathFinder.find_path(self.img.lines, splines,
splines_color)
prev_color = None
# loop to draw all lines and paths
for draw_info in line_index_list:
index = int(draw_info[0])
is_parallel = draw_info[1]
is_spline = draw_info[2]
line = self.img.lines[index]
curr_color = self.img.lines[index].color
if curr_color != prev_color:
prev_color = curr_color
self.penholder.set_color(*get_color(curr_color))
# ===== spline routine =====
if is_spline:
path_points = self.draw_path_coords(splines[index],
is_parallel)
self.penholder.set_color(*get_color(splines_color[0]))
# go to start of the curve and begin drawing
for i in range(0, 2):
# go to start of curve
goal_x, goal_y = path_points[0, 0], path_points[0, 1]
print("=== GOAL SET === {:.3f}, {:.3f}".format(
goal_x, goal_y))
if i == 1:
goal_x, goal_y = path_points[9, 0], path_points[9, 1]
# turn to goal
# self.tracker.update()
self.filter.update()
curr_x = self.filter.x
curr_y = self.filter.y
goal_theta = math.atan2(goal_y - curr_y, goal_x - curr_x)
if i == 1:
goal_theta = math.atan2(goal_y - path_points[0, 1],
goal_x - path_points[0, 0])
self.penholder.go_to(0.0)
self.go_to_angle(goal_theta)
self.go_to_goal(goal_x, goal_y)
# start drawing after correctly oriented
# uses only odometry during spline drawing
self.penholder.go_to(self.penholder_depth)
prev_base_speed = self.base_speed
self.filter.updateFlag = False
self.base_speed = 25
print("Draw!")
last_drew_index = 0
# draw the rest of the curve. Draws every 10th point.
for i in range(10, len(path_points), 5):
goal_x, goal_y = path_points[i, 0], path_points[i, 1]
print("=== GOAL SET === {:.3f}, {:.3f}".format(
goal_x, goal_y))
self.go_to_goal(goal_x, goal_y, useOdo=True)
print("\nlast drew index {}\n".format(last_drew_index))
if last_drew_index < len(path_points) - 1:
goal_x, goal_y = path_points[-1, 0], path_points[-1, 1]
print("=== GOAL SET === {:.3f}, {:.3f}".format(
goal_x, goal_y))
self.go_to_goal(goal_x, goal_y, useOdo=False)
# stop drawing and restore parameter values
self.base_speed = prev_base_speed
self.filter.updateFlag = True
self.penholder.go_to(0.0)
# ===== line routine =====
else:
for i in range(0, 2):
goal_x, goal_y = self.draw_coords(line,
is_parallel=is_parallel,
at_start=True)
if i == 1:
goal_x, goal_y = self.draw_coords(
line, is_parallel=is_parallel, at_start=False)
print("=== GOAL SET === {:.3f}, {:.3f}".format(
goal_x, goal_y))
# turn to goal
# self.tracker.update()
self.filter.update()
curr_x = self.filter.x
curr_y = self.filter.y
goal_theta = math.atan2(goal_y - curr_y, goal_x - curr_x)
self.penholder.go_to(0.0)
self.go_to_angle(goal_theta)
if i == 1:
# start drawing
self.penholder.go_to(self.penholder_depth)
print("Draw!")
self.go_to_goal(goal_x, goal_y)
# graph the final result
self.draw_graph()
self.create.stop()
def drive(self, theta, distance, speed):
# Sum all controllers and clamp
self.create.drive_direct(
max(min(int(theta + distance + speed), 500), -500),
max(min(int(-theta + distance + speed), 500), -500))
def sleep(self, time_in_sec):
"""Sleeps for the specified amount of time while keeping odometry up-to-date
Args:
time_in_sec (float): time to sleep in seconds
"""
start = self.time.time()
while True:
self.update()
t = self.time.time()
if start + time_in_sec <= t:
break
# gives coordinates to draw the lines correctly
# line: segment to be drawn
# at_start: set true to retun the first coordinate, set false for the second coordinate
# returns the x, y coordinates offset
def draw_coords(self, line, at_start=True, is_parallel=True):
if is_parallel:
theta = math.atan2(line.v[1] - line.u[1],
line.v[0] - line.u[0]) + math.pi / 2
if at_start:
return math.cos(theta) * self.robot_marker_distance + line.u[0], \
math.sin(theta) * self.robot_marker_distance + line.u[1]
else:
return math.cos(theta) * self.robot_marker_distance + line.v[0], \
math.sin(theta) * self.robot_marker_distance + line.v[1]
else:
theta = math.atan2(line.v[1] - line.u[1],
line.v[0] - line.u[0]) - math.pi / 2
if at_start:
return math.cos(theta) * self.robot_marker_distance + line.v[0], \
math.sin(theta) * self.robot_marker_distance + line.v[1]
else:
return math.cos(theta) * self.robot_marker_distance + line.u[0], \
math.sin(theta) * self.robot_marker_distance + line.u[1]
def draw_path_coords(self, result, is_parallel):
final_result = np.empty((0, 2))
if is_parallel:
for i in range(0, len(result) - 1):
theta = math.atan2(
result[i, 1] - result[i + 1, 1],
result[i, 0] - result[i + 1, 0]) - math.pi / 2
s = math.cos(theta) * self.robot_marker_distance + result[i, 0], \
math.sin(theta) * self.robot_marker_distance + result[i, 1]
final_result = np.vstack([final_result, s])
else:
for i in range(len(result) - 1, 1, -1):
theta = math.atan2(
result[i, 1] - result[i - 1, 1],
result[i, 0] - result[i - 1, 0]) - math.pi / 2
s = math.cos(theta) * self.robot_marker_distance + result[i, 0], \
math.sin(theta) * self.robot_marker_distance + result[i, 1]
final_result = np.vstack([final_result, s])
return final_result
def go_to_goal(self, goal_x, goal_y, useOdo=False):
while True:
state = self.update()
if state is not None:
if useOdo:
curr_x = self.odometry.x
curr_y = self.odometry.y
curr_theta = self.odometry.theta
else:
curr_x = self.filter.x
curr_y = self.filter.y
curr_theta = self.filter.theta
distance = math.sqrt(
math.pow(goal_x - curr_x, 2) +
math.pow(goal_y - curr_y, 2))
output_distance = self.pidDistance.update(
0, distance, self.time.time())
theta = math.atan2(goal_y - curr_y, goal_x - curr_x)
output_theta = self.pidTheta.update(curr_theta, theta,
self.time.time())
print("goal x,y = {:.3f}, {:.3f}, x,y = {:.3f}, {:.3f}".format(
goal_x, goal_y, curr_x, curr_y))
self.drive(output_theta, output_distance, self.base_speed)
if distance < 0.05:
self.create.drive_direct(0, 0)
break
self.sleep(0.01)
def go_to_angle(self, goal_theta):
curr_theta = self.filter.theta
while abs(-math.degrees(
math.atan2(math.sin(curr_theta - goal_theta),
math.cos(curr_theta - goal_theta)))) > 8:
curr_theta = self.filter.theta
print("goal_theta = {:.2f}, theta = {:.2f}".format(
math.degrees(goal_theta), math.degrees(curr_theta)))
output_theta = self.pidTheta.update(curr_theta, goal_theta,
self.time.time())
self.drive(output_theta, 0, 0)
self.sleep(0.01)
self.create.drive_direct(0, 0)
# debug function. Draws robot paths
def draw_graph(self):
# show drawing progress after each line segment is drawn
if self.debug_mode:
if len(self.odo) is not 0 and len(self.actual) is not 0:
x, y = zip(*self.odo)
a, b = zip(*self.actual)
plt.plot(x, y, color='red', label='Sensor path')
plt.plot(a,
b,
color='green',
label='Actual path',
linewidth=1.4)
self.odo = []
self.actual = []
for path in self.img.paths:
ts = np.linspace(0, 1.0, 100)
result = np.empty((0, 3))
for i in range(0, path.num_segments()):
for t in ts[:-2]:
s = path.eval(i, t)
result = np.vstack([result, s])
plt.plot(result[:, 0], result[:, 1], path.color)
path_points = self.draw_path_coords(result, True)
plt.plot(path_points[:, 0], path_points[:, 1], color='aqua')
path_points = self.draw_path_coords(result, False)
plt.plot(path_points[:, 0], path_points[:, 1], color='aqua')
for line in self.img.lines:
# draw lines
plt.plot([line.u[0], line.v[0]], [line.u[1], line.v[1]],
line.color)
theta = math.atan2(line.v[1] - line.u[1],
line.v[0] - line.u[0]) + math.pi / 2
plt.plot([
math.cos(theta) * self.robot_marker_distance + line.u[0],
math.cos(theta) * self.robot_marker_distance + line.v[0]
], [
math.sin(theta) * self.robot_marker_distance + line.u[1],
math.sin(theta) * self.robot_marker_distance + line.v[1]
], 'aqua')
theta = math.atan2(line.v[1] - line.u[1],
line.v[0] - line.u[0]) - math.pi / 2
plt.plot([
math.cos(theta) * self.robot_marker_distance + line.u[0],
math.cos(theta) * self.robot_marker_distance + line.v[0]
], [
math.sin(theta) * self.robot_marker_distance + line.u[1],
math.sin(theta) * self.robot_marker_distance + line.v[1]
], 'aqua')
plt.legend()
plt.show()
# updates odometry, filter, and tracker
def update(self):
state = self.create.update()
self.filter.update()
# self.tracker.update()
if state is not None:
self.odometry.update(state.leftEncoderCounts,
state.rightEncoderCounts)
# print("[{},{},{}]".format(self.odometry.x, self.odometry.y, math.degrees(self.odometry.theta)))
if self.debug_mode:
self.odo.append((self.filter.x, self.filter.y))
self.actual.append(
(self.create.sim_get_position()[0] - self.xi,
self.create.sim_get_position()[1] - self.yi))
return state
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)
