def filter_reference(): # Kombiniert Beschleunigunssensoren und Gyrosensoren
global sens_ref # Teilt Werte mit anderen Threads
offset = [0.0, 0.0] # Offset definiert Nullpunkt
sens_ref = [0.0, 0.0]
out = [0.0, 0.0]
iterations = 0
k = 0.025 # Beschleunigt Kalibrierungsvorgang durch mehr Iterations/t
time.sleep(1)
while True:
out[0] = (
offset[0]
+ 0.99 * (out[0] + reference_gyro_omega[0] * DT - offset[0])
+ 0.01
* (360 / (2 * math.pi))
* (math.atan2(reference_accelerometer_acc["y"], reference_accelerometer_acc["z"]) + math.pi)
) # Berechnet x-Rotation
out[1] = (
offset[1]
+ 0.99 * (out[1] + reference_gyro_omega[1] * DT - offset[1])
+ 0.01
* (360 / (2 * math.pi))
* (math.atan2(reference_accelerometer_acc["z"], reference_accelerometer_acc["x"]) + math.pi)
) # Berechnet y-Rotation
iterations += 1 # Zaehlt Ablaeufe
time.sleep(k * DT)
if iterations == 30 * (1 / DT): # Setzt Nullpunkt nach einer Anzahl Zyklen
offset[0] = 0.0 - out[0] # Nullpunkt x
offset[1] = 0.0 - out[1] # Nullpunkt y
out[0] = 0 # Setzt Ouput auf 0 um Inkrementierung zu vermeiden -> schnellere Anpassung
out[1] = 0 # Setzt Ouput auf 0 um Inkrementierung zu vermeiden -> schnellere Anpassung
k = 1
print "Sensor 1 calibrated"
sens_ref[0] = int(out[0]) # Teilt Wert als ganze Zahl
sens_ref[1] = int(out[1]) # Teilt Wert als ganze Zahl
def legIK(self, X, Y, Z, resolution):
""" Compute leg servo positions. """
ans = [0,0,0,0] # (coxa, femur, tibia)
try:
# first, make this a 2DOF problem... by solving coxa
ans[0] = radToServo(atan2(X,Y), resolution)
trueX = int(sqrt(sq(X)+sq(Y))) - self.L_COXA
im = int(sqrt(sq(trueX)+sq(Z))) # length of imaginary leg
# get femur angle above horizon...
q1 = -atan2(Z,trueX)
d1 = sq(self.L_FEMUR)-sq(self.L_TIBIA)+sq(im)
d2 = 2*self.L_FEMUR*im
q2 = acos(d1/float(d2))
ans[1] = radToServo(q1+q2, resolution)
# and tibia angle from femur...
d1 = sq(self.L_FEMUR)-sq(im)+sq(self.L_TIBIA)
d2 = 2*self.L_TIBIA*self.L_FEMUR;
ans[2] = radToServo(acos(d1/float(d2))-1.57, resolution)
except:
if self.debug:
"LegIK FAILED"
return [1024,1024,1024,0]
if self.debug:
print "LegIK:",ans
return ans
def update_sound(self):
"""
Used to update sound_sensor continuously depending on how far bird
would need to turn to face sound_origin
"""
a = math.radians(self.angle)
self.facing_vector = [math.cos(a), math.sin(a)] # get unit facing vector
if self.sound_timer > 0: # only perform calculations if bird has a
# sound in memory
self.sound_timer -= 1 # forget sound over time
# now get a unit vector in the direction of the sound
dist = ((self.x - self.sound_origin[0]) ** 2 + (
self.y - self.sound_origin[1]) ** 2) ** .5
self.sound_direction = [-(self.x - self.sound_origin[0]) / dist,
-(self.y - self.sound_origin[1]) / dist]
# use atan2 to get the difference between facing vector and
# direction vector into radians
a_dif = math.atan2(self.sound_direction[1],
self.sound_direction[0]) - math.atan2(
self.facing_vector[1], self.facing_vector[0])
self.sound_sensors = [max(0, a_dif), max(0, -a_dif)] # positive
# means left, negative means right
else:
self.sound_sensors = [0, 0]
self.sound_direction = [0, 0]
def filter_stabilized(): # Analog zu filter_reference()
global sens_stab
offset = [0.0, 0.0]
sens_stab = [0.0, 0.0]
out = [0.0, 0.0]
iterations = 0
k = 0.025
time.sleep(1)
while True:
out[0] = (
offset[0]
+ 0.99 * (out[0] + stabilized_gyro_omega[0] * DT - offset[0])
+ 0.01
* (360 / (2 * math.pi))
* (math.atan2(stabilized_accelerometer_acc["y"], stabilized_accelerometer_acc["z"]) + math.pi)
)
out[1] = (
offset[1]
+ 0.99 * (out[1] + stabilized_gyro_omega[1] * DT - offset[1])
+ 0.01
* (360 / (2 * math.pi))
* (math.atan2(stabilized_accelerometer_acc["z"], stabilized_accelerometer_acc["x"]) + math.pi)
)
iterations += 1
time.sleep(k * DT)
if iterations == 30 * (1 / DT):
offset[0] = 0.0 - out[0]
offset[1] = 0.0 - out[1]
out[0] = 0
out[1] = 0
print "Sensor 2 calibrated"
k = 1
sens_stab[0] = int(out[0])
sens_stab[1] = int(out[1])
def matrix_to_euler(rotmat):
'''Inverse of euler_to_matrix().'''
if not isinstance(rotmat, np.matrixlib.defmatrix.matrix):
# As this calculation relies on np.matrix algebra - convert array to
# matrix
rotmat = np.matrix(rotmat)
def cvec(x, y, z):
return np.matrix([[x, y, z]]).T
ex = cvec(1., 0., 0.)
ez = cvec(0., 0., 1.)
exs = rotmat.T * ex
ezs = rotmat.T * ez
enodes = np.cross(ez.T, ezs.T).T
if np.linalg.norm(enodes) < 1e-10:
enodes = exs
enodess = rotmat * enodes
cos_alpha = float((ez.T*ezs))
if cos_alpha > 1.:
cos_alpha = 1.
if cos_alpha < -1.:
cos_alpha = -1.
alpha = acos(cos_alpha)
beta = np.mod(atan2(enodes[1, 0], enodes[0, 0]), pi * 2.)
gamma = np.mod(-atan2(enodess[1, 0], enodess[0, 0]), pi*2.)
return unique_euler(alpha, beta, gamma)
def GetAngleOfLineBetweenTwoPoints(self, p1, p2, angle_unit="degrees"):
xDiff = p2.x() - p1.x()
yDiff = p2.y() - p1.y()
if angle_unit == "radians":
return atan2(yDiff, xDiff)
else:
return degrees(atan2(yDiff, xDiff))
def isRectangle(pts, seuil=0.05):
if len(pts) != 4:
raise ValueError('Input must be 4 points')
coords = pts.ravel().reshape(-1, 2)
cx, cy = np.mean([coord[0] for coord in coords]), np.mean(
[coord[1] for coord in coords])
dist = [distance((cx, cy), coord) for coord in coords]
res = 0
# print coords
for i in xrange(1, 4):
res += abs(dist[0] - dist[i])
bias = res / distance(coords[1], coords[2])
logging.info('Regtangle bias: %.3f', res)
logging.info('Ratio bias: %.3f', bias)
if bias < seuil:
line1 = coords[3] - coords[0]
line2 = coords[2] - coords[1]
mean_radian = - \
(math.atan2(line1[1], line1[0]) +
math.atan2(line2[1], line2[0])) / 2
inclination = math.degrees(mean_radian) # / np.pi * 90
logging.info('Document rotation: %.3f degree', inclination)
return True, mean_radian
else:
return False, None
def update_arrow(self, pt, size=10, angle=PI_OVER_FOUR):
line = None
ang = None
x, y = pt
if self.partial and not self.segments:
(x1, y1), (x2, y2) = self.start, self.end
dx, dy = x2 - x1, y2 - y1
ang = math.atan2(dy, dx)
elif self.partial and self.segments:
(xd, yd), (x1, y1) = self.segments[-1]
(x2, y2) = self.end
dx, dy = x2 - x1, y2 - y1
ang = math.atan2(dy, dx)
elif self.segments:
ang = self.segments[-1].get_angle()
if ang is not None:
ang0 = ang + PI - angle
ang1 = ang + PI + angle
x0, y0 = x + size * math.cos(ang0), y + size * math.sin(ang0)
x1, y1 = x + size * math.cos(ang1), y + size * math.sin(ang1)
line = (x0, y0), (x, y), (x1, y1)
self.arrow = line
def pointObjectToTarget(obj, targetLoc):
dx = targetLoc.x - obj.location.x;
dy = targetLoc.y - obj.location.y;
dz = targetLoc.z - obj.location.z;
xRad = math.atan2(dz, math.sqrt(dy**2 + dx**2)) + math.pi/2;
zRad = math.atan2(dy, dx) - math.pi/2;
obj.rotation_euler = mathutils.Euler((xRad, 0, zRad), 'XYZ');
def ecef_to_lla(ecef): #earths's radius in meters a = 6378137 #eccentricity e = 8.1819190842622e-2 asq = math.pow(a,2) esq = math.pow(e,2) x = ecef[0] y = ecef[1] z = ecef[2] b = math.sqrt( asq * (1-esq) ) bsq = math.pow(b,2) ep = math.sqrt( (asq - bsq)/bsq) p = math.sqrt( math.pow(x,2) + math.pow(y,2) ) th = math.atan2(a*z, b*p) lon = math.atan2(y,x) lat = math.atan2( (z + math.pow(ep,2)*b*math.pow(math.sin(th),3) ), (p - esq*a*math.pow(math.cos(th),3)) ) N = a/( math.sqrt(1-esq*math.pow(math.sin(lat),2)) ) alt = p / math.cos(lat) - N #mod lat to keep it between 0 and 2 pi lon = lon % (2*math.pi) #changing radians to degrees lat = math.degrees(lat) lon = math.degrees(lon) #normalizing angle lat = normalizeAngle(lat) lon = normalizeAngle(lon) ret = (lat, lon) return ret;
def update_searchlight_from_orientation_(self):
qw, qx, qy, qz = self.move.get_orientation()
# Convert quaternion to euler-angle.
# Formulas from:
# http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/
# but note that all values are negated.
azimuth = 0
elevation = 0
test = qx * qy + qz * qw
roll = math.asin(2 * test)
if test > 0.499:
azimuth = -2 * math.atan2(qx, qw)
elevation = math.pi / 2
elif test < -0.499:
azimuth = 2 * math.atan2(qx, qw)
elevation = 0
else:
azimuth = -math.atan2(2 * qy * qw - 2 * qx * qz,
1 - 2 * qy * qy - 2 * qz * qz)
elevation = math.atan2(2 * qx * qw - 2 * qy * qz ,
1 - 2 * qx * qx - 2 * qz * qz)
# print 'az %.2f el %.2f roll %.2f' % (
# azimuth * 57.2957795, elevation * 57.2957795, roll * 57.2957795)
min_elevation = -1
for searchlight in self.searchlights:
min_elevation = max(min_elevation, searchlight.config.elevation_lower_bound)
min_elevation = clamp_and_scale(min_elevation, -1, 1, 0, 90 * DEGREES_TO_RADIANS)
if elevation < min_elevation:
elevation = min_elevation
self.reactor.callFromThread(self.target_angle, azimuth, elevation)
def fusion_get_orientation(self):
'''
Fuses data from accelerometer and magnetometer. Same algorithm as the
original Adafruit 10-DOF function except pitch = -roll and roll = pitch
since this makes much more sense for the board layout.
Returns a tuple of (pitch, roll, heading)
'''
# Calculate pitch based only on accel
(accelX, accelY, accelZ) = self.accel_get_raw()
pitch = -atan2(accelY, accelZ)
# Calculate roll based on pitch and accel
if accelY * sin(-pitch) + accelZ * cos(-pitch) == 0:
roll = pi / 2 if accelX > 0 else -pi / 2
else:
roll = atan(-accelX / (accelY * sin(-pitch) + accelZ * cos(-pitch)))
# Calculate heading based on pitch, roll, and mag
(magX, magY, magZ) = self.mag_get_raw()
heading = atan2(magZ * sin(-pitch) - magY * cos(-pitch),
magX * cos(roll) +
magY * sin(roll) * sin(-pitch) +
magZ * sin(roll) * cos(-pitch))
return (pitch * 180 / pi, roll * 180 / pi, heading * 180 / pi)
def getValues(self):
accx = self.twos_comp_combine(self.BUS.read_byte_data(self.LSM, self.ACC_X_MSB), self.BUS.read_byte_data(self.LSM, self.ACC_X_LSB))
accy = self.twos_comp_combine(self.BUS.read_byte_data(self.LSM, self.ACC_Y_MSB), self.BUS.read_byte_data(self.LSM, self.ACC_Y_LSB))
accz = self.twos_comp_combine(self.BUS.read_byte_data(self.LSM, self.ACC_Z_MSB), self.BUS.read_byte_data(self.LSM, self.ACC_Z_LSB))
gyrox = self.twos_comp_combine(self.BUS.read_byte_data(self.GYRO, self.GYRO_X_MSB), self.BUS.read_byte_data(self.GYRO, self.GYRO_X_LSB))
gyroy = self.twos_comp_combine(self.BUS.read_byte_data(self.GYRO, self.GYRO_Y_MSB), self.BUS.read_byte_data(self.GYRO, self.GYRO_Y_LSB))
gyroz = self.twos_comp_combine(self.BUS.read_byte_data(self.GYRO, self.GYRO_Z_MSB), self.BUS.read_byte_data(self.GYRO, self.GYRO_Z_LSB))
rate_gyrox = gyrox*self.GYRO_ADD
rate_gyroy = gyroy*self.GYRO_ADD
rate_gyroz = gyroz*self.GYRO_ADD
if (not self.FIRST):
self.FIRST = True
self.gyroXangle = rate_gyrox*self.DT
self.gyroYangle = rate_gyroy*self.DT
self.gyroZangle = rate_gyroz*self.DT
else:
self.gyroXangle += rate_gyrox*self.DT
self.gyroYangle += rate_gyroy*self.DT
self.gyroZangle += rate_gyroz*self.DT
roll = int(round(math.degrees(math.atan2(accx, accz))))
pitch = int(round(math.degrees(math.atan2(accy, accz))))
print "Przechylenie: ", int(round(roll,0)), " Pochylenie: ", int(round(pitch,0))
self.FILTR_X = self.MULTIPLY*(roll)+(1-self.MULTIPLY)*self.gyroXangle
self.FILTR_Y = self.MULTIPLY*(pitch)+(1-self.MULTIPLY)*self.gyroYangle
print "Filtr przechylenie: ", int(round(self.FILTR_X,0)), " Filtr pochylenie: ", int(round(self.FILTR_Y,0))
return str(roll)+';'+str(pitch)
def matrix2angle(R):
''' compute three Euler angles from a Rotation Matrix. Ref: http://www.gregslabaugh.net/publications/euler.pdf
Args:
R: (3,3). rotation matrix
Returns:
x: yaw
y: pitch
z: roll
'''
# assert(isRotationMatrix(R))
if R[2,0] !=1 or R[2,0] != -1:
x = asin(R[2,0])
y = atan2(R[2,1]/cos(x), R[2,2]/cos(x))
z = atan2(R[1,0]/cos(x), R[0,0]/cos(x))
else:# Gimbal lock
z = 0 #can be anything
if R[2,0] == -1:
x = np.pi/2
y = z + atan2(R[0,1], R[0,2])
else:
x = -np.pi/2
y = -z + atan2(-R[0,1], -R[0,2])
return x, y, z
def draw_car(x,y,th,canv):
r = math.sqrt(math.pow(ROBOT_LENGTH,2) + math.pow(ROBOT_WIDTH,2))/2
x = x + 300.0/cm_to_pixels
y = y + 300.0/cm_to_pixels
# top left
phi = th + math.pi/2+ math.atan2(ROBOT_LENGTH,ROBOT_WIDTH)
topleft = (x + r*math.cos(phi),y+r*math.sin(phi))
#top right
phi = th + math.atan2(ROBOT_WIDTH,ROBOT_LENGTH)
topright = (x + r*math.cos(phi),y+r*math.sin(phi))
# bottom left
phi = th + math.pi + math.atan2(ROBOT_WIDTH,ROBOT_LENGTH)
bottomleft = (x + r*math.cos(phi),y+r*math.sin(phi))
# bottom right
phi = th + 3*math.pi/2 + math.atan2(ROBOT_LENGTH,ROBOT_WIDTH)
bottomright = (x + r*math.cos(phi),y+r*math.sin(phi))
canv.create_polygon(topleft[0]*cm_to_pixels,600 - topleft[1]*cm_to_pixels,
bottomleft[0]*cm_to_pixels,600 - bottomleft[1]*cm_to_pixels,
bottomright[0]*cm_to_pixels,600 - bottomright[1]*cm_to_pixels,
topright[0]*cm_to_pixels,600 - topright[1]*cm_to_pixels,
width = 1, outline = 'blue',fill = '')
x1 = x*cm_to_pixels
y1 = y*cm_to_pixels
canv.create_oval(x1-1,600-(y1-1),x1+1,600-(y1+1),outline = 'green',fill = 'green')
def __getCameraMovementAngles(self):
"""
@return: camera position as its movement angles (azimuth, elevation
and roll) in degres
@rtype: (float, float, float)
"""
camera = self.vtkapp.cameraPosition
top = self.vtkapp.cameraViewUp
if camera[0] != 0 or camera[2] != 0:
a = math.atan2(camera[0], camera[2])
camera = rotateY(-a, camera)
top = rotateY(-a, top)
b = math.atan2(camera[1], camera[2])
camera = rotateX(-b, camera)
top = rotateX(-b, top)
c = math.atan2(top[0], top[1])
#camera = rotateZ(c, camera)
top = rotateZ(c, top)
debugOutput("initial top: %s" % str(top))
debugOutput("initial camera: %s" % str(camera))
return tuple(x * 180 / math.pi for x in (a, b, c))
def to_geographic(self, x, y):
# Retrieve the locals.
A, E, PI4, PI2, P0, M0, X0, Y0, P1, P2, m1, m2, t1, t2, t0, n, F, rho0 = self._locals
# Subtract the false northing/easting.
x = x - X0
y = y - Y0
# Calculate the Longitude.
lon = math.atan2(x, rho0 - y) / n + M0
# Estimate the Latitude.
rho = math.sqrt(math.pow(x, 2.0) + math.pow(rho0 - y, 2.0))
t = math.pow(rho / (A * F), 1.0 / n)
lat = PI2 - (2.0 * math.atan2(t, 1.0))
# Substitute the estimate into the iterative calculation
# that converges on the correct Latitude value.
while True:
lat1 = lat
es = E * math.sin(lat1)
lat = PI2 - 2.0 * math.atan2(t * math.pow((1.0 - es) / (1.0 + es), E / 2.0), 1.0)
if math.fabs(lat - lat1) < 2.0e-9:
break
# Return lat/lon in degrees.
return math.degrees(lat), math.degrees(lon)
def coordangle_callback(data):
x = data.x
y = data.y
z = data.z
grip_angle = 0
d = math.sqrt(x**2 + y**2)
z_prime = z - shoulder_height
d -= math.cos(grip_angle)*wrist_endpoint
z_prime -= math.sin(grip_angle)*wrist_endpoint
l1 = shoulder_elbow
l2 = elbow_wrist
numerator = d**2 + z_prime**2 - l1**2 - l2**2
denominator = 2*l1*l2
elbow_angle = math.atan2(math.sqrt(1 - (numerator/denominator)**2), numerator / denominator)
if elbow_angle > 0:
elbow_angle = math.atan2(-math.sqrt(1 - (numerator / denominator)**2), numerator / denominator)
k1 = l1 + l2 * math.cos(elbow_angle)
k2 = l2 * math.sin(elbow_angle)
shoulder_angle = math.atan2(z_prime, d) - math.atan2(k2, k1)
wrist_angle = grip_angle - shoulder_angle - elbow_angle
base_angle = math.atan2(y, x)
next_angles.x = -base_angle + math.pi/2 #base angle
next_angles.y = -math.pi/2 + shoulder_angle #shoulder angle
next_angles.z = -elbow_angle - math.pi/2 #elbow angle
next_angles.w = wrist_angle + 0.17444 #wrist angle
assert -1.5708 <= next_angles.x <= 1.5708, next_angles.x
assert -0.8727 <= next_angles.y <= 0.8727, next_angles.y
assert -1.5708 <= next_angles.z <= 1.1775, next_angles.z
assert -0.7850 <= next_angles.w <= 0.7850, next_angles.w
rospy.loginfo("Base angle = %f, shoulder_angle = %f, elbow_angle = %f, wrist_angle = %f",next_angles.x,next_angles.y,next_angles.z,next_angles.w)
angle_pub.publish(next_angles)
def inputs(actual_q, altitude, target_q, target_altitude):
inpt = np.ones(4)*9.8/4 # TODO what range should these be in?
q = quaternion.qmul(quaternion.qinv(target_q), actual_q)
roll = math.atan2(2*(q[2]*q[3] + q[0]*q[1]), q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3])
yaw = math.atan2(2*(q[1]*q[2] + q[0]*q[3]), q[0]*q[0] + q[1]*q[1] - q[2]*q[2] - q[3]*q[3])
pitch = math.asin(-2*(q[1]*q[3] - q[0]*q[2]))
inpt[0] += 0.3*(target_altitude-altitude)
inpt[1] += 0.3*(target_altitude-altitude)
inpt[2] += 0.3*(target_altitude-altitude)
inpt[3] += 0.3*(target_altitude-altitude)
inpt = [max(0.3,x) for x in inpt]
inpt[0] -= 0.3*yaw
inpt[2] -= 0.3*yaw
inpt[1] += 0.3*yaw
inpt[3] += 0.3*yaw
inpt = [max(0,x) for x in inpt]
inpt[0] += 0.2*pitch
inpt[2] -= 0.2*pitch
inpt[1] += 0.2*roll
inpt[3] -= 0.2*roll
inpt = [max(0,x) for x in inpt]
return inpt
def _calc_det_angles_given_det_or_naz_constraint(
self, det_constraint, naz_constraint, theta, tau, alpha):
assert det_constraint or naz_constraint
if det_constraint:
# One of the detector angles is given (Section 5.1)
det_constraint_name, det_constraint = det_constraint.items()[0]
delta, nu, qaz = self._calc_remaining_detector_angles(
det_constraint_name, det_constraint, theta)
naz_qaz_angle = _calc_angle_between_naz_and_qaz(theta, alpha, tau)
naz = qaz - naz_qaz_angle
elif naz_constraint: # The 'detector' angle naz is given:
det_constraint_name, det_constraint = naz_constraint.items()[0]
naz_name, naz = det_constraint_name, det_constraint
assert naz_name == 'naz'
naz_qaz_angle = _calc_angle_between_naz_and_qaz(theta, alpha, tau)
qaz = naz - naz_qaz_angle
nu = atan2(sin(2 * theta) * cos(qaz), cos(2 * theta))
delta = atan2(sin(qaz) * sin(nu), cos(qaz))
delta_nu_pairs = self._generate_detector_solutions(
delta, nu, qaz, theta, det_constraint_name)
if not delta_nu_pairs:
raise DiffcalcException('No detector solutions were found,'
'please unconstrain detector limits')
delta, nu = self._choose_detector_solution(delta_nu_pairs)
return qaz, naz, delta, nu
def longest_lines(hull):
l = len(hull)
lines = [0] * l
for n in xrange(l):
x1, y1 = hull[n]
x2, y2 = hull[(n+1) % l]
lines[n] = {
'c1': (x1, y1),
'c2': (x2, y2),
'len': ( (x2-x1)**2 + (y2-y1)**2 ) ** 0.5,
'angle': math.atan2(y2 - y1, x2-x1),
}
#make straight-ish lines actually straight
n = 0
while n+1 < len(lines):
l1 = lines[n]
l2 = lines[(n+1) % len(lines)]
if abs(l1['angle'] - l2['angle']) / (math.pi*2) < 0.0027:
x1, y1 = c1 = l1['c1']
x2, y2 = c2 = l2['c2']
lines[n] = {
'c1': c1,
'c2': c2,
'len': ( (x2-x1)**2 + (y2-y1)**2 ) ** 0.5,
'angle': math.atan2(y2 - y1, x2-x1),
}
del lines[n+1]
else:
n += 1
lines.sort(key = lambda l: -l['len'])
return lines
def update_goals(self, dt):
'''update the velocities to match the goals'''
flagdist = collide.dist(self.pos, self.team.flag.pos)
if self.flag and collide.rect2circle(self.team.base.rect, (self.pos, constants.TANKRADIUS)):
self.team.map.scoreFlag(self.flag)
if not self.flag and self.team.flag.tank is None and flagdist < config['puppy_guard_zone']:
x,y = self.team.flag.pos
rad = 1 # config['puppy_guard_zone']
angle = math.atan2(self.pos[1]-y, self.pos[0]-x)
npos = [math.cos(angle) * rad + self.pos[0], math.sin(angle) * rad + self.pos[1]]
if not self.collision_at(npos, True):
self.pos = npos
## return the flag once you get on "your side"
# if self.flag and self.team.map.closest_base(self.pos) == self.team.base:
# self.team.map.scoreFlag(self.flag)
self.hspeed += self.accelx
self.vspeed += self.accely
max = 30
if collide.dist((0,0),(self.hspeed, self.vspeed)) > max:
dr = math.atan2(self.vspeed, self.hspeed)
self.hspeed = math.cos(dr) * max
self.vspeed = math.sin(dr) * max
def getArcParameters(self, arc, version):
xs = self.setUnit(arc['pos'][0], version)
ys = self.setUnit(arc['pos'][1], version)
x1 = self.setUnit(arc['ofsa'][0], version)
y1 = self.setUnit(arc['ofsa'][1], version)
x2 = self.setUnit(arc['ofsb'][0], version)
y2 = self.setUnit(arc['ofsb'][1], version)
angle = float("%.1f" % ((atan2(y2, x2) - atan2(y1, x1)) * 180 / 3.14))
#
x1 = self.setUnit(arc['ofsa'][0], version) + xs
y1 = self.setUnit(arc['ofsa'][1], version) + ys
[x2R, y2R] = self.obrocPunkt2([x1, y1], [xs, ys], angle)
#FreeCAD.Console.PrintWarning(u"angle = {0}\n".format(angle))
#FreeCAD.Console.PrintWarning(u"xs = {0}\n".format(xs))
#FreeCAD.Console.PrintWarning(u"ys = {0}\n".format(ys))
#FreeCAD.Console.PrintWarning(u"x1 = {0}\n".format(x1))
#FreeCAD.Console.PrintWarning(u"y1 = {0}\n".format(y1))
#FreeCAD.Console.PrintWarning(u"x2R = {0}\n".format(x2R))
#FreeCAD.Console.PrintWarning(u"y2R = {0}\n\n".format(y2R))
x2R = float("%.1f" % x2R)
y2R = float("%.1f" % y2R)
if angle < 0:
angle = angle - 360
width = self.setUnit(arc['width'], version)
return [x1, y1 * -1, x2R, y2R * -1, angle, width]
def pid(self, data):
#Function that does a z control over the heading for the ground vehicle
#Uses z from the ar tag and tries to reach it
#Calculate heading angle to goal
u_x = data.x - self.x
u_y = data.y - self.y
theta_g = math.atan2(u_y, u_x)
#Calculate error in heading
e_k = theta_g - self.theta
e_k = math.atan2(math.sin(e_k), math.cos(e_k))
#Calculate PID parameters
e_P = e_k
e_I = self.E_k + e_k
e_D = self.e_k_1 - e_k
#The controlled angular velocity
self.w = self.Kp*e_P + self.Ki*e_I + self.Kd*e_D
#Updates
self.E_k = e_I
self.e_k_1 = e_k
#Print statements for debugging
rospy.loginfo('PID called, w is: ')
rospy.loginfo(self.w)
goal = np.array((data.x, data.y))
rospy.loginfo('distance from goal: ')
rospy.loginfo(np.linalg.norm(self.pose-goal))
def solve_2R_inverse_kinematics(x,y,L1=1,L2=1):
"""For a 2R arm centered at the origin, solves for the joint angles
(q1,q2) that places the end effector at (x,y).
The result is a list of up to 2 solutions, e.g. [(q1,q2),(q1',q2')].
"""
D = vectorops.norm((x,y))
thetades = math.atan2(y,x)
if D == 0:
raise ValueError("(x,y) at origin, infinite # of solutions")
c2 = (D**2-L1**2-L2**2)/(2.0*L1*L2)
q2s = []
if c2 < -1:
print "solve_2R_inverse_kinematics: (x,y) inside inner circle"
return []
elif c2 > 1:
print "solve_2R_inverse_kinematics: (x,y) out of reach"
return []
else:
if c2 == 1:
q2s = [math.acos(c2)]
else:
q2s = [math.acos(c2),-math.acos(c2)]
res = []
for q2 in q2s:
thetaactual = math.atan2(math.sin(q2),L1+L2*math.cos(q2))
q1 = thetades - thetaactual
res.append((q1,q2))
return res
def speedsToReach( carrot, robotPose ) :
"""
Compute robot commands for the next path point
use 'follow the carrot' algorithm
:param carrot: Point to reach
:param robotPose: Robot collection storing robot position and orientation
:return: speeds of the robots in a dictionnary
"""
# robot angle
angleRobot = atan2( getBearing()['Y'], getBearing()['X'] )
# compute distance between the robot and the carrot
distanceCarrot = computeDistance( carrot, robotPose['Position'] )
# print "distance: ", distanceCarrot
# compute the angle to the carrot
angleCarrot = atan2( carrot['Y'] - robotPose['Position']['Y'], carrot['X'] - robotPose['Position']['X'] )
angleToCarrot = angleCarrot - angleRobot
if -pi > angleToCarrot :
angleToCarrot = ( 2 * pi ) + angleToCarrot
if pi < angleToCarrot :
angleToCarrot = ( 2 * pi ) - angleToCarrot
# print "angle: ", angleToCarrot
# compute angular speed
speeds = {}
speeds['angular'] = ( angleToCarrot ) / radPerSec
speeds['linear'] = ( speeds['angular'] * ( distanceCarrot / ( 2 * sin( angleToCarrot ) ) ) )
if maxLinearSpeed < speeds['linear'] :
speeds['linear'] = maxLinearSpeed
if -maxLinearSpeed > speeds['linear'] :
speeds['linear'] = -maxLinearSpeed
return speeds
def quat2eulerZYX (self,q):
euler = Vector3()
tol = self.quat2eulerZYX.tolerance
qww, qxx, qyy, qzz = q.w*q.w, q.x*q.x, q.y*q.y, q.z*q.z
qwx, qxy, qyz, qxz= q.w*q.x, q.x*q.y, q.y*q.z, q.x*q.z
qwy, qwz = q.w*q.y, q.w*q.z
test = -2.0 * (qxz - qwy)
if test > +tol:
euler.x = atan2 (-2.0*(qyz-qwx), qww-qxx+qyy-qzz)
euler.y = +0.5 * pi
euler.z = 0.0
return euler
elif test < -tol:
euler.x = atan2 (-2.0*(qyz-qwx), qww-qxx+qyy-qzz)
euler.y = -0.5 * pi
euler.z = tol
return euler
else:
euler.x = atan2 (2.0*(qyz+qwx), qww-qxx-qyy+qzz)
euler.y = asin (test)
euler.z = atan2 (2.0*(qxy+qwz), qww+qxx-qyy-qzz)
return euler
def on_mouse_press(x, y, button, modifiers):
#print player1.x, player1.y
#for n in range(0, player1.sides):
#print n, " ::: ", player1.vertices[2 * n + 2], player1.vertices[2 * n + 3], " : ", player1.colors[4*n + 4:4*n+8]
if button == mouse.MIDDLE:
player1.drawTarget = True
if button == mouse.LEFT and player1.sides > 3:
a1 = math.atan2(float(y - player1.y), float(x - player1.x))
side = 0
closest = 3.1415
a3 = 100
for n in range(1, player1.sides + 1):
vx = player1.vertices[2*n]
vy = player1.vertices[2*n + 1]
a2 = math.atan2((vy - player1.y), (vx - player1.x))
if abs(a2 - a1) < closest:
side = n
closest = abs(a2 - a1)
a3 = a2
vtx, clr = player1.removeSide(side)
#player1.x + math.cos(angle) * (player1.radius + 4), player1.y + math.sin(angle) * (player1.radius + 4)
b = bullet(angle = a1, vtx=vtx, color = clr)
game_objects.append(b)
def calc_ogp(self, params1, params2, freq):
"""
Calculates OGP values for both cores given best fit parameters
from a snap fit
"""
z_fact = -500.0/256.0
true_zero = 0.0 * z_fact
d_fact = 1e12/(2*np.pi*freq*1e6)
z1 = z_fact * params1[0][0]
sin1a = params1[0][1]
cos1a = params1[0][2]
amp1 = math.sqrt(sin1a**2 + cos1a**2)
dly1 = d_fact*math.atan2(sin1a, cos1a)
z2 = z_fact * params2[0][0]
sin2a = params2[0][1]
cos2a = params2[0][2]
amp2 = math.sqrt(sin2a**2 + cos2a**2)
dly2 = d_fact*math.atan2(sin2a, cos2a)
avz = (z1+z2)/2.0
avamp = (amp1+amp2)/2.0
a1p = 100*(avamp-amp1)/avamp
a2p = 100*(avamp-amp2)/avamp
avdly = (dly1+dly2)/2.0
ogp1 = (z1-true_zero, a1p, dly1-avdly)
ogp2 = (z2-true_zero, a2p, dly2-avdly)
return ogp1, ogp2
def angle (self, vector=None) : """angle between two vectors to go from vector to self :warning: orientation in dimension 3 is not garanteed :param vector: the second vector :type vector: NVector :return: an angle in radian :rtype: float""" if self.dim()==2 : if vector is None : vector=axis(0,dim=2) else : vector=vector.get_normalized() y=vector.cross(self)#[2] x=self.dot(vector) return math.atan2(y,x) else : if vector is None : vector=axis(0,self.dim()) else : vector=vector.get_normalized() axis_normal=vector.cross(self) if axis_normal.norm()<1e-6 : if self.dot(vector)>0 : return 0. else : return math.pi axis_proj=axis_normal.cross(vector) axis_proj.normalize() y=self.dot(axis_proj) x=self.dot(vector) return math.atan2(y,x)
def get_heading_in_degrees(self):
theta_radians = math.atan2(self.leading_point.y - self.trailing_point.y,
self.leading_point.x - self.trailing_point.x)
theta_degrees = (theta_radians + math.pi) * 360.0 / (2.0 * math.pi)
return theta_degrees
def draw_decision_boundary(w, b, col):
matplotlib.pyplot.axline(xy1=(0 - math.cos(math.atan2(w[1], w[0])) * b,
0 - math.sin(math.atan2(w[1], w[0])) * b),
xy2=(-(w[1]) - math.cos(math.atan2(w[1], w[0])) * b,
(w[0]) - math.sin(math.atan2(w[1], w[0])) * b), color=col)
def getInterpolatedArc(ptStart, ptArc, ptEnd, method, interValue):
coords = []
coords.append(ptStart)
center = CircularArc.getArcCenter(ptStart, ptArc, ptEnd)
if center == None:
coords.append(ptEnd)
g = QgsGeometry.fromPolyline(coords)
return g
cx = center.x()
cy = center.y()
px = ptArc.x()
py = ptArc.y()
r = ((cx - px) * (cx - px) + (cy - py) * (cy - py))**0.5
## If the method is "pitch" (=Pfeilhöhe) then
## we need to calculate the corresponding
## angle.
if method == "pitch":
myAlpha = 2.0 * math.acos(1.0 - (interValue / 1000) / r)
arcIncr = myAlpha
# print "myAlpha: " + str(myAlpha)
else:
arcIncr = interValue * math.pi / 180
# print "arcIncr: " + str(arcIncr)
a1 = math.atan2(ptStart.y() - center.y(), ptStart.x() - center.x())
a2 = math.atan2(ptArc.y() - center.y(), ptArc.x() - center.x())
a3 = math.atan2(ptEnd.y() - center.y(), ptEnd.x() - center.x())
# Clockwise
if a1 > a2 and a2 > a3:
sweep = a3 - a1
# Counter-clockwise
elif a1 < a2 and a2 < a3:
sweep = a3 - a1
# Clockwise, wrap
elif (a1 < a2 and a1 > a3) or (a2 < a3 and a1 > a3):
sweep = a3 - a1 + 2 * math.pi
# Counter-clockwise, wrap
elif (a1 > a2 and a1 < a3) or (a2 > a3 and a1 < a3):
sweep = a3 - a1 - 2 * math.pi
else:
sweep = 0.0
ptcount = int(math.ceil(math.fabs(sweep / arcIncr)))
if sweep < 0:
arcIncr *= -1.0
angle = a1
for i in range(0, ptcount - 1):
angle += arcIncr
if arcIncr > 0.0 and angle > math.pi:
angle -= 2 * math.pi
if arcIncr < 0.0 and angle < -1 * math.pi:
angle -= 2 * math.pi
x = cx + r * math.cos(angle)
y = cy + r * math.sin(angle)
point = QgsPoint(x, y)
coords.append(point)
# print str(point.toString())
if angle < a2 and (angle + arcIncr) > a2:
coords.append(ptArc)
if angle > a2 and (angle + arcIncr) < a2:
coords.append(ptArc)
coords.append(ptEnd)
g = QgsGeometry.fromPolyline(coords)
return g
h -= 2
print px, py
xmap = zeros([d * 2 + 1, d * 2 + 1])
for x in range(0, d * 2 + 1):
for y in range(0, d * 2 + 1):
if ((x - d) % (w * 2) == 0 or (x - d) %
(w * 2) == (w - px) * 2 - 1) and ((y - d) % (h * 2) == 0 or
(y - d) %
(h * 2) == (h - py) * 2 - 1):
xmap[y][x] = 1.0
print xmap
prevAngles = []
count = 0
for x in range(0, d * 2 + 1):
for y in range(0, d * 2 + 1):
if x != d or y != d:
dx = x - d
dy = y - d
distsquared = dx * dx + dy * dy
if xmap[y][x] == 1.0 and distsquared <= d * d:
a = math.atan2(y - d, x - d)
if not a in prevAngles:
prevAngles.append(a)
count += 1
print "Case #" + str(mastercount + 1) + ": " + str(count)
fp2.write("Case #" + str(mastercount + 1) + ": " + str(count) + "\n")
fp.close()
fp2.close()
def Ellipse_Tangent(self, alpha=0): #PointClass(0,0)
#große Halbachse, kleine Halbachse, rotation der Ellipse (rad), Winkel des Punkts in der Ellipse (rad)
phi = atan2(self.a * sin(alpha),
self.b * cos(alpha)) + self.rotation - pi / 2
return phi
def Ellipse_Grundwerte(self):
#Weitere Grundwerte der Ellipse, die nur einmal ausgerechnet werden müssen
self.rotation = atan2(self.vector.y, self.vector.x)
self.a = sqrt(self.vector.x**2 + self.vector.y**2)
self.b = self.a * self.ratio
def execute(self,agent):
agent.controller = calcController
#getting the coordinates of the goalposts
leftPost = Vector3([-sign(agent.team)*700 , 5100*-sign(agent.team), 200])
rightPost = Vector3([sign(agent.team)*700, 5100*-sign(agent.team), 200])
#center = Vector3([0, 5150*-sign(agent.team), 200])
#time stuff that we don't worry about yet
time_guess = 0 # this is set to zero, so its not really doing anything. Just assuming where the ball is right now
bloc = future(agent.ball,time_guess)
#vectors from the goalposts to the ball & to Gosling
ball_left = angle2(bloc,leftPost)
ball_right = angle2(bloc,rightPost)
agent_left = angle2(agent.me,leftPost)
agent_right = angle2(agent.me,rightPost)
#determining if we are left/right/inside of cone
if agent_left > ball_left and agent_right > ball_right:
goal_target = rightPost
elif agent_left > ball_left and agent_right < ball_right:
goal_target = None
elif agent_left < ball_left and agent_right < ball_right:
goal_target = leftPost
else:
goal_target = None
if goal_target != None:
#if we are outside the cone, this is the same as Gosling's old code
goal_to_ball = (agent.ball.location - goal_target).normalize()
goal_to_agent = (agent.me.location - goal_target).normalize()
difference = goal_to_ball - goal_to_agent
error = cap(abs(difference.data[0])+ abs(difference.data[1]),1,10)
else:
#if we are inside the cone, our line to follow is a vector from the ball to us (although it's still named 'goal_to_ball')
goal_to_ball = (agent.me.location - agent.ball.location).normalize()
error = cap( distance2D(bloc,agent.me) /1000,0,1)
#this is measuring how fast the ball is traveling away from us if we were stationary
ball_dpp_skew = cap(abs(dpp(agent.ball.location, agent.ball.velocity, agent.me.location, [0,0,0]))/80, 1,1.5)
#same as Gosling's old distance calculation, but now we consider dpp_skew which helps us handle when the ball is moving
target_distance =cap( (40 + distance2D(agent.ball.location,agent.me)* (error**2))/1.8, 0,4000)
target_location = agent.ball.location + Vector3([(goal_to_ball.data[0]*target_distance) * ball_dpp_skew, goal_to_ball.data[1]*target_distance,0])
#this also adjusts the target location based on dpp
ball_something = dpp(target_location,agent.ball.velocity, agent.me,[0,0,0])**2
if ball_something > 100: #if we were stopped, and the ball is moving 100uu/s away from us
ball_something = cap(ball_something,0,80)
correction = agent.ball.velocity.normalize()
correction = Vector3([correction.data[0]*ball_something,correction.data[1]*ball_something,correction.data[2]*ball_something])
target_location += correction #we're adding some component of the ball's velocity to the target position so that we are able to hit a faster moving ball better
#it's important that this only happens when the ball is moving away from us.
#another target adjustment that applies if the ball is close to the wall
extra = 4120 - abs(target_location.data[0])
if extra < 0:
# we prevent our target from going outside the wall, and extend it so that Gosling gets closer to the wall before taking a shot, makes things more reliable
target_location.data[0] = cap(target_location.data[0],-4120,4120)
target_location.data[1] = target_location.data[1] + (-sign(agent.team)*cap(extra,-500,500))
#getting speed, this would be a good place to modify because it's not very good
target_local = toLocal(agent.ball.location,agent.me)
angle_to_target = cap(math.atan2(target_local.data[1], target_local.data[0]),-3,3)
#distance_to_target = distance2D(agent.me, target_location)
speed= 2000 - (100*(1+angle_to_target)**2)
#picking our rendered target color based on the speed we want to go
colorRed = cap(int( (speed/2300) * 255),0,255)
colorBlue =cap(255-colorRed,0,255)
#see the rendering tutorial on github about this, just drawing lines from the posts to the ball and one from the ball to the target
agent.renderer.begin_rendering()
agent.renderer.draw_line_3d(bloc.data, leftPost.data, agent.renderer.create_color(255,255,0,0))
agent.renderer.draw_line_3d(bloc.data, rightPost.data, agent.renderer.create_color(255,0,255,0))
agent.renderer.draw_line_3d(agent.ball.location.data,target_location.data, agent.renderer.create_color(255,colorRed,0,colorBlue))
agent.renderer.draw_rect_3d(target_location.data, 10,10, True, agent.renderer.create_color(255,colorRed,0,colorBlue))
agent.renderer.end_rendering()
if ballReady(agent) == False or abs(agent.ball.location.data[1]) > 5050:
self.expired = True
return agent.controller(agent,target_location,speed)
def calc_perp(x,y,theta,pt1,pt2):
#this also needs to tell me if the xt,yt point has gone beyond pt2
skip = False
ld = 0.85
INF_ERROR = False
# ld = 0.85 #this worked best
# ld = 5
#find the equation of the line
x1,y1 = pt1
x2,y2 = pt2
try:
slope = (y2-y1)/(x2-x1)
except:
INF_ERROR = True
if not INF_ERROR:
print("[NO ERROR]")
phi = m.atan2((y2-y1),(x2-x1))
slope = (y2-y1)/(x2-x1)
intercept = y1 - slope*x1
xp = (x + slope*y - slope*intercept)/(1+slope**2)
yp = intercept + slope*xp
xt = xp + m.cos(phi)*ld
yt = yp + m.sin(phi)*ld
#checking if it has crossed pt2
check_angle = m.atan2((yt-y2),(xt-x2))
if check_angle == phi:
skip = True
xt = x2
yt = y2
#checking if the point is behind pt1
"""
MEASURES TO PREVENT WEIRD BEHAVIOUR
"""
check_angle2 = m.atan2((y1-yt),(x1-xt))
if check_angle2 == phi:
xt = x1 + m.cos(phi)*0.1
yt = y1 + m.sin(phi)*0.1
# elif abs(wrapToPi(phi-theta))>m.radians(80): #this means that the target point has cleared the pt1
# xt = x1 + m.cos(phi)*4
# yt = y1 + m.sin(phi)*4
# else:
# pass
else:
phi = m.atan2((y2-y1),(x2-x1))
print("[ZERO DIV ERROR]")
xp = x1
yp = y
sign = (y2-y1)/abs(y2-y1)
xt = xp
yt = yp + sign*1
#here also we need to check the crossing thresh
if sign>0:
if yt>y2:
skip = True
if yt<y1:
yt = y1
xt = x2
yt = y2
elif yt<y2:
skip = True
if yt>y1:
yt = y1
xt = x2
yt = y2
"""
MEASURES TO PREVENT WEIRD BEHAVIOUR
"""
check_angle2 = m.atan2((y1-yt),(x1-xt))
if check_angle2 == phi:
xt = x1 + m.cos(phi)*0.1
yt = y1 + m.sin(phi)*0.1
return(xp,yp,xt,yt,skip)
def path_track(path):
xstart, ystart = path[0]
start = [xstart,ystart,0]
goal_points = path
dummy_gp = copy.deepcopy(goal_points)
#need to calculate goal theta last two points
last_pt = dummy_gp[-1]
second_last_pt = dummy_gp[-2]
theta_g = m.atan2((last_pt[1]-second_last_pt[1]),(last_pt[0]-second_last_pt[0]))
goalx,goaly = path[-1]
goal = [goalx,goaly,theta_g]
# goal_points = [[2,2]]
x,y,theta = start
v = 1
s = 0
gp_array = np.array(goal_points)
x_traj = []
y_traj = []
skip = False
while len(dummy_gp) >1:
#first step would be to find the target point
target,proximity,skip = calc_target(x,y,theta,dummy_gp)
xt,yt = target
if proximity<0.1 or skip==True:
dummy_gp.pop(0)
if skip==True:
print(skip)
plt.cla()
plt.axis('scaled')
plt.xlim(-10,15)
plt.ylim(-10,15)
plt.plot(gp_array[:,0],gp_array[:,1],'--')
plt.plot(start[0],start[1],'co')
plt.plot(xt,yt,'ro')
if DRAW_DIFF:
draw_robot(x,y,theta)
if DRAW_PALLET:
dpj(x,y,theta,s)
x_traj.append(x)
y_traj.append(y)
plt.plot(x_traj,y_traj,'r')
x,y,theta,s = seek_one([x,y,theta],[xt,yt],goal)
# print(m.degrees(s))
plt.pause(0.0001)
if ALIGN_TO_GOAL_LINE:
pt1 = goal_points[-2]
pt2 = goal_points[-1]
while wrapToPi(abs(theta - goal[2]))>0.1:
_,_,xt,yt,_ = calc_perp(x,y,pt1,pt2)
plt.cla()
plt.axis('scaled')
plt.xlim(-5,15)
plt.ylim(-5,15)
plt.plot(gp_array[:,0],gp_array[:,1],'--')
plt.plot(start[0],start[1],'co')
plt.plot(xt,yt,'ro')
if DRAW_DIFF:
draw_robot(x,y,theta)
if DRAW_PALLET:
dpj(x,y,theta,s)
x_traj.append(x)
y_traj.append(y)
plt.plot(x_traj,y_traj,'r')
x,y,theta,s = seek_one([x,y,theta],[xt,yt],goal)
# print(m.degrees(s))
plt.pause(0.0001)
def path_track3(path,thetas):
# thetas = 0 #cancelling user defined theta
"""
SAMPLING STARTS HERE
"""
final_path = []
x,y = path[0]
sample_rate = 2 #best was 2
final_path.append([x,y])
for x,y in path:
xf,yf = final_path[-1]
if m.sqrt((xf-x)**2 + (yf-y)**2)>sample_rate:
final_path.append([x,y])
else:
continue
if path[-1] not in final_path:
final_path.append(path[-1])
"""
SAMPLING FINISHES HERE
"""
prev_path = path
path = final_path
prev_path_array = np.array(prev_path)
tic = time.time()
xstart, ystart = path[0]
start = [xstart,ystart,thetas]
goal_points = path
dummy_gp = copy.deepcopy(goal_points)
#need to calculate goal theta last two points
last_pt = dummy_gp[-1]
second_last_pt = dummy_gp[-2]
theta_g = -m.atan2((last_pt[1]-second_last_pt[1]),(last_pt[0]-second_last_pt[0]))
goalx,goaly = goal_points[-1]
goal = [goalx,goaly,theta_g]
x,y,theta = start
v = 1
s = 0
gp_array = np.array(goal_points)
x_traj = []
y_traj = []
skip = False
while m.sqrt((x - goal[0])**2 + (y - goal[1])**2)>0.2:
#first step would be to find the target point
target,proximity,skip = calc_target(x,y,theta,dummy_gp)
xt,yt = target
if (proximity<0.1 or skip==True) and len(dummy_gp)>2:
dummy_gp.pop(0)
if len(dummy_gp)==2:
print(dummy_gp)
angle = theta_g
xt = last_pt[0] + 0*m.cos(theta_g)
yt = last_pt[1] + 0*m.sin(-theta_g)
plt.plot(xt,yt,'ko')
if skip==True:
#need to set the value of target to something
target, proximity, skip = calc_target(x,y,theta,dummy_gp)
print(skip)
plt.cla()
plt.axis('scaled')
plt.xlim(-10,15)
plt.ylim(-10,15)
plt.plot(gp_array[:,0],gp_array[:,1],'m',label="Sampled-Target-path")
plt.plot(prev_path_array[:,0],prev_path_array[:,1],'c--',label="Actual-Target-path")
plt.plot(start[0],start[1],'co')
plt.plot(xt,yt,'ro')
if DRAW_DIFF:
draw_robot(x,y,theta)
if DRAW_PALLET:
dpj(x,y,theta,s)
x_traj.append(x)
y_traj.append(y)
plt.plot(x_traj,y_traj,'r',label="Actual-Path-taken")
x,y,theta,s = seek_one([x,y,theta],[xt,yt],goal)
# print(m.degrees(s))
plt.pause(0.0001)
if ALIGN_TO_GOAL_LINE:
pt1 = goal_points[-2]
pt2 = goal_points[-1]
while wrapToPi(abs(theta - goal[2]))>0.1:
_,_,xt,yt,_ = calc_perp(x,y,theta,pt1,pt2)
plt.cla()
plt.axis('scaled')
plt.xlim(-10,15)
plt.ylim(-10,15)
plt.plot(gp_array[:,0],gp_array[:,1],'m',label="Sampled-Target-path")
plt.plot(prev_path_array[:,0],prev_path_array[:,1],'c--',label="Actual-Target-path")
plt.plot(start[0],start[1],'co')
plt.plot(xt,yt,'ro')
if DRAW_DIFF:
draw_robot(x,y,theta)
if DRAW_PALLET:
dpj(x,y,theta,s)
x_traj.append(x)
y_traj.append(y)
plt.plot(x_traj,y_traj,'r',label="Actual-Path-taken")
x,y,theta,s = seek_one([x,y,theta],[xt,yt],goal)
# print(m.degrees(s))
plt.pause(0.0001)
print("Time taken: {} s".format(time.time()-tic))
plt.title('PID BASED CONSTANT SPEED PATH TRACKING OF A PALLET JACK')
plt.legend()
# plt.show()
return theta
def straighten(self):
print("Straight {}x at {:.2f} length ".format(len(self.edges),
self.length))
# Get edge angles in UV space
angles = {}
for edge in self.edges:
uv1 = self.vert_to_uv[edge.verts[0]][0].uv
uv2 = self.vert_to_uv[edge.verts[1]][0].uv
delta = uv2 - uv1
angle = math.atan2(delta.y, delta.x) % (math.pi / 2)
if angle >= (math.pi / 4):
angle = angle - (math.pi / 2)
angles[edge] = abs(angle)
# print("Angle {:.2f} degr".format(angle * 180 / math.pi))
# Pick edge with least rotation offset to U or V axis
edge_main = sorted(angles.items(), key=operator.itemgetter(1))[0][0]
print("Main edge: {} at {:.2f} degr".format(
edge_main.index, angles[edge_main] * 180 / math.pi))
# Rotate main edge to closest axis
uvs = [uv for v in edge_main.verts for uv in self.vert_to_uv[v]]
bpy.ops.uv.select_all(action='DESELECT')
for uv in uvs:
uv.select = True
uv1 = self.vert_to_uv[edge_main.verts[0]][0].uv
uv2 = self.vert_to_uv[edge_main.verts[1]][0].uv
diff = uv2 - uv1
angle = math.atan2(diff.y, diff.x) % (math.pi / 2)
if angle >= (math.pi / 4):
angle = angle - (math.pi / 2)
bpy.ops.uv.cursor_set(location=uv1 + diff / 2)
bpy.ops.transform.rotate(value=angle,
orient_axis='Z',
constraint_axis=(False, False, False),
orient_type='GLOBAL',
mirror=False,
use_proportional_edit=False)
# Expand edges and straighten
count = len(self.edges)
processed = [edge_main]
for i in range(count):
if (len(processed) < len(self.edges)):
verts = set([v for e in processed for v in e.verts])
edges_expand = [
e for e in self.edges if e not in processed and (
e.verts[0] in verts or e.verts[1] in verts)
]
verts_ends = [
v for e in edges_expand for v in e.verts if v in verts
]
print("Step, proc {} exp: {}".format(
[e.index for e in processed],
[e.index for e in edges_expand]))
if len(edges_expand) == 0:
continue
for edge in edges_expand:
# if edge.verts[0] in verts_ends and edge.verts[1] in verts_ends:
# print("Cancel at edge {}".format(edge.index))
# return
print(" E {} verts {} verts end: {}".format(
edge.index, [v.index for v in edge.verts],
[v.index for v in verts_ends]))
v1 = [v for v in edge.verts if v in verts_ends][0]
v2 = [v for v in edge.verts if v not in verts_ends][0]
# direction
previous_edge = [
e for e in processed
if e.verts[0] in edge.verts or e.verts[1] in edge.verts
][0]
prev_v1 = [v for v in previous_edge.verts if v != v1][0]
prev_v2 = [v for v in previous_edge.verts if v == v1][0]
direction = (self.vert_to_uv[prev_v2][0].uv -
self.vert_to_uv[prev_v1][0].uv).normalized()
for uv in self.vert_to_uv[v2]:
uv.uv = self.vert_to_uv[v1][0].uv + \
direction * self.edge_length[edge]
print("Procesed {}x Expand {}x".format(len(processed),
len(edges_expand)))
print("verts_ends: {}x".format(len(verts_ends)))
processed.extend(edges_expand)
# Select edges
uvs = list(
set([
uv for e in self.edges for v in e.verts
for uv in self.vert_to_uv[v]
]))
bpy.ops.uv.select_all(action='DESELECT')
for uv in uvs:
uv.select = True
# Pin UV's
bpy.ops.uv.pin()
bpy.ops.uv.unwrap(method='ANGLE_BASED', margin=0.001)
bpy.ops.uv.pin(clear=True)
def wrapToPi(theta):
return m.atan2(m.sin(theta),m.cos(theta))
def angle(point1, point2):
from math import sin, cos, sqrt, atan2, radians, degrees
delx = point2[0] - point1[0]
dely = point2[1] - point1[1]
return math.degrees(atan2(radians(delx), radians(dely)))
def calculateTrueNorthAngle(self, x, y):
return round(math.degrees(math.atan2(y, x)) - 90, 6)
def getArmBaseAng(self, finalPose):
# return self.getBestBaseAng(finalPose, math.atan2(finalPose.y, finalPose.x))
return math.atan2(finalPose.y, finalPose.x)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
setMotorSpeed(clientID, motor_handle, Vx)
setMotorPosition(clientID, steer_handle, 0)
vrep.simxSynchronousTrigger(clientID)
e = np.zeros((4,1))
while True:
_, vehicle_lin, vehicle_ang = vrep.simxGetObjectVelocity(clientID,vehicle_handle,vrep.simx_opmode_streaming)
_, vehicle_pos = vrep.simxGetObjectPosition(clientID,vehicle_handle,-1,vrep.simx_opmode_streaming)
_, vehicle_ori = vrep.simxGetObjectOrientation(clientID,vehicle_handle,-1,vrep.simx_opmode_streaming)
atan = math.atan2(vehicle_pos[1], vehicle_pos[0])
y = vehicle_pos[1]
psi = vehicle_ori[2] # in radians
ydot = (-math.sin(atan) * vehicle_lin[0] + math.cos(atan) * vehicle_lin[1])
psidot = vehicle_ang[2]
y_des = 0
psi_des =0
psidot_des = 0
e[0] = y - y_des
e[1] = ydot + Vx*(psi - psi_des)
e[2] = psi - psi_des
e[3] = psidot - psidot_des
def angle(x, y):
a = math.atan2(x, y)
return a if a >= 0 else a + 2 * math.pi
def tangentbug():
global goalVisible
global scan
goalfromlaser = rospy.Publisher('robot_1/robot2goal',Twist,queue_size=10)
robotController = rospy.Publisher('robot_1/cmd_vel',Twist,queue_size=10)
listener = tf.TransformListener()
rate = rospy.Rate(1.0)
while not rospy.is_shutdown():
try:
# trans is a list of 3 elements x,y,z of the transform. rot is a list of
# 4 elements of the quaternion for the transform
(trans,rot) = listener.lookupTransform('robot1/trueOdom','robot1/goal',rospy.Time(0))
twi = Twist()
twi.linear.x = trans[0]
twi.linear.y = trans[1]
twi.linear.z = trans[2]
#ignore angular orientations since we only care about getting to the goal position
twi.angular = Vector3(0.0,0.0,0.0)
control = Twist()
control.linear = Vector3(0.0,0.0,0.0)
control.angular = Vector3(0.0,0.0,0.0)
angle2goal = math.atan2(trans[1],trans[0])
sensorIndex = int((angle2goal-scan.angle_min)/scan.angle_increment)
# check if we can see the goal directly. Move towards it if we can
if (scan.ranges[sensorIndex] >= maxRange):
goalVisible = 1
if (angle2goal > 0):
control.linear.x = 0.3
control.angular.z = 0.2
elif (angle2goal < 0):
control.linear.x = 0.3
control.angular.z = -0.2
else:
control.linear.x = 0.3
control.angular.z = 0.0
else:
goalVisible = 0
# if we can't see the goal directly, check for the best direction of travel
if goalVisible == 0:
bestAngle = 0.0
besti = 0
bestDist = 10000.0
realAngle = 0.0
for i in range(len(scan.ranges)):
# check for discontinuties within a specified threshold
if (i>0) and (abs(scan.ranges[i]-scan.ranges[i-1]) > discThresh):
# output the index for the discontinuity and the angle value and the distance to that discontinuity
discDist = scan.ranges[i]
if discDist==float('Inf'):
discDist = scan.range_max
dAng = scan.angle_min + i * scan.angle_increment
xDist = discDist * math.sin(dAng)
yDist = discDist * math.cos(dAng)
heurDist = math.sqrt((twi.linear.x-xDist) ** 2 + (twi.linear.y-yDist) ** 2)
if ((heurDist + discDist) < bestDist):
bestDist = heurDist + discDist
bestAngle = dAng
besti = i
# drive towards the best heuristic or turn towards it if we're not facing it already
if ((bestAngle) > 0):
control.linear.x = 0.2
control.angular.z = 0.3
elif ((bestAngle) < 0):
control.linear.x = 0.2
control.angular.z = -0.3
else:
control.linear.x = 0.2
control.angular.z = 0.0
# prioritize avoiding obstacles
if (besti > 90) and (besti < (len(scan.ranges)-90)):
if scan.ranges[besti+20] < 2.0:
control.linear.x = 0.2
control.angular.z = 0.5
elif scan.ranges[besti-20] < 2.0:
control.linear.x = 0.2
control.angular.z = -0.5
elif (besti > 90) and (besti < (len(scan.ranges)-90)):
if scan.ranges[besti+30] < 2.0:
control.linear.x = 0.2
control.angular.z = 0.5
elif scan.ranges[besti-30] < 2.0:
control.linear.x = 0.2
control.angular.z = -0.5
# if obstacles are too close to the robot, prioritize avoiding them
j = int(len(scan.ranges)/2) - 70
m = int(len(scan.ranges)/2) - 10
k = int(len(scan.ranges)/2) + 70
n = int(len(scan.ranges)/2) + 10
for i in range(j,m):
if (scan.ranges[i] < 2.5):
control.linear.x = 0.0
control.angular.z = 0.5
for i in range(n,k):
if (scan.ranges[i] < 2.5):
control.linear.x = 0.0
control.angular.z = -0.5
# stop moving if we're close enough to the goal
if math.sqrt((twi.linear.x) ** 2 + (twi.linear.y) ** 2) < 0.5:
control.linear.x = 0.0
control.linear.y = 0.0
robotController.publish(control)
goalfromlaser.publish(twi)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
rate.sleep()
def corner_to_center_box3d(boxes_corner, coordinate='camera', calib_mat=None):
# (N, 8, 3) -> (N, 7) x,y,z,h,w,l,ry/z
if coordinate == 'lidar':
for idx in range(len(boxes_corner)):
boxes_corner[idx] = lidar_to_camera_point(boxes_corner[idx], calib_mat=calib_mat)
ret = []
for roi in boxes_corner:
if cfg.CORNER2CENTER_AVG: # average version
roi = np.array(roi)
h = abs(np.sum(roi[:4, 1] - roi[4:, 1]) / 4)
w = np.sum(
np.sqrt(np.sum((roi[0, [0, 2]] - roi[3, [0, 2]])**2)) +
np.sqrt(np.sum((roi[1, [0, 2]] - roi[2, [0, 2]])**2)) +
np.sqrt(np.sum((roi[4, [0, 2]] - roi[7, [0, 2]])**2)) +
np.sqrt(np.sum((roi[5, [0, 2]] - roi[6, [0, 2]])**2))
) / 4
l = np.sum(
np.sqrt(np.sum((roi[0, [0, 2]] - roi[1, [0, 2]])**2)) +
np.sqrt(np.sum((roi[2, [0, 2]] - roi[3, [0, 2]])**2)) +
np.sqrt(np.sum((roi[4, [0, 2]] - roi[5, [0, 2]])**2)) +
np.sqrt(np.sum((roi[6, [0, 2]] - roi[7, [0, 2]])**2))
) / 4
x, y, z = np.sum(roi, axis=0) / 8
y = y + h/2
# warn("x {} y {} z {}".format(x, y, z))
ry = np.sum(
math.atan2(roi[2, 0] - roi[1, 0], roi[2, 2] - roi[1, 2]) +
math.atan2(roi[6, 0] - roi[5, 0], roi[6, 2] - roi[5, 2]) +
math.atan2(roi[3, 0] - roi[0, 0], roi[3, 2] - roi[0, 2]) +
math.atan2(roi[7, 0] - roi[4, 0], roi[7, 2] - roi[4, 2]) +
math.atan2(roi[0, 2] - roi[1, 2], roi[1, 0] - roi[0, 0]) +
math.atan2(roi[4, 2] - roi[5, 2], roi[5, 0] - roi[4, 0]) +
math.atan2(roi[3, 2] - roi[2, 2], roi[2, 0] - roi[3, 0]) +
math.atan2(roi[7, 2] - roi[6, 2], roi[6, 0] - roi[7, 0])
) / 8
if w > l:
w, l = l, w
ry = angle_in_limit(ry + np.pi / 2)
else: # max version
h = max(abs(roi[:4, 1] - roi[4:, 1]))
w = np.max(
np.sqrt(np.sum((roi[0, [0, 2]] - roi[3, [0, 2]])**2)) +
np.sqrt(np.sum((roi[1, [0, 2]] - roi[2, [0, 2]])**2)) +
np.sqrt(np.sum((roi[4, [0, 2]] - roi[7, [0, 2]])**2)) +
np.sqrt(np.sum((roi[5, [0, 2]] - roi[6, [0, 2]])**2))
)
l = np.max(
np.sqrt(np.sum((roi[0, [0, 2]] - roi[1, [0, 2]])**2)) +
np.sqrt(np.sum((roi[2, [0, 2]] - roi[3, [0, 2]])**2)) +
np.sqrt(np.sum((roi[4, [0, 2]] - roi[5, [0, 2]])**2)) +
np.sqrt(np.sum((roi[6, [0, 2]] - roi[7, [0, 2]])**2))
)
x, y, z = np.sum(roi, axis=0) / 8
y = y + h/2
ry = np.sum(
math.atan2(roi[2, 0] - roi[1, 0], roi[2, 2] - roi[1, 2]) +
math.atan2(roi[6, 0] - roi[5, 0], roi[6, 2] - roi[5, 2]) +
math.atan2(roi[3, 0] - roi[0, 0], roi[3, 2] - roi[0, 2]) +
math.atan2(roi[7, 0] - roi[4, 0], roi[7, 2] - roi[4, 2]) +
math.atan2(roi[0, 2] - roi[1, 2], roi[1, 0] - roi[0, 0]) +
math.atan2(roi[4, 2] - roi[5, 2], roi[5, 0] - roi[4, 0]) +
math.atan2(roi[3, 2] - roi[2, 2], roi[2, 0] - roi[3, 0]) +
math.atan2(roi[7, 2] - roi[6, 2], roi[6, 0] - roi[7, 0])
) / 8
if w > l:
w, l = l, w
ry = angle_in_limit(ry + np.pi / 2)
ret.append([x, y, z, h, w, l, ry])
# warn("z: {}".format(x))
if coordinate == 'lidar':
ret = camera_to_lidar_box(np.array(ret), calib_mat=calib_mat)
return np.array(ret)
def observation_model(xVeh, iFeature, Map):
Delta = Map[0:2, iFeature:(iFeature+1)]-xVeh[0:2]
z = np.array([[np.linalg.norm(Delta)], [
atan2(Delta[1], Delta[0]) - xVeh[2, 0]]])
z[1, 0] = angle_wrap(z[1, 0])
return z
def _is_hit_constrain(self, q_now, q_near):
result = 0
counter = 1
theta = atan2(q_near[1] - q_now[1], q_near[0] - q_now[0])
_c = cos(theta)
_s = sin(theta)
t = np.zeros(16)
s = np.zeros(16)
while counter <= self.obstacle_list.size:
for obs_index in range(1, self.obstacle_list.size):
bound = [
self.obstacle_list[obs_index].x -
self.obstacle_list[obs_index].r / 2,
self.obstacle_list[obs_index].x +
self.obstacle_list[obs_index].r / 2,
self.obstacle_list[obs_index].y -
self.obstacle_list[obs_index].r / 2,
self.obstacle_list[obs_index].y +
self.obstacle_list[obs_index].r / 2
]
t[0] = (q_near[0] - bound[0] + self.car_length / 2 * _c) / _s
s[0] = (q_near[1] - bound[2] + t[0] * _c +
self.car_length / 2 * _s) / (bound[3] - bound[2])
t[1] = (q_near[0] - bound[1] + self.car_length / 2 * _c) / _s
s[1] = (q_near[1] - bound[2] + t[1] * _c +
self.car_length / 2 * _s) / (bound[3] - bound[2])
t[2] = (bound[2] - q_near[1] - self.car_length / 2 * _s) / _c
s[2] = (q_near[0] - bound[2] - t[2] * _s +
self.car_length / 2 * _c) / (bound[1] - bound[0])
t[3] = (bound[3] - q_near[1] - self.car_length / 2 * _s) / _c
s[3] = (q_near[0] - bound[0] - t[3] * _s +
self.car_length / 2 * _c) / (bound[1] - bound[0])
t[4] = (q_near[0] - bound[0] - self.car_length / 2 * _c) / _s
s[4] = (q_near[1] - bound[2] + t[4] * _c -
self.car_length / 2 * _s) / (bound[3] - bound[2])
t[5] = (q_near[0] - bound[1] - self.car_length / 2 * _c) / _s
s[5] = (q_near[1] - bound[2] + t[5] * _c * self.car_length /
2 * _s) / (bound[3] - bound[2])
t[6] = (bound[2] - q_near[1] + self.car_length / 2 * _s) / _c
s[6] = (q_near[0] - bound[0] - t[6] * _s -
self.car_length / 2 * _c) / (bound[1] - bound[0])
t[7] = (bound[3] - q_near[1] + self.car_length / 2 * _s) / _c
s[7] = (q_near[0] - bound[0] - t[7] * _s -
self.car_length / 2 * _c) / (bound[1] - bound[0])
t[8] = (bound[0] - q_near[0] + self.car_length / 2 * _s) / _c
s[8] = (q_near[1] - bound[2] + t[8] * _s +
self.car_length / 2 * _c) / (bound[3] - bound[2])
t[9] = (bound[1] - q_near[0] + self.car_length / 2 * _s) / _c
s[9] = (q_near[1] - bound[2] + t[9] * _s -
self.car_length / 2 * _c) / (bound[3] - bound[2])
t[10] = (bound[2] - q_near[1] - self.car_length / 2 * _c) / _s
s[10] = (q_near[0] - bound[0] + t[10] * _c -
self.car_length / 2 * _s) / (bound[1] - bound[0])
t[11] = (bound[3] - q_near[1] - self.car_length / 2 * _c) / _s
s[11] = (q_near[0] - bound[0] + t[11] * _c -
self.car_length / 2 * _s) / (bound[1] - bound[0])
t[12] = (bound[0] - q_near[0] - self.car_length / 2 * _s) / _c
s[12] = (q_near[1] - bound[2] + t[12] * _s -
self.car_length / 2 * _c) / (bound[3] - bound[2])
t[13] = (bound[1] - q_near[0] - self.car_length / 2 * _s) / _c
s[13] = (q_near[1] - bound[2] + t[13] * _s -
self.car_length / 2 * _c) / (bound[3] - bound[2])
t[14] = (bound[2] - q_near[1] + self.car_length / 2 * _c) / _s
s[14] = (q_near[0] - bound[0] + t[14] * _c -
self.car_length / 2 * _s) / (bound[1] - bound[0])
t[15] = (bound[3] - q_near[1] + self.car_length / 2 * _c) / _s
s[15] = (q_near[0] - bound[0] + t[15] * _c +
self.car_length / 2 * _s) / (bound[1] - bound[0])
for parameter_checker in range(0, 15):
if (t[parameter_checker] >= -self.car_length / 2
and t[parameter_checker] <= self.car_length / 2
and s[parameter_checker] >= 0
and s[parameter_checker] <= 1):
result = 1
# end if
# end for (parameter_checker)
# end for (obs_index)
counter = counter + 1
#end while (counter)
return result
def get_path_position_orientation(pose, my_path, timestamp, sync):
if pose:
# Print some of the pose data to the terminal
data = pose.get_pose_data()
w_r = data.rotation.w
x_r = -data.rotation.z
y_r = data.rotation.x
z_r = -data.rotation.y
pitch = -math.asin(2.0 * (x_r*z_r - w_r*y_r)) * 180.0 / math.pi;
roll = math.atan2(2.0 * (w_r*x_r + y_r*z_r), w_r*w_r - x_r*x_r - y_r*y_r + z_r*z_r) * 180.0 / math.pi
yaw = math.atan2(2.0 * (w_r*z_r + x_r*y_r), w_r*w_r + x_r*x_r - y_r*y_r - z_r*z_r) * 180.0 / math.pi
#create path
pose = PoseStamped()
pose.pose.position.x = -data.translation.z
pose.pose.position.z = data.translation.y
pose.pose.position.y = -data.translation.x
pose.pose.orientation.x = pitch
pose.pose.orientation.z = -yaw
pose.pose.orientation.y = roll
pose.pose.orientation.w = 1
pose.header.frame_id = 'camera'
if sync:
#print("trajectory timestamp: ",timestamp)
t1 = (timestamp / 100000000)
t2 = (t1 - int(t1)) * 100000
#t1 = timestamp / 1000.0
time = rospy.Time(secs=int(t2), nsecs = int((t2 - int(t2))*100))
#print("trajectory time: ",time)
my_path.poses.append(pose)
position = [pose.pose.position.x, pose.pose.position.y, pose.pose.position.z]
orientation = [pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z]
odom = Odometry()
odom.header.frame_id = 'camera'
if sync:
odom.header.stamp = time
odom.pose.pose.position.x = data.translation.x
odom.pose.pose.position.z = data.translation.y
odom.pose.pose.position.y = data.translation.z
odom.pose.pose.orientation.x = -x_r
odom.pose.pose.orientation.z = z_r
odom.pose.pose.orientation.y = y_r
odom.pose.pose.orientation.w = w_r
odom.pose.covariance = np.diag([1e-1, 1e-1, 1e-1, 1e-2, 1e-2, 1e-2]).ravel()
odom.twist.twist.linear.x = data.velocity.x
odom.twist.twist.linear.z = data.velocity.y
odom.twist.twist.linear.y = -data.velocity.z
odom.twist.twist.angular.x = data.acceleration.x
odom.twist.twist.angular.y = data.acceleration.y
odom.twist.twist.angular.z = data.acceleration.z
odom.twist.covariance = np.diag([1e-1, 1e-1, 1e-1, 1e-2, 1e-2, 1e-2]).ravel()
return my_path, position, orientation, odom
def plot(self):
"""Opens a window, draws the graph into the window.
Requires Tk, and of course a windowing system.
"""
import Tkinter as tk
import math
window = tk.Tk()
canvas_size = 400
drawing = tk.Canvas(window, height=canvas_size, width=canvas_size, background="white")
n_nodes = self.n_nodes
radius = 150
node_radius = 10
drawing.create_text(200, 10, text="Network:" + str(id(self)))
list_of_coordinates = [(radius * math.sin(2 * math.pi * n / n_nodes) + canvas_size / 2, radius * math.cos(2 * math.pi * n / n_nodes) + canvas_size / 2) for n in range(n_nodes)]
for linksto, node in enumerate(self.adjacency):
for linksfrom, link in enumerate(node):
if linksto == linksfrom and link == True:
angle = math.atan2(list_of_coordinates[linksto][1] - 200,
list_of_coordinates[linksto][0] - 200)
drawing.create_line(list_of_coordinates[linksto][0] + node_radius * math.cos(angle),
list_of_coordinates[linksto][1] + node_radius * math.sin(angle),
list_of_coordinates[linksto][0] + node_radius * 2 * (math.cos(angle + 20)),
list_of_coordinates[linksto][1] + node_radius * 2 * math.sin(angle + 20),
list_of_coordinates[linksto][0] + node_radius * 4 * (math.cos(angle)),
list_of_coordinates[linksto][1] + node_radius * 4 * math.sin(angle),
list_of_coordinates[linksto][0] + node_radius * 2 * math.cos(angle - 20),
list_of_coordinates[linksto][1] + node_radius * 2 * (math.sin(angle - 20)),
list_of_coordinates[linksto][0] + node_radius * math.cos(angle),
list_of_coordinates[linksto][1] + node_radius * math.sin(angle),
smooth=True, joinstyle="round", fill="black", width=2, arrow="last"
)
elif link == True:
angle = math.atan2(list_of_coordinates[linksto][1] - list_of_coordinates[linksfrom][1],
list_of_coordinates[linksto][0] - list_of_coordinates[linksfrom][0])
drawing.create_line(list_of_coordinates[linksfrom][0] + node_radius * math.cos(angle),
list_of_coordinates[linksfrom][1] + node_radius * math.sin(angle),
list_of_coordinates[linksto][0] - node_radius * math.cos(angle),
list_of_coordinates[linksto][1] - node_radius * math.sin(angle),
fill="black", width=2, arrow="last")
for node_ctr, (x, y) in enumerate(list_of_coordinates):
if type(self) != Network:
node_color = "white"
text_color = "black"
elif self.state == num.array([[]]):
node_color = "white"
text_color = "black"
else:
if self.state[-1][node_ctr] == True:
node_color = "black"
text_color = "white"
else:
node_color = "white"
text_color = "black"
drawing.create_oval(x - node_radius, y - node_radius, x + node_radius, y + node_radius, width=2, fill=node_color)
drawing.create_text(x, y, text=str(node_ctr), fill=text_color, font="Arial")
drawing.pack()
window.mainloop()
T_B_L_door = velma.getTf("B", "right_door")
ldX,ldY,ldZ = T_B_L_door.p
ldYaw,ldPitch,ldRoll = T_B_L_door.M.GetEulerZYX()
Rot = PyKDL.Rotation.RotZ(ldYaw + math.pi)
tempX = ldX + math.sin(ldYaw) * 0.2835 + math.cos(ldYaw) * (0.3 + step)
tempY = ldY - math.cos(ldYaw) * 0.2835 + math.sin(ldYaw) * (0.3 + step)
Trans = PyKDL.Vector(tempX,tempY,ldZ + 0.7)
dest_cab = PyKDL.Frame(Rot,Trans)
# VELMA IS READY FOR MOVING
print("Preparing velma for the task (torso and right hand)")
T_B_Cab = velma.getTf("B", "cabinet")
cabX,cabY,cabZ = T_B_Cab.p
cab_angle = math.atan2(cabY,cabX)
if cab_angle < math.pi/2 and cab_angle > -math.pi/2 :
q_map_start['torso_0_joint'] = cab_angle
elif cab_angle > math.pi/2 :
q_map_start['torso_0_joint'] = math.pi/2 - 0.03
else :
q_map_start['torso_0_joint'] = -math.pi/2 + 0.03
planAndExecute(q_map_start)
# UNTESTED NEEDS MORE TESTS
print("moving hand to cabinet")
T_B_L_door = velma.getTf("B", "right_door")
ldX,ldY,ldZ = T_B_L_door.p
# Step 2.2 Calculate Exponential factor from intrinsic impedance
ej = cmath.exp(-2*thickness*dj);
# Step 2.3 Calculate reflection coeficient using current layer
# intrinsic impedance and the below layer impedance
belowImpedance = impedances[j + 1];
rj = (wj - belowImpedance)/(wj + belowImpedance);
re = rj*ej;
Zj = wj * ((1 - re)/(1 + re));
impedances[j] = Zj;
# Step 3. Compute apparent resistivity from top layer impedance
Z = impedances[0];
absZ = abs(Z);
apparentResistivity.append((absZ * absZ)/(mu * w))
phase.append(math.atan2(Z.imag, Z.real))
print('');
print('time taken = ', time.clock() - start, 's');
# Plot the results in 2 axes. Phase (degree) and apparent resistivity (ohm meter)
fig, ax = plt.subplots(2, 1, figsize=(6, 4))
ax[0].loglog(frequencies,apparentResistivity,'r-o'),
ax[0].set_ylabel("$\\rho_a \ (\Omega m)$", fontsize=12)
ax[1].semilogx(frequencies,np.array(phase)*180/np.pi,'b-o'),
ax[1].set_ylabel("$\phi \ (^{\circ})$", fontsize=12)
ax[1].set_ylim([0., 90.])
def __getitem__(self, index):
"""
Get the pixels of an image, and a random synthetic scene graph for that
image constructed on-the-fly from its COCO object annotations. We assume
that the image will have height H, width W, C channels; there will be O
object annotations, each of which will have both a bounding box and a
segmentation mask of shape (M, M). There will be T triples in the scene
graph.
Returns a tuple of:
- image: FloatTensor of shape (C, H, W)
- objs: LongTensor of shape (O,)
- boxes: FloatTensor of shape (O, 4) giving boxes for objects in
(x0, y0, x1, y1) format, in a [0, 1] coordinate system
- masks: LongTensor of shape (O, M, M) giving segmentation masks for
objects, where 0 is background and 1 is object.
- triples: LongTensor of shape (T, 3) where triples[t] = [i, p, j]
means that (objs[i], p, objs[j]) is a triple.
"""
image_id = self.image_ids[index]
abs_image = []
filename = self.image_id_to_filename[image_id]
image_path = os.path.join(self.image_dir, filename)
with open(image_path, 'rb') as f:
with PIL.Image.open(f) as image:
# unscaled image
abs_image = np.array(image)
WW, HH = image.size
image = self.transform(image.convert('RGB'))
H, W = self.image_size
objs, boxes, masks = [], [], []
# absolute masks (unscaled)
# can later convert masks into self.image_size
# scale if scaling factor is known (s = self.image_size/HH, etc)
abs_boxes, abs_masks = [], []
obj_contours = []
for object_data in self.image_id_to_objects[image_id]:
objs.append(object_data['category_id'])
x, y, w, h = object_data['bbox']
# Normalized coordinates, preserves aspect ratio
x0 = x / WW
y0 = y / HH
x1 = (x + w) / WW
y1 = (y + h) / HH
boxes.append(torch.FloatTensor([x0, y0, x1, y1]))
# unscaled boxes in true image coords (e.g side len = 4 => coords [0,3]
abs_boxes.append(torch.FloatTensor([x, y, x + w - 1, y + h - 1]))
# This will give a numpy array of shape (HH, WW)
mask = seg_to_mask(object_data['segmentation'], WW, HH)
# Crop the mask according to the bounding box, being careful to
# ensure that we don't crop a zero-area region
mx0, mx1 = int(round(x)), int(round(x + w))
my0, my1 = int(round(y)), int(round(y + h))
mx1 = max(mx0 + 1, mx1)
my1 = max(my0 + 1, my1)
mask = mask[my0:my1, mx0:mx1]
# unscaled mask
abs_mask = mask
# masks resized to 64x64
mask = imresize(255.0 * mask, (self.mask_size, self.mask_size),
mode='constant', anti_aliasing=True)
mask = torch.from_numpy((mask > 128).astype(np.int64))
# unscaled/resized mask
abs_mask = torch.from_numpy((abs_mask > 0).astype(np.int64))
masks.append(mask)
# add unscaled/resized mask
abs_masks.append(abs_mask)
# Extract contours for each scaled mask
max_contour_pts = 25
pad = 1
obj_contour = None
# Add padding to capture edge contours
mask_pad = np.pad(mask, ((pad,pad),(pad,pad)), 'constant')
obj_contour = measure.find_contours(mask_pad, 0.99)
obj_contour = np.concatenate(obj_contour, axis=0) - pad # remove padding
subsample = int(np.ceil(len(obj_contour)/max_contour_pts))
obj_contour = obj_contour[0::subsample] # min val = 1
if len(obj_contour) < max_contour_pts:
num_pad = max_contour_pts - len(obj_contour)
obj_contour = np.concatenate([obj_contour, np.zeros((num_pad, 2))], axis=0)
# points visualized in (y,x) order
orig_contour = np.copy(obj_contour)
# contour points are in (y,x) order
obj_contour[:,[0, 1]] = obj_contour[:,[1, 0]]
# normalize to size of object mask (which is self.mask_size)
obj_contour[:,0] = obj_contour[:,0]/self.mask_size # x
obj_contour[:,1] = obj_contour[:,1]/self.mask_size # y
obj_contours.append(torch.FloatTensor(obj_contour.flatten()))
import matplotlib.pyplot as plt
fig, ax = plt.subplots()
ax.imshow(mask)
print(h, w, triplet_mask.shape)
ax.scatter(orig_contour[:,1],orig_contour[:,0], linewidth=0.5)
plt.show()
pdb.set_trace()
# Add dummy __image__ object
objs.append(self.vocab['object_name_to_idx']['__image__'])
boxes.append(torch.FloatTensor([0, 0, 1, 1]))
masks.append(torch.ones(self.mask_size, self.mask_size).long())
# unscaled box/mask for singleton case: [x0, y0, x1, y1]
abs_boxes.append(torch.FloatTensor([0, 0, WW, HH]))
abs_masks.append(torch.ones(HH, WW).long())
# add empty contours with all zeros
obj_contours.append(torch.FloatTensor(np.zeros((max_contour_pts, 2)).flatten()))
objs = torch.LongTensor(objs)
# merge a list of Tensors into one Tensor
boxes = torch.stack(boxes, dim=0)
masks = torch.stack(masks, dim=0)
abs_boxes = torch.stack(abs_boxes, dim=0)
obj_contours = torch.stack(obj_contours, dim=0)
box_areas = (boxes[:, 2] - boxes[:, 0]) * (boxes[:, 3] - boxes[:, 1])
# Compute centers of all objects
obj_centers = []
_, MH, MW = masks.size()
for i, obj_idx in enumerate(objs):
x0, y0, x1, y1 = boxes[i]
mask = (masks[i] == 1)
xs = torch.linspace(x0, x1, MW).view(1, MW).expand(MH, MW)
ys = torch.linspace(y0, y1, MH).view(MH, 1).expand(MH, MW)
if mask.sum() == 0:
mean_x = 0.5 * (x0 + x1)
mean_y = 0.5 * (y0 + y1)
else:
mean_x = xs[mask].mean()
mean_y = ys[mask].mean()
obj_centers.append([mean_x, mean_y])
obj_centers = torch.FloatTensor(obj_centers)
# Add triples
triples = []
num_objs = objs.size(0)
__image__ = self.vocab['object_name_to_idx']['__image__']
real_objs = []
if num_objs > 1:
real_objs = (objs != __image__).nonzero().squeeze(1)
for cur in real_objs:
choices = [obj for obj in real_objs if obj != cur]
if len(choices) == 0 or not self.include_relationships:
break
# by default, randomize
###############
if self.seed != 0:
random.seed(self.seed)
#################
other = random.choice(choices)
##################
if self.seed != 0:
random.seed(self.seed)
##################
if random.random() > 0.5:
s, o = cur, other
else:
s, o = other, cur
# Check for inside / surrounding
sx0, sy0, sx1, sy1 = boxes[s]
ox0, oy0, ox1, oy1 = boxes[o]
d = obj_centers[s] - obj_centers[o]
theta = math.atan2(d[1], d[0])
if sx0 < ox0 and sx1 > ox1 and sy0 < oy0 and sy1 > oy1:
p = 'surrounding'
elif sx0 > ox0 and sx1 < ox1 and sy0 > oy0 and sy1 < oy1:
p = 'inside'
elif theta >= 3 * math.pi / 4 or theta <= -3 * math.pi / 4:
p = 'left of'
elif -3 * math.pi / 4 <= theta < -math.pi / 4:
p = 'above'
elif -math.pi / 4 <= theta < math.pi / 4:
p = 'right of'
elif math.pi / 4 <= theta < 3 * math.pi / 4:
p = 'below'
# add heuristics here
p = self.vocab['pred_name_to_idx'][p]
triples.append([s, p, o])
#########################################################
# front_back_flag = False
# s_obj_type = self.vocab['object_idx_to_name'][s]
# o_obj_type = self.vocab['object_idx_to_name'][o]
# if s_obj_type in self.instance_whitelist and o_obj_type in self.instance_whitelist:
# if sy1 > oy1 and oy1 > sy0 and ox1 > sx0 and ox0 < sx1:
# q = 'infront of'
# front_back_flag = True
# elif sy1 < oy1 and oy1 < sy0 and ox1 < sx0 and ox0 > sx1:
# q = 'behind'
# front_back_flag = True
# if front_back_flag:
# q = self.vocab['pred_name_to_idx'][q]
# triples.append([s, q, o])
#########################################################
# Add __in_image__ triples
O = objs.size(0)
in_image = self.vocab['pred_name_to_idx']['__in_image__']
for i in range(O - 1):
triples.append([i, in_image, O - 1])
triples = torch.LongTensor(triples)
# get bounding boxes for triples
num_triples = triples.size(0)
s, p, o = triples.chunk(3, dim=1)
s_boxes, o_boxes = abs_boxes[s], abs_boxes[o]
triplet_boxes = torch.cat([torch.squeeze(s_boxes), torch.squeeze(o_boxes)], dim=1)
triplet_masks = []
triplet_contours = []
superboxes = []
#print(triples)
# masks will all be resized to fixed size (32x32)
for n in range(0, num_triples):
s_idx, o_idx = s[n], o[n] # index into object-related arrays
s_mask = abs_masks[s_idx] # unscaled subject mask
o_mask = abs_masks[o_idx] # unscaled object mask
# find dimensions of encapsulating triplet superbox
min_x = np.min([triplet_boxes[n][0], triplet_boxes[n][4]])
min_y = np.min([triplet_boxes[n][1], triplet_boxes[n][5]])
max_x = np.max([triplet_boxes[n][2], triplet_boxes[n][6]])
max_y = np.max([triplet_boxes[n][3], triplet_boxes[n][7]])
superboxes.append([min_x, min_y, max_x, max_y])
#print('----------------------------------')
#print(superboxes[n])
min_x, min_y = int(round(min_x)), int(round(min_y))
max_x, max_y = int(round(max_x)), int(round(max_y))
h = max_y - min_y + 1
w = max_x - min_x + 1
#print([min_x, min_y, max_x, max_y])
#print('[', 0, 0, w-1, h-1, ']')
# create empty mask the size of superbox
self.triplet_mask_size = 32
triplet_mask = np.zeros((h, w))
# superbox shift offset
dx, dy = min_x, min_y
#print('dx, dy: ', dx, dy)
# indices must be integers
bbs = np.array(np.round(triplet_boxes[n])).astype(int)
#print("subj:", bbs[:4], "[", bbs[0] - dx, bbs[1] - dy, bbs[2] - dx, bbs[3] - dy, "]") #
#print("obj:", bbs[4:], "[", bbs[4] - dx, bbs[5] - dy, bbs[6] - dx, bbs[7] - dy, "]") #
#print('size: ', s_mask.shape)
# python array indexing: (0,640) is RHS non-inclusive
# subject mask
mask_h, mask_w = s_mask.shape[0], s_mask.shape[1]
x0, y0 = bbs[0] - dx, bbs[1] - dy
x0, y0 = max(x0, 0), max(y0, 0)
x1, y1 = x0 + mask_w, y0 + mask_h # this should be ok: w = 5, [0:5]
x1, y1 = min(x1, w), min(y1, h)
assert triplet_mask[y0:y1, x0:x1].shape == s_mask[0:y1 - y0, 0:x1 - x0].shape, print('s_mask mismatch shape: ', triplet_mask[y0:y1, x0:x1].shape, s_mask[0:y1 - y0, 0:x1 - x0].shape)
# implicite: label subject points as value 1
triplet_mask[y0:y1, x0:x1] = s_mask[0:y1 - y0, 0:x1 - x0]
# resize
triplet_mask = imresize(255.0 * triplet_mask, (self.triplet_mask_size, self.triplet_mask_size),
mode='constant', anti_aliasing=True)
triplet_mask = (triplet_mask > 128).astype(np.int64)
#pdb.set_trace()
# contour points for subject
max_contour_pts = 50
pad = 10
s_contours = None
s_triplet_mask_pad = np.pad(triplet_mask, ((pad,pad),(pad,pad)), 'constant')
# problem: sometimes small contours are downsampled away
s_contours = measure.find_contours(s_triplet_mask_pad, 0.99)
# in rare case object gets subsampled away
if len(s_contours) < 1:
s_contours = np.zeros((max_contour_pts, 2)) # if this is concatenated, becomes 1x100D
else:
s_contours = np.concatenate(s_contours, axis=0) - pad # remove padding
subsample = int(np.ceil(len(s_contours)/max_contour_pts))
s_contours = s_contours[0::subsample] # min val = 1
if len(s_contours) < max_contour_pts:
num_pad = max_contour_pts - len(s_contours)
s_contours = np.concatenate([s_contours, np.zeros((num_pad, 2))], axis=0)
#s_ch = ConvexHull(s_contours)
#pdb.set_trace()
# need to deal with case for singleton triples
#if(self.vocab['object_idx_to_name'][objs[o[n]]] == '__image__'):
#import matplotlib.pyplot as plt
#print('--------------------------------')
#pdb.set_trace()
#plt.imshow(triplet_mask)
#plt.imshow(s_mask)
#plt.show()
#plt.imshow(triplet_image)
#plt.show()
#else:
o_contours = None
if(self.vocab['object_idx_to_name'][objs[o[n]]] != '__image__'):
# object mask
mask_h, mask_w = o_mask.shape[0], o_mask.shape[1]
x0, y0 = bbs[4] - dx, bbs[5] - dy
x0, y0 = max(x0, 0), max(y0, 0)
x1, y1 = x0 + mask_w, y0 + mask_h
x1, y1 = min(x1, w), min(y1, h)
#assert triplet_mask[y0:y1, x0:x1].shape == o_mask[0:y1 - y0, 0:x1 - x0].shape, print('mismatch shape: ', triplet_mask[y0:y1, x0:x1].shape, o_mask[0:y1 - y0, 0:x1 - x0].shape)
o_triplet_mask = np.zeros((h, w))
o_triplet_mask[y0:y1, x0:x1] = o_mask[0:y1 - y0, 0:x1 - x0]
o_triplet_mask = imresize(255.0 * o_triplet_mask, (self.triplet_mask_size, self.triplet_mask_size),
mode='constant', anti_aliasing=True)
o_triplet_mask = (o_triplet_mask > 128).astype(np.int64)
# OR triplet masks to deal with areas of overlap
triplet_mask = np.logical_or(triplet_mask, o_triplet_mask).astype(np.int64)
# label object pixel value 2
triplet_mask += o_triplet_mask
# contour points for object
#o_triplet_mask = np.zeros((h, w))
#o_triplet_mask[y0:y1, x0:x1] = o_mask[0:y1 - y0, 0:x1 - x0]
o_triplet_mask_pad = np.pad(o_triplet_mask, ((pad,pad),(pad,pad)), 'constant')
# get contours
o_contours = measure.find_contours(o_triplet_mask_pad, 0.99)
# in rare case object gets subsampled away
if len(o_contours) < 1:
o_contours = np.zeros((max_contour_pts, 2))
else:
o_contours = np.concatenate(o_contours, axis=0) - pad # removed padding
subsample = int(np.ceil(len(o_contours)/max_contour_pts))
o_contours = o_contours[0::subsample] # min val = 1
if len(o_contours) < max_contour_pts:
num_pad = max_contour_pts - len(o_contours)
o_contours = np.concatenate([o_contours, np.zeros((num_pad, 2))], axis=0)
if s_contours is not None and o_contours is not None:
assert s_contours.shape == o_contours.shape, pdb.set_trace()
contours = np.concatenate([s_contours, o_contours], axis=0)
elif s_contours is not None: # singleton (o_contours is None)
assert s_contours.shape == np.zeros((max_contour_pts, 2)).shape, pdb.set_trace()
contours = np.concatenate([s_contours, np.zeros((max_contour_pts, 2))], axis=0)
# just in case
if len(contours) > 2*max_contour_pts:
contours = contours[0:2*max_contour_pts]
elif len(contours) < 2*max_contour_pts:
num_pad = max_contour_pts - len(contours)
contours = np.concatenate([contours, np.zeros((num_pad, 2))], axis=0)
# points visualized in (y,x) order
orig_contours = np.copy(contours)
# contour points are (y,x)
contours[:,[0, 1]] = contours[:,[1, 0]]
# normalize to size of padded triplet mask
contours[:,0] = contours[:,0]/self.triplet_mask_size # w, x
contours[:,1] = contours[:,1]/self.triplet_mask_size # h, y
triplet_contours.append(torch.FloatTensor(contours.flatten()))
triplet_mask = torch.from_numpy(triplet_mask)
triplet_masks.append(triplet_mask)
#import matplotlib.pyplot as plt
#fig, ax = plt.subplots()
#ax.imshow(triplet_mask)
#print(h, w, triplet_mask.shape)
#ax.scatter(orig_contours[:,1],orig_contours[:,0], linewidth=0.5)
#ax.scatter(s_contours[:,1],s_contours[:,0], linewidth=0.5)
#if o_contours is not None:
# ax.scatter(o_contours[:,1],o_contours[:,0], linewidth=0.5)
#plt.show()
# merge multiple tensors into one
triplet_masks = torch.stack(triplet_masks, dim=0)
triplet_contours = torch.stack(triplet_contours, dim=0)
#print('----- end processing of image --------')
# this gets passed to coco_collate
return image, objs, boxes, masks, triples, triplet_masks, triplet_contours
def curve3_recursive_bezier( points, x1, y1, x2, y2, x3, y3, level = 0 ):
if level > curve_recursion_limit:
return
# Calculate all the mid-points of the line segments
# -------------------------------------------------
x12 = (x1 + x2) / 2.
y12 = (y1 + y2) / 2.
x23 = (x2 + x3) / 2.
y23 = (y2 + y3) / 2.
x123 = (x12 + x23) / 2.
y123 = (y12 + y23) / 2.
dx = x3 - x1
dy = y3 - y1
d = math.fabs((x2-x3)*dy - (y2-y3)*dx)
if d > curve_collinearity_epsilon:
# Regular case
# ------------
if d*d <= m_distance_tolerance_square * (dx*dx + dy*dy):
# If the curvature doesn't exceed the distance_tolerance value
# we tend to finish subdivisions.
if m_angle_tolerance < curve_angle_tolerance_epsilon:
points.append( (x123,y123) )
return
# Angle & Cusp Condition
da = math.fabs(math.atan2(y3 - y2, x3 - x2) - math.atan2(y2 - y1, x2 - x1))
if da >= math.pi:
da = 2*math.pi - da
if da < m_angle_tolerance:
# Finally we can stop the recursion
points.append( (x123,y123) )
return
else:
# Collinear case
# --------------
da = dx*dx + dy*dy
if da == 0:
d = calc_sq_distance(x1, y1, x2, y2)
else:
d = ((x2 - x1)*dx + (y2 - y1)*dy) / da
if d > 0 and d < 1:
# Simple collinear case, 1---2---3, we can leave just two endpoints
return
if(d <= 0):
d = calc_sq_distance(x2, y2, x1, y1)
elif d >= 1:
d = calc_sq_distance(x2, y2, x3, y3)
else:
d = calc_sq_distance(x2, y2, x1 + d*dx, y1 + d*dy)
if d < m_distance_tolerance_square:
points.append( (x2,y2) )
return
# Continue subdivision
# --------------------
curve3_recursive_bezier( points, x1, y1, x12, y12, x123, y123, level + 1 )
curve3_recursive_bezier( points, x123, y123, x23, y23, x3, y3, level + 1 )
def angle_of_vector(point1, point2):
rad = math.atan2(point2[1] - point1[1], point2[0] - point1[0])
return math.degrees(rad)
def curve4_recursive_bezier( points, x1, y1, x2, y2, x3, y3, x4, y4, level=0):
if level > curve_recursion_limit:
return
# Calculate all the mid-points of the line segments
# -------------------------------------------------
x12 = (x1 + x2) / 2.
y12 = (y1 + y2) / 2.
x23 = (x2 + x3) / 2.
y23 = (y2 + y3) / 2.
x34 = (x3 + x4) / 2.
y34 = (y3 + y4) / 2.
x123 = (x12 + x23) / 2.
y123 = (y12 + y23) / 2.
x234 = (x23 + x34) / 2.
y234 = (y23 + y34) / 2.
x1234 = (x123 + x234) / 2.
y1234 = (y123 + y234) / 2.
# Try to approximate the full cubic curve by a single straight line
# -----------------------------------------------------------------
dx = x4 - x1
dy = y4 - y1
d2 = math.fabs(((x2 - x4) * dy - (y2 - y4) * dx))
d3 = math.fabs(((x3 - x4) * dy - (y3 - y4) * dx))
s = int((d2 > curve_collinearity_epsilon) << 1) + int(d3 > curve_collinearity_epsilon)
if s == 0:
# All collinear OR p1==p4
# ----------------------
k = dx*dx + dy*dy
if k == 0:
d2 = calc_sq_distance(x1, y1, x2, y2)
d3 = calc_sq_distance(x4, y4, x3, y3)
else:
k = 1. / k
da1 = x2 - x1
da2 = y2 - y1
d2 = k * (da1*dx + da2*dy)
da1 = x3 - x1
da2 = y3 - y1
d3 = k * (da1*dx + da2*dy)
if d2 > 0 and d2 < 1 and d3 > 0 and d3 < 1:
# Simple collinear case, 1---2---3---4
# We can leave just two endpoints
return
if d2 <= 0:
d2 = calc_sq_distance(x2, y2, x1, y1)
elif d2 >= 1:
d2 = calc_sq_distance(x2, y2, x4, y4)
else:
d2 = calc_sq_distance(x2, y2, x1 + d2*dx, y1 + d2*dy)
if d3 <= 0:
d3 = calc_sq_distance(x3, y3, x1, y1)
elif d3 >= 1:
d3 = calc_sq_distance(x3, y3, x4, y4)
else:
d3 = calc_sq_distance(x3, y3, x1 + d3*dx, y1 + d3*dy)
if d2 > d3:
if d2 < m_distance_tolerance_square:
points.append( (x2, y2) )
return
else:
if d3 < m_distance_tolerance_square:
points.append( (x3, y3) )
return
elif s == 1:
# p1,p2,p4 are collinear, p3 is significant
# -----------------------------------------
if d3 * d3 <= m_distance_tolerance_square * (dx*dx + dy*dy):
if m_angle_tolerance < curve_angle_tolerance_epsilon:
points.append((x23, y23) )
return
# Angle Condition
# ---------------
da1 = math.fabs(math.atan2(y4 - y3, x4 - x3) - math.atan2(y3 - y2, x3 - x2))
if da1 >= math.pi:
da1 = 2*math.pi - da1
if da1 < m_angle_tolerance:
points.extend( [(x2, y2),(x3, y3)] )
return
if m_cusp_limit != 0.0:
if da1 > m_cusp_limit:
points.append( (x3, y3) )
return
elif s == 2:
# p1,p3,p4 are collinear, p2 is significant
# -----------------------------------------
if d2 * d2 <= m_distance_tolerance_square * (dx*dx + dy*dy):
if m_angle_tolerance < curve_angle_tolerance_epsilon:
points.append( (x23, y23) )
return
# Angle Condition
# ---------------
da1 = math.fabs(math.atan2(y3 - y2, x3 - x2) - math.atan2(y2 - y1, x2 - x1))
if da1 >= math.pi:
da1 = 2*math.pi - da1
if da1 < m_angle_tolerance:
points.extend( [(x2, y2),(x3, y3)] )
return
if m_cusp_limit != 0.0:
if da1 > m_cusp_limit:
points.append( (x2, y2) )
return
elif s == 3:
# Regular case
# ------------
if (d2 + d3)*(d2 + d3) <= m_distance_tolerance_square * (dx*dx + dy*dy):
# If the curvature doesn't exceed the distance_tolerance value
# we tend to finish subdivisions.
if m_angle_tolerance < curve_angle_tolerance_epsilon:
points.append( (x23, y23) )
return
# Angle & Cusp Condition
# ----------------------
k = math.atan2(y3 - y2, x3 - x2)
da1 = math.fabs(k - math.atan2(y2 - y1, x2 - x1))
da2 = math.fabs(math.atan2(y4 - y3, x4 - x3) - k)
if da1 >= math.pi:
da1 = 2*math.pi - da1
if da2 >= math.pi:
da2 = 2*math.pi - da2
if da1 + da2 < m_angle_tolerance:
# Finally we can stop the recursion
# ---------------------------------
points.append( (x23, y23) )
return
if m_cusp_limit != 0.0:
if da1 > m_cusp_limit:
points.append( (x2, y2) )
return
if da2 > m_cusp_limit:
points.append( (x3, y3) )
return
# Continue subdivision
# --------------------
curve4_recursive_bezier( points, x1, y1, x12, y12, x123, y123, x1234, y1234, level + 1 )
curve4_recursive_bezier( points, x1234, y1234, x234, y234, x34, y34, x4, y4, level + 1 )
def ecliptic_to_angle(ecliptic):
return math.atan2(ecliptic[1], ecliptic[0])