def control_velocity(): global pid, v_est rospy.init_node('controller_velocity') motor_pub = rospy.Publisher('motor_pwm', ECU, queue_size = 10) rospy.Subscriber('v_ref', Vector3, v_ref_callback, queue_size=10) rospy.Subscriber('state_estimate', Z_KinBkMdl, state_estimate_callback, queue_size=10) # set node rate rateHz = 50 dt = 1.0 / rateHz rate = rospy.Rate(rateHz) # initialize motor_pwm and v_est at neutral v_est = 0 motor_pwm = 90 # use pid to correct velocity, begins with set point at zero p = rospy.get_param("controller/p") i = rospy.get_param("controller/i") d = rospy.get_param("controller/d") pid = PID(P=p, I=i, D=d) # main loop while not rospy.is_shutdown(): u = pid.update(v_est, dt) motor_pwm = motor_pwm + u # send command signal motor_cmd = ECU(motor_pwm, 0) motor_pub.publish(motor_cmd) rate.sleep()
def fitness(self, individual, target): """ Check the final value of running the simulation and the time constant tau where y is ~63.2% of the target value. """ dt = self._sample_period tau = self._tau pid = PID(Kp=individual[0], Ki=individual[1], Kd=individual[2]) system = self._system_cls(*self._system_args, **self._system_kwargs) t = range(0, self._rise_time, dt) y = [] for i in t: output = system.measurement pid.update(output, target, dt=dt) y.append(output) system.update(pid.control) if tau is not None: t632 = self._time_near(t, y, 0.632 * target) tau_diff = abs(tau - t632) else: tau_diff = 0 return abs(target - y[-1]) + tau_diff
def main_auto(): # initialize ROS node rospy.init_node('auto_mode', anonymous=True) rospy.Subscriber('imu', TimeData, imu_callback) rospy.Subscriber('encoder', Encoder, encoder_callback) rospy.Subscriber('speed', SPEED, speed_callback) nh = rospy.Publisher('mot', MOT, queue_size = 10) log = rospy.Publisher('state', STATE, queue_size = 10) # set node rate rateHz = 50 rate = rospy.Rate(rateHz) dt = 1.0 / rateHz p = rospy.get_param("longitudinal/p") i = rospy.get_param("longitudinal/i") d = rospy.get_param("longitudinal/d") pid = PID(P=1, I=1, D=0) # main loop while not rospy.is_shutdown(): # publish command signal # TODO global v_x, Ww_Reff, desired_slip_ratio slip_ratio = (Ww_Reff -v_x) /Ww_Reff err_slip_ratio = slip_ratio -desired_slip_ratio motor_PWM = pid.update(err_slip_ratio) # motor_PWM = 90 nh.publish(MOT(motor_PWM)) log.publish(STATE(Vx, Ww_Reff, err)) # wait rate.sleep(sleep_time)
def main_auto(): # initialize ROS node rospy.init_node('auto_mode', anonymous=True) nh = rospy.Publisher('serv', SERV, queue_size = 10) rospy.Subscriber('imu', TimeData, imu_callback) # set node rate rateHz = 50 dt = 1.0 / rateHz rate = rospy.Rate(rateHz) # use simple pid control to keep steering straight p = rospy.get_param("lateral/p") i = rospy.get_param("lateral/i") d = rospy.get_param("lateral/d") pid = PID(P=p, I=i, D=d) # main loop while not rospy.is_shutdown(): # get steering wheel command u = pid.update(err, dt) servoCMD = angle_2_servo(u) # send command signal serv_cmd = SERV(servoCMD) nh.publish(serv_cmd) # wait rate.sleep()
def __init__(self): # Set PID structures to intial values self.rightPID = PID(-200, 0, 0, 0, 0, 0, 0) self.rightPID.setPoint(0) self.leftPID = PID(200, 0, 0, 0, 0, 0, 0) self.leftPID.setPoint(0) self.somethingWentWrong = False # Ensure that data is fresh from both wheels self.newDataM1 = False self.newDataM2 = False self.newData = False # IMU structures self.currentLeftV = MovingAverage() self.currentRightV = MovingAverage() # Roboclaw self.myRoboclaw = RoboClaw() # ROS subscribers rospy.Subscriber("/cmd_vel", Twist, self.commandCallback) rospy.Subscriber("/imu1", Imu, self.imu1Callback) rospy.Subscriber("/imu2", Imu, self.imu2Callback) time.sleep(1) self.lastDataTime = time.time() self.repeatCount = 0 self.currentM1Measurement = 0 self.currentM2Measurement = 0 self.lastM1Measurement = 0 self.lastM2Measurement = 0
def __init__(self): self.left_wall_pid = PID(14, 2, 0.5) self.right_wall_pid = PID(14, 2, 0.5) self.left_wall_pid.setpoint = 0.055 self.right_wall_pid.setpoint = 0.055 self.goal_angle = None self.goal_distance = None self.start_position = None self.position = None self.start_angle = None self.angle = None self.left_dist = 0 self.right_dist = 0 self.front_dist = 0 self.commands = [] self.busy = False self.callback = None self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1) rospy.Subscriber("/wheel_odom", Odometry, self.update) rospy.Subscriber("/sensors/ir_left", Float32, self.ir_left) rospy.Subscriber("/sensors/ir_right", Float32, self.ir_right) rospy.Subscriber("/sensors/ir_front", Float32, self.ir_front)
def __init__(self, quad, max_force=10, min_force=0.02): self.quad = quad # set max and min forces self.max_force = max_force self.min_force = min_force # commands self.command_pitch = 0 self.command_roll = 0 self.command_yaw = 0 self.command_acceleration = 0 # acceleration set points self.set_point_acceleration = 0 self.acceleration = PID(4, 0.8, 2) self.acceleration.setPoint(self.set_point_acceleration) # Pitch set point self.set_point_pitch = 0 self.pitch = PID(0.02, 0.001, 0.10) self.pitch.setPoint(self.set_point_pitch) # Roll set point self.set_point_roll = 0 self.roll = PID(0.02, 0.001, 0.10) self.roll.setPoint(self.set_point_roll) # Yaw set point self.lock_yaw = True self.set_point_yaw = 90 self.yaw = PID(0.2, 0.001, 1) self.yaw.setPoint(self.set_point_yaw)
def __init__(self): Node.__init__(self) self.x_pid = PID(0.5, 0.0001, 0.001, 300) self.t_pid = PID(0.5, 0.005, 0.0, 0.87) self.target_pos = None self.last_seen = 0 self.kick_offset = 50 self.finishCt = 0
def __init__(self): Node.__init__(self) self.x = PID(0.5, 0.0001, 0.001) self.y = PID(0.5, 0.005, 0.0) self.target_pos = None self.last_seen = 0 self.kick_offset = 50 self.finishCt = 0
class PID_Posicion(object): """docstring for PID_Posicion""" def __init__(self, kp = 1.0 , ki =0.0, kd =0.0 , pinA = 4, pinB = 21): self.pid = PID(kp , ki , kd) self.motor = Motor_encoder(pinA , pinB) def SetPoint(self , setpoint): self.pid.setPoint(abs(setpoint)) self.motor.avance(setpoint)
def __init__(self, P=2.0, I=0.0, D=1.0, Derivator=0, Integrator=0, \ Integrator_max=500, Integrator_min=-500, set_point=10): print "in pid44: ", P,I,D self.current=[0, 0, 0, 0, 0] self.data_number=0 PID.__init__(self, P, I, D, Derivator=0, Integrator=0, \ Integrator_max=500, Integrator_min=-500) PID.setPoint(self, set_point)
def integrate(self, n, state, dt): pid = PID(kp=self.kp, ki=self.ki, kd=self.kd, angle=self.pidAngle, dt=dt) history = np.array([state]) theta = Robot._get_gyroscope(state[2]) for i in range(n): self.u = pid.execute_pid(theta) state = rk4(self.dynamics, state, i, dt) theta = Robot._get_gyroscope(Robot._get_gyroscope(state[2])) history = np.append(history, [state], axis=0) return history
def __init__(self): Node.__init__(self) self.x = PID(0.5, 0.001, 0.001, 300) self.y = PID(0.5, 0.001, 0.0, 180) # was 0.0005 self.target_pos = None self.last_seen = 0 self.kick_offset = 50 self.finishCt = 0 b = world_objects.getObjPtr(core.WO_BALL) self.initialTheta = b.visionBearing
def __init__(self, sensor1, sensor2, ir_device_func, diff_k_values=(0,0,0), dist_k_values=(0,0,0)): self.sensor1 = sensor1 self.sensor2 = sensor2 self.get_values = ir_device_func self.diff_pid = PID() p1,d1,i1 = diff_k_values # self.diff_pid.set_k_values(1.1, 0.04, 0.0) self.diff_pid.set_k_values(p1, d1, i1) self.dist_pid = PID() p2,d2,i2 = dist_k_values self.dist_pid.set_k_values(p2, d2, i2)
def __init__(self, P=2.0, I=0.0, D=1.0, Derivator=0, Integrator=0, \ Integrator_max=500, Integrator_min=-500, set_point=10): ''' initial ''' print "in pid_1: input parameters ", P,I,D,set_point, "to init PID" self.count=0 PID.__init__(self, P, I, D, Derivator, Integrator, \ Integrator_max, Integrator_min) PID.setPoint(self, set_point)
class Interrupt(object): """Clase elaborada por Luis Borbolla para interrupcion por hardware en raspberry pi """ def __init__(self, pin , pin_pwm , pin_pwmR): self.avance = 0 self.pin = pin self.pin_pwmR = pin_pwmR self.cuenta = 0 self.index = 0 self.velocidad = 0 self.tiempo_anterior = time.time() self.tiempo_actual = 0 self.pin_pwm = pin_pwm GPIO.setmode(GPIO.BOARD) GPIO.setup(self.pin, GPIO.IN, pull_up_down=GPIO.PUD_UP) GPIO.add_event_detect(self.pin, GPIO.FALLING, callback=self.interrupcion, bouncetime=5) GPIO.setup(24, GPIO.IN, pull_up_down=GPIO.PUD_DOWN) self.pwm_llanta = PWM(self.pin_pwm) self.pwm_llantar = PWM(self.pin_pwmR) self.pid_vel = PID() def setup(self , setPoint): self.pwm_llanta.start() if setPoint >0: self.pwm_llanta.set_duty(setPoint/15.0) else: self.pwm_llantar.set_duty(setPoint/15.0) self.pid_vel.setPoint(setPoint) def interrupcion(self,channel): #print 'velocidad = %s' % self.pin self.cuenta += 1 self.tiempo_actual = time.time() self.avance += 10 prom_tiempos = (self.tiempo_actual-self.tiempo_anterior)/2 self.velocidad = 9.974556675147593/prom_tiempos #2.5*25.4*m.pi/20 == 9.974556675147593 self.tiempo_anterior = self.tiempo_actual error = self.pid_vel.update(self.velocidad) self.pwm_llanta.update(error/15.0) def esperar_int(self): try: print "Waiting for rising edge on port 24" GPIO.wait_for_edge(24, GPIO.RISING) print "Rising edge detected on port 24. Here endeth the third lesson." except KeyboardInterrupt: GPIO.cleanup() # clean up GPIO on CTRL+C exit GPIO.cleanup() # clean up GPIO on normal exit
class motor_control(): def __init__(self): #pid loop for each motor self.pid0=PID(P=0.0001, I=0.00001) self.pid1=PID(P=0.0001, I=0.00001) self.high_power_limit=0.70 self.low_power_limit=0.15 self.low_rpm_limit=250 self.deadband=0.1 self.pub = rospy.Publisher('motor_power', MotorPower, queue_size=10) self.motor_power=MotorPower() self.motor_power.power1=0 self.motor_power.power2=0 def set_motor_power(self): self.motor_power.power1=self.pid0.PID+self.motor_power.power1 self.motor_power.power2=self.pid1.PID+self.motor_power.power2 if self.pid0.getPoint() > 0 and self.pid0.getPoint() < self.low_rpm_limit: self.motor_power.power1=self.low_power_limit elif self.pid0.getPoint() < 0 and self.pid0.getPoint() > -self.low_rpm_limit: self.motor_power.power1=-self.low_power_limit elif self.pid0.getPoint() == 0: self.motor_power.power1=0 elif self.motor_power.power1 > self.high_power_limit: self.motor_power.power1=self.high_power_limit elif self.motor_power.power1 < self.low_power_limit: # if self.motor_power.power1 > -self.low_power_limit+self.deadband and self.motor_power.power1 < self.low_power_limit-self.deadband: # self.motor_power.power1=0 if self.motor_power.power1 < 0 and self.motor_power.power1 > -self.low_power_limit: self.motor_power.power1 = -self.low_power_limit elif self.motor_power.power1 < -self.high_power_limit: self.motor_power.power1= -self.high_power_limit else: self.motor_power.power1=self.low_power_limit if self.pid1.getPoint() > 0 and self.pid1.getPoint() < self.low_rpm_limit: self.motor_power.power2=self.low_power_limit elif self.pid1.getPoint() < 0 and self.pid1.getPoint() > -self.low_rpm_limit: self.motor_power.power2=-self.low_power_limit elif self.pid1.getPoint() == 0: self.motor_power.power2=0 elif self.motor_power.power2 > self.high_power_limit: self.motor_power.power2=self.high_power_limit elif self.motor_power.power2 < self.low_power_limit: if self.motor_power.power2 < 0 and self.motor_power.power2 > -self.low_power_limit: self.motor_power.power2 = -self.low_power_limit elif self.motor_power.power2 < -self.high_power_limit: self.motor_power.power2= -self.high_power_limit else: self.motor_power.power2=self.low_power_limit
def __init__(self): rospy.on_shutdown(self.stop) rospy.wait_for_service('uniboard_service') self.uniboard_service = rospy.ServiceProxy('uniboard_service', communication) rospy.Subscriber("/cmd_vel", Twist, self.set_vel) rospy.Subscriber("/vel", vel_update, self.update) self.vel_pid = PID(self.drive, 0.5, 0.01, 0.0,[-2, 2], [-1, 1], 'vel_pid') self.vel_pid.start() self.rot_pid = PID(self.rotation, 0.5, 0.01, 0.0, [-2.0, 2.0], [-0.5, 0.5], 'rot_pid') self.rot_pid.start() # The offset value between wheel power to drive rotation self.rotation_offset = 0.0
def __init__(self): self.linear_pid = PID(95, 85, 1, 62.915) self.angular_pid = PID(6, 4, 0.1, 3.125) self.linear_pid.setpoint = 0 self.angular_pid.setpoint = 0 self.left_pub = rospy.Publisher('/wheel_left/duty', Int16, queue_size=1) self.right_pub = rospy.Publisher('/wheel_right/duty', Int16, queue_size=1) rospy.Subscriber("/cmd_vel", Twist, self.update_setpoints) rospy.Subscriber("/wheel_odom", Odometry, self.update_duty)
def model(self, p): model = nengo.Network() with model: system = System(p.D, p.D, dt=p.dt, seed=p.seed, motor_noise=p.noise, sense_noise=p.noise, scale_add=p.scale_add, motor_scale=10, motor_delay=p.delay, sensor_delay=p.delay, motor_filter=p.filter, sensor_filter=p.filter) self.system_state = [] self.system_times = [] self.system_desired = [] def minsim_system(t, x): r = system.step(x) self.system_state.append(system.state) self.system_times.append(t) self.system_desired.append(signal.value(t)) return r minsim = nengo.Node(minsim_system, size_in=p.D, size_out=p.D) state_node = nengo.Node(lambda t: system.state) pid = PID(p.Kp, p.Kd, p.Ki, tau_d=p.tau_d) control = nengo.Node(lambda t, x: pid.step(x[:p.D], x[p.D:]), size_in=p.D*2) nengo.Connection(minsim, control[:p.D], synapse=0) nengo.Connection(control, minsim, synapse=None) if p.adapt: adapt = nengo.Ensemble(p.n_neurons, dimensions=p.D, radius=p.radius) nengo.Connection(minsim, adapt, synapse=None) conn = nengo.Connection(adapt, minsim, synapse=p.synapse, function=lambda x: [0]*p.D, solver=ZeroDecoder(), learning_rule_type=nengo.PES()) conn.learning_rule_type.learning_rate *= p.learning_rate nengo.Connection(control, conn.learning_rule, synapse=None, transform=-1) signal = Signal(p.D, p.period, dt=p.dt, max_freq=p.max_freq, seed=p.seed) desired = nengo.Node(signal.value) nengo.Connection(desired, control[p.D:], synapse=None) #self.p_desired = nengo.Probe(desired, synapse=None) #self.p_q = nengo.Probe(state_node, synapse=None) return model
class ControllerDaemon(AtlasDaemon): def _init(self): self.logger.info("Initializing controller daemon.") self.temp_target = 19 self.update_time = 0 self.period = 5 * 60 self.duty_cycle = 0 self.pid = PID() self.pid.setPoint(self.temp_target) self.pin = OutputPin("P8_10") self.pin.set_low() self.heater = State.off self.subscriber = self.get_subscriber(Topic("temperature", "ferm1_wort")) self.wort_temp = Average(self.subscriber.topic.data) topic = Topic("temperature", "ferm1_wort_average", self.wort_temp.get_value()) self.publisher_average_temp = self.get_publisher(topic) topic = Topic("pid", "ferm1_pid", self.pid.update(self.wort_temp.get_value())) self.publisher_pid = self.get_publisher(topic) topic = Topic("state", "ferm1_heating", self.heater) self.publisher_heater = self.get_publisher(topic) topic = Topic("temperature", "ferm1_target", self.temp_target) self.publisher_target = self.get_publisher(topic) topic = Topic("percentage", "ferm1_duty_cycle", self.duty_cycle) self.publisher_duty_cycle = self.get_publisher(topic) self.logger.info("Controller initialized.") def _loop(self): self.wort_temp.update(self.subscriber.topic.data) self.publisher_average_temp.publish(self.wort_temp.get_value()) pid = self.pid.update(self.wort_temp.get_value()) self.publisher_pid.publish(pid) self.publisher_target.publish(self.temp_target) if pid < 1: self.duty_cycle = pid else: self.duty_cycle = 1 self.publisher_duty_cycle.publish(self.duty_cycle) if self.update_time + self.period < time.time(): self.update_time = time.time() if 0 < self.duty_cycle: self.heater = State.on self.pin.set_high() elif self.update_time + self.duty_cycle * self.period < time.time(): if self.heater == State.on: self.heater = State.off self.pin.set_low() self.publisher_heater.publish(self.heater) time.sleep(1)
def __init__(self, yaw_controller, p_brake): # TODO: Implement # defining 2 PID controllers self.throttle_pid = PID(kp=0.4, ki=0.02, kd=0.7, mn=-1.0, mx=1.0) self.brake_pid = PID(kp=p_brake, ki=0.0, kd=0.0, mn=0.0, mx=100000.) self.yaw_controller = yaw_controller self.des_speed_filter = LowPassFilter2(1.5, 0.) self.throttle_brake_offs = -1.0 self.throttle_direct = 0.0 self.standstill_velocity = 0.01 self.standstill_brake = -2.0*p_brake*self.throttle_brake_offs self.standstill_filter_time = 0.75 self.standstill_filter = LowPassFilter2(self.standstill_filter_time, 0.0)
def __init__(self): super(Boat, self).__init__() self.lat = 0 self.lon = 0 self.fix = False self.waypoints = [] self.gps = GpsReader(self) self.gps.start() self.mag = TelemetryReader() self.dirPID = PID(35,0,0) self.speedPID = PID(-0.005,0,0)
def __init__(self, frame): self.frame = frame self.pubNav = rospy.Publisher('cmd_vel', Twist, queue_size=1) self.listener = TransformListener() self.pidX = PID(35, 10, 0.0, -20, 20, "x") self.pidY = PID(-35, -10, -0.0, -20, 20, "y") self.pidZ = PID(4000, 3000.0, 2000.0, 10000, 60000, "z") self.pidYaw = PID(-50.0, 0.0, 0.0, -200.0, 200.0, "yaw") self.state = Controller.Idle self.goal = Pose() rospy.Subscriber("goal", Pose, self._poseChanged) rospy.Service("takeoff", std_srvs.srv.Empty, self._takeoff) rospy.Service("land", std_srvs.srv.Empty, self._land)
def __init__(self, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): self.wheel_base = wheel_base self.steer_ratio = steer_ratio self.min_speed = 5 self.max_lat_accel = max_lat_accel self.previous_dbw_enabled = False self.min_angle = -max_steer_angle self.max_angle = max_steer_angle self.linear_pid = PID(0.9, 0.001, 0.0004, self.min_angle, self.max_angle) #self.cte_pid = PID(0.4, 0.1, 0.1, self.min_angle, self.max_angle) self.cte_pid = PID(0.4, 0.1, 0.2, self.min_angle, self.max_angle) self.tau = 0.2 self.ts = 0.1 self.low_pass_filter = LowPassFilter(self.tau, self.ts)
def __init__(self): #pid loop for each motor self.pid0=PID(P=0.0001, I=0.00001) self.pid1=PID(P=0.0001, I=0.00001) self.high_power_limit=0.70 self.low_power_limit=0.15 self.low_rpm_limit=250 self.deadband=0.1 self.pub = rospy.Publisher('motor_power', MotorPower, queue_size=10) self.motor_power=MotorPower() self.motor_power.power1=0 self.motor_power.power2=0
def __init__(self): PID.__init__(self) self.setKp(0.05) self.setKi(0.003) self.setKd(0.00075) self.setPoint(0) self.theta = 0 self.thetaA = 0 self.thetaG = 0 self.lwEffort = 0 self.rwEffort = 0 self.Effort = 0 self.yawEffort = 0 self.lwEffortPublisher = rospy.Publisher('selbie/selbie_lj_effort_controller/command', Float64, queue_size=10) self.rwEffortPublisher = rospy.Publisher('selbie/selbie_rj_effort_controller/command', Float64, queue_size=10)
def __init__(self, sensor_reader, led, database=None, file=None): self.sensor_reader = sensor_reader self.led = led self.database = database self.output_file = file self.pid = PID(0.495, 0.594, 0.0, MIN_VALUE, MAX_VALUE, 0) self.red_pid = PID(0.495, 0.594, 0.0, MIN_VALUE, 65, 0) self.green_pid = PID(0.495, 0.594, 0.0, MIN_VALUE, 80, 0) self.blue_pid = PID(0.495, 0.594, 0.0, MIN_VALUE, 70, 0) # self.pid = PID(2.0, 0.0, 0.0, MIN_VALUE, MAX_VALUE, 0) self.last_reads = {} self.mode = MANUAL
def __init__(self): pygame.sprite.Sprite.__init__(self) #call Sprite intializer self.image, self.rect = load_image('boat.png', -1) self.image = scale(self.image, (50, 100)) screen = pygame.display.get_surface() self.area = screen.get_rect() self.rect.topleft = 10, 10 self.original = self.image self.direction = 0 # 0 - 360 self.speed = 1 self.rudder = 0 self.motor = 0.3 self.dirPID = PID(0.3,0,0) self.speedPID = PID(-0.001,0,0)
def __init__(self, wheel_base, wheel_radius, steer_ratio, max_lat_accel, max_steer_angle, decel_limit, vehicle_mass): self.yaw_controller = YawController( wheel_base, steer_ratio, 0.1, # min_speed max_lat_accel, max_steer_angle, ) # PID coefficients kp = 0.3 ki = 0.1 kd = 0.0 # For convenience (no real limits on throttle): mn_th = 0.0 # Minimal throttle mx_th = 0.2 # Maximal throttle self.throttle_controller = PID(kp, ki, kd, mn_th, mx_th) tau = 0.5 # 1 / (2pi*tau) = cutoff frequency ts = 0.02 # Sample time self.vel_lpf = LowPassFilter(tau, ts) self.wheel_radius = wheel_radius self.decel_limit = decel_limit self.vehicle_mass = vehicle_mass self.last_time = rospy.get_time()
class Controller(object): def __init__(self, vehicle_mass_in, fuel_capacity_in, brake_deadband_in, decel_limit_in, accel_limit_in, wheel_radius_in, wheel_base_in, steer_ratio_in, max_lat_accel_in, max_steer_angle_in): self.yaw_controller = YawController(wheel_base_in, steer_ratio_in, 0.1, max_lat_accel_in, max_steer_angle_in) kp = 0.3 ki = 0.1 kd = 0.0 # ki = 0.01 # kd = 0.1 min_throttle = 0.0 max_throttle = 0.10 self.throttle_controller = PID(kp, ki, kd, min_throttle, max_throttle) tau = 0.5 sampling_time = 0.02 # 50 Hz self.vel_lpf = LowPassFilter(tau, sampling_time) self.vehicle_mass = vehicle_mass_in self.fuel_capacity = fuel_capacity_in self.brake_deadband = brake_deadband_in self.decel_limit = decel_limit_in self.accel_limit = accel_limit_in self.wheel_radius = wheel_radius_in self.last_time = rospy.get_time() def control(self, current_vel, dbw_enabled, linear_vel, angular_vel): # TODO: Change the arg, kwarg list to suit your needs # Return throttle, brake, steer if not dbw_enabled: self.throttle_controller.reset() return 0., 0., 0. current_vel = self.vel_lpf.filt(current_vel) # rospy.logwarn("Angular vel: {0}".format(angular_vel)) # rospy.logwarn("Target vel: {0}".format(linear_vel)) # rospy.logwarn("Current vel: {0}".format(current_vel)) steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel) # rospy.logwarn("Steering : {0}".format(steering)) vel_error = linear_vel - current_vel self.last_vel = current_vel #rospy.logwarn("vel_error: {0}".format(vel_error)) current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time throttle = self.throttle_controller.step(vel_error, sample_time) brake = 0 # Changed to 0.015 fix car stopping way beyond stopline if linear_vel == 0. and current_vel < 0.015: throttle = 0. # Testing purpose did not work #brake = 400 # N*m Hold car at stop light brake = 700 # N*m Hold car at stop light ## To be changed to 700 above for Carla. Done. elif throttle < 0.1 and vel_error < 0: throttle = 0. decel = max(vel_error, self.decel_limit) brake = abs(decel) * self.vehicle_mass * self.wheel_radius #rospy.logwarn("Decel: {0}".format(decel)) #rospy.logwarn("Brake: {0}".format(brake)) return throttle, brake, steering
class Controller(object): def __init__(self, vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): # TODO: Implement self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle) kp = 0.3 ki = 0.001 kd = 0.6 mn = 0. mx = 0.3 self.throttle_controller = PID(kp, ki, kd, mn, mx) tau = 0.5 ts = 0.02 self.vel_lpf = LowPassFilter(tau, ts) self.vehicle_mass = vehicle_mass self.fuel_capacity = fuel_capacity self.brake_deadband = brake_deadband self.decel_limit = decel_limit self.accel_limit = accel_limit self.wheel_radius = wheel_radius self.last_time = rospy.get_time() self.iteration_count = 0 def control(self, current_vel, dbw_enabled, tld_enabled, linear_vel, angular_vel): # TODO: Change the arg, kwarg list to suit your needs # Return throttle, brake, steer # rospy.logwarn("dbw_enabled: {0}".format(dbw_enabled)) if not dbw_enabled: self.throttle_controller.reset() return 0., 0., 0. current_vel = self.vel_lpf.filt(current_vel) steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel) vel_error = linear_vel - current_vel self.last_vel = current_vel current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time throttle = self.throttle_controller.step(vel_error, sample_time) brake = 0 if ((linear_vel == 0. and current_vel < 0.5) or (not tld_enabled)): throttle = 0 brake = 700 # N*m # 700 for real car - simulator would only require 400 elif throttle < 0.1 and vel_error < 0: throttle = 0 decel = max(vel_error, self.decel_limit) brake = min(2 * abs(decel) * self.vehicle_mass * self.wheel_radius, 700) if (bDEBUG & ((self.iteration_count % DEBUG_THRESHOLD) == 0)): rospy.logwarn( "----------------------------------------------------------------------" ) #rospy.logwarn("Target angular vel: {0}".format(angular_vel)) rospy.logwarn("Target velocity : {0}".format(linear_vel)) rospy.logwarn("Current velocity : {0}".format(current_vel)) rospy.logwarn("Throttle : {0}".format(throttle)) rospy.logwarn("Braking : {0}".format(brake)) self.iteration_count += 1 return throttle, brake, steering
class Controller(object): def __init__(self, vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): # TODO: Implement self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle) kp = 0.3 ki = 0.1 kd = 0. mn = 0. #Minimum throttle value mx = 0.2 #Maximum throttle value self.throttle_controller = PID(kp, ki, kd, mn, mx) tau = 0.5 #1/(2pi*tau) = cutoff frequency ts = .02 #Sample time self.vel_lpf = LowPassFilter(tau, ts) self.vehicle_mass = vehicle_mass self.fuel_capacity = fuel_capacity self.brake_deadband = brake_deadband self.decel_limit = decel_limit self.accel_limit = accel_limit self.wheel_radius = wheel_radius self.last_time = rospy.get_time() def control(self, current_vel, dbw_enabled, linear_vel, angular_vel): # TODO: Change the arg, kwarg list to suit your needs # Return throttle, brake, steer if not dbw_enabled: self.throttle_controller.reset() return 0., 0., 0. current_vel = self.vel_lpf.filt(current_vel) steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel) vel_error = linear_vel - current_vel self.last_vel = current_vel current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time throttle = self.throttle_controller.step(vel_error, sample_time) brake = 0 if linear_vel == 0. and current_vel < 0.1: throttle = 0 brake = 400 #N*m - to hold the car in place if we are stopped at a light. Acceleration ~ 1m/s^2 elif throttle < .1 and vel_error < 0: throttle = 0 decel = max(vel_error, self.decel_limit) brake = abs( decel) * self.vehicle_mass * self.wheel_radius # Torque N*m return throttle, brake, steering
class Controller(object): def __init__(self, vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): # TODO: Implement # create and initialize YawController object self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle) # set PID parameters and threshholds kp = 0.3 ki = 0.1 kd = 0 mn = 0 # minimum throttle value mx = 0.2 # maximum throttle value # create and initialize PID controller object self.throttle_controller = PID(kp, ki, kd, mn, mx) # set filter parameters tau = 0.5 # 1/(2pi * tau) = cutoff frequency ts = .02 # sample time # create and initialize low pass filter to compensate for velocity oscilations self.vel_lpf = LowPassFilter(tau, ts) # load parameter buffers self.vehicle_mass = vehicle_mass self.fuel_capacity = fuel_capacity self.brake_deadband = brake_deadband self.decel_limit = decel_limit self.accel_limit = accel_limit self.wheel_radius = wheel_radius self.last_time = rospy.get_time() def control(self, current_vel, dbw_enabled, linear_vel, angular_vel): # TODO: Change the arg, kwarg list to suit your needs # Return throttle, brake, steer if not dbw_enabled: # reset throttle control when dbw system is disabled to avoid instabilities after system is reenebled self.throttle_controller.reset() return 0., 0., 0. current_vel = self.vel_lpf.filt(current_vel) # get steering set point steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel) vel_error = linear_vel - current_vel self.last_vel = current_vel current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time # get throttle set point throttle = self.throttle_controller.step(vel_error, sample_time) brake = 0 if linear_vel == 0. and current_vel < 0.1: # come to a full stop when velocity is small and set point is zero throttle = 0 brake = 700 elif throttle < .1 and vel_error < 0: # brake torque intensity/limitation logic throttle = 0 decel = max(vel_error, self.decel_limit) brake = abs(decel) * self.vehicle_mass * self.wheel_radius return throttle, brake, steering
class Controller(object): def __init__(self, *args, **kwargs): # TODO: Implement self.max_abs_angle = kwargs.get('max_steer_angle') self.fuel_capacity = kwargs.get('fuel_capacity') self.vehicle_mass = kwargs.get('vehicle_mass') self.wheel_radius = kwargs.get('wheel_radius') self.accel_limit = kwargs.get('accel_limit') self.decel_limit = kwargs.get('decel_limit') self.brake_deadband = kwargs.get('brake_deadband') self.max_acceleration = 1.5 self.wheel_base = kwargs.get('wheel_base') self.steer_ratio = kwargs.get('steer_ratio') self.max_steer_angle = kwargs.get('max_steer_angle') self.max_lat_accel = kwargs.get('max_lat_accel') self.yaw_controller = YawController( self.wheel_base, self.steer_ratio, 0.1, self.max_lat_accel, self.max_steer_angle) kp = 0.3 ki = 0.1 kd = 0. mn = 0. # Minimum throttle value mx = 0.2 # Max throttle value self.throttle_controller = PID(kp, ki, kd, mn, mx) tau = 0.5 # 1/(2pi*tau) = cutoff frequency ts = .02 # Sample time self.vel_lpf = LowPassFilter(tau, ts) #self.brake_lpf = LowPassFilter(0.5, 0.02) #total_vehicle_mass = self.vehicle_mass + self.fuel_capacity * GAS_DENSITY # 1.774,933 # max torque (1.0 throttle) and max brake torque (deceleration lmt) #self.max_acc_torque = total_vehicle_mass * self.max_acceleration * self.wheel_radius #567 #self.max_brake_torque = total_vehicle_mass * abs(self.decel_limit) * self.wheel_radius #2095 self.last_time = rospy.get_time() def control(self, current_vel, dbw_enabled, linear_vel, angular_vel): # TODO: Change the arg, kwarg list to suit your needs # Return throttle, brake, steer if not dbw_enabled: self.throttle_controller.reset() return 0., 0., 0. current_vel = self.vel_lpf.filt(current_vel) steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel) vel_error = linear_vel - current_vel self.last_vel = current_vel current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time throttle = self.throttle_controller.step(vel_error, sample_time) brake = 0 if linear_vel == 0. and current_vel < 0.1: throttle = 0 brake = 400 # N*m - to hold the car in place if we are stopped at a light. Accel ~ 1m/s**2 elif throttle < .1 and vel_error < 0: throttle = 0 decel = max(vel_error, self.decel_limit) brake = abs(decel)*self.vehicle_mass*self.wheel_radius # Torque N*m return throttle, brake, steering
class Controller(object): def __init__(self, vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): # TODO: Implement self.yaw_controller = YawController(wheel_base, steer_ratio, MIN_SPEED, max_lat_accel, max_steer_angle) # Some experimentaly set parameters kp = 0.3 ki = 0.1 kd = 0.0 mn = 0.0 # Min throttle mx = 0.5 # Max throttle self.throttle_controller = PID(kp, ki, kd, mn, mx) tau = 0.2 / 2.4 # tau = rise_time / 2.4 self.vel_lpf = MyLowPassFilter(tau, SAMPLE_TIME) self.vehicle_mass = vehicle_mass self.fuel_capacity = fuel_capacity self.brake_deadband = brake_deadband self.decel_limit = decel_limit self.accel_limit = accel_limit self.wheel_radius = wheel_radius # States self.prev_time = rospy.get_time() self.prev_vel = 0 def control(self, current_vel, dbw_enabled, linear_vel, angular_vel): # TODO: Change the arg, kwarg list to suit your needs # Return throttle, brake, steer if not dbw_enabled: self.throttle_controller.reset() return 0, 0, 0 # Measured velocity current_vel = self.vel_lpf.filt(current_vel) # TODO: Steering could be dampened for better control ? steering = self.yaw_controller.get_steering( linear_vel, angular_vel, current_vel) # linear_vel, angular_vel, current_vel vel_error = linear_vel - current_vel self.prev_vel = current_vel current_time = rospy.get_time() sample_time = current_time - self.prev_time self.prev_time = current_time throttle = self.throttle_controller.step(vel_error, sample_time) brake = 0 # It means full brake torque on all wheels if linear_vel == 0 and current_vel < MIN_SPEED: throttle = 0 brake = 700 # In [N], to be enough to keep car still elif throttle < 0.1 and vel_error < 0: # Basically here we are going faster then setpoint so we will # brake up to brake limit throttle = 0 decel = max(vel_error, self.decel_limit) brake = abs(decel) * self.vehicle_mass * self.wheel_radius # [Nm] return throttle, brake, steering
class Controller(object): def __init__(self, vehicle_mass=None, fuel_capacity=None, brake_deadband=None, decel_limit=None, accel_limit=None, wheel_radius=None, wheel_base=None, steer_ratio=None, max_lat_accel=None, max_steer_angle=None): self.vehicle_mass = vehicle_mass self.fuel_capacity = fuel_capacity self.brake_deadband = brake_deadband self.decel_limit = decel_limit self.accel_limit = accel_limit self.wheel_radius = wheel_radius self.wheel_base = wheel_base self.steer_ratio = steer_ratio self.max_lat_accel = max_lat_accel self.max_steer_angle = max_steer_angle self.controller_steering = YawController( wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle) self.controller_throttle = PID(0.3, 0.01, 0.1, 0.0, 0.2) self.lowpass = LowPassFilter(0.5, 1. / 50.) self.last_time = rospy.get_time() self.last_vel = 0.0 def control(self, linear_vel, angular_vel, current_vel, dbw_enabled): # TODO: Change the arg, kwarg list to suit your needs # Return throttle, brake, steer if not dbw_enabled: self.controller_throttle.reset() return 0.0, 0.0, 0.0 # low pass the current velocity current_vel = self.lowpass.filt(current_vel) # update time current_time = rospy.get_time() step_time = current_time - self.last_time self.last_time = current_time # steering steering = self.controller_steering.get_steering( linear_vel, angular_vel, current_vel) # velocity vel_error = linear_vel - current_vel self.last_vel = current_vel # logic on throttle and brake throttle = self.controller_throttle.step(vel_error, step_time) brake = 0.0 if linear_vel == 0 and current_vel < 0.1: # we want to stop throttle = 0.0 brake = 700 # N*m elif throttle < 0.1 and vel_error < 0.0: # we want to decelerate throttle = 0.0 decelerate = max(vel_error, self.decel_limit) brake = abs(decelerate) * self.vehicle_mass * self.wheel_radius rospy.logwarn('Throttle {} - Brake {} - Steer {}'.format(throttle, brake, steering)) return throttle, brake, steering
class CF_model(): def __init__(self): rospy.init_node("dynamic_model", anonymous=True) self.topic = rospy.get_param("~topic") rospy.Subscriber(self.topic + "/cmd_vel", Twist, self.new_attitude_setpoint) rospy.Subscriber(self.topic + "/cmd_pos", Position, self.new_position_setpoint) rospy.Subscriber("/init_pose", PointStamped, self.new_init_position) self.pub_pos = rospy.Publisher(self.topic + "/out_pos", PoseStamped, queue_size=1000) self.pub_ack = rospy.Publisher("/init_pose_ack", String, queue_size=1000) self.msg = PoseStamped() self.isInit = False # System state: position, linear velocities, # attitude and angular velocities self.cf_state = CF_state() # Import the crazyflie physical paramters # - These parameters are obtained from different sources. # - For these parameters and the dynamical equations refer # to : DESIGN OF A TRAJECTORY TRACKING CONTROLLER FOR A # NANOQUADCOPTER # Luis, C., & Le Ny, J. (August, 2016) self.cf_physical_params = CF_parameters() # Import the PID gains (from the firmware) self.cf_pid_gains = CF_pid_params() # Main CF variables initialization (if needed) self.simulation_freq = rospy.Rate( int(1 / self.cf_physical_params.DT_CF)) ###################### # Initialize PID ###################### # Out from the PIDs, values of # r, p, y, thrust self.desired_rpy = np.zeros(3) # Comes from the external position controller self.desired_thrust = 0.0 ###################### # Angular velocities ###################### self.wx_pid = PID(self.cf_pid_gains.KP_WX, self.cf_pid_gains.KI_WX, self.cf_pid_gains.KD_WX, self.cf_pid_gains.INT_MAX_WX, self.cf_pid_gains.WX_DT) self.wy_pid = PID(self.cf_pid_gains.KP_WY, self.cf_pid_gains.KI_WY, self.cf_pid_gains.KD_WY, self.cf_pid_gains.INT_MAX_WY, self.cf_pid_gains.WY_DT) self.wz_pid = PID(self.cf_pid_gains.KP_WZ, self.cf_pid_gains.KI_WZ, self.cf_pid_gains.KD_WZ, self.cf_pid_gains.INT_MAX_WZ, self.cf_pid_gains.WZ_DT) self.desired_ang_vel = np.zeros(3) ###################### # Attitudes ###################### self.roll_pid = PID(self.cf_pid_gains.KP_ROLL, self.cf_pid_gains.KI_ROLL, self.cf_pid_gains.KD_ROLL, self.cf_pid_gains.INT_MAX_ROLL, self.cf_pid_gains.ROLL_DT) self.pitch_pid = PID(self.cf_pid_gains.KP_PITCH, self.cf_pid_gains.KI_PITCH, self.cf_pid_gains.KD_PITCH, self.cf_pid_gains.INT_MAX_PITCH, self.cf_pid_gains.PITCH_DT) self.yaw_pid = PID(self.cf_pid_gains.KP_YAW, self.cf_pid_gains.KI_YAW, self.cf_pid_gains.KD_YAW, self.cf_pid_gains.INT_MAX_YAW, self.cf_pid_gains.YAW_DT) self.att_pid_counter = 0 self.att_pid_counter_max = int(self.cf_physical_params.DT_ATT_PIDS / self.cf_physical_params.DT_CF) - 1 self.desired_att = np.zeros(3) ###################### # Angular velocities ###################### self.vx_pid = PID(self.cf_pid_gains.KP_VX, self.cf_pid_gains.KI_VX, self.cf_pid_gains.KD_VX, self.cf_pid_gains.INT_MAX_VX, self.cf_pid_gains.VX_DT) self.vy_pid = PID(self.cf_pid_gains.KP_VY, self.cf_pid_gains.KI_VY, self.cf_pid_gains.KD_VY, self.cf_pid_gains.INT_MAX_VY, self.cf_pid_gains.VY_DT) self.vz_pid = PID(self.cf_pid_gains.KP_VZ, self.cf_pid_gains.KI_VZ, self.cf_pid_gains.KD_VZ, self.cf_pid_gains.INT_MAX_VZ, self.cf_pid_gains.VZ_DT) self.desired_lin_vel = np.zeros(3) ###################### # Angular velocities ###################### self.x_pid = PID(self.cf_pid_gains.KP_X, self.cf_pid_gains.KI_X, self.cf_pid_gains.KD_X, self.cf_pid_gains.INT_MAX_X, self.cf_pid_gains.X_DT) self.y_pid = PID(self.cf_pid_gains.KP_Y, self.cf_pid_gains.KI_Y, self.cf_pid_gains.KD_Y, self.cf_pid_gains.INT_MAX_Y, self.cf_pid_gains.Y_DT) self.z_pid = PID(self.cf_pid_gains.KP_Z, self.cf_pid_gains.KI_Z, self.cf_pid_gains.KD_Z, self.cf_pid_gains.INT_MAX_Z, self.cf_pid_gains.Z_DT) self.desired_pos = np.zeros(3) ############################ # Communication control ############################ self.out_pos_counter = 0 self.out_pos_counter_max = int(self.cf_physical_params.DT_POS_PIDS / self.cf_physical_params.DT_CF) - 1 self.mode = "POS" self.time_processing = [] self.delta_time = [] ########################### # Math operations functions ########################### # Rotation matrix around X def rot_x(self, alpha): cos_a = cos(alpha) sin_a = sin(alpha) M = np.array([[1, 0, 0], [0, cos_a, sin_a], [0, -sin_a, cos_a]]) return M # Rotation matrix around Y def rot_y(self, beta): cos_b = cos(beta) sin_b = sin(beta) M = np.array([[cos_b, 0, -sin_b], [0, 1, 0], [sin_b, 0, cos_b]]) return M # Rotation matrix around Z def rot_z(self, gamma): cos_g = cos(gamma) sin_g = sin(gamma) M = np.array([[cos_g, sin_g, 0], [-sin_g, cos_g, 0], [0, 0, 1]]) return M # Rotation matrix - from the body frame to the inertial frame def rot_m(self, roll, pitch, yaw): rotx = self.rot_x(roll) roty = self.rot_y(pitch) rotz = self.rot_z(yaw) rot = np.dot(np.dot(rotx, roty), rotz) return rot def rotation_matrix(self, roll, pitch, yaw): return np.array( [[cos(pitch) * cos(yaw), cos(pitch) * sin(yaw), -sin(pitch)], [ sin(roll) * sin(pitch) * cos(yaw) - cos(roll) * sin(yaw), sin(roll) * sin(pitch) * sin(yaw) + cos(roll) * cos(yaw), sin(roll) * cos(pitch) ], [ cos(roll) * sin(pitch) * cos(yaw) + sin(roll) * sin(yaw), cos(roll) * sin(pitch) * sin(yaw) - sin(roll) * cos(yaw), cos(roll) * cos(pitch) ]]) def euler_matrix(self, roll, pitch, yaw): cos_roll = cos(roll) sin_roll = sin(roll) cos_pitch = cos(pitch) + 1e-12 tan_pitch = tan(pitch) return np.array([[1, sin_roll * tan_pitch, cos_roll * tan_pitch], [0, cos_roll, -sin_roll], [0, sin_roll / cos_pitch, cos_roll / cos_pitch]]) def limit(self, value): return max(min(self.cf_physical_params.PWM_MAX, value), self.cf_physical_params.PWM_MIN) ########################### # Callback function ########################### def new_attitude_setpoint(self, twist_msg): ##############!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! ### DUDA EN EL CODIGO DEL SERVER DE LO QUE ES m_X_trim ###################################################### self.mode = "ATT" self.desired_att[0] = twist_msg.linear.y self.desired_att[1] = -twist_msg.linear.x self.desired_ang_vel[2] = twist_msg.angular.z self.desired_thrust = min(twist_msg.linear.z, 60000) ########################### # Callback function ########################### def new_position_setpoint(self, position_msg): ##############!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! ### DUDA EN EL CODIGO DEL SERVER DE LO QUE ES m_X_trim ###################################################### self.mode = "POS" self.desired_pos[0] = position_msg.x self.desired_pos[1] = position_msg.y self.desired_pos[2] = position_msg.z self.desired_att[2] = position_msg.yaw def new_init_position(self, position_msg): if (self.topic == position_msg.header.frame_id): self.cf_state.position = np.array([ position_msg.point.x, position_msg.point.y, position_msg.point.z ]) self.desired_pos = np.array([ position_msg.point.x, position_msg.point.y, position_msg.point.z ]) self.isInit = True msgID = String() msgID = position_msg.header.frame_id self.pub_ack.publish(msgID) ########################### # Single step simulation ########################### def apply_simulation_step(self): ########################### # Main simulation loop # - The CF works at a rate of 1000Hz, # in the same way, we are simulating # at the same frequency ########################### # New simulated state new_state = CF_state() rotation_matrix = self.rot_m(self.cf_state.attitude[0], self.cf_state.attitude[1], self.cf_state.attitude[2]) euler_matrix = self.euler_matrix(self.cf_state.attitude[0], self.cf_state.attitude[1], self.cf_state.attitude[2]) self.cf_state.getMotorRotationSpeed() self.cf_state.addMotorsRotationsSpeed() self.cf_state.getForces() self.cf_state.getMomentums() new_state.lin_vel = np.dot( np.linalg.inv(rotation_matrix), self.cf_state.forces ) / self.cf_physical_params.DRONE_MASS - np.array([ 0, 0, self.cf_physical_params.G ]) # - np.cross(self.cf_state.ang_vel, self.cf_state.lin_vel) new_state.position = self.cf_state.lin_vel preoperation = self.cf_state.momentums - np.cross( self.cf_state.ang_vel, np.dot(self.cf_physical_params.INERTIA_MATRIX, self.cf_state.ang_vel)) new_state.ang_vel = np.dot(self.cf_physical_params.INV_INERTIA_MATRIX, preoperation) new_state.attitude = np.dot(euler_matrix, self.cf_state.ang_vel) for i in range(0, 3): self.cf_state.position[i] = self.cf_state.position[i] + ( new_state.position[i] * self.cf_physical_params.DT_CF) self.cf_state.attitude[i] = self.cf_state.attitude[i] + ( new_state.attitude[i] * self.cf_physical_params.DT_CF) self.cf_state.lin_vel[i] = self.cf_state.lin_vel[i] + ( new_state.lin_vel[i] * self.cf_physical_params.DT_CF) self.cf_state.ang_vel[i] = self.cf_state.ang_vel[i] + ( new_state.ang_vel[i] * self.cf_physical_params.DT_CF) self.cf_state.ang_vel_deg = self.cf_state.ang_vel * 180.0 / np.pi self.cf_state.attitude_deg = self.cf_state.attitude * 180.0 / np.pi # Ground constraints if self.cf_state.position[2] <= 0: self.cf_state.position[2] = 0 if self.cf_state.lin_vel[2] <= 0: self.cf_state.lin_vel[:] = 0. self.cf_state.attitude[:-1] = 0. self.cf_state.ang_vel[:] = 0. def run_pos_pid(self): self.desired_lin_vel = np.array([ self.x_pid.update(self.desired_pos[0], self.cf_state.position[0]), self.y_pid.update(self.desired_pos[1], self.cf_state.position[1]), self.z_pid.update(self.desired_pos[2], self.cf_state.position[2]) ]) def run_lin_vel_pid(self): raw_pitch = self.vy_pid.update(self.desired_lin_vel[1], self.cf_state.lin_vel[1]) raw_roll = self.vx_pid.update(self.desired_lin_vel[0], self.cf_state.lin_vel[0]) # Current YAW raw_yaw = -self.cf_state.attitude[2] # Transformation to the drone body frame self.desired_att[1] = -(raw_roll * cos(raw_yaw)) - (raw_pitch * sin(raw_yaw)) self.desired_att[0] = -(raw_pitch * cos(raw_yaw)) + (raw_roll * sin(raw_yaw)) self.desired_att[0] = max( min(self.cf_pid_gains.MAX_ATT, self.desired_att[0]), -self.cf_pid_gains.MAX_ATT) self.desired_att[1] = max( min(self.cf_pid_gains.MAX_ATT, self.desired_att[1]), -self.cf_pid_gains.MAX_ATT) raw_thrust = self.vz_pid.update(self.desired_lin_vel[2], self.cf_state.lin_vel[2]) self.desired_thrust = max( (raw_thrust * 1000 + self.cf_physical_params.BASE_THRUST), self.cf_physical_params.PWM_MIN) def run_att_pid(self): self.desired_ang_vel = np.array([ self.roll_pid.update(self.desired_att[0], self.cf_state.attitude_deg[0]), self.pitch_pid.update(-self.desired_att[1], self.cf_state.attitude_deg[1]), self.yaw_pid.update(self.desired_att[2], self.cf_state.attitude_deg[2]) ]) def run_ang_vel_pid(self): self.desired_rpy = np.array([ self.wx_pid.update(self.desired_ang_vel[0], self.cf_state.ang_vel_deg[0]), -self.wy_pid.update(self.desired_ang_vel[1], self.cf_state.ang_vel_deg[1]), -self.wz_pid.update(self.desired_ang_vel[2], self.cf_state.ang_vel_deg[2]) ]) self.rpyt_2_motor_pwm() def log_state(self): rospy.loginfo( "\n Nuevo estado: \n Position: " + str(self.cf_state.position) + "\n Lin. Velocity: " + str(self.cf_state.lin_vel) + "\n Attitude: " + str(self.cf_state.attitude * 180 / np.pi) + "\n Ang. velocities: " + str(self.cf_state.ang_vel * 180 / np.pi) + "\n Delta time: " + str(sum(self.delta_time) / len(self.delta_time)) + "\n Processing time " + str(sum(self.time_processing) / len(self.time_processing))) self.time_processing = [] self.delta_time = [] def rpyt_2_motor_pwm(self): # Inputs r = self.desired_rpy[0] p = self.desired_rpy[1] y = self.desired_rpy[2] thrust = self.desired_thrust ########################## # Function that transform the output # r, p, y, thrust of the PIDs, # into the values of the PWM # applied to each motor ########################## R = r / 2.0 P = p / 2.0 Y = y self.cf_state.motor_pwm[0] = int(self.limit(thrust - R + P + Y)) self.cf_state.motor_pwm[1] = int(self.limit(thrust - R - P - Y)) self.cf_state.motor_pwm[2] = int(self.limit(thrust + R - P + Y)) self.cf_state.motor_pwm[3] = int(self.limit(thrust + R + P - Y)) def publishPose(self): self.msg.header.frame_id = "/base_link" self.msg.pose.position.x = self.cf_state.position[0] self.msg.pose.position.y = self.cf_state.position[1] self.msg.pose.position.z = self.cf_state.position[2] quaternion = tf.transformations.quaternion_from_euler( self.cf_state.attitude[0], self.cf_state.attitude[1], self.cf_state.attitude[2]) self.msg.pose.orientation.w = quaternion[3] self.msg.pose.orientation.x = quaternion[0] self.msg.pose.orientation.y = quaternion[1] self.msg.pose.orientation.z = quaternion[2] self.pub_pos.publish(self.msg) br = tf.TransformBroadcaster() br.sendTransform(self.cf_state.position, quaternion, rospy.Time.now(), self.topic + "/base_link", "/base_link") def run(self): tic_init = time.time() while (not rospy.is_shutdown()): if (self.isInit): self.delta_time.append(time.time() - tic_init) tic_init = time.time() self.apply_simulation_step() if (self.att_pid_counter == self.att_pid_counter_max): self.att_pid_counter = 0 self.run_att_pid() self.run_ang_vel_pid() else: self.att_pid_counter = self.att_pid_counter + 1 if (self.out_pos_counter == self.out_pos_counter_max): self.out_pos_counter = 0 self.run_pos_pid() self.run_lin_vel_pid() self.publishPose() #self.log_state() else: self.out_pos_counter = self.out_pos_counter + 1 self.time_processing.append(time.time() - tic_init) # Wait for the cycle left time self.simulation_freq.sleep()
import sys import tornado.ioloop import tornado.web import datetime import time import random import face import person import cv2 import tracker import random from pid import PID #curr_coords = None #initBB = None panPID = PID(0.16, 0.00, 0.00) # panPID.initialize() tiltPID = PID(0.24, 0.00, 0.00) #tiltPID.initialize() class MainHandler(tornado.web.RequestHandler): def set_extra_headers(self): self.set_header("Cache-Control", "no-store, no-cache, must-revalidate, max-age=0") def get(self): self.write('<img src="img/camera.jpeg?t=' + str(time.time()) + '" />') def post(self): global cvtracker
class Controller(object): def __init__(self, vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): # Init yaw controller self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle) # PID Params kp = 0.3 ki = 0.1 kd = 0.0 mn = 0.0 # Min throttle value mx = 0.2 # Maximum throttle value self.throttle_controller = PID(kp, ki, kd, mn, mx) # LP filter params tau = 0.5 ts = 0.02 self.vel_lpf = LowPassFilter(tau, ts) # Other infos self.vehice_mass = vehicle_mass self.fuel_capacity = fuel_capacity self.brake_deadband = brake_deadband self.decel_limit = decel_limit # Unused self.accel_limit = accel_limit # Unused self.wheel_radius = wheel_radius self.last_time = rospy.get_time() def control(self, current_vel, dbw_enabled, linear_vel, angular_vel): # If manual control is enabled, don't do anything if not dbw_enabled: # Avoid error acuumulation self.throttle_controller.reset() return 0.0, 0.0, 0.0 # Filter current velocity current_vel = self.vel_lpf.filt(current_vel) # Calculate steering angle steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel) # Check what is current vel error from target vel vel_error = linear_vel - current_vel # Update last vel self.last_vel = current_vel # Calculate time step (needed for PID) current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time # Use PID to calculate throttle throttle = self.throttle_controller.step(vel_error, sample_time) brake = 0 # If target velocity is 0 and current velocity is very small that means we need to stop if linear_vel == 0. and current_vel < 0.1: throttle = 0 # Apply brake brake = 400 # N * m # If throttle is smal and we are going over target velocity elif throttle < 0.1 and vel_error < 0: # Let go of throttle throttle = 0 decel = max(vel_error, self.decel_limit) # apply brake - slightly brake = abs(decel) * self.vehice_mass * self.wheel_radius # Torque N * m return throttle, brake, steering
def __init__(self, fc=100.0, ns='kr6'): self.fc = fc self.rate = rospy.Rate(self.fc) # 100 Hz self.Tc = 1.0 / self.fc self._joint_states = JointState self._cmd_joint = JointState self._cmd_vel = TwistStamped # self.tip_pose = Pose self.ft_sensor = WrenchStamped() self.ft_twist = Twist() # self.x_pid = PID(30.0, 0.0, 1.0, 1.0, -1.0) # self.y_pid = PID(30.0, 0.0, 1.0, 1.0, -1.0) # self.z_pid = PID(30.0, 0.0, 1.0, 1.0, -1.0) self.q1_pid = PID(10.0, 0.0, 0.0, 1.0, -1.0) self.q2_pid = PID(10.0, 0.0, 0.0, 1.0, -1.0) self.q3_pid = PID(10.0, 0.0, 0.0, 1.0, -1.0) self.q4_pid = PID(10.0, 0.0, 0.0, 1.0, -1.0) self.q5_pid = PID(10.0, 0.0, 0.0, 1.0, -1.0) self.q6_pid = PID(10.0, 0.0, 0.0, 1.0, -1.0) self.iter = 0 self.iter_data = 0 # self.home = np.array([0.0, -math.pi / 2, math.pi / 2, 0.0, -math.pi / 2 + 0.3, 0.0]).reshape((6, 1)) # self.home = np.array([0.0,0.0,0.0,0.0,0.0,0.0]).reshape((6,1)) self.home = np.array([ -0.0 * math.pi, -math.pi / 2, math.pi / 2, 0.0, -math.pi / 2, 0.0 ]).reshape((6, 1)) # self.q0 = self.home.copy() self.w = np.zeros((6, 1)) self.e = np.zeros((6, 1)) self.u = np.zeros((6, 1)) self.v = np.zeros((6, 1)) # Joint limits self.uMin = np.zeros((6, 1)) self.uMax = np.zeros((6, 1)) self.uMin[0] = -np.pi * 170 / 180 # Default -170 self.uMin[1] = -np.pi * 190 / 180 # Default -190 self.uMin[2] = -np.pi * 79 / 180 # Default -120 self.uMin[3] = -np.pi * 185 / 180 # Default -185 self.uMin[4] = -np.pi * 115 / 180 # Default -120 self.uMin[5] = -np.pi * 350 / 180 # Default -350 self.uMax[0] = np.pi * 170 / 180 # Default 170 self.uMax[1] = np.pi * 45 / 180 # Default 45 self.uMax[2] = np.pi * 156 / 180 # Default 156 self.uMax[3] = np.pi * 185 / 180 # Default 185 self.uMax[4] = np.pi * 95 / 180 # Default 120 self.uMax[5] = np.pi * 350 / 180 # Default 350 self.joint_names = [ 'joint_a1', 'joint_a2', 'joint_a3', 'joint_a4', 'joint_a5', 'joint_a6' ] # self.joint_positions = np.array([self.q1,self.q2,self.q3,self.q4,self.q5,self.q6]) self.joint_positions = self.home self.trajectory = JointTrajectory() # self.trajectory.header = Header(stamp=rospy.Time.now(), frame_id='base', seq=self.iter) self.trajectory.joint_names = self.joint_names # self.trajectory.points.append(JointTrajectoryPoint(positions = np.array([0.0,-1.57,1.57,-0.0,-1.57,-0.0]), time_from_start = rospy.Duration(1.0,0.0))) self.trajectory.points.append( JointTrajectoryPoint(positions=self.joint_positions, time_from_start=rospy.Duration(0, 500000000))) # self.kin = kuka_kinematics('kr6') # print '\n*** Kuka Description ***\n' # self.kin.print_robot_description() # print '\n*** Kuka KDL Chain ***\n' # self.kin.print_kdl_chain() # # FK Position # print '\n*** Kuka Position FK ***\n' # print self.kin.forward_position_kinematics() # # FK Velocity # # print '\n*** Kuka Velocity FK ***\n' # # kin.forward_velocity_kinematics() # # IK # print '\n*** Kuka Position IK ***\n' # pos = [4.45426035e-01, 0.0, 9.69999830e-01] # rot = [0.0, 3.98163384e-04, 0.0, 9.99999921e-01] # print self.kin.inverse_kinematics(pos) # position, don't care orientation # print '\n*** Kuka Pose IK ***\n' # print self.kin.inverse_kinematics(pos, rot) # position & orientation # # Jacobian # print '\n*** Kuka Jacobian ***\n' # print self.kin.jacobian() # # Jacobian Transpose # print '\n*** Kuka Jacobian Tranpose***\n' # print self.kin.jacobian_transpose() # # Jacobian Pseudo-Inverse (Moore-Penrose) # print '\n*** Kuka Jacobian Pseudo-Inverse (Moore-Penrose)***\n' # print self.kin.jacobian_pseudo_inverse() # # Joint space mass matrix # print '\n*** Kuka Joint Inertia ***\n' # print self.kin.inertia() # # Cartesian space mass matrix # print '\n*** Kuka Cartesian Inertia ***\n' # print self.kin.cart_inertia() self.new_cmd_joint = False self.new_cmd_vel = False self.new_msg = False self.DH = [] self.Tn = [] self.Twe = np.eye(4) self.p = np.zeros((6, 1)) self.pd = np.zeros((6, 1)) self.base = np.eye(4) self.base[0, 3] = -0.46 self.base[1, 3] = 0 self.base[2, 3] = 2.05075 # 2.05275 # self.base[2,3]=2.055 self.J0 = [] self.Jn = [] self.Jrec = [] self.n_joints = 6 self.name = [None] * self.n_joints self.q = [None] * self.n_joints self.dq = [None] * self.n_joints self.tool = 0.127 + 0.035 # +0.0225 self.p_ee = Point() self.robot_pose = PoseStamped() self.tool_pose = PoseStamped() self.object_pose = PoseStamped() self.ns = ns self.pubTrajectory_ = rospy.Publisher( "/position_trajectory_controller/command", JointTrajectory, queue_size=1) self.pubTrajectory = rospy.Publisher( self.ns + "/position_trajectory_controller/command", JointTrajectory, queue_size=1) self.pub_coords = rospy.Publisher('/input_coords', Point, queue_size=1) self.pub_tool = rospy.Publisher('/tool', PoseStamped, queue_size=1) self.pub_robot = rospy.Publisher('/base_robot', PoseStamped, queue_size=1) self.pub_object = rospy.Publisher('/object', PoseStamped, queue_size=1) self.pub_ft = rospy.Publisher('/ft', Twist, queue_size=1) self._cbJointState = rospy.Subscriber(self.ns + "/joint_states", JointState, self.callback_joint_states, queue_size=1) self._cbCmdJoint = rospy.Subscriber(self.ns + "/cmd_joint", JointState, self.callback_cmd_joint, queue_size=1) self._cbCmdVel = rospy.Subscriber(self.ns + "/cmd_vel", TwistStamped, self.callback_cmd_vel, queue_size=1) #self._cbFTSensor= rospy.Subscriber(self.ns + "/axia80/ft_sensor_topic", WrenchStamped, self.callback_ft_sensor, queue_size=1) #Simulation #self._cbJointState = rospy.Subscriber("/joint_states", JointState, self.callback_joint_states, queue_size=1) #self._cbCmdJoint = rospy.Subscriber("/cmd_joint", JointState, self.callback_cmd_joint, queue_size=1) #self._cbCmdVel = rospy.Subscriber("/cmd_vel", TwistStamped, self.callback_cmd_vel, queue_size=1) #self._cbFTSensor= rospy.Subscriber("/axia80/ft_sensor_topic", WrenchStamped, self.callback_ft_sensor, queue_size=1) #Simulation self._cbFTSensor = rospy.Subscriber("/netft_data_172_1_10_1", WrenchStamped, self.callback_ft_sensor, queue_size=1) #ATI Axia80 real self.go_home()
class Controller(object): def __init__(self, vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): # TODO: Implement self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle) kp = 0.3 ki = 0.1 kd = 0. mn = 0. mx = 0.2 self.throttle_controller = PID(kp, ki, kd, mn, mx) tau = 0.5 ts = 0.02 self.vel_lpf = LowPassFilter(tau, ts) self.vehicle_mass = vehicle_mass self.fuel_capacity = fuel_capacity self.brake_deadband = brake_deadband self.decel_limit = decel_limit self.accel_limit = accel_limit self.wheel_radius = wheel_radius self.last_time = rospy.get_time() def control(self, pro_linear_vel, pro_angular_vel, curr_velocity, dbw_enabled): # TODO: Change the arg, kwarg list to suit your needs # Return throttle, brake, steer if not dbw_enabled: self.throttle_controller.reset( ) # Need reset pid when the car is taked over return 0.0, 0.0, 0.0 curr_velocity = self.vel_lpf.filt(curr_velocity) steering = self.yaw_controller.get_steering(pro_linear_vel, pro_angular_vel, curr_velocity) vel_error = pro_linear_vel - curr_velocity self.last_vel = curr_velocity current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time throttle = self.throttle_controller.step(vel_error, sample_time) brake = 0 if pro_linear_vel == 0. and curr_velocity < 0.1: throttle = 0 brake = 400 elif throttle < 0.1 and vel_error < 0: throttle = 0 decel = max(vel_error, self.decel_limit) brake = abs(decel) * self.vehicle_mass * self.wheel_radius return throttle, brake, steering
class Controller(object): def __init__(self, vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): # TODO: Implement self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle) kp = 0.3 ki = 0.1 kd = 0. mn = 0. # minimum throttle value mx = 0.2 # maximum throttle value self.throttle_controller = PID(kp, ki, kd, mn, mx) tau = 0.5 # 1/(2pi*tau) = cutoff frequency ts = .02 # sample time self.vel_lpf = LowPassFilter(tau, ts) self.vehicle_mass = vehicle_mass self.fuel_capacity = fuel_capacity self.brake_deadband = brake_deadband self.decel_limit = decel_limit self.accel_limit = accel_limit self.wheel_radius = wheel_radius self.last_time = rospy.get_time() def control(self, current_vel, dbw_enabled, linear_vel, angular_vel): # TODO: Change the arg, kwarg list to suit your needs # Return throttle, brake, steer # print 'dbw status: ', dbw_enabled rospy.logwarn("DBW Status: {0}".format(dbw_enabled)) if not dbw_enabled: self.throttle_controller.reset() return 0., 0., 0. current_vel = self.vel_lpf.filt(current_vel) # rospy.logwarn("Angular vel: {0}".format(angular_vel)) # rospy.logwarn("Target velocity: {0}".format(linear_vel)) # rospy.logwarn("Target angular velocity: {0}\n".format(angular_vel)) # rospy.logwarn("Current velocity: {0}".format(current_vel)) # rospy.logwarn("Filtered velocity: {0}".format(self.vel_lpf.get())) steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel) vel_error = linear_vel - current_vel self.last_vel = current_vel current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time throttle = self.throttle_controller.step(vel_error, sample_time) brake = 0 if linear_vel == 0. and current_vel < 0: throttle = 0 brake = 400 # N.m, to hold car in place, acceleration ~ 1 m/s2 elif throttle < .1 and vel_error < 0: throttle = 0 decel = max(vel_error, self.decel_limit) brake = abs( decel) * self.vehicle_mass * self.wheel_radius # Torque N.m return throttle, brake, steering
if ptu_offset_mode == 1: #Command PTU to point at sun ptu.ephem_point(ep, imu=imu, target='sun', init=False) if ptu_offset_mode == 2: #Command PTU to point at moon ptu.ephem_point(ep, imu=imu, target='moon', init=False) else: track_mode = default_track_mode filter_mode = default_filter_mode ptu_offset_mode = default_ptu_offset_mode #Initiate PID control loop pid_x = PID( step_size='eighth' ) #pid_x will control azimuth ptu motor (assuming orientation of ss is correct) pid_x.SetKp(kp_x) pid_x.SetKi(ki_x) pid_x.SetKd(kd_x) pid_y = PID( step_size='eighth' ) #pid_y will control azimuth ptu motor (assuming orientation of ss is correct) pid_y.SetKp(kp_y) pid_y.SetKi(ki_y) pid_y.SetKd(kd_y) #Set ptu=None if not using tracking to ensure PTU is not moved after initial offset if track_mode == 4: ptu.ptu.close()
class Controller(object): def __init__(self, *args, **kwargs): rospy.loginfo('TwistController: Start init') self.sampling_rate = kwargs["sampling_rate"] self.decel_limit = kwargs["decel_limit"] self.accel_limit = kwargs["accel_limit"] # brake_deadband is the interval in which the brake would be ignored # the car would just be allowed to slow by itself/coast to a slower speed self.brake_deadband = kwargs["brake_deadband"] self.vehicle_mass = kwargs["vehicle_mass"] self.fuel_capacity = kwargs["fuel_capacity"] self.wheel_radius = kwargs["wheel_radius"] # bunch of parameters to use for the Yaw (steering) controller self.wheel_base = kwargs["wheel_base"] self.steer_ratio = kwargs["steer_ratio"] self.max_lat_accel = kwargs["max_lat_accel"] self.max_steer_angle = kwargs["max_steer_angle"] self.delta_t = 1 / self.sampling_rate self.brake_torque_const = (self.vehicle_mass + self.fuel_capacity \ * GAS_DENSITY) * self.wheel_radius self.low_pass_filter = LowPassFilter(0.2, self.delta_t) # Initialise speed PID, with tuning parameters # Will use this PID for the speed control self.velocity_pid = PID(0.9, 0.0005, 0.07, self.decel_limit, self.accel_limit) # Initialise Yaw controller - this gives steering values using # vehicle attributes/bicycle model # Need to have some minimum speed before steering is applied self.yaw_controller = YawController( wheel_base=self.wheel_base, steer_ratio=self.steer_ratio, min_speed=5.0, max_lat_accel=self.max_lat_accel, max_steer_angle=self.max_steer_angle) rospy.loginfo('TwistController: Complete init') rospy.loginfo('TwistController: Steer ratio = ' + str(self.steer_ratio)) def control(self, target_linear, target_angular, current_linear): throttle, brake, steering = 0.0, 0.0, 0.0 error_velocity = target_linear - current_linear # use velocity controller compute desired accelaration acc_desired = self.velocity_pid.step(error_velocity, self.delta_t) if acc_desired > 0.0: throttle = self.low_pass_filter.filt(acc_desired) brake = 0.0 else: throttle = 0.0 brake = 0.0 if abs(acc_desired) > self.brake_deadband: # don't bother braking unless over the deadband level # make sure we do not brake to hard if abs(acc_desired) > abs(self.decel_limit): brake = abs(self.decel_limit) * self.brake_torque_const else: brake = abs(acc_desired) * self.brake_torque_const # steering - yaw controller takes desired linear, desired angular, current linear as params #steering = target_angular * self.steer_ratio steering = self.yaw_controller.get_steering(target_linear, target_angular, current_linear) if abs(steering) <> 0.0: #rospy.loginfo('TwistController: Steering = ' + str(steering)) rospy.loginfo('Veer: Steering = ' + str(steering) + ', required = ' + str(target_angular)) return throttle, brake, steering def reset(self): self.velocity_pid.reset()
def __init__(self): # TODO: Implement self.Acc_controller = PID(0.5,0.00,0.1,mn=0.1,mx=1.0) self.yaw_controller = YawController()
def __init__(self): rospy.init_node("dynamic_model", anonymous=True) self.topic = rospy.get_param("~topic") rospy.Subscriber(self.topic + "/cmd_vel", Twist, self.new_attitude_setpoint) rospy.Subscriber(self.topic + "/cmd_pos", Position, self.new_position_setpoint) rospy.Subscriber("/init_pose", PointStamped, self.new_init_position) self.pub_pos = rospy.Publisher(self.topic + "/out_pos", PoseStamped, queue_size=1000) self.pub_ack = rospy.Publisher("/init_pose_ack", String, queue_size=1000) self.msg = PoseStamped() self.isInit = False # System state: position, linear velocities, # attitude and angular velocities self.cf_state = CF_state() # Import the crazyflie physical paramters # - These parameters are obtained from different sources. # - For these parameters and the dynamical equations refer # to : DESIGN OF A TRAJECTORY TRACKING CONTROLLER FOR A # NANOQUADCOPTER # Luis, C., & Le Ny, J. (August, 2016) self.cf_physical_params = CF_parameters() # Import the PID gains (from the firmware) self.cf_pid_gains = CF_pid_params() # Main CF variables initialization (if needed) self.simulation_freq = rospy.Rate( int(1 / self.cf_physical_params.DT_CF)) ###################### # Initialize PID ###################### # Out from the PIDs, values of # r, p, y, thrust self.desired_rpy = np.zeros(3) # Comes from the external position controller self.desired_thrust = 0.0 ###################### # Angular velocities ###################### self.wx_pid = PID(self.cf_pid_gains.KP_WX, self.cf_pid_gains.KI_WX, self.cf_pid_gains.KD_WX, self.cf_pid_gains.INT_MAX_WX, self.cf_pid_gains.WX_DT) self.wy_pid = PID(self.cf_pid_gains.KP_WY, self.cf_pid_gains.KI_WY, self.cf_pid_gains.KD_WY, self.cf_pid_gains.INT_MAX_WY, self.cf_pid_gains.WY_DT) self.wz_pid = PID(self.cf_pid_gains.KP_WZ, self.cf_pid_gains.KI_WZ, self.cf_pid_gains.KD_WZ, self.cf_pid_gains.INT_MAX_WZ, self.cf_pid_gains.WZ_DT) self.desired_ang_vel = np.zeros(3) ###################### # Attitudes ###################### self.roll_pid = PID(self.cf_pid_gains.KP_ROLL, self.cf_pid_gains.KI_ROLL, self.cf_pid_gains.KD_ROLL, self.cf_pid_gains.INT_MAX_ROLL, self.cf_pid_gains.ROLL_DT) self.pitch_pid = PID(self.cf_pid_gains.KP_PITCH, self.cf_pid_gains.KI_PITCH, self.cf_pid_gains.KD_PITCH, self.cf_pid_gains.INT_MAX_PITCH, self.cf_pid_gains.PITCH_DT) self.yaw_pid = PID(self.cf_pid_gains.KP_YAW, self.cf_pid_gains.KI_YAW, self.cf_pid_gains.KD_YAW, self.cf_pid_gains.INT_MAX_YAW, self.cf_pid_gains.YAW_DT) self.att_pid_counter = 0 self.att_pid_counter_max = int(self.cf_physical_params.DT_ATT_PIDS / self.cf_physical_params.DT_CF) - 1 self.desired_att = np.zeros(3) ###################### # Angular velocities ###################### self.vx_pid = PID(self.cf_pid_gains.KP_VX, self.cf_pid_gains.KI_VX, self.cf_pid_gains.KD_VX, self.cf_pid_gains.INT_MAX_VX, self.cf_pid_gains.VX_DT) self.vy_pid = PID(self.cf_pid_gains.KP_VY, self.cf_pid_gains.KI_VY, self.cf_pid_gains.KD_VY, self.cf_pid_gains.INT_MAX_VY, self.cf_pid_gains.VY_DT) self.vz_pid = PID(self.cf_pid_gains.KP_VZ, self.cf_pid_gains.KI_VZ, self.cf_pid_gains.KD_VZ, self.cf_pid_gains.INT_MAX_VZ, self.cf_pid_gains.VZ_DT) self.desired_lin_vel = np.zeros(3) ###################### # Angular velocities ###################### self.x_pid = PID(self.cf_pid_gains.KP_X, self.cf_pid_gains.KI_X, self.cf_pid_gains.KD_X, self.cf_pid_gains.INT_MAX_X, self.cf_pid_gains.X_DT) self.y_pid = PID(self.cf_pid_gains.KP_Y, self.cf_pid_gains.KI_Y, self.cf_pid_gains.KD_Y, self.cf_pid_gains.INT_MAX_Y, self.cf_pid_gains.Y_DT) self.z_pid = PID(self.cf_pid_gains.KP_Z, self.cf_pid_gains.KI_Z, self.cf_pid_gains.KD_Z, self.cf_pid_gains.INT_MAX_Z, self.cf_pid_gains.Z_DT) self.desired_pos = np.zeros(3) ############################ # Communication control ############################ self.out_pos_counter = 0 self.out_pos_counter_max = int(self.cf_physical_params.DT_POS_PIDS / self.cf_physical_params.DT_CF) - 1 self.mode = "POS" self.time_processing = [] self.delta_time = []
class Controller(object): def __init__(self, *args, **kwargs): # TODO: Implement self.wheel_base = None self.steer_ratio = None self.min_speed = None self.max_lat_accel = None self.max_steer_angle = None self.accel_limit = None self.decel_limit = None self.brake_active = False self.prev_time = None self._test_timer = None self._test_light_state = 0 for key in kwargs: if key == "wheel_base": self.wheel_base = kwargs[key] elif key == "steer_ratio": self.steer_ratio = kwargs[key] elif key == "min_speed": self.min_speed = kwargs[key] elif key == "max_lat_accel": self.max_lat_accel = kwargs[key] elif key == "max_steer_angle": self.max_steer_angle = kwargs[key] elif key == "accel_limit": self.accel_limit = kwargs[key] elif key == "decel_limit": self.decel_limit = kwargs[key] elif key == "fuel_capacity": self.fuel_capacity = kwargs[key] elif key == "wheel_radius": self.wheel_radius = kwargs[key] elif key == "vehicle_mass": self.vehicle_mass = kwargs[key] elif key == "max_velocity": self.max_velocity = kwargs[key] self.yaw_controller = YawController(self.wheel_base, self.steer_ratio,\ self.min_speed, self.max_lat_accel, self.max_steer_angle) self.pid_throttle = PID(kp=15., ki=0.0, kd=0.5) self.debug_counter = 0 pass def apply_brake(self): acceleration = self.accel_limit return (self.vehicle_mass + self.fuel_capacity * GAS_DENSITY) * acceleration * self.wheel_radius def control(self, *args, **kwargs): full_brake = self.apply_brake() if not kwargs["dbw_enabled"] or self.prev_time is None: self.prev_time = rospy.Time.now().to_sec() return 0.0, 0.0, 0.0, 0.0, 0.0 brake = 0 current_time = rospy.Time.now().to_sec() # TODO: Change the arg, kwarg list to suit your needs current_velocity_linear = kwargs["current_velocity_linear"] target_velocity_linear = kwargs["target_velocity_linear"] target_velocity_angular = kwargs["target_velocity_angular"] distance_to_light = kwargs["distance_to_light"] light_state = kwargs["light_state"] # create pid for throttle sample_time, cte sample_time = max(current_time - self.prev_time, SAMPLE_TIME_MIN) #---------------------------------------------------------- #---------- Possible scenarios ---------------------------- #---------------------------------------------------------- # Note: Becuase the simulator preformance slows down with camera # I had to test my traffic_light handling code with following scenarios: # 1- set light to red for 10 seconds : vehicle should slow down and apply brake to full stop # 2- the set light to green: vehicle should start moving without exceeding max acceleration # 3- then set light to yellow: vehicle should apply soft brake (~10) to slow down vehicle # 4- repeat from the top # delay = 15.0 # target_velocity_linear.x = 10 # if self._test_timer is None: # self._test_timer = current_time + delay # self._test_light_state =0 # distance_to_light = 50 # light_state = self._test_light_state # if current_time > self._test_timer : # if self._test_light_state == 0: # self._test_light_state = 2 # elif self._test_light_state == 2: # self._test_light_state = 1 # elif self._test_light_state == 1: # self._test_light_state = 0 # light_state = self._test_light_state # self._test_timer = current_time + delay #-------------------------------------------------------- if kwargs["number_waypoints_ahead"] < 150: # ENDING points ahead should brake and stop the car if kwargs["number_waypoints_ahead"] < 25: brake = full_brake # ~360 elif kwargs["number_waypoints_ahead"] < 50: brake = full_brake * 0.5 elif kwargs["number_waypoints_ahead"] < 75: brake = full_brake * 0.1 elif kwargs["number_waypoints_ahead"] < 125: brake = full_brake * 0.05 else: brake = full_brake * 0.01 # ~3.6 throttle = 0.0 steer = self.yaw_controller.get_steering(0, 0, current_velocity_linear.x) return throttle, brake, steer, current_time, self._test_light_state else: max_speed_mps = self.max_velocity * ONE_MPH target_velocity = min(max_speed_mps, target_velocity_linear.x) v_error = target_velocity - current_velocity_linear.x # if light is red apply brake based on how close it is to the light if light_state == 0: if distance_to_light < 15: brake = full_brake elif distance_to_light < 25: brake = 20 else: brake = 10 self.pid_throttle.reset() return 0., brake, 0., current_time, self._test_light_state elif light_state == 1: #Yellow brake = 10. return 0., brake, 0., current_time, self._test_light_state else: brake = 0. v_decel = self.decel_limit * sample_time v_accel = self.accel_limit * sample_time v_error = min(v_error, v_accel) v_error = max(v_error, v_decel) throttle = self.pid_throttle.step(error=v_error, sample_time=sample_time) throttle = max(throttle, 0.) # call yaw controller linear_velocity, angular_velocity, current_velocity steer = self.yaw_controller.get_steering(target_velocity_linear.x, target_velocity_angular.z,\ current_velocity_linear.x) self.prev_time = current_time # Return throttle, brake, steer return throttle, brake, steer, current_time, self._test_light_state
class Controller(object): def __init__(self, *args, **kwargs): # TODO: Implement self.vehicle_mass = args[0] self.fuel_capacity = args[1] self.brake_deadband = args[2] self.decel_limit = args[3] self.accel_limit = args[4] self.wheel_radius = args[5] self.wheel_base = args[6] self.steer_ratio = args[7] self.max_lat_accel = args[8] self.max_steer_angle = args[9] self.velocity_pid = PID(VELOCITY_Kp, VELOCITY_Ki, VELOCITY_Kd, -abs(self.decel_limit), abs(self.accel_limit)) self.accel_pid = PID(ACCEL_Kp, ACCEL_Ki, ACCEL_Kd) self.steering_cntrl = YawController(self.wheel_base, self.steer_ratio, MIN_SPEED, self.max_lat_accel, self.max_steer_angle) self.accel_lpf = LowPassFilter(ACCEL_Tau, ACCEL_Ts) #pass def control(self, *args, **kwargs): # TODO: Change the arg, kwarg list to suit your needs # Return throttle, brake, steer #return 1., 0., 0. current_time = args[0] last_cmd_time = args[1] control_period = args[2] twist_cmd = args[3] current_velocity = args[4] dbw_enabled = args[5] if (current_time - last_cmd_time) > 10 * control_period: self.velocity_pid.reset() self.accel_pid.reset() vehicle_mass = self.vehicle_mass + self.fuel_capacity * GAS_DENSITY + 2 * WEIGHT_PERSON velocity_error = twist_cmd.twist.linear.x - current_velocity.twist.linear.x if abs(twist_cmd.twist.linear.x) < (1.0 * ONE_MPH): self.velocity_pid.reset() accel_cmd = self.velocity_pid.step(velocity_error, control_period) if twist_cmd.twist.linear.x <= 1e-2: accel_cmd = min(accel_cmd, -530 / vehicle_mass / self.wheel_radius) elif twist_cmd.twist.linear.x < MIN_SPEED: twist_cmd.twist.angular.z = twist_cmd.twist.angular.z * MIN_SPEED / twist_cmd.twist.linear.x twist_cmd.twist.linear.x = MIN_SPEED if dbw_enabled: if accel_cmd >= 0: throttle_val = self.accel_pid.step( accel_cmd - self.accel_lpf.get(), control_period) else: throttle_val = 0.0 self.accel_pid.reset() if accel_cmd < -self.brake_deadband or twist_cmd.twist.linear.x < MIN_SPEED: brake_val = -accel_cmd * vehicle_mass * self.wheel_radius else: brake_val = 0.0 steering_val = self.steering_cntrl.get_steering( twist_cmd.twist.linear.x, twist_cmd.twist.angular.z, current_velocity.twist.linear.x ) #+ STEER_Kp * (twist_cmd.twist.angular.z - current_velocity.twist.angular.z) return throttle_val, brake_val, steering_val else: self.velocity_pid.reset() self.accel_pid.reset() return 0.0, 0.0, 0.0 def filter_accel_value(self, value): self.accel_lpf.filt(value) def get_filtered_accel(self): return self.accel_lpf.get()
plt.plot(pltx, plt_value, color='red') plt.show() if __name__ == '__main__': # Period of simulation T = 0.04 # Parameters of PID Kp = 3 Ti = 15 Td = 0.3 # Initialize PID and Model p = PID(T, Kp, Ti, Td) model = Model(T) target_value = 50 current_value = 0 _t = time.time() loop = True while loop: if time.time() - _t > 10: loop = False t0 = time.time()
class Controller(object): def __init__(self, sampling_rate, vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): self.vehicle_mass = vehicle_mass self.fuel_capacity = fuel_capacity self.brake_deadband = brake_deadband self.decel_limit = decel_limit self.accel_limit = accel_limit self.wheel_radius = wheel_radius self.sampling_rate = sampling_rate tau = LPF_ACCEL_TAU # 1/(2pi*tau) = cutoff frequency self.sampling_time = 1. / self.sampling_rate # rospy.logwarn('TwistController: Sampling rate = ' + str(self.sampling_rate)) # rospy.logwarn('TwistController: Sampling time = ' + str(self.sampling_time)) self.prev_vel = 0.0 self.current_accel = 0.0 self.acc_lpf = LowPassFilter(tau, self.sampling_time) self.brake_torque_const = (self.vehicle_mass + self.fuel_capacity * GAS_DENSITY) * self.wheel_radius # Initialise PID controller for speed control self.pid_spd_ctl = PID(PID_SPDCTL_P, PID_SPDCTL_I, PID_SPDCTL_D, self.decel_limit, self.accel_limit) # second controller to get throttle signal between 0 and 1 self.accel_pid = PID(PID_ACC_P, PID_ACC_I, PID_ACC_D, 0.0, 0.8) # Initialise Yaw controller for steering control self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle) # self.last_time = rospy.get_time() def reset(self): #self.accel_pid.reset() self.pid_spd_ctl.reset() def control(self, current_vel, dbw_enabled, linear_vel, angular_vel): """Returns (throttle, brake, steer), or None""" throttle, brake, steering = 0.0, 0.0, 0.0 if not dbw_enabled: self.reset() return None vel_error = linear_vel - current_vel # calculate current acceleration and smooth using lpf accel_temp = self.sampling_rate * (self.prev_vel - current_vel) # update self.prev_vel = current_vel self.acc_lpf.filt(accel_temp) self.current_accel = self.acc_lpf.get() # use velocity controller compute desired accelaration desired_accel = self.pid_spd_ctl.step(vel_error, self.sampling_time) if desired_accel > 0.0: if desired_accel < self.accel_limit: throttle = self.accel_pid.step( desired_accel - self.current_accel, self.sampling_time) else: throttle = self.accel_pid.step( self.accel_limit - self.current_accel, self.sampling_time) brake = 0.0 else: throttle = 0.0 # reset just to be sure self.accel_pid.reset() if abs(desired_accel) > self.brake_deadband: # don't bother braking unless over the deadband level # make sure we do not brake to hard if abs(desired_accel) > abs(self.decel_limit): brake = abs(self.decel_limit) * self.brake_torque_const else: brake = abs(desired_accel) * self.brake_torque_const steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel) # throttle = self.throttle_controller.step(vel_error, sample_time) # brake = 0 # # if linear_vel == 0. and current_vel < 0.1: # throttle = 0 # brake = 400 # N*m - to hold the car in place if we are stopped at a light. Acceleration - 1m/s^2 # # elif throttle < .1 and vel_error < 0: # throttle = 0 # decel = max(vel_error, self.decel_limit) # brake = abs(decel) * self.vehicle_mass * self.wheel_radius # Torque N*m return throttle, brake, steering
class Controller(object): def __init__(self, vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): self.yaw_controller = YawController(wheel_base, steer_ratio, min_speed=0.1, max_lat_accel=max_lat_accel, max_steer_angle=max_steer_angle) # Throttle PID controller parameters # Tuned using Ziegler-Nichols method, temporarily using mx=1.0 # see https://en.wikipedia.org/wiki/PID_controller#Ziegler%E2%80%93Nichols_method # Which gives the parameters Ku=2.1, Tu=1.4s # Which in turn results in: kp = 1.26 ki = 1.8 kd = 0.294 # Throttle range, 0 is minimum. 0.2 max for safety and comfort, real max is 1.0 mn = 0.0 mx = 0.2 self.throttle_controller = PID(kp, ki, kd, mn, mx) # Lowpass filter for measured velocity, the sensor is noisy tau = 0.5 # 1/(2*pi*tau) = cutoff frequency ts = 0.02 # Sample time at 50 Hz self.vel_lpf = LowPassFilter(tau, ts) self.vehicle_mass = vehicle_mass self.fuel_capacity = fuel_capacity self.brake_deadband = brake_deadband self.decel_limit = decel_limit self.accel_limit = accel_limit self.wheel_radius = wheel_radius self.last_time = rospy.get_time() def control(self, current_vel, dbw_enabled, linear_vel, angular_vel): if not dbw_enabled: self.throttle_controller.reset() return 0.0, 0.0, 0.0 if DEBUG_LOG: rospy.logwarn("Angular vel: {0}".format(angular_vel)) rospy.logwarn("Target velocity: {0}".format(linear_vel)) rospy.logwarn("Target angular velocity: {0}".format(angular_vel)) rospy.logwarn("Current velocity: {0}".format(current_vel)) rospy.logwarn("Filtered velocity: {0}".format(self.vel_lpf.get())) current_vel = self.vel_lpf.filt(current_vel) steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel) vel_error = linear_vel - current_vel current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time throttle = self.throttle_controller.step(vel_error, sample_time) brake = 0 # If we want to stop and are not going too fast, brake with known-good torque for staying still if linear_vel == 0.0 and current_vel < 0.1: throttle = 0 brake = 700 # Nm - according to Slack comments, minimum 550 Nm required, but use 700 to be safer # If no/low throttle was indicated and we want to slow down (decelerate), give zero throttle and brake elif throttle < 0.1 and vel_error < 0.0: throttle = 0.0 decel = max(vel_error, self.decel_limit) # decel_limit is negative as well brake = abs( decel ) * self.vehicle_mass * self.wheel_radius # Braking torque in Nm # return 1., 0., 0. # For debugging only, full gas ahead! return throttle, brake, steering
class Controller(object): def __init__(self, vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): # TODO: Implement self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle) kp = 0.3 ki = 0.1 kd = 0. mn = 0. mx = 0.2 self.throttle_controller = PID(kp, ki, kd, mn, mx) tau = 0.5 ts = .02 self.vel_lowpass = LowPassFilter(tau, ts) self.vehicle_mass = vehicle_mass self.fuel_capacity = fuel_capacity self.brake_deadband = brake_deadband self.decel_limit = decel_limit self.accel_limit = accel_limit self.wheel_radius = wheel_radius self.wheel_base = wheel_base self.steer_ratio = steer_ratio self.max_lat_accel = max_lat_accel self.max_steer_angle = max_steer_angle self.last_time = rospy.get_time() def control(self, linear_vel, angular_vel, current_vel, dbw_enabled): # TODO: Change the arg, kwarg list to suit your needs # Return throttle, brake, steer if not dbw_enabled: self.throttle_controller.reset() return 0., 0., 0. current_vel = self.vel_lowpass.filt(current_vel) steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel) vel_error = linear_vel - current_vel self.last_vel = current_vel current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time throttle = self.throttle_controller.step(vel_error, sample_time) brake = 0. if linear_vel == 0 and current_vel < 0.1: throttle = 0 brake = 400 # N*m - to hold the car in place elif throttle < .1 and vel_error < 0: throttle = 0 decel = max(vel_error, self.decel_limit) brake = abs( decel ) * self.vehicle_mass * self.wheel_radius # torque calculation return throttle, brake, steering
class Controller(object): ### DBWVideo { def __init__(self, vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): self.yaw_controller = YawController( wheel_base, steer_ratio, 0.1, # min speed [m/s] max_lat_accel, max_steer_angle) kp = 0.3 ki = 0.1 kd = 0. mn = 0. # min throttle value mx = 0.2 # max throttle value self.throttle_controller = PID(kp, ki, kd, mn, mx) tau = 0.5 # 1/(2pi*tau) = cutoff freq ts = .02 # sample time self.vel_lpf = LowPassFilter(tau, ts) self.vehicle_mass = vehicle_mass self.fuel_capacity = fuel_capacity self.brake_deadband = brake_deadband self.decel_limit = decel_limit self.accel_limit = accel_limit self.wheel_radius = wheel_radius self.last_time = rospy.get_time() ### DBWVideo } ### DBWVideo { def control(self, current_vel, dbw_enabled, linear_vel, angular_vel): # TODO: Change the arg, kwarg list to suit your needs # Return throttle, brake, steer if not dbw_enabled: self.throttle_controller.reset() return 0., 0., 0. current_vel = self.vel_lpf.filt(current_vel) ## rospy.logwarn("angular_vel : {0}".format(angular_vel )) ## rospy.logwarn("linear_vel : {0}".format(linear_vel )) ## rospy.logwarn("angular_vel : {0}".format(angular_vel )) ## rospy.logwarn("current_vel : {0}".format(current_vel )) ## rospy.logwarn("self.vel_lpf.get(): {0}".format(self.vel_lpf.get())) steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel) vel_error = linear_vel - current_vel self.last_vel = current_vel current_time = rospy.get_time() sample_time = current_time = self.last_time self.last_time = current_time throttle = self.throttle_controller.step(vel_error, sample_time) brake = 0 if linear_vel == 0. and current_vel < 0.1: throttle = 0 brake = 700 # N*M elif throttle < .1 and vel_error < 0: throttle = 0 decel = max(vel_error, self.decel_limit) brake = abs(decel) * self.vehicle_mass * self.wheel_radius # Nm # TODO amedveczki - 1 bug which can be optionally fixed: wandering a little bit in the lane. # autoware code doesn't recompute the trajectory until certain distance waypoint/angle was passed # 11:00 in video - waypoint_follower.cpp -> update all the time, func which checks if waypoints are being followed -> just follow all the time return throttle, brake, steering
def dock(conn, speed_limit = 1.0): #Setup KRPC '''sc = conn.space_center v = sc.active_vessel t = sc.target_docking_port ap = v.auto_pilot rf = v.orbit.body.reference_frame #Setup Auto Pilot ap.reference_frame = rf ap.target_direction = tuple(x * -1 for x in t.direction(rf)) ap.engage()''' #create PIDs upPID = PID(.75,.25,1) rightPID = PID(.75,.25,1) forwardPID = PID(.75,.2,.5) proceed=False current = None target = None connector_separated = False #'proceed' is a flag that signals that we're lined up and ready to dock. # Otherwise the program will try to line up 10m from the docking port. #LineUp and then dock - in the same loop with 'proceed' controlling whether #we're headed for the 10m safe point, or headed forward to dock. while True: current = vessel.parts.controlling.docking_port target = conn.space_center.target_docking_port if current is None: print('等待选中指令舱连接器') elif connector_separated is False: vessel.control.activate_next_stage() connector_separated = True elif target is None: print('等待选中目标连接器') else: sc = conn.space_center v = sc.active_vessel t = sc.target_docking_port ap = v.auto_pilot rf = v.orbit.body.reference_frame #Setup Auto Pilot ap.reference_frame = rf ap.target_direction = tuple(x * -1 for x in t.direction(rf)) ap.target_roll = 0 ap.engage() ap.wait() offset = getOffsets(v, t) #grab data and compute setpoints velocity = getVelocities(v, t) if proceedCheck(offset): #Check whether we're lined up and ready to dock proceed = True setpoints = getSetpoints(offset, proceed, speed_limit) upPID.setpoint(setpoints.up) #set PID setpoints rightPID.setpoint(setpoints.right) forwardPID.setpoint(setpoints.forward) v.control.up = (-1.0/8.0) * upPID.update(velocity.up) #steer vessel v.control.right = (-1.0/8.0) * rightPID.update(velocity.right) #v.control.forward = (1/8.0) * forwardPID.update(velocity.forward) print('前后%f, 上下:%f, 左右:%f' % (v.control.forward, v.control.up, v.control.right))
class Controller(object): # TODO: Implement def __init__(self, vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle) # PID hyperparameters kp = 0.3 ki = 0.1 kd = 0 # Throttle values min, max mn = 0. mx = 0.2 self.throttle_controller = PID(kp, ki, kd, mn, mx) # Low pass filter tau = 0.5 # cutoff-frequence ts = 0.02 # sample time = 50 Hz self.vel_lpf = LowPassFilter(tau, ts) self.vehicle_mass = vehicle_mass self.fuel_capacity = fuel_capacity self.brake_deadband = brake_deadband self.decel_limit = decel_limit self.accel_limit = accel_limit self.wheel_radius = wheel_radius self.last_time = rospy.get_time() def control(self, current_vel, dbw_enabled, linear_vel, angular_vel): # TODO: Change the arg, kwarg list to suit your needs # Return throttle, brake, steer if not dbw_enabled: self.throttle_controller.reset() return 0., 0., 0. current_vel = self.vel_lpf.filt(current_vel) if IS_DEBUG: rospy.loginfo("Angular vel: {0}".format(angular_vel)) rospy.loginfo("Target vel: {0}".format(linear_vel)) rospy.loginfo("Target ang vel: {0}".format(angular_vel)) rospy.loginfo("Current vel: {0}".format(current_vel)) rospy.loginfo("Filtered vel: {0}".format(self.vel_lpf.get())) steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel) vel_error = linear_vel - current_vel self.last_vel = current_vel current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time throttle = self.throttle_controller.step(vel_error, sample_time) brake = 0 if linear_vel == 0. and current_vel < 0.1: throttle = 0 brake = 700 # Nm --> needed to hold car in place elif throttle < 0.1 and vel_error < 0: throttle = 0 decel = max(vel_error, self.decel_limit) # limit brake value brake = abs(decel) * self.vehicle_mass * self.wheel_radius if IS_DEBUG: rospy.logwarn("vel error: {0}, throttle: {1}, brake: {2}".format( vel_error, throttle, brake)) return throttle, brake, steering
class Controller(object): def __init__(self, *args, **kwargs): min_speed = 0.02 # Define controllers @TODO Tune parameters self.controller_steer = YawController(rospy.get_param('~wheel_base'), \ rospy.get_param('~steer_ratio'), \ min_speed, \ rospy.get_param('~max_lat_accel'),\ rospy.get_param('~max_steer_angle')) self.controller_acceleration = PID(kp=5., ki=.5, kd=.5, mn=rospy.get_param('~decel_limit'), mx=rospy.get_param('~accel_limit')) # Define low pass filters @TODO Tune parameters self.lowpass_steer = LowPassFilter(tau=3., ts=1.0) self.lowpass_acceleration = LowPassFilter(tau=5., ts=1.0) return # Resets all controllers def reset(self): self.controller_acceleration.reset() """ Computes x,y magnitude for each object that brings an x, y value. """ def _get_magn(self, data): return math.sqrt(data.x**2 + data.y**2) def control(self, proposed_linear_v, proposed_angular_v,\ current_linear_v, current_angular_v, \ is_dbw_enabled, sample_time,): # Values from current run desired_velocity = self._get_magn(proposed_linear_v) desired_angular_velocity = proposed_angular_v.z current_velocity = self._get_magn(current_linear_v) # Velocity error - steered by acceleration #print("desired_velocity= {} \t current_velocity= {} \t error_vel={}".format(desired_velocity, current_velocity, error_vel)) # Steering Command cmd_steer = self.controller_steer.get_steering( desired_velocity, desired_angular_velocity, current_velocity) cmd_steer = self.lowpass_steer.filt(cmd_steer) # Acceleration Command - slow down when steering error_vel = (desired_velocity - current_velocity) cmd_accel = self.controller_acceleration.step(error_vel, sample_time) cmd_accel = self.lowpass_acceleration.filt(cmd_accel) # Convert acceleration to throttle and brake cmd_throttle = 0.0 cmd_brake = 0.0 if cmd_accel > 0.: # Accelerate proportional to accel cmd cmd_throttle = cmd_accel / rospy.get_param('~accel_limit') else: if abs(cmd_accel) < rospy.get_param('~brake_deadband'): cmd_accel = 0.0 # Brake - compute needed torque brake_torque = rospy.get_param('~wheel_radius') * ( rospy.get_param('~vehicle_mass') + rospy.get_param('~fuel_capacity') * GAS_DENSITY) * -1. * cmd_accel cmd_brake = min(brake_torque, 3250.) return cmd_throttle, cmd_brake, cmd_steer
class Controller(object): def __init__(self, wheel_base, wheel_radius, steer_ratio, max_lat_accel, max_steer_angle, decel_limit, vehicle_mass): self.yaw_controller = YawController( wheel_base, steer_ratio, 0.1, # min_speed max_lat_accel, max_steer_angle, ) # PID coefficients kp = 0.3 ki = 0.1 kd = 0.0 # For convenience (no real limits on throttle): mn_th = 0.0 # Minimal throttle mx_th = 0.2 # Maximal throttle self.throttle_controller = PID(kp, ki, kd, mn_th, mx_th) tau = 0.5 # 1 / (2pi*tau) = cutoff frequency ts = 0.02 # Sample time self.vel_lpf = LowPassFilter(tau, ts) self.wheel_radius = wheel_radius self.decel_limit = decel_limit self.vehicle_mass = vehicle_mass self.last_time = rospy.get_time() def control(self, current_vel, dbw_enabled, linear_vel, angular_vel): if not dbw_enabled: self.throttle_controller.reset() return 0.0, 0.0, 0.0 filtered_vel = self.vel_lpf.filt(current_vel) # rospy.logwarn( # '\nAngular vel: {}\n' # 'Target velocity: {}\n' # 'Current velocity: {}\n' # 'Filtered velocity: {}\n' # .format(angular_vel, linear_vel, current_vel, filtered_vel) # ) steering = self.yaw_controller.get_steering(linear_vel, angular_vel, filtered_vel) vel_error = linear_vel - filtered_vel current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time throttle = self.throttle_controller.step(vel_error, sample_time) brake = 0 if linear_vel == 0 and filtered_vel < 0.1: throttle = 0 brake = 400 # [N*m] -- to hold the car in place if we are stopped # at a light. Acceleration ~ 1 m/s/s elif throttle < 0.1 and vel_error < 0: throttle = 0 decel = max(vel_error, self.decel_limit) brake = abs( decel) * self.vehicle_mass * self.wheel_radius # Torque [N*m] return throttle, brake, steering
class HeightControl: ''' Class implements ROS node for cascade (z, vz) PID control for MAV height. Subscribes to: /morus/pose - used to extract z-position of the vehicle /morus/velocity - used to extract vz of the vehicle /morus/pos_ref - used to set the reference for z-position /morus/vel_ref - used to set the reference for vz-position (useful for testing velocity controller) Publishes: /morus/mot_vel_ref - referent value for thrust in terms of motor velocity (rad/s) /morus/pid_z - publishes PID-z data - referent value, measured value, P, I, D and total component (useful for tuning params) /morus/pid_vz - publishes PID-vz data - referent value, measured value, P, I, D and total component (useful for tuning params) Dynamic reconfigure is used to set controller params online. ''' def __init__(self): ''' Initialization of the class. ''' self.start_flag = False # indicates if we received the first measurement self.config_start = False # flag indicates if the config callback is called for the first time self.z_sp = 1.0 # z-position set point self.z_ref_filt = 0 # z ref filtered self.z_mv = 0 # z-position measured value self.pid_z = PID() # pid instance for z control self.vz_sp = 0 # vz velocity set_point self.vz_mv = 0 # vz velocity measured value self.pid_vz = PID() # pid instance for z-velocity control ######################################################### ######################################################### # Add parameters for z controller self.pid_z.set_kp(100) self.pid_z.set_ki(1) self.pid_z.set_kd(100) # Add parameters for vz controller self.pid_vz.set_kp(1) #87.2) self.pid_vz.set_ki(0.0) self.pid_vz.set_kd(0) #10.89) ######################################################### ######################################################### self.pid_z.set_lim_high(500) # max vertical ascent speed self.pid_z.set_lim_low(-500) # max vertical descent speed self.pid_vz.set_lim_high(500) # max velocity of a motor self.pid_vz.set_lim_low(-500) # min velocity of a motor self.mot_speed = 0 # referent motors velocity, computed by PID cascade self.t_old = 0 rospy.Subscriber('pose', PoseStamped, self.pose_cb) rospy.Subscriber('velocity', Odometry, self.vel_cb) rospy.Subscriber('vel_ref', Vector3, self.vel_ref_cb) rospy.Subscriber('pos_ref', Vector3, self.pos_ref_cb) self.pub_pid_z = rospy.Publisher('pid_z', PIDController, queue_size=1) self.pub_pid_vz = rospy.Publisher('pid_vz', PIDController, queue_size=1) self.mot_ref_pub = rospy.Publisher('mot_vel_ref', Float64, queue_size=1) self.pub_mot = rospy.Publisher('/gazebo/command/motor_speed', Actuators, queue_size=1) #self.pub_gm_mot = rospy.Publisher('collectiveThrust', GmStatus, queue_size=1) self.cfg_server = Server(MmcuavZCtlParamsConfig, self.cfg_callback) self.rate = rospy.get_param('~rate', 100) self.ros_rate = rospy.Rate(self.rate) # attitude control at 100 Hz self.t_start = rospy.Time.now() def run(self): ''' Runs ROS node - computes PID algorithms for z and vz control. ''' while not self.start_flag and not rospy.is_shutdown(): print 'Waiting for pose measurements.' rospy.sleep(0.5) print "Starting height control." self.t_old = rospy.Time.now() #self.t_old = datetime.now() while not rospy.is_shutdown(): rospy.sleep(1.0 / float(self.rate)) ######################################################## ######################################################## # Implement cascade PID control here. # Reference for z is stored in self.z_sp. # Measured z-position is stored in self.z_mv. # If you want to test only vz - controller, the corresponding reference is stored in self.vz_sp. # Measured vz-velocity is stored in self.vz_mv # Resultant referent value for motor velocity should be stored in variable mot_speed. # When you implement attitude control set the flag self.attitude_ctl to 1 #self.gm_attitude_ctl = 1 # don't forget to set me to 1 when you implement attitude ctl t = rospy.Time.now() dt = (t - self.t_old).to_sec() #t = datetime.now() #dt = (t - self.t_old).total_seconds() if dt > 1.05 / float(self.rate) or dt < 0.95 / float(self.rate): #print dt pass self.t_old = t # (m_uav + m_arms)/(C*4) self.mot_speed_hover = math.sqrt(9.81 * (2.083 + 0.208 * 4) / (8.54858e-06 * 4.0)) # prefilter for reference #a = 0.1 #self.z_ref_filt = (1-a) * self.z_ref_filt + a * self.z_sp vz_ref = self.pid_z.compute(self.z_sp, self.z_mv, dt) print "vz_ref", vz_ref self.mot_speed = self.mot_speed_hover + \ self.pid_vz.compute(vz_ref, self.vz_mv, dt) print "mot_speed", self.mot_speed ######################################################## ######################################################## # gas motors are used to control attitude # publish referent motor velocity to attitude controller mot_speed_msg = Float64(self.mot_speed) self.mot_ref_pub.publish(mot_speed_msg) # Publish PID data - could be useful for tuning self.pub_pid_z.publish(self.pid_z.create_msg()) self.pub_pid_vz.publish(self.pid_vz.create_msg()) def pose_cb(self, msg): ''' Pose (6DOF - position and orientation) callback. :param msg: Type PoseWithCovarianceStamped ''' if not self.start_flag: self.start_flag = True self.z_mv = msg.pose.position.z def vel_cb(self, msg): ''' Velocity callback (linear velocity - x,y,z) :param msg: Type Vector3Stamped ''' #if not self.start_flag: # self.start_flag = True self.vz_mv = msg.twist.twist.linear.z def vel_ref_cb(self, msg): ''' Referent velocity callback. Use received velocity values when during initial tuning velocity controller (i.e. when position controller still not implemented). :param msg: Type Vector3 ''' self.vz_sp = msg.z def pos_ref_cb(self, msg): ''' Referent position callback. Received value (z-component) is used as a referent height. :param msg: Type Vector3 ''' self.z_sp = msg.z def cfg_callback(self, config, level): """ Callback for dynamically reconfigurable parameters (P,I,D gains for height and velocity controller) """ print "CFG callback" if not self.config_start: # callback is called for the first time. Use this to set the new params to the config server config.z_kp = self.pid_z.get_kp() config.z_ki = self.pid_z.get_ki() config.z_kd = self.pid_z.get_kd() config.vz_kp = self.pid_vz.get_kp() config.vz_ki = self.pid_vz.get_ki() config.vz_kd = self.pid_vz.get_kd() self.config_start = True else: # The following code just sets up the P,I,D gains for all controllers self.pid_z.set_kp(config.z_kp) self.pid_z.set_ki(config.z_ki) self.pid_z.set_kd(config.z_kd) self.pid_vz.set_kp(config.vz_kp) self.pid_vz.set_ki(config.vz_ki) self.pid_vz.set_kd(config.vz_kd) # this callback should return config data back to server return config
class Controller(object): def __init__(self, vehicle_mass, fuel_capacity, brake_deadband, decel_limit, accel_limit, wheel_radius, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): self.yaw_controller = YawController(wheel_base, steer_ratio, 0.1, max_lat_accel, max_steer_angle) kp = 0.3 ki = 0.1 kd = 0.0 mn = 0.0 # Minimum throttle value mx = 0.2 # Maximum throttle value self.throttle_controller = PID(kp, ki, kd, mn, mx) tau = 0.5 # 1/(2pi*tau) = cutoff frequency ts = 0.02 # Sample time self.vel_lpf = LowPassFilter(tau, ts) self.vehicle_mass = vehicle_mass self.fuel_capacity = fuel_capacity self.brake_deadband = brake_deadband self.decel_limit = decel_limit self.accel_limit = accel_limit self.wheel_radius = wheel_radius self.last_time = rospy.get_time() def control(self, current_vel, dbw_enabled, linear_vel, angular_vel): if not dbw_enabled: self.throttle_controller.reset() return 0., 0., 0. current_vel = self.vel_lpf.filt(current_vel) #rospy.logwarn("Angular vel: {0}".format(angular_vel)) #rospy.logwarn("Target velocity: {0}".format(linear_vel)) #rospy.logwarn("Target angular velocity: {0}\n".format(angular_vel)) #rospy.logwarn("Current velocity: {0}".format(current_vel)) #rospy.logwarn("Filtered velocity: {0}".format(self.vel_lpf.get())) steering = self.yaw_controller.get_steering(linear_vel, angular_vel, current_vel) vel_error = linear_vel - current_vel self.last_vel = current_vel current_time = rospy.get_time() sample_time = current_time - self.last_time self.last_time = current_time throttle = self.throttle_controller.step(vel_error, sample_time) brake = 0 if linear_vel == 0.0 and current_vel < 0.1: throttle = 0 brake = 700 # N*m - to hold car in place if we are stopped at a light. elif throttle < 0.1 and vel_error < 0: throttle = 0 decel = max(vel_error, self.decel_limit) brake = abs( decel) * self.vehicle_mass * self.wheel_radius # Torque N*m return throttle, brake, steering