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)