def test_angle():
with pytest.raises(ValueError):
Angle(0, 0, 1)
with pytest.raises(ValueError):
Angle(-1, 1, 2)
with pytest.raises(ValueError):
Angle(1, -1, 2)
with pytest.raises(ValueError):
Angle(1, 0, -2)
a1 = Angle(0, 1, 2)
a2 = Angle(0, 2, 1)
assert a1 == a2
a3 = Angle(1, 2, 0)
assert a3 != a1
assert a3 != a2
a4 = Angle(0, 1, 3)
assert a4 != a1
# a funky setup to trigger the first return statement in __eq__ of angle
a1 = Angle(0, 1, 2)
a2 = Angle(1, 2, 3)
the_same = a1 == a2
assert not the_same
def angle_from_p1_to_p2(p1, p2):
p1 = np.asarray(p1)
p2 = np.asarray(p2)
v = p2 - p1
if v[0] < 0:
return Angle(radians=np.arctan(v[1] / v[0]) + math.pi)
return Angle(radians=np.arctan(v[1] / v[0]))
def Result(self, msgs, anss):
""" Parse answer from message
:param msgs: messages sent to instrument
:param anss: answers got from instrument
:returns: dictionary
"""
msgList = re.split('\|', msgs)
ansList = re.split('\|', anss)
res = {}
for msg, ans in zip(msgList, ansList):
if len(msg.strip()) == 0:
continue
# get command id form message
ansBufflist = re.split('\n=', ans)
commandID = ansBufflist[0]
if commandID == self.codes['HA']:
res['hz'] = Angle(float(ansBufflist[1]), 'PDEG')
elif commandID == self.codes['VA']:
res['v'] = Angle(float(ansBufflist[1]), 'PDEG')
elif commandID == self.codes['SD']:
res['distance'] = float(ansBufflist[1])
elif commandID == self.codes['EASTING']:
res['easting'] = float(ansBufflist[1])
elif commandID == self.codes['NORTHING']:
res['northing'] = float(ansBufflist[1])
elif commandID == self.codes['ELE']:
res['elevation'] = float(ansBufflist[1])
# TODO add all codes!
return res
def __init__(self):
super().__init__()
# this decides which side of the screen to spawn at.
self.start = random.randint(1, 4)
# uses the function from point to find the x and y
self.center.x = self.center.generate_asteroid_x(self.start)
self.center.y = self.center.generate_asteroid_y(self.start)
# uses angle object and its methods
# to find the direction it should go
angle = Angle()
self.direction = angle.generate_angle(self.start)
# uses the direction to get the correct speed
self.velocity.dx = math.cos(math.radians(
self.direction)) * BIG_ROCK_SPEED
self.velocity.dy = math.sin(math.radians(
self.direction)) * BIG_ROCK_SPEED
self.radius = BIG_ROCK_RADIUS
# starts at 0, but this will increase as the astroids advance
self.rotation = 0
self.dr = 1
self.texture = arcade.load_texture("images/meteorGrey_big1.png")
self.width = 50
self.height = 50
def run(self):
""" generate observetion list
:returns: list of observation dicts ordered by hz
"""
observations = []
for coo in self.coords:
if self.station_id == coo['id']:
#skip station
continue
obs = {}
d_north = coo['north'] - self.station_north
d_east = coo['east'] - self.station_east
d_elev = coo['elev'] - self.station_elev - self.station_ih
bearing = math.atan2(d_east, d_north)
dist = math.hypot(d_east, d_north)
zenith = math.atan2(dist, d_elev)
obs['id'] = coo['id']
obs['ih'] = self.station_ih
obs['hz'] = Angle(bearing).Positive()
obs['v'] = Angle(zenith).Positive()
obs['distance'] = math.hypot(dist, d_elev)
obs['code'] = 'ATR'
obs['faces'] = self.faces
if 'code' in coo and coo['code'] in modes1:
obs['code'] = coo['code']
observations.append(obs)
observations = sorted(observations, key=lambda a: a['hz'].GetAngle())
obs = {}
obs['station'] = self.station_id
obs['ih'] = self.station_ih
observations.insert(0, obs)
return observations
def make_angle(lineM, lineN):
meet = intersection(lineM, lineN)
A = Angle(lineM.endPoint1, meet, lineN.endPoint1)
B = Angle(lineM.endPoint1, meet, lineN.endPoint2)
C = Angle(lineM.endPoint2, meet, lineN.endPoint1)
D = Angle(lineM.endPoint2, meet, lineN.endPoint2)
return (A, B, C, D)
def GetNext(self):
""" Get fields in dictionary from next line considering filter
:returns: values in dict, empty dict on EOF
"""
res = {}
w = self.GetLine()
if self.state != self.RD_OK:
return res
w = w.strip('\n\r')
buf = re.split('[\{\}]', w)
for ww in buf:
if len(ww) > 2:
www = ww.split(' ')
key = int(www[0])
if key in self.codes:
if key in (7, 8, 21): # angles
# angles in DMS?
if re.search('-', www[1]):
res[self.codes[key]] = Angle(www[1], 'DMS')
else:
res[self.codes[key]] = Angle(float(www[1]))
elif key in (3, 6, 9, 11, 20, 37, 38, 39):
res[self.codes[key]] = float(www[1]) # numeric
elif key == 112:
res[self.codes[key]] = int(www[1]) # numeric
elif key == 51:
try:
res[self.codes[key]] = time.strptime(
' '.join(www[1:]), '%Y-%m-%d %H:%M:%S')
except:
pass # skip if datetime format is not valid
else:
res[self.codes[key]] = ' '.join(www[1:])
return res
def GetNext(self):
""" Get fields in dictionary from next line considering filter
:returns: values in dict, empty dict on EOF
"""
res = {}
w = self.GetLine().strip('\n\r')
buf = re.split('[\{\}]', w)
for ww in buf:
if len(ww) > 2:
www = ww.split(' ')
key = int(www[0])
if key in self.codes:
if key in (7, 8, 21): # angles
# angles in DMS?
if re.search('-', www[1]):
res[self.codes[key]] = Angle(www[1], 'DMS')
else:
res[self.codes[key]] = Angle(float(www[1]))
elif key in (3, 6, 9, 11, 37, 38, 39):
res[self.codes[key]] = float(www[1]) # numeric
elif key == 112:
res[self.codes[key]] = int(www[1]) # numeric
else:
res[self.codes[key]] = ' '.join(www[1:])
return res
def Result(self, msgs, anss):
""" Parse answer from message
:param msgs: messages sent to instrument
:param anss: answers got from instrument
:returns: dictionary
"""
msgList = msgs.split('|')
ansList = anss.split('|')
res = {}
for msg, ans in zip(msgList, ansList):
if len(msg.strip()) == 0:
continue
# get command id form message
ansBufflist = ans.split('\n')
for ans1 in ansBufflist:
if '=' in ans1:
buf = ans1.strip('\r|').split('=')
commandID = int(buf[0])
if commandID == self.codes['HA']:
res['hz'] = Angle(float(buf[1]), 'PDEG')
elif commandID == self.codes['VA']:
res['v'] = Angle(float(buf[1]), 'PDEG')
elif commandID == self.codes['SD']:
res['distance'] = float(buf[1])
elif commandID == self.codes['EASTING']:
res['east'] = float(buf[1])
elif commandID == self.codes['NORTHING']:
res['north'] = float(buf[1])
elif commandID == self.codes['ELE']:
res['elev'] = float(buf[1])
# TODO add all codes!
return res
def get_minute_hour_angle(self):
self.set_second_angle()
self.set_minute_angle()
self.set_hour_angle()
minute_hour_angle = Angle(self.minute_angle.angle -
self.hour_angle.angle)
minute_hour_angle.convert_to_normal_angle()
return minute_hour_angle
def ChangeFace(self):
""" Change face
:returns: empty dictionary
"""
msg = self.measureUnit.ChangeFaceMsg()
if msg is None:
angles = self.GetAngles()
angles['hz'] += Angle(180, 'DEG')
angles['v'] = Angle(360, 'DEG') - angles['v']
return self.Move(angles['hz'], angles['v'])
return self._process(msg)
def get_bounds(self):
rc = GeoRect()
rc.left = Angle.degrees(180)
rc.right = Angle.degrees(-180)
rc.top = Angle.degrees(-90)
rc.bottom = Angle.degrees(90)
for wp in self.__list:
rc.left = min(rc.left, wp.location.lon)
rc.right = max(rc.right, wp.location.lon)
rc.top = max(rc.top, wp.location.lat)
rc.bottom = min(rc.bottom, wp.location.lat)
return rc
def get_bounds(self):
rc = GeoRect()
rc.left = Angle.degrees(180)
rc.right = Angle.degrees(-180)
rc.top = Angle.degrees(-90)
rc.bottom = Angle.degrees(90)
for wp in self.__list:
rc.left = min(rc.left, wp.location.lon)
rc.right = max(rc.right, wp.location.lon)
rc.top = max(rc.top, wp.location.lat)
rc.bottom = min(rc.bottom, wp.location.lat)
return rc
def create_empty_data_struct(self):
COF = Joint(None, None, "COF")
COM = Joint(None, None, "COF")
C7 = Joint(None, None, "COF")
angle_lower = Angle(None, None, "angle_lower")
angle_trunk = Angle(None, None, "angle_trunk")
phase = OLD_PHASE_OUT_PHASE(None, None, None, name="phase")
return Move(cof=COF,
com=COM,
c7=C7,
angle_l=angle_lower,
angle_t=angle_trunk,
phase=phase)
def __get_earth_rotation_since_observation(elapsed_seconds):
"""
convert seconds into angles
total sec/86164.1*360d00.0
:param elapsed_seconds: second between ref time and observed time
:return: hour angle
"""
full_angle = Angle.from_string("360d00.0")
rotation = round(elapsed_seconds / 86164.1000, 5)
print("full_angle_:" + full_angle.str)
print("rotation_:" + str(rotation))
print("get_earth_rotation_:" + Angle.from_decimal(rotation).str)
return Angle.from_decimal(rotation)
def Result(self, msg, ans):
""" process the answer from GNSS
:param msg: MNEA message to get
:param ans: NMEA message from GNSS unit
:returns: processed message or None if msg and ans do not match
"""
res = {}
if ans[3:len(msg) + 3] != msg:
return None
# check checksum
data, cksum = re.split('\*', ans)
cksum1 = 0
for s in data[1:]:
cksum1 ^= ord(s)
if ('0x' + cksum).lower() != hex(cksum1).lower():
logging.error(' Checksum error')
return None
anslist = ans.split(',')
if msg == 'GGA':
# no fix
if int(anslist[6]) == 0:
return None
try:
mul = 1 if anslist[3] == 'N' else -1
res['latitude'] = Angle(mul * float(anslist[2]), 'NMEA')
mul = 1 if anslist[5] == 'E' else -1
res['longitude'] = Angle(mul * float(anslist[4]), 'NMEA')
res['quality'] = int(anslist[6])
res['nsat'] = int(anslist[7])
res['altitude'] = float(anslist[9])
res['hdop'] = float(anslist[8])
if self.date_time is not None:
res['datetime'] = self.date_time
self.date_time = None
except:
logging.error(" invalid nmea sentence: " + ans)
return None
elif msg == 'ZDA':
try:
# TODO microseconds
self.date_time = datetime(int(anslist[4]), int(anslist[3]),
int(anslist[2]),
int(anslist[1][0:2]),
int(anslist[1][2:4]),
int(anslist[1][4:6]))
except:
logging.error(" invalid nmea sentence: " + ans)
return None
return res
def dispatch(Correction, values):
try:
if 'correctedDistance' in values or 'correctedAzimuth' in values:
raise ValueError('invalid keys present')
correction = Correction(Angle.parse(values['lat']),
Angle.parse(values['long']),
Angle.parse(values['altitude']),
Angle.parse(values['assumedLat']),
Angle.parse(values['assumedLong']))
values['correctedDistance'] = correction.correctedDistance()
values['correctedAzimuth'] = str(correction.correctedAzimuth())
except Exception as e:
values['error'] = str(e)
return values
def create_bridge_config(spec):
grapple_points = spec.grapple_points
arm = spec.initial
min_lengths = spec.min_lengths
max_lengths = spec.max_lengths
#print(min_lengths)
#print(max_lengths)
angles = []
lengths = []
for i in range(len(arm.lengths)):
if i == 0:
angles.append(Angle(math.radians(random.randint(-180, 180))))
else:
angles.append(Angle(math.radians(random.randint(-165, 165))))
lengths.append(random.uniform(min_lengths[i], max_lengths[i]))
#angles.append(Angle)
#print(angles)
#print(lengths)
#print(arm.points[0][0])
#print(arm.points[0][1])
#print(arm.points[1])
sample = make_robot_config_from_ee1(arm.points[0][0], arm.points[0][1],
angles, lengths, True)
#for points in sample.points:
#print(points)
sample_points = sample.points[-2]
#print("--------------")
#print(sample_points)
delta_y = grapple_points[1][1] - sample_points[1]
delta_x = grapple_points[1][0] - sample_points[0]
new_angle = delta_y / delta_x
last_angle = Angle.tan(new_angle)
sum_angles = 0
for angle in range(len(angles) - 1):
if angle == 0:
sum_angles = angles[angle].in_degrees()
else:
sum_angles = 180 + angles[angle].in_degrees()
second_last_angle = 360 - sum_angles - last_angle
angles[-1] = Angle(math.radians(360 + second_last_angle))
lengths[-1] = math.sqrt(delta_x**2 + delta_y**2)
bridge_2_config = make_robot_config_from_ee1(arm.points[0][0],
arm.points[0][1], angles,
lengths, True, False)
while (not individual_config_collision_checking(spec, bridge_2_config)):
bridge_2_config = create_bridge_config(spec)
return bridge_2_config
def generate_sample(spec, config, index):
angles = []
lengths = []
for i in range(spec.num_segments):
angles.append(Angle(random.uniform(-165, 165)))
if spec.min_lengths != spec.max_lengths:
lengths.append(
random.uniform(spec.min_lengths[i], spec.max_lengths[i]))
else:
lengths = config.lengths
if index % 2 == 0:
next_config = make_robot_config_from_ee1(spec.grapple_points[index][0],
spec.grapple_points[index][1],
angles,
lengths,
ee1_grappled=True,
ee2_grappled=False)
else:
next_config = make_robot_config_from_ee2(spec.grapple_points[index][0],
spec.grapple_points[index][1],
angles,
lengths,
ee1_grappled=False,
ee2_grappled=True)
if detect_collision(spec, next_config):
node = GraphNode(spec, next_config)
return node
else:
return generate_sample(spec, config, index)
def __init__(self,
shape,
coordinate_system,
rotation=Angle.from_degrees(7.5)):
self.shape = shape
self.rotation = rotation
self.coordinate_system = coordinate_system
def testMakeFromBondsWithSuccess(self):
b1 = bond = Bond(angleTests.a1, angleTests.a2, 2, None)
b2 = bond = Bond(angleTests.a2, angleTests.a3, 2, None)
angle = Angle.makeFromBonds(b1, b2)
self.assertEqual(angle.get_a_1(), angleTests.a1)
self.assertEqual(angle.get_a_2(), angleTests.a2)
self.assertEqual(angle.get_a_3(), angleTests.a3)
def get_greenwich_hour_angle(year, month, day, hour, minute, second):
"""
= relative_prime_meridian + earth rotation
:param year:
:param month:
:param day:
:param hour:
:param minute:
:param second:
:return:
"""
reference_datetime_str = str(year) + ",01,01,00,00,00"
observation_datetime_str = str(year) + ',' + str(month) + ',' + str(
day) + ','
observation_datetime_str += str(hour) + ',' + str(minute) + ',' + str(
second)
observation_datetime = datetime.strptime(observation_datetime_str,
'%Y,%m,%d,%H,%M,%S')
reference_datetime = datetime.strptime(reference_datetime_str,
'%Y,%m,%d,%H,%M,%S')
elapsed_sed_since_ref = (observation_datetime -
reference_datetime).total_seconds()
relative_pm = Aries.__get_relative_prime_meridian(year)
earth_rotation = Aries.__get_earth_rotation_since_observation(
elapsed_sed_since_ref)
print("relative_pm" + relative_pm.str)
print("earth_rotation" + earth_rotation.str)
return Angle.add(relative_pm, earth_rotation)
def process_gyro(gyro_data, timestamp):
"""
Computes the change in rotation angle, based on gyroscope measurements.
It accepts gyro_data, an rs2_vector containing measurements retrieved from gyroscope,
and timestamp, the timestamp of the current frame from gyroscope stream.
"""
global first
global last_timestamp_gyro
# On the first iteration use only data from accelerometer
# to set the camera's initial position
if first:
last_timestamp_gyro = timestamp
return
# Initialize gyro angle with data from gyro
# gyro_data.x : Pitch
# gyro_data.y : Yaw
# gyro_data.z : Roll
gyro_angle = Angle(gyro_data.x, gyro_data.y, gyro_data.z)
# Compute the difference between arrival times of previous and current gyro frames
dt_gyro = (timestamp - last_timestamp_gyro) / 1000.0
last_timestamp_gyro = timestamp
# Change in angle equals gyro measurements * time passed since last measurement
gyro_angle = gyro_angle * dt_gyro
# Apply the calculated change of angle to the current angle (theta)
global mutex
global theta
mutex.acquire()
theta.add(-gyro_angle.z, -gyro_angle.y, gyro_angle.x)
mutex.release()
return theta
def setup_sensors(self, angle, origin):
self.origin = origin
offset = -62
for line in self.lines:
self.detected[line].update_vector(
calculate_vector(origin, -Angle(angle.degree + offset).radians,
self.detected[line].sensor_size))
offset += 31
def correctedAzimuth(self):
return Angle(
math.degrees(
math.acos((math.sin(math.radians(self.getLatitude())) -
math.sin(math.radians(self.getAssumedLatitude())) *
self.intermediateDistance()) /
(math.cos(math.radians(self.getAssumedLatitude())) *
math.cos(math.asin(self.intermediateDistance()))))))
def Result(self, msg, ans):
""" process the answer from GNSS
:param msg: MNEA message to get
:param ans: NMEA message from GNSS unit
:returns: processed message or None if msg and ans do not match
"""
res = {}
if ans[3:len(msg) + 3] != msg:
return None
# check checksum
data, cksum = re.split('\*', ans)
cksum1 = 0
for s in data[1:]:
cksum1 ^= ord(s)
if ('0x' + cksum).lower() != hex(cksum1).lower():
logging.error(' Checksum error')
return None
anslist = ans.split(',')
if msg == 'GGA':
# no fix
if int(anslist[6]) == 0:
return None
try:
hour = int(anslist[1][0:2])
minute = int(anslist[1][2:4])
second = int(anslist[1][4:6])
if len(anslist[1]) > 6:
ms = int(float(anslist[1][6:]) * 1000)
else:
ms = 0
d = date.today()
res['datetime'] = datetime(d.year, d.month, d.day, hour,
minute, second, ms)
mul = 1 if anslist[3] == 'N' else -1
res['latitude'] = Angle(mul * float(anslist[2]), 'NMEA')
mul = 1 if anslist[5] == 'E' else -1
res['longitude'] = Angle(mul * float(anslist[4]), 'NMEA')
res['quality'] = int(anslist[6])
res['nsat'] = int(anslist[7])
res['altitude'] = float(anslist[9])
res['hdop'] = float(anslist[8])
except:
logging.error(" invalid nmea sentence: " + ans)
return None
return res
def set_hour_angle(self):
if self.time.hour > 12:
hour = self.time.hour - 12
else:
hour = self.time.hour
self.hour_angle = Angle(hour / 12 * 360 + self.time.minute /
(60 * 12) * 360 + self.time.second /
(60 * 60 * 12) * 360)
def get_all_transit_intervals(target,
ephemeris,
start_date,
end_date,
onepersite=False,
verbose=False,
vverbose=False,
exptime=0):
intervals = []
titles = []
sitenames = []
telescopes = file_to_dicts(
'/home/jeastman/lcogt/scheduler/transits/rise_set-0.2.10/telescopes.dat'
)
for telescope in telescopes:
if telescope['status'] == 'online':
site = {
'name': telescope['name'].split(".")[2],
'latitude': Angle(degrees=telescope['latitude']),
'longitude': Angle(degrees=telescope['longitude']),
'horizon': telescope['horizon'],
'ha_limit_neg': telescope['ha_limit_neg'],
'ha_limit_pos': telescope['ha_limit_pos'],
}
if not site['name'] in sitenames or not onepersite:
sitenames.append(site['name'])
site_intervals, site_titles = get_transit_intervals(
target,
ephemeris,
site,
start_date,
end_date,
verbose=verbose,
vverbose=vverbose,
exptime=exptime)
# print site_intervals, site_titles, start_date, end_date, target
for start, stop in site_intervals:
intervals.append((start, stop))
for title in site_titles:
titles.append(title)
return intervals, titles
def PicMes(self, photoName, targetType=None):
'''Measure angles between the target and the optical axis
:param photoName: name of the photo
:param targetType: type of the target
:returns: horizontal (hz) and vertical (v) correction angle in dictionary
'''
ok = False
while not ok:
print(photoName)
file = open(photoName, 'w+b')
print((int(self._affinParams[0, 3]), int(self._affinParams[1, 3])))
ang = self.GetAngles()
self.TakePhoto(
file,
(int(self._affinParams[0, 3]), int(self._affinParams[1, 3])))
file.close()
try:
img = cv2.imread(photoName, 1)
picCoord = rec.recogChessPattern(img)
print(picCoord)
ok = True
except:
pass
img[int(picCoord[1]), :] = [0, 255, 255]
img[:, int(picCoord[0])] = [0, 255, 255]
cv2.imwrite(photoName, img)
angles = {}
angles['hz'] = Angle(1 / math.sin(ang['v'].GetAngle('RAD')) *
(self._affinParams[0, 1] *
(picCoord[0] - round(self._affinParams[0, 0])) +
self._affinParams[0, 2] *
(picCoord[1] - round(self._affinParams[1, 0]))))
angles['v'] = Angle(self._affinParams[1, 1] *
(picCoord[0] - round(self._affinParams[0, 0])) +
self._affinParams[1, 2] *
(picCoord[1] - round(self._affinParams[1, 0])))
return angles
def __get_relative_prime_meridian(year):
"""
- total progression = 100d42.6 + cumulative prog + leap progs
- cumulative progression: delta(year-2001) * -0d14.31667
- leap progression: (leap years elapsed) * 0d59.0
:param year: observation year
:return: angle of prime meridian
"""
reference_rotation = Angle.from_string("100d42.6")
# cumulative progression: delta(year-2001) * -0d14.31667
annual_gha_decrement = Angle.from_string("-0d14.32")
delta_year = year - Aries.REFERENCE_YEAR
cumulative_progression = Angle.multiply(annual_gha_decrement,
delta_year)
# leap progression: (leap years elapsed) * 0d59.0
daily_rotation = Angle.from_string("0d59.0")
leap_years = math.floor((year - Aries.REFERENCE_YEAR) / 4)
leap_progression = Angle.multiply(daily_rotation, leap_years)
# total progression = 100d42.6 + cumulative prog + leap progs
total_progression = Angle.add(reference_rotation,
cumulative_progression)
total_progression = Angle.add(total_progression, leap_progression)
print("total progression" + total_progression.str)
return total_progression
def __init__(self, args_opt):
super(Simulation, self).__init__()
self.control = controller(args_opt)
self.md_info = md_information(self.control)
self.bond = Bond(self.control, self.md_info)
self.angle = Angle(self.control)
self.dihedral = Dihedral(self.control)
self.nb14 = NON_BOND_14(self.control, self.dihedral,
self.md_info.atom_numbers)
self.nb_info = nb_infomation(self.control, self.md_info.atom_numbers,
self.md_info.box_length)
self.LJ_info = Lennard_Jones_Information(self.control)
self.liujian_info = Langevin_Liujian(self.control,
self.md_info.atom_numbers)
self.pme_method = Particle_Mesh_Ewald(self.control, self.md_info)
self.box_length = Tensor(
np.asarray(self.md_info.box_length, np.float32), mstype.float32)
self.file = None
def __init__(self, left = 0, right = 0, top = 0, bottom = 0):
if isinstance(left, Angle):
self.left = left
else:
self.left = Angle.degrees(left)
if isinstance(right, Angle):
self.right = right
else:
self.right = Angle.degrees(right)
if isinstance(top, Angle):
self.top = top
else:
self.top = Angle.degrees(top)
if isinstance(bottom, Angle):
self.bottom = bottom
else:
self.bottom = Angle.degrees(bottom)
def __init__(self, camera, pnts_pattern):
self.rep = Representation()
self.h = Homography(pnts_pattern)
self.tcp = TCP()
self.a = Angle()
self.camera = camera
self.hom = np.zeros((3, 3))
self.inv_hom = np.zeros((3, 3))
self.Frame = np.zeros((3, 3))
self.inv_Frame = np.zeros((3, 3))
self.hom_final_camera = np.zeros((3, 3))
def __parse_winpilot_coordinate(self, str):
str = str.lower()
if str.endswith("s") or str.endswith("w"):
negative = True;
str = str.rstrip("sw")
else:
negative = False;
str = str.rstrip("ne")
str = str.split(":")
if len(str) < 2:
return None
a = Angle.dms(int(str[0]), float(str[1]))
if (negative):
a.flip()
return a
def measurement_predict_function_angle(objs, noise, obj_indices, noise_indices):
return Angle.fromRadians(objs[obj_indices[0]].toRadians()+noise[noise_indices[0],0])
def measurement_predict_function(objects, noise, obj_indices, noise_indices):
#pdb.set_trace()
return Angle.fromRadians(objects[obj_indices[0]].toRadians()+noise[noise_indices[0],0])
def measurement_predict_function(objects, noise):
#pdb.set_trace()
return [Vector(objects[0].v+noise[0:2]),Angle.fromRadians(objects[1].toRadians()+noise[2,0]),Vector(objects[2].v+noise[3:5]),Angle.fromRadians(objects[3].toRadians()+noise[5,0])]
from numpy import matrix, random, identity, linalg
from math import pi, tan, sin, cos, sqrt, atan
from time import clock
from quaternion import Quaternion
from angle import Angle
from angle3d import Angle3D
from vector import Vector
from unscented_kalman_filter_objects import *
from kalman_util import add_noise
from filter_system import *
from testPr2 import *
import pdb
objects = [Vector(matrix([[1.1],[-.4],[tZ]])),Angle.fromRadians(0),Vector(matrix([[1.1],[0],[tZ]])),Angle.fromRadians(0),Vector(matrix([[1.3],[0],[0]])),Angle.fromRadians(pi/2),Vector(matrix([[0.],[0.],[0.]])),Angle.fromRadians(0)]
covars = [matrix([[.1**2,0,0],[0,.1**2,0],[0,0,1e-10]]),(.3**2)*matrix(identity(1)),matrix([[.1**2,0,0],[0,.1**2,0],[0,0,1e-10]]),(.3**2)*matrix(identity(1)),matrix([[.07**2,0,0],[0,.03**2,0],[0,0,1e-10]]),(.2**2)*matrix(identity(1)),(.00001**2)*matrix(identity(3)),(.00001**2)*matrix(identity(1))]
covar_process_noise = append_matrices([(.00001**2)*matrix(identity(12)),(.005**2)*matrix(identity(2)),.000000001*matrix(identity(1)),((pi/36)**2)*matrix(identity(1))])
## Mess with this to increase observation variance
#typicalErrProbs.obsVar = 10*typicalErrProbs.obsVar
covar_measurement_noise = append_matrices([typicalErrProbs.obsVar[0:4,0:4],typicalErrProbs.obsVar[0:4,0:4],typicalErrProbs.obsVar[0:4,0:4]])
def state_update_none_vec(command, objs, noise, obj_indices, noise_indices):
return add_noise([objs[obj_indices[0]]],noise[noise_indices[0]:noise_indices[0]+3])[0]
def state_update_none_ang(command, objs, noise, obj_indices, noise_indices):
return add_noise([objs[obj_indices[0]]],noise[noise_indices[0]:noise_indices[0]+1])[0]
def state_update_move_ang(command, objs, noise, obj_indices, noise_indices):
return add_noise([Angle.fromRadians(objs[obj_indices[0]].toRadians()-pi/6*command)],noise[noise_indices[0]])[0]
from math import sin, cos, pi
from numpy import matrix, random, identity
from kalman_util import append_matrices, add_noise
from quaternion import Quaternion
from angle import Angle
from angle3d import Angle3D
from vector import Vector
from unscented_kalman_filter_objects import *
import pdb
objects = [Vector(matrix([[1],[0]])),Angle.fromRadians(pi/2),Vector(matrix([[-1],[0]])),Angle.fromRadians(-pi/2)]
covars = [.1*matrix(identity(2)),matrix([[(pi/6)**2]]),.1*matrix(identity(2)),matrix([[(pi/6)**2]])]
def update_function(command, objects, noise):
#pdb.set_trace()
return [Vector(objects[0].v+matrix([[(command[0]+noise[0,0])*cos(objects[1].toRadians())],[(command[0]+noise[0,0])*sin(objects[1].toRadians())]])),Angle.fromRadians(pi/12+objects[1].toRadians()+noise[1,0]),Vector(objects[2].v+matrix([[(command[1]+noise[2,0])*cos(objects[3].toRadians())],[(command[1]+noise[2,0])*sin(objects[3].toRadians())]])),Angle.fromRadians(pi/12+objects[3].toRadians()+noise[3,0])]
def measurement_predict_function(objects, noise):
#pdb.set_trace()
return [Vector(objects[0].v+noise[0:2]),Angle.fromRadians(objects[1].toRadians()+noise[2,0]),Vector(objects[2].v+noise[3:5]),Angle.fromRadians(objects[3].toRadians()+noise[5,0])]
covar_process_noise = append_matrices((matrix([[.005**2]]),matrix([[(pi/36)**2]]),matrix([[.005**2]]),matrix([[(pi/36)**2]])))
covar_measurement_noise = append_matrices(((.005**2)*matrix(identity(2)),matrix([[(pi/36)**2]]),(.005**2)*matrix(identity(2)),matrix([[(pi/36)**2]])))
k = UnscentedKalmanFilter(objects, covars, update_function, measurement_predict_function, [None,None,None,None], covar_process_noise, covar_measurement_noise,True)
realPos = [Vector(matrix([[1],[0]])),Angle.fromRadians(pi/2),Vector(matrix([[-1],[0]])),Angle.fromRadians(-pi/2)]
def update_function(command, objects, noise):
#pdb.set_trace()
return [Vector(objects[0].v+matrix([[(command[0]+noise[0,0])*cos(objects[1].toRadians())],[(command[0]+noise[0,0])*sin(objects[1].toRadians())]])),Angle.fromRadians(pi/12+objects[1].toRadians()+noise[1,0]),Vector(objects[2].v+matrix([[(command[1]+noise[2,0])*cos(objects[3].toRadians())],[(command[1]+noise[2,0])*sin(objects[3].toRadians())]])),Angle.fromRadians(pi/12+objects[3].toRadians()+noise[3,0])]
def update_function_angle(command, objs, noise, obj_indices, noise_indices):
return Angle.fromRadians(pi/12+objs[obj_indices[0]].toRadians()+noise[noise_indices[0],0])
class Cal_camera():
def __init__(self, camera, pnts_pattern):
self.rep = Representation()
self.h = Homography(pnts_pattern)
self.tcp = TCP()
self.a = Angle()
self.camera = camera
self.hom = np.zeros((3, 3))
self.inv_hom = np.zeros((3, 3))
self.Frame = np.zeros((3, 3))
self.inv_Frame = np.zeros((3, 3))
self.hom_final_camera = np.zeros((3, 3))
def get_pxls_homography(self, image, scale=1):
pts_image = self.h.read_image(image, scale)
return pts_image
def calculate_homography(self, pxls_pattern):
self.hom = self.h.calculate(pxls_pattern)
self.inv_hom = linalg.inv(self.hom)
def get_pxl_origin(self, folder, scale=1):
pxl_pnts = []
for f in sorted(folder):
im_uEye = cv2.imread(f)
pxl_pnts.append(self.tcp.read_image(im_uEye, scale))
return pxl_pnts
def get_pxl_orientation(self, folder, scale=1):
for f in sorted(folder):
name = basename(f)
if name == "move_x.jpg":
im_uEye = cv2.imread(f)
pxl_pnts_x = self.tcp.read_image(im_uEye, scale)
elif name == "move_y.jpg":
im_uEye = cv2.imread(f)
pxl_pnts_y = self.tcp.read_image(im_uEye, scale)
elif name == "move_o.jpg":
im_uEye = cv2.imread(f)
pxl_pnts_origin = self.tcp.read_image(im_uEye, scale)
return pxl_pnts_origin, pxl_pnts_x, pxl_pnts_y
def calculate_TCP_orientarion(self, pxl_pnts, pxl_pnts_origin, pxl_pnts_x, pxl_pnts_y, dx, dy):
pxl_TCP = self.tcp.calculate_origin(pxl_pnts)
print "TCP :", pxl_TCP
factor, angle_y, angle_x = self.tcp.calculate_orientation(pxl_pnts_origin, pxl_pnts_x, pxl_pnts_y, dx, dy)
return pxl_TCP, factor, angle_y, angle_x
def calculate_angle_TCP(self, origin, axis_x, pattern):
pnt_origin = self.rep.transform(self.hom, origin)
pnt_x = self.rep.transform(self.hom, axis_x)
pnt_pattern = self.rep.transform(self.hom, pattern)
l_1 = np.float32([pnt_origin[0], pnt_x[0]])
l_2 = pnt_pattern[0:2]
vd1 = self.a.director_vector(l_1[0], l_1[1])
vd2 = self.a.director_vector(l_2[0], l_2[1])
angle = self.a.calculate_angle(vd1, vd2)
return angle
def calculate_frame(self, pxl_TCP, angle):
pnts_TCP = self.rep.transform(self.hom, pxl_TCP)[0]
a = np.deg2rad(angle)
self.Frame = np.float32([[np.cos(a), -np.sin(a), pnts_TCP[0]],
[np.sin(a), np.cos(a), pnts_TCP[1]],
[0, 0, 1]])
self.Orientation = np.float32([[np.cos(a), -np.sin(a), 0],
[np.sin(a), np.cos(a), 0],
[0, 0, 1]])
self.inv_Orientation = linalg.inv(self.Orientation)
self.inv_Frame = linalg.inv(self.Frame)
def calculate_hom_final(self, img, pnts, corners, pnts_final):
# pxls_camera = self.rep.transform(self.inv_hom, pnts)
im_measures = self.rep.define_camera(img.copy(), self.hom)
#------------------ Data in (c)mm
image_axis = self.rep.draw_axis_camera(im_measures, pnts)
#------------------
pnts_Frame = pnts
pnts_camera = self.rep.transform(self.Frame, pnts_Frame)
image_axis_tcp = self.rep.draw_axis_camera(image_axis, pnts_camera)
#------------------
corners_Frame = corners
corners_camera = self.rep.transform(self.Frame, corners_Frame)
img = self.rep.draw_points(image_axis_tcp, corners_camera)
#------------------
pxls_corner = self.rep.transform(self.inv_hom, corners_camera)
self.hom_final_camera, status = cv2.findHomography(pxls_corner.copy(), pnts_final)
self.write_config_file()
return self.hom_final_camera
def write_config_file(self):
hom_vis = self.hom_final_camera
hom_TCP = np.dot(self.inv_hom, self.Frame)
inv_hom_TCP = np.dot(self.inv_Frame, self.hom)
data = dict(
hom_vis=hom_vis.tolist(),
hom=hom_TCP.tolist(),
inv_hom=inv_hom_TCP.tolist(),
)
filename = '../../config/' + self.camera + '_config.yaml'
with open(filename, 'w') as outfile:
yaml.dump(data, outfile)
print data
def visualize_data(self):
print "Homography: "
print self.hom
print "Frame: "
print self.Frame
print "Final homography: "
print self.hom_final_camera
def draw_pattern_axis(self, pnts, img):
pnts_axis_pattern = pnts
pxls_axis_pattern = self.rep.transform(self.inv_hom, pnts_axis_pattern)
pnt_axis_pattern_final = self.rep.transform(self.hom_final_camera, pxls_axis_pattern)
img_pattern_NIT = self.rep.draw_axis_camera(img, pnt_axis_pattern_final)
return img_pattern_NIT
def draw_TCP_axis(self, pnts, img):
pnts_axis = pnts
pnts_axis_TCP = self.rep.transform(self.Frame, pnts_axis)
pxls_axis_TCP = self.rep.transform(self.inv_hom, pnts_axis_TCP)
pnt_axis_TCP_final = self.rep.transform(self.hom_final_camera, pxls_axis_TCP)
img_final_axis = self.rep.draw_axis_camera(img, pnt_axis_TCP_final)
return img_final_axis
def draw_axis(self, pnts, img):
img_TCP = self.draw_pattern_axis(pnts, img)
img_final = self.draw_TCP_axis(pnts, img_TCP)
return img_final
def update_function(command, objs, noise, obj_indices, noise_indices):
#pdb.set_trace()
return Angle.fromRadians(pi/12+objs[obj_indices[0]].toRadians()+noise[noise_indices[0],0])
from math import sin, cos, pi
from numpy import matrix, random, identity
from kalman_util import append_matrices, add_noise
from quaternion import Quaternion
from angle import Angle
from angle3d import Angle3D
from vector import Vector
from unscented_kalman_filter_objects import *
from filter_system import *
import pdb
objects = [Angle.fromRadians(pi/2)]
covars = [matrix([[(pi/1006)**2]])]
def update_function(command, objs, noise, obj_indices, noise_indices):
#pdb.set_trace()
return Angle.fromRadians(pi/12+objs[obj_indices[0]].toRadians()+noise[noise_indices[0],0])
def measurement_predict_function(objects, noise, obj_indices, noise_indices):
#pdb.set_trace()
return Angle.fromRadians(objects[obj_indices[0]].toRadians()+noise[noise_indices[0],0])
covar_process_noise = matrix([[(pi/36)**2]])
covar_measurement_noise = matrix([[(pi/36)**2]])
fs = FilterSystem()
fs.addObject('ObjA')
def __init__(self,
lon = Angle.degrees(0),
lat = Angle.degrees(0)):
self.lon = lon
self.lat = lat
def generate_interactions_o(self,interaction_name,bond="harmonic",SPB=False,radius=10,cutoff=1.15,reducedv_factor=1,khun=1.):
if bond == "harmonic" :
temp1 = "bond_style harmonic\n"
temp2 = "bond_coeff %i %.2f %.2f"
ene = 350
if bond == "fene":
if SPB:
temp1 = "bond_style hybrid harmonic fene\n"
temp2 = "bond_coeff %i fene %.2f %.2f %.2f %.2f"
else:
temp1 = "bond_style fene\n"
temp2 = "bond_coeff %i %.2f %.2f %.2f %.2f"
ene = 30 * 1
Bond = [temp1]
if bond == "fene":
Bond.append("special_bonds fene\n")
ene_ratio=1#.35
#Bond.append("special_bonds 0.0 1.0 1.0\n")
if SPB and False:
#print radius
Pair = ["pair_style hybrid lj/cut 3.0 gauss/cut %.2f \n"%(2*radius) + "pair_modify shift yes\n"]
else:
Pair = ["pair_style lj/cut 1.4 \n" + "pair_modify shift yes\n"]
Angle = ["angle_style cosine/delta\n"]
Angle = ["angle_style harmonic\n"]
keyl = range(1,len(self.natom)+1)
for t1 in keyl:
for t2 in keyl:
if t2 >= t1:
if not self.liaison.has_key("%s-%s"%(t1,t2)):
print "Warning liaison between {0} and {1} not defined".format(t1,t2)
dist,tybe_b = self.liaison["%s-%s"%(t1,t2)]
if cutoff is not None:
cut = dist*cutoff
else:
cut = dist*pow(2.,1/6.)
odist = copy.deepcopy(dist)
#dist=1
if bond == "fene":
Bond.append(temp2 % (tybe_b,ene_ratio * ene/(dist*dist),1.5*dist,ene_ratio,dist) +"\n")
else:
Bond.append(temp2 % (tybe_b, ene,dist) +"\n")
dist = odist
precise =""
if SPB and False:
precise = "lj/cut"
reduced = 1
if t1 == t2 and t1 == 3:
reduced = 1
else:
reduced = reducedv_factor
Pair.append("""pair_coeff %s %s %s %.1f %.2f %.2f\n"""%(t1,t2,precise,ene_ratio,dist*reduced,cut*reduced))
if self.angle_def is not None and self.Angle != []:
for t3 in keyl:
if t3 >= t2:
dist,tybe_b = self.angle_def["%s-%s-%s"%(t1,t2,t3)]
k=khun/2. * dist # dist = 1 if no ribo involved else 0
Angle.append("angle_coeff %i %.3f 180.0\n"%(tybe_b,k))
if SPB:
if bond == "fene":
Bond.append("bond_coeff %i harmonic 0 0 \n"%(self.liaison["spb"][1]))
spbond = "bond_coeff %i harmonic %.1f %.1f\n"%(self.liaison["spb"][1],10,microtubule/realsigma)
else:
Bond.append("bond_coeff %i 0 0 \n"%(self.liaison["spb"][1]))
spbond = "bond_coeff %i %.1f %.1f\n"%(self.liaison["spb"][1],10,microtubule/realsigma)
n_i = len(diameter)/2
for t1 in range(len(diameter) ):
Pair.append("""pair_coeff %i %i %s 0. %.2f %.2f\n"""%(t1+1,num_particle["spb"],precise,dist,cut))
Pair.append("""pair_coeff %i %i %s 0. %.2f %.2f\n"""%(num_particle["spb"],num_particle["spb"],precise,dist,cut))
g = open(interaction_name,"w")
g.write("".join(Bond)+"\n")
g.write("".join(Pair)+"\n")
if self.Angle != []:
g.write("".join(Angle))
