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