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
