def __init__(self, ter=None, AS=0, DS=0):
self.Ter = list(ter) if ter is not None else None
self.N = 0 if self.Ter is None else len(self.Ter)
self.AS = AS
self.DS = DS
self.maxV = 0.0
self.VSP = 0.0
self.upA = 0.0
self.upV = 0.0
self.rV = 0.0
self.upX = 0.0
self.dX = 0.0
self.E = 0.0
self.upAF = 0.0
self.T = []
self.X = []
self.rX = []
self.V = []
self.rV = []
self.K = []
self.F = []
self.VSp = []
self.M = 4.0
self.MinVSF = 0.1
self.vK1_pid = PID(0.3, 0.1, 0.3, 0.0, 1.0)
def sim_PID():
np.random.seed(42)
def rSV(val, delta):
return max(0, min(100,
val + (delta * np.random.random() - delta / 2.0)))
t1 = 10.0
PV = 10.0
SV = [rSV(50, 100)]
T = np.arange(0, t1, PID.dt)
sSV = SV[-1]
for _t in T[1:]:
r = np.random.random()
if r < 0.98:
SV.append(SV[-1])
continue
sSV = rSV(sSV, 50)
SV.append(sSV)
pid1 = PID(0.8, 0.2, 0.001, -10, 10)
pid2 = PID2(0.8, 0.2, 0.001, -10, 10)
pid1.sim(PV, SV, T)
pid2.sim(PV, SV, T)
plt.show()
def sim_PointNav():
P = 2
D = 0.1
AAk = 0.5
pid = PID(P, 0, D, 0, 10)
accel = np.arange(0.1, 1.1, 0.1)
distance = 500
distanceF = 0.1
cols = color_grad(len(accel))
for i, a in enumerate(accel):
d = [distance]
v = [0]
act = [0]
t = 0
A = [0]
while d[-1] > 0:
pid.kp = P * a / clampL(v[-1], 0.01)
act.append(pid.update(d[-1] * distanceF))
aa = a * AAk
if v[-1] < pid.action:
if A[-1] < a: A.append(A[-1] + a * AAk * dt)
else: A.append(a)
elif v[-1] > pid.action:
if A[-1] > -a: A.append(A[-1] - a * AAk * dt)
else: A.append(-a)
elif A[-1] > 0: A.append(A[-1] - a * AAk * dt)
elif A[-1] < 0: A.append(A[-1] + a * AAk * dt)
else: A.append(0)
# print 'd=% 8.4f, v=% 8.4f, act=% 8.4f, A=% 8.4f' % (d[-1], v[-1], act[-1], A[-1])
v.append(v[-1] + A[-1] * dt)
d.append(d[-1] - v[-1] * dt)
t += dt
print 'accel=%.2f, time: %.1fs' % (a, t)
print len(d), len(A)
plt.subplot(3, 1, 1)
plt.plot(d, v, color=cols[i], label="accel=%.2f" % a)
plt.ylabel("V")
plt.xlabel("distance")
plt.subplot(3, 1, 2)
plt.plot(d, act, color=cols[i], label="accel=%.2f" % a)
plt.ylabel("act")
plt.xlabel("distance")
plt.subplot(3, 1, 3)
plt.plot(d, A, color=cols[i], label="accel=%.2f" % a)
plt.ylabel("real accel");
plt.xlabel("distance")
plt.legend()
plt_show_maxed()
def sim_Rotation():
def _plot(r, c, n, X, Y, aa, ylab):
plt.subplot(r, c, n)
plt.plot(X, Y, label=("V: AA=%.1f" % aa))
plt.xlabel("time (s)")
plt.ylabel(ylab)
MaxAngularA = np.arange(0.5, 30, 5);
start_error = 0.8 * np.pi
end_time = 10.0
def test(c, n, pid):
p = pid.kp;
d = pid.kd
for aa in MaxAngularA:
A = [start_error]
V = [0.0]
S = [0.0]
T = [0.0]
def plot(c, n, Y, ylab):
_plot(3, c, n, T, Y, aa, ylab)
pid.kp = p / aa
pid.kd = d / aa
while T[-1] < end_time:
S.append(pid.update(A[-1]))
V.append(V[-1] - S[-1] * aa * dt)
A.append(A[-1] + V[-1] * dt)
T.append(T[-1] + dt)
plot(c, n, np.fromiter(A, float) / np.pi * 180.0, 'Angle')
plot(c, c + n, V, 'Angular velocity')
plot(c, c * 2 + n, S, 'Steering')
test(2, 1, PID(6.0, 0, 10.0, -1, 1))
test(2, 2, PID(4.0, 0, 6.0, -1, 1))
legend()
plt.show()
def run_alt(self, alt, salt=0.0, thrust=20.0, pid=PID(0.5, 0.0, 0.5, -9.9, 9.9)):
self.pid = pid
self._vK = self.vK2
self.cthrust = self.M * 9.81
self.maxV = 0.0
self.VSP = 0.0
self.upV = 0.0
self.upX = salt if self.Ter is None else self.Ter[0] + 10
self.Vsp = [self.maxV]
self.dVsp = [0]
self.init(thrust)
d = pid.kd
def on_frame():
alt_err = alt - self.dX
if self.AS > 0 or self.DS > 0:
if alt_err > 0:
self.pid.kp = clamp(0.01 * abs(alt_err) / clampL(self.upV, 1), 0.0, d)
elif alt_err < 0:
self.pid.kp = clamp(self.twr ** 2 / clampL(-self.upV, 1), 0.0, clampH(d * self.twr / 2, d))
else: self.pid.kp = d
self.pid.kd = d / clampL(self.dX, 1)
if alt_err < 0: alt_err = alt_err / clampL(self.dX, 1)
self.maxV = self.pid.update(alt_err)
print self.pid, alt_err, clamp(0.01 * abs(alt_err) / clampL(self.upV, 1), 0.0, d), d
if self.N > 0:
dV = (self.upV - self.dV) / clampL(self.dX, 1)
if alt_err < 0:
# print '% f %+f = %f' % (dV/clampL(self.dX/50, 1), clampL(alt_err*self.dX/5000*self.twr, self.pid.min*10), dV/clampL(self.dX/50, 1) + clampL(alt_err*self.dX/5000*self.twr, self.pid.min*10))
dV = dV + clampL(alt_err * self.dX / 500 * self.twr, self.pid.min * 10)
else:
dV = clampL(dV, 0)
# print dV
self.dVsp.append(dV)
self.maxV += dV
self.Vsp.append(self.maxV)
self.phys_loop(on_frame)
from __future__ import print_function
from __future__ import print_function
import numpy as np
import matplotlib.pyplot as plt
from scipy.optimize import curve_fit
from common import clampL, clampH, clamp01, lerp, PID, PID2, dt, center_deg
if __name__ == '__main__':
amp = np.pi
bearing_pid = PID(amp, 0.001, 1.0, -amp, amp)
av_pid = PID(1.0, 0.05, 0, -1.0, 1.0)
def timelines(t, *lines):
n = len(lines)
for i in range(n):
plt.subplot(n, 1, i + 1)
plt.plot(t, lines[i])
plt.show()
def sym(angle, maxAA, max_t, aaF, draw=False):
t = [0]
a = [angle]
av = [0]
avn = [0]
st = [0]
bearing_pid.reset()
av_pid.reset()
bearing_pid.D = aaF
def __init__(self, debug=False):
super(DriveNode, self).__init__(name='DriveNode', debug=debug)
"""
------------------------------------------------------------------------------------------
Publishers
------------------------------------------------------------------------------------------
"""
# Publisher of speed to motor driver and odometry to other nodes
self._speedout_pub = HALSpeedOutPublisher()
self._odom_pub = OdometryPublisher()
"""
------------------------------------------------------------------------------------------
Parameters
------------------------------------------------------------------------------------------
"""
# Robot physical dimensions
self._track_width = rospy.get_param('Track Width', 0.3937)
self._wheel_radius = rospy.get_param('Wheel Radius', 0.0785)
# Main loop rate in hz
drive_node_rate = rospy.get_param('Drive Node Rate', 10)
self._loop_rate = rospy.Rate(drive_node_rate)
self._loop_period = 1.0 / float(drive_node_rate)
max_wheel_rpm = rospy.get_param('Max Wheel RPM', 95.0)
max_wheel_ang_vel = rpm_to_rps(max_wheel_rpm, self._wheel_radius)
# Calculate max unicycle velocities based on wheel maximum, track width and wheel radius
max_lin_vel, max_ang_vel = uni_max(max_wheel_ang_vel,
self._track_width,
self._wheel_radius)
max_angular_accel = rospy.get_param('Max Angular Accel', 0.15)
max_angular_decel = rospy.get_param('Max Angular Decel', 0.15)
self._angular_max = AngularMax(ccw=max_ang_vel,
cw=-max_ang_vel,
accel=max_angular_accel,
decel=-max_angular_decel,
wheel=max_ang_vel)
max_linear_accel = rospy.get_param('Max Linear Accel', 0.035)
max_linear_decel = rospy.get_param('Max Linear Decel', 0.035)
self._linear_max = LinearMax(forward=max_lin_vel,
backward=-max_lin_vel,
accel=max_linear_accel,
decel=-max_linear_decel)
# PID controllers
linear_kp = rospy.get_param('Linear Gain Kp', 0.8)
linear_ki = rospy.get_param('Linear Gain Ki', 0.0)
linear_kd = rospy.get_param('Linear Gain Kd', 0.0)
self._lin_pid = PID('linear', linear_kp, linear_ki, linear_kd, 0.0,
max_lin_vel, self._loop_period)
angular_kp = rospy.get_param('Angular Gain Kp', 0.8)
angular_ki = rospy.get_param('Angular Gain Ki', 0.0)
angular_kd = rospy.get_param('Angular Gain Kd', 0.0)
self._ang_pid = PID('angular', angular_kp, angular_ki, angular_kd, 0.0,
max_ang_vel, self._loop_period)
# State variable storage
self._vel_state = VelocityState()
self._odometry = OdometryState()
self._cmd_vel_state = CommandVelocityState()
rospy.loginfo(
STATUS_FMT.format(self._track_width, self._wheel_radius,
self._wheel_radius * 2,
self._wheel_radius * 2 * math.pi,
max_wheel_ang_vel, max_ang_vel, max_lin_vel,
max_angular_accel, max_linear_accel))
"""
-------------------------------------------------------------------------------------------
Subscribers
-------------------------------------------------------------------------------------------
"""
# Note: Declare subscribers at the end to prevent premature callbacks
# Subscriber for Twist messages
self._twist_sub = rospy.Subscriber("cmd_vel",
Twist,
self._twist_cmd_cb,
queue_size=1)
# Subscriber for motor driver speed, distance, and orientation messages
self._speedin_sub = rospy.Subscriber("HALSpeedIn",
HALSpeedIn,
self._speedin_cb,
queue_size=1)
self._positionin_sub = rospy.Subscriber("HALPositionIn",
HALPositionIn,
self._positionin_cb,
queue_size=1)
self._headingin_sub = rospy.Subscriber("HALHeadingIn",
HALHeadingIn,
self._headingin_cb,
queue_size=1)
self._eulerin_sub = rospy.Subscriber("HALEulerIn",
HALEulerIn,
self._eulerin_cb,
queue_size=1)
class DriveNode(BaseNode):
"""
The DriveNode is responsible for:
* accepting Twist messages to be sent to the motor driver.
* ensuring max linear/angular velocity and applying acceleration limits.
* reading speed, distance and heading data from the hardware drivers, calculating and publishing
odometry.
"""
def __init__(self, debug=False):
super(DriveNode, self).__init__(name='DriveNode', debug=debug)
"""
------------------------------------------------------------------------------------------
Publishers
------------------------------------------------------------------------------------------
"""
# Publisher of speed to motor driver and odometry to other nodes
self._speedout_pub = HALSpeedOutPublisher()
self._odom_pub = OdometryPublisher()
"""
------------------------------------------------------------------------------------------
Parameters
------------------------------------------------------------------------------------------
"""
# Robot physical dimensions
self._track_width = rospy.get_param('Track Width', 0.3937)
self._wheel_radius = rospy.get_param('Wheel Radius', 0.0785)
# Main loop rate in hz
drive_node_rate = rospy.get_param('Drive Node Rate', 10)
self._loop_rate = rospy.Rate(drive_node_rate)
self._loop_period = 1.0 / float(drive_node_rate)
max_wheel_rpm = rospy.get_param('Max Wheel RPM', 95.0)
max_wheel_ang_vel = rpm_to_rps(max_wheel_rpm, self._wheel_radius)
# Calculate max unicycle velocities based on wheel maximum, track width and wheel radius
max_lin_vel, max_ang_vel = uni_max(max_wheel_ang_vel,
self._track_width,
self._wheel_radius)
max_angular_accel = rospy.get_param('Max Angular Accel', 0.15)
max_angular_decel = rospy.get_param('Max Angular Decel', 0.15)
self._angular_max = AngularMax(ccw=max_ang_vel,
cw=-max_ang_vel,
accel=max_angular_accel,
decel=-max_angular_decel,
wheel=max_ang_vel)
max_linear_accel = rospy.get_param('Max Linear Accel', 0.035)
max_linear_decel = rospy.get_param('Max Linear Decel', 0.035)
self._linear_max = LinearMax(forward=max_lin_vel,
backward=-max_lin_vel,
accel=max_linear_accel,
decel=-max_linear_decel)
# PID controllers
linear_kp = rospy.get_param('Linear Gain Kp', 0.8)
linear_ki = rospy.get_param('Linear Gain Ki', 0.0)
linear_kd = rospy.get_param('Linear Gain Kd', 0.0)
self._lin_pid = PID('linear', linear_kp, linear_ki, linear_kd, 0.0,
max_lin_vel, self._loop_period)
angular_kp = rospy.get_param('Angular Gain Kp', 0.8)
angular_ki = rospy.get_param('Angular Gain Ki', 0.0)
angular_kd = rospy.get_param('Angular Gain Kd', 0.0)
self._ang_pid = PID('angular', angular_kp, angular_ki, angular_kd, 0.0,
max_ang_vel, self._loop_period)
# State variable storage
self._vel_state = VelocityState()
self._odometry = OdometryState()
self._cmd_vel_state = CommandVelocityState()
rospy.loginfo(
STATUS_FMT.format(self._track_width, self._wheel_radius,
self._wheel_radius * 2,
self._wheel_radius * 2 * math.pi,
max_wheel_ang_vel, max_ang_vel, max_lin_vel,
max_angular_accel, max_linear_accel))
"""
-------------------------------------------------------------------------------------------
Subscribers
-------------------------------------------------------------------------------------------
"""
# Note: Declare subscribers at the end to prevent premature callbacks
# Subscriber for Twist messages
self._twist_sub = rospy.Subscriber("cmd_vel",
Twist,
self._twist_cmd_cb,
queue_size=1)
# Subscriber for motor driver speed, distance, and orientation messages
self._speedin_sub = rospy.Subscriber("HALSpeedIn",
HALSpeedIn,
self._speedin_cb,
queue_size=1)
self._positionin_sub = rospy.Subscriber("HALPositionIn",
HALPositionIn,
self._positionin_cb,
queue_size=1)
self._headingin_sub = rospy.Subscriber("HALHeadingIn",
HALHeadingIn,
self._headingin_cb,
queue_size=1)
self._eulerin_sub = rospy.Subscriber("HALEulerIn",
HALEulerIn,
self._eulerin_cb,
queue_size=1)
def _twist_cmd_cb(self, msg):
"""
Callback registered to receive cmd_vel messages
Averages the receipt times of cmd_vel messages for use in velocity processing
Stores velocity for later processing
:param msg: the cmd_vel message
:return: None
"""
self._cmd_vel_state.add_delta(rospy.Time.now())
self._cmd_vel_state.velocity.set(v=msg.linear.x, w=msg.angular.z)
def _speedin_cb(self, msg):
"""
Callback registered to receive HALSpeedIn messages
Receives and stores unicycle speed
:param msg: the HALSpeedIn message (see HALSpeedIn.msg)
:return: None
"""
self._odometry.set_velocity(msg.linear, msg.angular)
def _positionin_cb(self, msg):
"""
Callback registered to receive HALPositionIn messages
Receives and stores x/y position
:param msg: the HALPositionIn message (see HALPositionIn.msg)
:return: None
"""
self._odometry.set_position(msg.x, msg.y)
def _headingin_cb(self, msg):
"""
:param msg:
:return:
"""
self._odometry.set_orientation(yaw=msg.heading)
def _eulerin_cb(self, msg):
"""
Callback registered to receive HALEulerIn messages
Receives and stores roll, pitch and yaw
:param msg: the HALEulerIn message
:return: None
"""
#self._odometry.set_orientation(msg.roll, msg.pitch, msg.yaw)
def _publish_odom(self):
"""
Publishes odometry on the odom topic
:return: None
"""
if self._odometry.is_ready():
self._odom_pub.publish(self._odometry.x, self._odometry.y,
self._odometry.velocity.linear,
self._odometry.velocity.angular,
self._odometry.heading)
def _velocity_has_changed(self):
return (not isclose(self._vel_state.target_velocity.linear,
self._vel_state.last_velocity.linear)
or not isclose(self._vel_state.target_velocity.angular,
self._vel_state.last_velocity.angular))
def _safety_check(self):
"""
Perform safety/sanity checking on the command velocity before passing through the velocity pipeline
if the command velocity is active (we're getting velocity commands) and command velocity is recent
- constrain the commanded velocity to mechanical and user-defined limits
- ensure requested angular velocity can be achieved
- store in target velocity
if the command velocity is stale or not being received at all
- zero out target velocity
:return: None
"""
# TODO-Add a velocity check for 'too' large velocity change with resulting adjustment that is proportional
if self._cmd_vel_state.is_active():
if self._cmd_vel_state.is_recent(num_msgs=5, max_time=2.0):
v_initial = self._cmd_vel_state.velocity.linear
w_initial = self._cmd_vel_state.velocity.angular
# Ensure linear and angular velocity are within mecahnical and user-defined ranges
v = constrain(v_initial, self._linear_max.backward,
self._linear_max.forward)
w = constrain(w_initial, self._angular_max.cw,
self._angular_max.ccw)
# Ensure the specified angular velocity can be achieved.
#
# Even though velocity is constrained within defined limits, it is necessary to check that the resulting
# velocities are achievable. For example if both wheels are moving at the maximum linear velocity and
# an angular velocity is specified, it is necessary to reduce the linear velocity in order to achieve
# the angular velocity.
# Note: Will adjust linear velocity if necessary
v, w = ensure_w(v, w, self._track_width, self._wheel_radius,
self._angular_max.wheel)
rospy.loginfo(
"SafetyCheck: initial {:.3f} {:.3f}, constrained {:.3f} {:.3f}"
.format(v_initial, w_initial, v, w))
self._vel_state.target_velocity.set(v, w)
else:
rospy.logwarn(
"No message received for {} msgs or {} secs".format(
5, 1.0))
self._vel_state.zero_target()
else:
rospy.logwarn("No command velocity active")
self._vel_state.zero_target()
def _limit_accel(self):
lv_initial = self._vel_state.last_velocity
tv_initial = self._vel_state.target_velocity
# Calculate the desired and max velocity increments for linear and angular velocities
v_inc, max_v_inc = calc_vel_inc(self._vel_state.target_velocity.linear,
self._vel_state.last_velocity.linear,
self._linear_max.accel,
self._linear_max.decel,
self._loop_period)
w_inc, max_w_inc = calc_vel_inc(
self._vel_state.target_velocity.angular,
self._vel_state.last_velocity.angular, self._angular_max.accel,
self._angular_max.decel, self._loop_period)
# Create normalized vectors for desired (v_inv/w_inc) and maximum (max_v_inc/max_w_inc) velocity increments
Av, Aw = normalize_vector(v_inc, w_inc)
Bv, Bw = normalize_vector(max_v_inc, max_w_inc)
# Use the angle between the vectors to determine which increment will dominate
theta = math.atan2(Bw, Bv) - math.atan2(Aw, Av)
if theta < 0.0:
try:
max_v_inc = (max_w_inc * math.fabs(v_inc)) / math.fabs(w_inc)
except ZeroDivisionError:
pass
else:
try:
max_w_inc = (max_v_inc * math.fabs(w_inc)) / math.fabs(v_inc)
except ZeroDivisionError:
pass
# Calculate the velocity delta to be applied
v_delta = math.copysign(1.0, v_inc) * min(math.fabs(v_inc),
math.fabs(max_v_inc))
w_delta = math.copysign(1.0, w_inc) * min(math.fabs(w_inc),
math.fabs(max_w_inc))
# Apply the velocity delta to the last velocity
self._vel_state.target_velocity.linear = self._vel_state.last_velocity.linear + v_delta
self._vel_state.target_velocity.angular = self._vel_state.last_velocity.angular + w_delta
rospy.logdebug("LimitAccel - initial {} {}, resulting {} {}".format(
lv_initial, tv_initial, self._vel_state.last_velocity,
self._vel_state.target_velocity))
def _track_velocity(self):
"""
Track linear and angular velocity using PID controller
Note: Only track when command velocity is active; otherwise, zero out published velocity
:return:
"""
rospy.logdebug("TV Entry - O: {}, T: {}, L: {}".format(
self._odometry.velocity, self._vel_state.target_velocity,
self._vel_state.last_velocity))
delta_v = self._lin_pid.update(self._odometry.velocity.linear)
delta_w = self._ang_pid.update(self._odometry.velocity.angular)
self._lin_pid.print(rospy.logdebug)
self._ang_pid.print(rospy.logdebug)
rospy.logdebug("TV - dv {:02.3f}, dw {:02.3f}".format(
delta_v, delta_w))
self._vel_state.target_velocity.linear += delta_v
self._vel_state.target_velocity.angular += delta_w
rospy.logdebug("TV Exit - O: {}, T: {}, L: {}".format(
self._odometry.velocity, self._vel_state.target_velocity,
self._vel_state.last_velocity))
def _process_velocity(self):
rospy.logdebug("PV Entry - T: {}, L: {}".format(
self._vel_state.target_velocity, self._vel_state.last_velocity))
self._safety_check()
if self._velocity_has_changed():
rospy.logdebug("Velocity Changed")
self._limit_accel()
else:
rospy.logdebug("Velocity Unchanged")
self._vel_state.target_velocity.linear = self._vel_state.last_velocity.linear
self._vel_state.target_velocity.angular = self._vel_state.last_velocity.angular
#self._lin_pid.set_target(self._vel_state.target_velocity.linear)
#self._ang_pid.set_target(self._vel_state.target_velocity.angular)
rospy.logdebug("PV Exit - T: {}, L: {}".format(
self._vel_state.target_velocity, self._vel_state.last_velocity))
def _update_speed(self):
linear, angular = 0.0, 0.0
if self._vel_state.target_velocity.linear != 0.0 or self._vel_state.target_velocity.angular != 0.0:
linear, angular = self._vel_state.target_velocity.linear, self._vel_state.target_velocity.angular
self._speedout_pub.publish(SpeedData(linear=linear, angular=angular))
def start(self):
rospy.loginfo("DriveNode starting ...")
self._speedout_pub.publish(SpeedData(linear=0.0, angular=0.0))
rospy.loginfo("DriveNode started")
def loop(self):
rospy.loginfo("DriveNode loop starting ...")
while not rospy.is_shutdown():
# Smooth velocity
self._process_velocity()
# Update Speed
self._update_speed()
# Track velocity
#self._track_velocity()
self._vel_state.last_velocity.linear = self._vel_state.target_velocity.linear
self._vel_state.last_velocity.angular = self._vel_state.target_velocity.angular
# Publish odometry
self._publish_odom()
rospy.loginfo(
"Linear(T/O): {:6.3f}/{:6.3f}, Angular(T/O): {:6.3f}/{:6.3f}".
format(self._vel_state.target_velocity.linear,
self._odometry.velocity.linear,
self._vel_state.target_velocity.angular,
self._odometry.velocity.angular))
self._loop_rate.sleep()
self.shutdown()
rospy.loginfo("DriveNode loop exiting")
def shutdown(self):
rospy.loginfo("DriveNode shutdown")
if IR in XBMC_dic.keys():
kodi(XBMC_dic[IR])
elif codeIR[0] in extra_esp_keys:
e = ESP()
e.Go_parallel(extra_esp_keys[IR])
else:
logger.info('speaking only ' + IR)
Speak(IR)
last = [IR, datetime.datetime.now()]
except:
logger.error('error : ' + str(sys.exc_info()))
Speak('error in lirc module')
sleep(2)
sleep(0.3)
if __name__ == '__main__':
# with daemon.DaemonContext():
try:
from common import LOGGER, PID
logger = LOGGER('lirc')
# logger = log # compatibility
logger.info('starting lirc')
from time import sleep
from KODI_control import kodi
PID()
import lirc
sockid = lirc.init("test", blocking=False)
Start()
except:
logger.error('error on initiation: ' + str(sys.exc_info()))
class VSF_sim(object):
t1 = 5.0
def __init__(self, ter=None, AS=0, DS=0):
self.Ter = list(ter) if ter is not None else None
self.N = 0 if self.Ter is None else len(self.Ter)
self.AS = AS
self.DS = DS
self.maxV = 0.0
self.VSP = 0.0
self.upA = 0.0
self.upV = 0.0
self.rV = 0.0
self.upX = 0.0
self.dX = 0.0
self.E = 0.0
self.upAF = 0.0
self.T = []
self.X = []
self.rX = []
self.V = []
self.rV = []
self.K = []
self.F = []
self.VSp = []
self.M = 4.0
self.MinVSF = 0.1
self.vK1_pid = PID(0.3, 0.1, 0.3, 0.0, 1.0)
def vK1(self):
# upAF = 0.0
# if self.upA < 0 and self.AS > 0: upAF = (1.0/self.AS)*2
# elif self.DS > 0: upAF = (1.0/self.DS)*1
# upAF *= -self.upA*0.04
# self.VSP = self.maxV+(2.0+upAF)/self.twr
# self.E = self.VSP-self.upV
return self.vK1_pid.update(self.E)
def vK2(self):
# print('Thrust %.2f, W %.2f, H %.2f, upV %.2f, E %.2f, upA %.2f, upAF %.3f, dVSP %.2f, K0 %.3f, K1 %.3f'
# % (self.cthrust, self.M*9.81, self.upX, self.upV, self.maxV-self.upV, self.upA, upAF, (2.0+upAF)/self.twr,
# clamp01(self.E/2.0/(clampL(self.upA/self.K1+1.0, self.L1)**2)),
# clamp01(self.E/2.0/(clampL(self.upA/self.K1+1.0, self.L1)**2)+upAF)))
# return clampL(clamp01(self.E/2.0/(clampL(self.upA/self.K1+1.0, self.L1)**2)+upAF), 0.05)
# if self.upV <= self.maxV:
K = clamp01(self.E * 0.5 + self.upAF)
# else:
# K = clamp01(self.E*self.upA*0.5+self.upAF)
return clampL(K, self.MinVSF)
_vK = vK2
@property
def vK(self):
self.E = self.VSP - self.upV
return self._vK()
def init(self, thrust):
self.thrust = thrust
self.add_thrust = 0
self.twr = thrust / 9.81 / self.M
self.upA = 0.0
self.dV = self.upV
self.dX = self.upX - self.Ter[0] if self.N > 0 else 0
self.T = [0.0]
self.A = [self.upA]
self.V = [self.upV]
self.rV = [self.dV]
self.X = [self.upX]
self.rX = [self.dX]
self.K = [self.vK]
self.F = [0.0]
def update_upAF(self):
self.upAF = 0.0
if self.upA < 0 < self.AS: self.upAF = (1.0 / self.AS) * 2
elif self.DS > 0: self.upAF = (1.0 / self.DS) * 1
self.upAF = -self.upA * 0.2 * self.upAF
self.VSP = self.maxV + (2.0 + self.upAF) / self.twr
def phys_loop(self, on_frame=lambda: None):
i = 1
while self.dX >= 0 and (self.T[-1] < self.t1 or i < self.N):
self.T.append(self.T[-1] + dt)
self.update_upAF()
on_frame()
# VSF
self.K.append(self.vK)
# thrust
rthrust = self.thrust * self.K[-1]
speed = self.AS if rthrust > self.cthrust else self.DS
self.cthrust = lerp(self.cthrust, rthrust, speed * dt) if speed > 0 else rthrust
# dynamics
self.upA = (self.add_thrust + self.cthrust - self.M * 9.81) / self.M
self.upV += self.upA * dt
self.upX += self.upV * dt
self.dX = self.upX - (self.Ter[i] if self.N > 0 else 0)
self.dV = EWA(self.dV, (self.dX - self.rX[-1]) / dt, 0.1)
# logging
self.F.append(self.cthrust * dt)
self.A.append(self.upA)
self.V.append(self.upV)
self.rV.append(self.dV)
self.X.append(self.upX)
self.rX.append(self.dX)
i += 1
def run(self, upV, maxV=1.0, thrust=20.0, vK=None):
if vK is not None: self._vK = vK
self.cthrust = self.M * 9.81
self.maxV = maxV
self.VSP = maxV
self.upV = upV
self.upX = 100.0
self.init(thrust)
self.phys_loop()
def run_impulse(self, dthrust=10, impulse=0.1, maxV=1.0, thrust=20.0):
self._vK = self.vK2
self.cthrust = self.M * 9.81
self.maxV = maxV
self.VSP = maxV
self.upV = maxV
self.upX = 100.0
self.init(thrust)
def on_frame():
if 3 < self.T[-1] < 3 + impulse:
self.add_thrust = dthrust
else: self.add_thrust = 0
self.twr = (self.thrust + self.add_thrust) / 9.81 / self.M
self.phys_loop(on_frame)
def run_alt(self, alt, salt=0.0, thrust=20.0, pid=PID(0.5, 0.0, 0.5, -9.9, 9.9)):
self.pid = pid
self._vK = self.vK2
self.cthrust = self.M * 9.81
self.maxV = 0.0
self.VSP = 0.0
self.upV = 0.0
self.upX = salt if self.Ter is None else self.Ter[0] + 10
self.Vsp = [self.maxV]
self.dVsp = [0]
self.init(thrust)
d = pid.kd
def on_frame():
alt_err = alt - self.dX
if self.AS > 0 or self.DS > 0:
if alt_err > 0:
self.pid.kp = clamp(0.01 * abs(alt_err) / clampL(self.upV, 1), 0.0, d)
elif alt_err < 0:
self.pid.kp = clamp(self.twr ** 2 / clampL(-self.upV, 1), 0.0, clampH(d * self.twr / 2, d))
else: self.pid.kp = d
self.pid.kd = d / clampL(self.dX, 1)
if alt_err < 0: alt_err = alt_err / clampL(self.dX, 1)
self.maxV = self.pid.update(alt_err)
print self.pid, alt_err, clamp(0.01 * abs(alt_err) / clampL(self.upV, 1), 0.0, d), d
if self.N > 0:
dV = (self.upV - self.dV) / clampL(self.dX, 1)
if alt_err < 0:
# print '% f %+f = %f' % (dV/clampL(self.dX/50, 1), clampL(alt_err*self.dX/5000*self.twr, self.pid.min*10), dV/clampL(self.dX/50, 1) + clampL(alt_err*self.dX/5000*self.twr, self.pid.min*10))
dV = dV + clampL(alt_err * self.dX / 500 * self.twr, self.pid.min * 10)
else:
dV = clampL(dV, 0)
# print dV
self.dVsp.append(dV)
self.maxV += dV
self.Vsp.append(self.maxV)
self.phys_loop(on_frame)
# print(("Start velocity: %f\n"
# "K1 %f; L1 %f; K2 %f L2 %f\n"
# "Fuel consumed: %f\n")
# % (upV, self.K1, self.L1, self.K2, self.L2, sum(self.F)))
def describe(self):
from scipy import stats
cols = ('X', 'rX', 'V', 'rV', 'Vsp', 'dVsp', 'K')
for n in cols:
c = getattr(self, n)
d = stats.describe(c)
print '%s:' % n
print ' sum: ', sum(c)
print ' min-max:', d.minmax
print ' mean: ', d.mean
print ' median: ', np.median(c)
print ' std: ', np.std(c)
print ' sem: ', stats.sem(c)
print ' skew: ', d.skewness
print ' hist: '
h = np.histogram(c, normed=True)
for i, e in enumerate(h[1][1:]):
e0 = h[1][i]
print '[% 8.3f : % 8.3f]: %s' % (e0, e, "#" * int(h[0][i] * 80))
print ''
print '\n'
def _plot(self, r, c, n, Y, ylab):
plt.subplot(r, c, n)
plt.plot(self.T, Y, label=("V: twr=%.1f" % self.twr))
plt.ylabel(ylab)
def legend(self):
plt.legend(bbox_to_anchor=(1.05, 1), loc=2, borderaxespad=0.)
def plot_a(self, r, c, n):
self._plot(r, c, n, self.A, ylab="vertical acceleration (m/s2)")
def plot_vs(self, r, c, n):
self._plot(r, c, n, self.V, ylab="vertical speed (m/s)")
def plot_ter(self, r, c, n):
if self.N == 0: return
self._plot(r, c, n, self.Ter[:len(self.T)], ylab="terrain (m)")
def plot_alt(self, r, c, n):
self._plot(r, c, n, self.X, ylab="altitude (m)")
def plot_ralt(self, r, c, n):
self._plot(r, c, n, self.rX, ylab="relative altitude (m)")
def plot_vsp(self, r, c, n):
self._plot(r, c, n, self.Vsp, ylab="VSP (m/s)")
def plot_dvsp(self, r, c, n):
if len(self.dVsp) != len(self.T): return
self._plot(r, c, n, self.dVsp, ylab="dVSP (m/s)")
def plot_k(self, r, c, n):
self._plot(r, c, n, self.K, ylab="K")
from __future__ import print_function
import numpy as np
import matplotlib.pyplot as plt
from multiprocessing import Pool, cpu_count
from scipy.optimize import curve_fit
from common import clampL, clampH, clamp01, lerp, PID, PID2, PID3, dt, center_deg, plt_show_maxed
if __name__ == '__main__':
rad2deg = 180 / np.pi
bearing_pid = PID(1, 0.0, 0.0, 0, np.pi * 10)
av_pid = PID3(
1.0,
0.1,
0,
-1.0,
1.0,
3 * dt,
)
def timelines(t, *lines):
n = len(lines)
ax1 = None
for i, line in enumerate(lines):
ax = plt.subplot(n, 1, i + 1, sharex=ax1)
plt.plot(t, line)
plt.grid(b=False,
which='major',
axis='x',