def move():
"""
x = tur.xcor() + v0 *0.1* cos(alpha) * t
y = tur.ycor() + v0 * sin(alpha) * t - (g * t ** 2) / 2
:return:
"""
g = 9.8
v0 = 25
alpha = 67 * 3.14 / 180
t = 0
i = 0
while i != 2:
h_max = (v0**2 * sin(alpha)**2) / (2 * g)
print('h_max = ', h_max)
while t != 22.5:
t += 0.1
x = tur.xcor() + v0 * 0.2 * cos(alpha) * t
y = tur.ycor() + v0 * 0.8 * sin(alpha) * t - (g * t**2) / 2
print('UP', x, y)
tur.goto(x, y)
tur.stamp()
if tur.ycor() <= 0:
break
break
print('x = ', c(tur.xcor()), 'y = ', c(tur.ycor()))
tur.goto(x, y)
t += 0.5
i += 1
def rtolatlong(r_p, planet):
# From PCPF to LLA through Bowring's method https://www.mathworks.com/help/aeroblks/ecefpositiontolla.html;jsessionid=2ae36964c7d5f2115d2c21286db0?nocookie=true
x_p = r_p[0]
y_p = r_p[1]
z_p = r_p[2]
f = (planet.Rp_e - planet.Rp_p) / planet.Rp_e # flattening
e = (1 - (1 - f)**2) # ellipticity (NOTE = considered as square)
r = (x_p**2 + y_p**2)**0.5
# Calculate initial guesses for reduced latitude (latr) and planet-detic latitude (latd)
latr = at(z_p / ((1 - f) * r)) # reduced latitude
latd = at((z_p + (e * (1 - f) * planet.Rp_e * s(latr)**3) / (1 - e)) /
(r - e * planet.Rp_e * c(latr)**3))
# Recalculate reduced latitude based on planet-detic latitude
latr2 = at((1 - f) * s(latd) / c(latd))
diff = latr - latr2
# Iterate until reduced latitude converges
while diff > 1e-10:
latr = latr2
latd = at((z_p + (e * (1 - f) * planet.Rp_e * s(latr)**3) / (1 - e)) /
(r - e * planet.Rp_e * c(latr)**3))
latr2 = at((1 - f) * s(latd) / c(latd))
diff = latr - latr2
lat = latd
#Calculate longitude
lon = atan2(y_p, x_p) # -180<lon<180
#Calculate altitude
N = planet.Rp_e / (
1 - e * s(lat)**2)**0.5 # radius of curvature in the vertical prime
alt = r * c(lat) + (z_p + e * N * s(lat)) * s(lat) - N
return [alt, lat, lon]
def make_mass_matrix(robot):
"""
:param robot:
:return: mass matrix
"""
I, m, l, r = robot.unpack()
theta_1 = robot.q[0]
theta_2 = robot.q[1]
theta_3 = robot.q[2]
M_11 = I[1][1] * s(theta_2) ** 2 + I[2][1] * s(theta_2 + theta_3) ** 2 + \
I[0][2] + I[1][2] * c(theta_2) ** 2 + \
I[1][2] * c(theta_2 + theta_3) ** 2
M_12 = 0
M_13 = 0
M_21 = 0
M_22 = I[1][0] + I[2][0] + m[2] * l[0] ** 2 + \
m[1] * r[0] ** 2 + m[1] ** 2 + \
2 * m[2] * l[0] * r[1] * c(theta_3)
M_23 = I[2][0] + m[2] * r[1] ** 2 + \
m[2] * l[0] * r[1] * c(theta_3)
M_31 = 0
M_32 = I[2][0] + m[2] * r[1] ** 2 + \
m[2] * l[1] * r[1] * c(theta_3)
M_33 = I[2][0] + m[2] * r[1]**2
M = np.matrix([[M_11, M_12, M_13], [M_21, M_22, M_23], [M_31, M_32, M_33]])
return M
def r_pintor_i(r_p, v_p, planet, t, t_prev):
# From PCPF (planet centered/planet fixed) to PCI (planet centered inertial)
rot_angle = -np.linalg.norm(planet.omega) * (t + t_prev) # rad
L_pi = [[c(rot_angle), s(rot_angle), 0], [-s(rot_angle),
c(rot_angle), 0], [0, 0, 1]]
r_i = np.inner((L_pi), r_p)
v_i = np.inner((L_pi), (v_p + np.cross(planet.omega, r_p)))
return r_I, v_I
def alfadeltartor(R_RA_DEC):
# From Geocentric Celestial Reference Frame (GCRF) to PCI (Planet Centered Inertial)
R = R_RA_DEC[0]
RA = R_RA_DEC[1]
DEC = R_RA_DEC[2]
x = R * c(DEC) * c(RA)
y = R * c(DEC) * s(RA)
z = R * s(DEC)
return [x, y, z]
def r_intor_p(r_i, v_i, planet, t, t_prev):
# From PCI (planet centered inertial) to PCPF (planet centered/planet fixed)
rot_angle = np.linalg.norm(planet.omega) * (t + t_prev) # rad
# print(t,rot_angle)
L_pi = [[c(rot_angle), s(rot_angle), 0], [-s(rot_angle),
c(rot_angle), 0], [0, 0, 1]]
r_p = np.inner(L_pi, r_i)
#V_pf_pci = v_i - np.cross(planet.omega, r_i); #planet relative velocity in PCI frame
v_p = np.inner(L_pi, (v_i - np.cross(planet.omega, r_i)))
return r_p, v_p
def make_coriolis_matrix(robot):
"""sin
sin
:param: robot
:return: coriolis matrix
"""
I, m, l, r = robot.unpack
theta_1 = robot.q[0]
theta_2 = robot.q[1]
theta_3 = robot.q[2]
theta_23 = theta_2 + theta_3
C = np.zeros(shape=(3,3))
gamma_001 = 0.5*( I[1][2] - I[2][2] - m[1]*r[1]**2)*s(2*theta_2) + 0.5*( I[2][1] - I[2][2] )*s(2*theta_23) \
-m[2]*(l[1]*c(theta_2) + r[2]*c(theta_23))*(l[1]*s(theta_2) + r[2] *s(theta_23))
gamma_002 = 0.5*(I[2][1] - I[2][2])*c(2*theta_23) -m[2]*r[2]*s(theta_23)*(l[1] *c(theta_2) + r[2]*c(theta_23))
gamma_010 = 0.5*(I[1][1] - I[1][2] - m[1]*r[1]**2 )*c(2*theta_2) + 0.5*( I[2][1] - I[2][2])*c(2*theta_2) \
- m[2]*( l[1]*c(theta_2) + r[2]*c(theta_23) )*( l[1]*s(theta_2) + r[2]*s(theta_23) )
gamma_020 = 0.5*( I[2][1] - I[2][2] )*s(2*theta_23) - m[2]*r[2]*s(theta_23)*( l[1]*c(theta_2) + r[2]*c(theta_23))
gamma_100 = 0.5*(I[1][2] - I[1][1] + m[1]*r[1]**2)*s(2*theta_2) + 0.5*(I[2][2] - I[2][1])*s(2*theta_23) + \
m[2]*( l[1]*c(theta_2) + r[2]*c(theta_23) )*(l[1]*s(theta_2) + r[2]*s(theta_23) )
gamma_112 = -l[1]*m[2]*r[2]*s(theta_3)
gamma_121 = gamma_112
gamma_122 = gamma_112
gamma_200 = 0.5*(I[2][2] - I[2][1]) + m[1]*r[2]*s(theta_23)*(l[1]*c(theta_2) + r[2]*c(theta_23))
gamma_211 = l[1]*m[2]*r[2]*s(theta_3)
C[0,0] = gamma_001 + gamma_002
C[0, 1] = gamma_010
C[0, 2] = gamma_020
C[1,0] = gamma_100
C[1, 1] = gamma_112
C[1, 2] = gamma_121 + gamma_122
C[2,0] = gamma_200
C[2,1] = gamma_211
return np.asmatrix(C)
def latlongtoNED(H_LAN_LON):
lon = H_LAN_LON[2]
lat = H_LAN_LON[1]
# Compute first in xyz coordinates(z: north pole, x - z plane: contains r, y: completes right - handed set)
uDxyz = [-c(lat), 0, -s(lat)]
uNxyz = [-s(lat), 0, c(lat)]
uExyz = [0, 1, 0]
# Rotate by longitude to change to PCPF frame
L3 = [[c(lon), -s(lon), 0], [s(lon), c(lon), 0], [0, 0, 1]]
uN = np.inner(L3, uNxyz)
uE = np.inner(L3, uExyz)
uD = np.inner(L3, uDxyz)
return [uD, uN, uE]
def poseFromOdom(X0, Y0, THETA0, v, v_var, om, om_var):
X = []
Y = []
THETA = []
X.append(X0)
Y.append(Y0)
THETA.append(THETA0)
dt = 0.1
for i in range(1, len(v)):
nv = np.random.normal(0, v_var)
nom = np.random.normal(0, om_var)
x = X[i - 1] + dt * c(THETA[i - 1]) * (v[i] + nv)
y = Y[i - 1] + dt * s(THETA[i - 1]) * (v[i] + nv)
theta = THETA[i - 1] + dt * (om[i] + nom)
X.append(x)
Y.append(y)
THETA.append(theta)
X = np.asarray(X)
Y = np.asarray(Y)
THETA = np.asarray(THETA)
return (X, Y, THETA)
def rtoalfadeltar(r):
# From PCI (Planet Centered Inertial) to Geocentric Celestial Reference Frame (GCRF)
#Conversion between x,y,z and right ascension (RA), declination (dec), distance from the center of the planet (r)
x = r[0]
y = r[1]
z = r[2]
r = (x**2 + y**2 + z**2)**(0.5)
l, m, n = x / r, y / r, z / r
dec = asin(n)
if m > 0:
RA = ac(l / c(dec))
else:
RA = 2 * np.pi - ac(l / c(dec))
return [r, RA, dec]
def latlongtor(LATLONGH, planet, alfa_g0, t,
t0): #maybe need to add t_prev orbit
#From Geodetic to PCI
phi = LATLONGH[0] #geodetic latitude
lam = LATLONGH[1] #longitude
h = LATLONGH[2] #altitude
a = planet.Rp_e
b = planet.Rp_p
e = (1 - b**2 / a**2)**0.5
alfa = lam + alfa_g0 + planet.omega[2] * (t - t0)
const = a / (1 - e**2 * s(phi)**2) + h
x = const * c(phi) * c(alfa)
y = const * c(phi) * s(alfa)
z = const * s(phi)
return [x, y, z]
def get_r_hat(in_blocks, out_blocks):
from math import ceil as c
inb = in_blocks
r_hat = tuple([
inb.shape[i] * (c(out_blocks.shape[i] / in_blocks.shape[i]))
for i in range(in_blocks.ndim)
])
array = in_blocks.array
message = 'Cannot find r hat'
assert (all(array.shape[i] % r_hat[i] == 0 for i in (0, 1, 2))), message
return r_hat
def solution(n, stations, w):
d = []
a = 0
for i in range(1, len(stations)):
d.append(stations[i] - w - 1 - stations[i - 1] - w)
d.append(stations[0] - w - 1)
d.append(n - stations[-1] - w)
for i in d:
if i <= 0: continue
a += c(i / (2 * w + 1))
return a
def update_states(self, ft, tx, ty, tz):
roll_ddot = ((self.Iy - self.Iz) /
self.Ix) * (self.pitch_dot * self.yaw_dot) + tx / self.Ix
pitch_ddot = ((self.Iz - self.Ix) /
self.Iy) * (self.roll_dot * self.yaw_dot) + ty / self.Iy
yaw_ddot = ((self.Ix - self.Iy) /
self.Iz) * (self.roll_dot * self.pitch_dot) + tz / self.Iz
x_ddot = -(ft / self.m) * (s(self.roll) * s(self.yaw) +
c(self.roll) * c(self.yaw) * s(self.pitch))
y_ddot = -(ft / self.m) * (c(self.roll) * s(self.yaw) * s(self.pitch) -
c(self.yaw) * s(self.roll))
z_ddot = -1 * (self.g - (ft / self.m) * (c(self.roll) * c(self.pitch)))
self.roll_dot += roll_ddot * DT
self.roll += self.roll_dot * DT
self.pitch_dot += pitch_ddot * DT
self.pitch += self.pitch_dot * DT
self.yaw_dot += yaw_ddot * DT
self.yaw += self.yaw_dot * DT
self.x_dot += x_ddot * DT
self.x += self.x_dot * DT
self.y_dot += y_ddot * DT
self.y += self.y_dot * DT
self.z_dot += z_ddot * DT
self.z += self.z_dot * DT
self.states = np.array([
self.roll, self.pitch, self.yaw, self.roll_dot, self.pitch_dot,
self.yaw_dot, self.x_dot, self.y_dot, self.z_dot, self.x, self.y,
self.z
]).T
self.record_states()
def get_jacobian_matricies(robot):
"""
:param robot:
:return:
"""
I, m, l, r = robot.unpack()
theta_1 = robot.q[0]
theta_2 = robot.q[1]
theta_3 = robot.q[2]
J_1 = np.zeros(shape=(6, 3))
J_1[5, 0] = 1
J_2 = np.matrix([[-r[0] * c(theta_2), 0, 0], [0, 0, 0], [0, -r[0], 0],
[0, -1, 0], [-s(theta_2), 0, 0], [c(theta_2), 0, 0]])
J_3 = np.matrix([[l[1] * c(theta_2) - r[1] * c(theta_2 + theta_3), 0, 0],
[0, l[0] * s(theta_1), 0],
[0, -l[1] - l[0] * c(theta_3), -r[1]], [0, -1, -1],
[-s(theta_2 + theta_3), 0, 0],
[c(theta_2 + theta_3), 0, 0]])
return (J_1, J_2, J_3)
def from_dh(dh_params):
d, theta, a, alpha = dh_params
return Frame(
np.array([[
c(theta), -s(theta) * c(alpha),
s(theta) * s(alpha), a * c(theta)
],
[
s(theta),
c(theta) * c(alpha), -c(theta) * s(alpha),
a * s(theta)
], [0., s(alpha), c(alpha), d], [0., 0., 0., 1.]]))
def make_gravity_matrix(robot):
"""
:param robot:
:return: gravity matix
"""
I, m, l, r = robot.unpack()
theta_1 = robot.q[0]
theta_2 = robot.q[1]
gravity = 9.81
G_1 = 0
G_2 = -(m[1] * r[0] + m[2] * l[0]) * gravity * c(theta_2) \
- m[2] * r[1] * c(theta_2 + theta_3)
G_3 = -m[2] * gravity * r[1] * c(theta_2 + theta_3)
G = np.matrix([[G_1], [G_2], [G_3]])
return G
def from_dh_modified(dh_params):
d, theta, a, alpha = dh_params
return Frame(
np.array([[c(theta), -s(theta), 0, a],
[
s(theta) * c(alpha),
c(theta) * c(alpha), -s(alpha), -s(alpha) * d
],
[
s(theta) * s(alpha),
c(theta) * s(alpha),
c(alpha),
c(alpha) * d
], [0., 0., 0., 1.]]))
def main():
n, x = map(int, input().split())
a = list(map(int, input().split()))
a.sort()
i = n - 1
r = 0
while n > 0:
# for i in range(n-1,0,-1):
nd = c(x / a[i])
i -= nd
n -= nd
r += 1
if nd == 1:
n -= 1
i -= 1
print(r)
def T(i, j):
theta_i = math.radians(dh_table[i][3])
a_j = dh_table[j][1]
alpha_j = math.radians(dh_table[j][0])
d_i = dh_table[i][2]
return np.array([[c(theta_i), -s(theta_i), 0, a_j],
[
c(alpha_j) * s(theta_i),
c(alpha_j) * c(theta_i), -s(alpha_j),
-d_i * s(alpha_j)
],
[
s(alpha_j) * s(theta_i),
s(alpha_j) * c(theta_i),
c(alpha_j), d_i * c(alpha_j)
], [0, 0, 0, 1]])
def fk(robot):
"""
:param robot:
:return:
"""
I, m, l, r = robot.unpack()
theta_1 = robot.q[0]
theta_2 = robot.q[1]
theta_3 = robot.q[2]
pose_1 = (0, 0, l[0])
pose_2 = (l[1] * c(theta_2) * c(theta_1), l[1] * c(theta_2) * c(theta_1),
l[0] + l[2] * s(theta_2))
pose_3 = (
( l[1]*c(theta_2) + l[2]*c(theta_2 + theta_3) )*c(theta_1), \
( l[1]*c(theta_2) + l[2]*c(theta_2 + theta_3))*s(theta_1), \
( l[0] + l[1]*s(theta_1) + l[2]*s(theta_2+theta_3))
)
return (pose_1, pose_2, pose_3)
def ik(robot, pose):
"""
:param robot:
:param pose:
:return:
"""
I, m, l, r = robot.unpack()
x = pose[0]
y = pose[1]
z = pose[2]
theta_1 = math.atan2(y, z)
theta_3 = -math.acos(
(x * x + y * y + (z - l[0])**2 - l[1] * l[1] - l[2] * [2]) /
(2 * l[1] * [2])) - 0.5 * math.pi
theta_2 = math.atan2(z - l[0], math.sqrt(x * x, y * y)) - math.atan2(
l[2] * s(theta_3), l[1] + l[2] * c(theta_3))
return (theta_1, theta_2, theta_3)
def fk(joints):
"""
:param robot:
:return:
"""
l = helper.get_lengths()
theta_1 = joints[0]
theta_2 = joints[1]
theta_3 = joints[2]
pose_1 = (0,0,l[0])
pose_2 = ( l[1]*c(theta_2)*c(theta_1), l[1]*c(theta_2)*s(theta_1), l[0] + l[2]*s(theta_2) )
pose_3 = (
( l[1]*c(theta_2) + l[2]*c(theta_2 + theta_3) )*c(theta_1), \
( l[1]*c(theta_2) + l[2]*c(theta_2 + theta_3))*s(theta_1), \
( l[0] + l[1]*s(theta_2) + l[2]*s(theta_2+theta_3))
)
return pose_1, pose_2, pose_3
def century(year):
return c(year/100)
from math import sin as s, cos as c, log as ln x = 30 v = (s(x) + c(x) - ln(x)) print(v)
def extrensic_matrix_IC(params: dict):
"""
Return the extrensic matrix for converting from camera
to world coordinates.
"""
from math import sin as s
from math import cos as c
from math import radians
y = radians(params["yaw"]) # yaw [radians]
p = radians(params["pitch"]) # pitch [radians]
r = radians(params["roll"]) # roll [radians]
x_ext = params["x_t"] # Translation x [meters]
y_ext = params["y_t"] # Translation y [meters]
z_ext = params["z_t"] # Translation z [meters]
# Rotation from camera to vehicle
R_CV = np.array([[
c(r) * c(p),
c(r) * s(p) * s(y) - s(r) * c(y),
c(r) * s(p) * c(y) + s(r) * s(y)
],
[
s(r) * c(p),
s(r) * s(p) * s(y) + c(r) * c(y),
s(r) * s(p) * c(y) - c(y) * s(r)
], [-s(p), c(p) * s(y), c(p) * c(y)]])
# Translation from camera to vehicle
t_CV = np.array([[x_ext], [y_ext], [z_ext]])
return np.hstack((R_CV, t_CV))
from math import sqrt as s, pow as p, ceil as c, floor, pi, e
x = s(26)
print("Floor value " +
str(floor(x))) #floor is the lower value while rounding figure
print("Ceil value " +
str(c(x))) #ceil is the upper value while rounding figure
print("Power of " + str(p(2, 31)))
print("Pi value " + str(pi))
print("e value " + str(e))
import math as m
x = m.sqrt(26)
print("Floor value " +
str(m.floor(x))) #floor is the lower value while rounding figure
print("Ceil value " +
str(m.ceil(x))) #ceil is the upper value while rounding figure
print("Power of " + str(m.pow(2, 31)))
print("Pi value " + str(m.pi))
print("e value " + str(m.e))
from jolly.map import LocationMask as FiringArc
from math import floor as f
from math import ceil as c
_tests = {
'RF' : lambda x, y: x >= 0 and y + f(x / 2) <= 0,
'LF' : lambda x, y: x <= 0 and y + f(-x / 2) <= 0,
'R' : lambda x, y: x >= 0 and y + f(x / 2) >= 0 and y - c(x / 2) <= 0,
'L' : lambda x, y: x <= 0 and y + f(-x / 2) >= 0 and y - c(-x / 2) <= 0,
'RR' : lambda x, y: x >= 0 and y - c(x / 2) >= 0,
'LR' : lambda x, y: x <= 0 and y - c(-x / 2) >= 0,
'FH' : lambda x, y: y <= 0,
'RH' : lambda x, y: x % 2 == 0 and y >= 0 or y > 0,
'RP' : lambda x, y: (y + f(x / 2) * 3 + (0, 2)[x % 2]) <= 0,
'LP' : lambda x, y: (y + f(-x / 2) * 3 + (0, 2)[x % 2]) <= 0,
'RPR' : lambda x, y: (y + f(-x / 2) * 3 + (0, 2)[x % 2]) >= 0,
'LPR' : lambda x, y: (y + f(x / 2) * 3 + (0, 2)[x % 2]) >= 0,
'ALL' : lambda x, y: True }
del f
del c
RF = FiringArc('RF', (_tests['RF'],))
LF = FiringArc('LF', (_tests['LF'],))
R = FiringArc('R', (_tests['R'],))
L = FiringArc('L', (_tests['L'],))
RR = FiringArc('RR', (_tests['RR'],))
LR = FiringArc('LR', (_tests['LR'],))
FH = FiringArc('FH', (_tests['FH'],))
RH = FiringArc('RH', (_tests['RH'],))
RP = FiringArc('RP', (_tests['RP'],))
from math import floor as f, ceil as c, sqrt
myNum = -44
myNumStr = str(myNum)
print("This is a string number " + myNumStr)
# print("This is a non string number " + myNum)
print("Absolute:\t" + str(abs(myNum)))
print("Power:\t\t" + str(pow(myNum, 2)))
print("Max:\t\t" + str(max(1, 3, 5, 6)))
print("Round\t\t" + str(round(4.49)))
print("Floor\t\t" + str(f(4.99)))
print("Ceil\t\t" + str(c(4.01)))
print("Sqrt\t\t" + str(sqrt(144)))
inNumber = input("Enter a number\n")
print("The number you entered is " + str(inNumber))
from math import ceil as c
n, q = map(int, input().split())
l = [int(i) for i in input().split()]
for i in range(q):
a, b = map(int, input().split())
t = (b * (b + 1)) / 2
a = a - 1
r = (a * (a + 1)) / 2
#print(t,a)
t = t - r
o = b - a
#print(o)
print(c(t // o))
def transform_matrix(px, py, pz, r, p, y):
"""given pose coordinates px,py,pz and euler angles
for roll, pitch, yaw, return the 4*4 homogenous
matrix representing that transformation from base"""
# this is just the matrix representing translation by px, py, pz
# then rotation around z, y, x in that order (yaw, pitch, roll)
transform = np.asarray([[
c(y) * c(p),
c(y) * s(p) * s(r) - s(y) * c(r),
c(y) * s(p) * c(r) + s(y) * s(r), px
],
[
s(y) * c(p),
s(y) * s(p) * s(r) + c(y) * c(r),
s(y) * s(p) * c(r) - c(y) * s(r), py
], [-s(p), c(p) * s(r),
c(p) * c(r), pz], [0, 0, 0, 1]])
# aren't we glad we made it compact?
return transform
import math as m # m ist neu der Name des mathematischen Module v = m.sin(m.pi) print v from math import log as ln v = ln(5) print v from math import sin as s, cos as c, log as ln x = m.pi v = s(x)*c(x) + ln(x) print v
