def test_cartesian_vector_normalize_sphere():
a = (180, 91)
r = (0, 89)
v = CartesianVector.from_spherical(r=1.0, alpha=d2r(a[0]), delta=d2r(a[1]))
x = [r2d(i) for i in v.normalized_angles]
assert (round(x[0], 12), round(x[1], 12)) == r
a = (180, -91)
r = (0, -89)
v = CartesianVector.from_spherical(r=1.0, alpha=d2r(a[0]), delta=d2r(a[1]))
x = [r2d(i) for i in v.normalized_angles]
assert (round(x[0], 12), round(x[1], 12)) == r
a = (0, 91)
r = (180, 89)
v = CartesianVector.from_spherical(r=1.0, alpha=d2r(a[0]), delta=d2r(a[1]))
x = [r2d(i) for i in v.normalized_angles]
assert (round(x[0], 12), round(x[1], 12)) == r
a = (0, -91)
r = (180, -89)
v = CartesianVector.from_spherical(r=1.0, alpha=d2r(a[0]), delta=d2r(a[1]))
x = [r2d(i) for i in v.normalized_angles]
assert (round(x[0], 12), round(x[1], 12)) == r
a = (120, 280)
r = (120, -80)
v = CartesianVector.from_spherical(r=1.0, alpha=d2r(a[0]), delta=d2r(a[1]))
x = [r2d(i) for i in v.normalized_angles]
assert (round(x[0], 12), round(x[1], 12)) == r
a = (375, 45) # 25 hours, 45 degrees
r = (15, 45)
v = CartesianVector.from_spherical(r=1.0, alpha=d2r(a[0]), delta=d2r(a[1]))
x = [r2d(i) for i in v.normalized_angles]
assert (round(x[0], 12), round(x[1], 12)) == r
a = (-375, -45)
r = (345, -45)
v = CartesianVector.from_spherical(r=1.0, alpha=d2r(a[0]), delta=d2r(a[1]))
x = [r2d(i) for i in v.normalized_angles]
assert (round(x[0], 12), round(x[1], 12)) == r
a = (-375, -91)
r = (165, -89)
v = CartesianVector.from_spherical(r=1.0, alpha=d2r(a[0]), delta=d2r(a[1]))
x = [r2d(i) for i in v.normalized_angles]
assert (round(x[0], 12), round(x[1], 12)) == r
def test_cartesian_vector_normalize_sphere():
a = (180, 91)
r = (0, 89)
v = CartesianVector.from_spherical(r=1.0, alpha=d2r(a[0]), delta=d2r(a[1]))
x = [r2d(i) for i in v.normalized_angles]
assert (round(x[0], 12), round(x[1], 12)) == r
a = (180, -91)
r = (0, -89)
v = CartesianVector.from_spherical(r=1.0, alpha=d2r(a[0]), delta=d2r(a[1]))
x = [r2d(i) for i in v.normalized_angles]
assert (round(x[0], 12), round(x[1], 12)) == r
a = (0, 91)
r = (180, 89)
v = CartesianVector.from_spherical(r=1.0, alpha=d2r(a[0]), delta=d2r(a[1]))
x = [r2d(i) for i in v.normalized_angles]
assert (round(x[0], 12), round(x[1], 12)) == r
a = (0, -91)
r = (180, -89)
v = CartesianVector.from_spherical(r=1.0, alpha=d2r(a[0]), delta=d2r(a[1]))
x = [r2d(i) for i in v.normalized_angles]
assert (round(x[0], 12), round(x[1], 12)) == r
a = (120, 280)
r = (120, -80)
v = CartesianVector.from_spherical(r=1.0, alpha=d2r(a[0]), delta=d2r(a[1]))
x = [r2d(i) for i in v.normalized_angles]
assert (round(x[0], 12), round(x[1], 12)) == r
a = (375, 45) # 25 hours, 45 degrees
r = (15, 45)
v = CartesianVector.from_spherical(r=1.0, alpha=d2r(a[0]), delta=d2r(a[1]))
x = [r2d(i) for i in v.normalized_angles]
assert (round(x[0], 12), round(x[1], 12)) == r
a = (-375, -45)
r = (345, -45)
v = CartesianVector.from_spherical(r=1.0, alpha=d2r(a[0]), delta=d2r(a[1]))
x = [r2d(i) for i in v.normalized_angles]
assert (round(x[0], 12), round(x[1], 12)) == r
a = (-375, -91)
r = (165, -89)
v = CartesianVector.from_spherical(r=1.0, alpha=d2r(a[0]), delta=d2r(a[1]))
x = [r2d(i) for i in v.normalized_angles]
assert (round(x[0], 12), round(x[1], 12)) == r
def log_stanley(cross_track_error, cross_track_steering, heading_error,
desired_yaw, current_yaw, delta, normalized_delta):
print("cross track error: {}, heading error: {}".format(
cross_track_error, r2d(heading_error)))
print("cross track steering: {}".format(r2d(cross_track_steering)))
print("desired yaw: {}".format(r2d(desired_yaw)))
print("current yaw: {}".format(r2d(current_yaw)))
print("steering angle: {}".format(r2d(delta)))
print("normalized steering angle [-1.22, 1.22]: {}".format(
r2d(normalized_delta)))
def convolve(self, x, y, z, inweight):
"""
convolve a measurement onto a range of grid coordinates
"""
nwidth = int(4.1 * self.FWHM * self.dpi)
ix = self.ii(x)
jy = self.jj(y)
x0 = angles.d2r(x) # convert to radians
if x0 < self.xminrad:
x0 = x0 + (2. * np.pi)
elif x0 > self.xmaxrad:
x0 = x0 - (2. * np.pi)
y0 = angles.d2r(y)
for iii in range(-nwidth, nwidth):
iix = ix + iii
if iix < 0:
iix = self.img_width + iix
elif iix >= self.img_width:
iix = iix - self.img_width
xx = self.xx(iix)
rx = angles.d2r(xx) # convert to radians
for jjj in range(-nwidth, nwidth):
jjy = jy + jjj
if jjy < 0:
jjy = self.img_height + jjy
elif jjy >= self.img_height:
jjy = jjy - self.img_height
yy = self.yy(jjy)
ry = angles.d2r(yy) # conver to radians
if ry < self.yminrad:
ry = ry + (2. * np.pi)
elif ry > self.ymaxrad:
ry = ry - (2. * np.pi)
# finally get angular separation in degrees
r = angles.r2d(angles.sep(x0, y0, rx, ry))
# if r > 4.*self.FWHM: # round convolving function
if r > 3. * self.FWHM: # round convolving function
continue
# add the colvolved measurement to the grid.
self.xy(iix, jjy, z, r, inweight)
return
nOn = nOn + 1
nRead = nRead + 1
if aTheta > offThetaA and aTheta < offThetaB:
if nOff == 0:
offSpectra = copy.deepcopy( rs)
else:
offSpectra.ydataA = offSpectra.ydataA + rs.ydataA
nOff = nOff + 1
nRead = nRead + 1
# utcs[nRead] = rs.utc
# lsts[nRead] = rs.lst
print 'Min Theta: ', angles.r2d( minTheta)
print 'N On: ', nOn, ' N Off: ', nOff
print 'Read ',nRead,' Spectra with ',nData,' Spectral channels '
if nOn < 1:
print 'no On Spectra found'
exit()
onSpectra.ydataA = scalefactor*onSpectra.ydataA/float(nOn)
if nOff < 1:
print 'no Off Spectra found'
exit()
offSpectra.ydataA = scalefactor*offSpectra.ydataA/float(nOff)
def exeLevelNorm(self):
""" Called for run level RUNLVL_NORMAL. Do not call externally.
"""
path = self.path()
pt = self.loc()
## Return if no path set!
if path == None:
logging.debug('No path set.')
return
logging.debug('Point: %s' % pt)
logging.debug('>>>>>> Current target: %s' % path.get())
feedback = path.isOnPath(pt, self.THRESHOLD_DIST, self.THRESHOLD_ANGLE)
status = feedback['status']
angleCorrection = int(angles.r2d(float(feedback['angleCorrection'])))
## Collision detection first
if (self.collisionLocked):
if (self.obstacle == None):
# Unlock collisionLocked
self._dispatcherClient.send(9002, 'say', {'text': 'Obstacle cleared. Move forward!'})
logging.debug('Obstacle cleared. Move forward!')
self.collisionLocked = False
time.sleep(5)
logging.debug('Time to clear obstacle has passed.')
elif (self.obstacle == 'FRONT'):
self._dispatcherClient.send(9002, 'say', {'text': 'Obstacle ahead. Turn left or right!'})
logging.debug('Obstacle ahead. Turn left or right!')
elif (self.obstacle == 'LEFT'):
self._dispatcherClient.send(9002, 'say', {'text': 'Obstacle on the left!'})
logging.debug('Obstacle on the left!')
elif (self.obstacle == 'RIGHT'):
self._dispatcherClient.send(9002, 'say', {'text': 'Obstacle on the right!'})
logging.debug('Obstacle on the right!')
# Do not execute below
return
if (status is Point.REACHED):
##TODO: Implement print to voice
checkpointName = '' ## initialize it first
if (path.isAtDest() is False):
if ((self._hasPassedStart == False) and (self.__model['path'].ref == 0)):
self.__model['path'].next()
self._hasPassedStart = True # So this does not trigger again at start
self.achievedNode = 0
# elif ( (self.__model['path'].ref != 0) and (self.achievedNode == int(self.__model['path'].ref - 1)) ):
else:
if (self.achievedNode == (self.__model['path'].ref - 1)):
self.achievedNode = self.__model['path'].ref # Update to current node before incrementing
##TODO: Fix this tomorrow
try:
currNode = self.__model['path'].get() ## Get current node
checkpointName = currNode.name()
except:
logging.error('Oops. Invalid name?')
self.__model['path'].next()
## Store the last value, i.e. 'delay'
if checkpointName == '':
self._dispatcherClient.send(9002, 'say', {'text': 'Checkpoint reached!'})
else:
self._dispatcherClient.send(9002, 'say', {'text': 'Checkpoint ' + checkpointName + ' reached!'})
logging.debug('checkpoint reached!')
else:
self._dispatcherClient.send(9002, 'say', {'text': 'Destination ' + checkpointName + ' reached!'})
self._resetNavParams() # Reset all navigation params and await new path
logging.debug('Reached destination, done!')
pass
elif (status is Point.MOVE_FORWARD):
self._dispatcherClient.send(9002, 'say', {'text': 'Move forward!'})
logging.debug('move forward!')
pass
elif (status is Point.OUT_OF_PATH):
self._dispatcherClient.send(9002, 'say', {'text': 'Out of path!'})
logging.debug('Out of path!')
## DEPRECATED --------------------------------
# self.getPathTo( self.path().dest().name() )
## /DEPRECATED --------------------------------
pass
elif (status is Point.ALIGNED):
##TODO: Implement print to voice
# self._dispatcherClient.send(9002, 'say', {'text': 'Out of path!'})
logging.debug('Point aligned!')
pass
elif (status is Point.TURN_LEFT):
self._dispatcherClient.send(9002, 'say', {'text': 'Turn left ' + str(angleCorrection) + ' degrees' })
logging.debug('Turn left ' + str(angleCorrection) + ' degrees')
pass
elif (status is Point.TURN_RIGHT):
self._dispatcherClient.send(9002, 'say', {'text': 'Turn right ' + str(angleCorrection) + ' degrees' })
logging.debug('Turn right ' + str(angleCorrection) + ' degrees')
pass
else:
## Oops uncaught feedback
logging.warn('Oops, did we account for all feedback flags?')
pass
pass
"""
7.С начала суток прошло H часов, M минут, S секунд (0 ≤ H < 12, 0 ≤ M < 60, 0 ≤ S < 60).
По данным числам H, M, S определите угол (в градусах), на который повернулаcь часовая стрелка с начала суток и выведите его в виде действительного числа.
"""
import math
import angles
def second_arrow(second):
return second * math.pi / 30
def minute_arrow(minute, second):
return (minute * math.pi / 30) + (second_arrow(second) / 60)
def hour_arrow(hour, minute, second):
return ((hour * math.pi) / 12) + (minute_arrow(minute, second) / 24)
hour = float(input("Hour = "))
minute = float(input("Minute = "))
second = float(input("Second = "))
# angle = (hour * (360/12)) + (minute * (360/60)) + (second * (360/3600)
# ang = hour + minute/60 + second/3600
angle = hour_arrow(hour, minute, second) * angles.r2d(math.pi)
print("Angle = ", angle)
def dec_deg(self):
return angles.r2d(self.obs.dec)
def ra_deg(self):
return angles.r2d(self.obs.ra)
def dec_deg(self):
return angles.r2d(self.obsdoc.dec)
def ra_deg(self):
return angles.r2d(self.obsdoc.ra)
def normalize_angles(cls, radian_vector):
return np.asarray(map(lambda x: angles.d2r(angles.normalize(angles.r2d(x), -180, +180)), radian_vector))
def angle(p, q, r) -> float:
# Note: the +plus+ is a concatenation of tuples here
pq = angles.bear(*map(angles.d2r, p + q))
qr = angles.bear(*map(angles.d2r, q + r))
return (((angles.r2d(pq - qr) + 540) % 360) - 180)
def normalizeRad(self, value): deg = angles.r2d(float(value)) return angles.d2r(angles.normalize(deg, -180, 180))
