def vonmisesvariate(self, mu, kappa): """Circular data distribution. mu is the mean angle, expressed in radians between 0 and 2*pi, and kappa is the concentration parameter, which must be greater than or equal to zero. If kappa is equal to zero, this distribution reduces to a uniform random angle over the range 0 to 2*pi. """ random = self.random if kappa <= 1e-06: return TWOPI * random() a = 1.0 + _sqrt(1.0 + 4.0 * kappa * kappa) b = (a - _sqrt(2.0 * a)) / (2.0 * kappa) r = (1.0 + b * b) / (2.0 * b) while 1: u1 = random() z = _cos(_pi * u1) f = (1.0 + r * z) / (r + z) c = kappa * (r - f) u2 = random() if u2 < c * (2.0 - c) or u2 <= c * _exp(1.0 - c): break u3 = random() if u3 > 0.5: theta = mu % TWOPI + _acos(f) else: theta = mu % TWOPI - _acos(f) return theta
def vonmisesvariate(self, mu, kappa): """Circular data distribution. mu is the mean angle, expressed in radians between 0 and 2*pi, and kappa is the concentration parameter, which must be greater than or equal to zero. If kappa is equal to zero, this distribution reduces to a uniform random angle over the range 0 to 2*pi. """ random = self.random if kappa <= 1e-06: return TWOPI * random() s = 0.5 / kappa r = s + _sqrt(1.0 + s * s) while 1: u1 = random() z = _cos(_pi * u1) d = z / (r + z) u2 = random() if u2 < 1.0 - d * d or u2 <= (1.0 - d) * _exp(d): break q = 1.0 / r f = (q + z) / (1.0 + q * z) u3 = random() if u3 > 0.5: theta = (mu + _acos(f)) % TWOPI else: theta = (mu - _acos(f)) % TWOPI return theta
def vonmisesvariate(self, mu, kappa): random = self.random if kappa <= 9.9999999999999995e-007: return TWOPI * random() a = 1.0 + _sqrt(1.0 + 4.0 * kappa * kappa) b = (a - _sqrt(2.0 * a)) / 2.0 * kappa r = (1.0 + b * b) / 2.0 * b while 1: u1 = random() z = _cos(_pi * u1) f = (1.0 + r * z) / (r + z) c = kappa * (r - f) u2 = random() if u2 >= c * (2.0 - c): pass if not (u2 > c * _exp(1.0 - c)): break u3 = random() if u3 > 0.5: theta = mu % TWOPI + _acos(f) else: theta = mu % TWOPI - _acos(f) return theta
def vonmisesvariate(self, mu, kappa): """Circular data distribution. mu is the mean angle, expressed in radians between 0 and 2*pi, and kappa is the concentration parameter, which must be greater than or equal to zero. If kappa is equal to zero, this distribution reduces to a uniform random angle over the range 0 to 2*pi. """ # mu: mean angle (in radians between 0 and 2*pi) # kappa: concentration parameter kappa (>= 0) # if kappa = 0 generate uniform random angle # Based upon an algorithm published in: Fisher, N.I., # "Statistical Analysis of Circular Data", Cambridge # University Press, 1993. # Thanks to Magnus Kessler for a correction to the # implementation of step 4. random = self.random if kappa <= 1e-6: return TWOPI * random() a = 1.0 + _sqrt(1.0 + 4.0 * kappa * kappa) b = (a - _sqrt(2.0 * a)) / (2.0 * kappa) r = (1.0 + b * b) / (2.0 * b) while 1: u1 = random() z = _cos(_pi * u1) f = (1.0 + r * z) / (r + z) c = kappa * (r - f) u2 = random() if not (u2 >= c * (2.0 - c) and u2 > c * _exp(1.0 - c)): break u3 = random() if u3 > 0.5: theta = (mu % TWOPI) + _acos(f) else: theta = (mu % TWOPI) - _acos(f) return theta
def vonmisesvariate(self, mu: float, kappa: float) -> float: """Circular data distribution. mu is the mean angle, expressed in radians between 0 and 2*pi, and kappa is the concentration parameter, which must be greater than or equal to zero. If kappa is equal to zero, this distribution reduces to a uniform random angle over the range 0 to 2*pi. """ # mu: mean angle (in radians between 0 and 2*pi) # kappa: concentration parameter kappa (>= 0) # if kappa = 0 generate uniform random angle # Based upon an algorithm published in: Fisher, N.I., # "Statistical Analysis of Circular Data", Cambridge # University Press, 1993. # Thanks to Magnus Kessler for a correction to the # implementation of step 4. random = self.random if kappa <= 1e-6: return TWOPI * random() a = 1.0 + _sqrt(1.0 + 4.0 * kappa * kappa) b = (a - _sqrt(2.0 * a)) / (2.0 * kappa) r = (1.0 + b * b) / (2.0 * b) while 1: u1 = random() z = _cos(_pi * u1) f = (1.0 + r * z) / (r + z) c = kappa * (r - f) u2 = random() if u2 < c * (2.0 - c) or u2 <= c * _exp(1.0 - c): break u3 = random() if u3 > 0.5: theta = (mu % TWOPI) + _acos(f) else: theta = (mu % TWOPI) - _acos(f) return theta
def vonmisesvariate(self, mu, kappa): """Circular data distribution. mu is the mean angle, expressed in radians between 0 and 2*pi, and kappa is the concentration parameter, which must be greater than or equal to zero. If kappa is equal to zero, this distribution reduces to a uniform random angle over the range 0 to 2*pi. """ # mu: mean angle (in radians between 0 and 2*pi) # kappa: concentration parameter kappa (>= 0) # if kappa = 0 generate uniform random angle # Based upon an algorithm published in: Fisher, N.I., # "Statistical Analysis of Circular Data", Cambridge # University Press, 1993. # Thanks to Magnus Kessler for a correction to the # implementation of step 4. random = self.random if kappa <= 1e-6: return TWOPI * random() s = 0.5 / kappa r = s + _sqrt(1.0 + s * s) while 1: u1 = random() z = _cos(_pi * u1) d = z / (r + z) u2 = random() if u2 < 1.0 - d * d or u2 <= (1.0 - d) * _exp(d): break q = 1.0 / r f = (q + z) / (1.0 + q * z) u3 = random() if u3 > 0.5: theta = (mu + _acos(f)) % TWOPI else: theta = (mu - _acos(f)) % TWOPI return theta
def vonmisesvariate(self, mu, kappa): random = self.random if kappa <= 1e-06: return TWOPI * random() s = 0.5 / kappa r = s + _sqrt(1.0 + s * s) while True: u1 = random() z = _cos(_pi * u1) d = z / (r + z) u2 = random() if u2 < 1.0 - d * d or u2 <= (1.0 - d) * _exp(d): break q = 1.0 / r f = (q + z) / (1.0 + q * z) u3 = random() if u3 > 0.5: theta = (mu + _acos(f)) % TWOPI else: theta = (mu - _acos(f)) % TWOPI return theta
def vonmisesvariate(self, mu, kappa): random = self.random if kappa <= 1e-06: return TWOPI*random() s = 0.5/kappa r = s + _sqrt(1.0 + s*s) while True: u1 = random() z = _cos(_pi*u1) d = z/(r + z) u2 = random() if u2 < 1.0 - d*d or u2 <= (1.0 - d)*_exp(d): break q = 1.0/r f = (q + z)/(1.0 + q*z) u3 = random() if u3 > 0.5: theta = (mu + _acos(f)) % TWOPI else: theta = (mu - _acos(f)) % TWOPI return theta
def vonmisesvariate(self, mu, kappa): random = self.random if kappa <= 1e-06: return TWOPI * random() a = 1.0 + _sqrt(1.0 + 4.0 * kappa * kappa) b = (a - _sqrt(2.0 * a)) / (2.0 * kappa) r = (1.0 + b * b) / (2.0 * b) while 1: u1 = random() z = _cos(_pi * u1) f = (1.0 + r * z) / (r + z) c = kappa * (r - f) u2 = random() if u2 < c * (2.0 - c) or u2 <= c * _exp(1.0 - c): break u3 = random() if u3 > 0.5: theta = mu % TWOPI + _acos(f) else: theta = mu % TWOPI - _acos(f) return theta
def vonmisesvariate(self, mu, kappa): random = self.random if (kappa <= 9.9999999999999995e-007): return (TWOPI * random()) a = (1.0 + _sqrt((1.0 + ((4.0 * kappa) * kappa)))) b = ((a - _sqrt((2.0 * a))) / (2.0 * kappa)) r = ((1.0 + (b * b)) / (2.0 * b)) while (1): u1 = random() z = _cos((_pi * u1)) f = ((1.0 + (r * z)) / (r + z)) c = (kappa * (r - f)) u2 = random() if (not (((u2 >= (c * (2.0 - c))) and (u2 > (c * _exp( (1.0 - c))))))): break u3 = random() if (u3 > 0.5): theta = ((mu % TWOPI) + _acos(f)) else: theta = ((mu % TWOPI) - _acos(f)) return theta
def vonmisesvariate(self, mu, kappa): # mu: mean angle (in radians between 0 and 2*pi) # kappa: concentration parameter kappa (>= 0) # if kappa = 0 generate uniform random angle # Based upon an algorithm published in: Fisher, N.I., # "Statistical Analysis of Circular Data", Cambridge # University Press, 1993. # Thanks to Magnus Kessler for a correction to the # implementation of step 4. random = self.random if kappa <= 1e-6: return TWOPI * random() a = 1.0 + _sqrt(1.0 + 4.0 * kappa * kappa) b = (a - _sqrt(2.0 * a)) / (2.0 * kappa) r = (1.0 + b * b) / (2.0 * b) while 1: u1 = random() z = _cos(_pi * u1) f = (1.0 + r * z) / (r + z) c = kappa * (r - f) u2 = random() if not (u2 >= c * (2.0 - c) and u2 > c * _exp(1.0 - c)): break u3 = random() if u3 > 0.5: theta = (mu % TWOPI) + _acos(f) else: theta = (mu % TWOPI) - _acos(f) return theta
def acos(c): """acos - Safe inverse cosine Input argument c is shrunk to admissible interval to avoid case where a small rounding error causes a math domain error. """ from math import acos as _acos if c > 1: c = 1 if c < -1: c = -1 return _acos(c)
def get_spherical_rotatation(p1, p2, width, height, theta_multiplier): v1 = get_sphere_mapping(p1[0], p1[1], width, height) v2 = get_sphere_mapping(p2[0], p2[1], width, height) d = min(max([dot(v1, v2), -1]), 1) if abs(d - 1.0) < 0.000001: return None raxis = norm( cross(v1, v2) ) rtheta = theta_multiplier * rad2deg * _acos(d) glPushMatrix() glLoadIdentity() glRotatef(rtheta, *raxis) mat = (c_float*16)() glGetFloatv(GL_MODELVIEW_MATRIX, mat) glPopMatrix() return mat
def get_spherical_rotatation(p1, p2, width, height, theta_multiplier): v1 = get_sphere_mapping(p1[0], p1[1], width, height) v2 = get_sphere_mapping(p2[0], p2[1], width, height) d = min(max([dot(v1, v2), -1]), 1) if abs(d - 1.0) < 0.000001: return None raxis = norm(cross(v1, v2)) rtheta = theta_multiplier * rad2deg * _acos(d) pgl.glPushMatrix() pgl.glLoadIdentity() pgl.glRotatef(rtheta, *raxis) mat = (c_float * 16)() pgl.glGetFloatv(pgl.GL_MODELVIEW_MATRIX, mat) pgl.glPopMatrix() return mat
def _theta(self, k_y, k_z, k): """Polar angle. Part of coordinate transformation from k-space to (theta, phi)-space. """ return _acos(_csqrt(k**2 - k_y**2 - k_z**2).real / k)
def Angle( self, other ): ''' Angle between two 3 vectors. ''' return _acos( self.Unit() ** other.Unit() )
def cinematicaInversa(state): global x global y global z global tilt_x global tilt_y global tilt_z global _last_x global _last_y global _last_z global joint_angles #print("x: " + str(x) + " y: " + str(y) + " z: " + str(z)) if (x < 0): print("erro: posicao (" + str([x, y, z]) + ") fora do alcance do braco robotico") #return last valid position x = _last_x y = _last_y z = _last_z return joint_angles, False #a error is given when the desired position is invalid try: #main calculations x_const = x - _D0 x_const_pow = x_const * x_const y_const = y y_const_pow = y_const * y_const base_dist = _sqrt(x_const_pow + y_const_pow) d = _sqrt(x_const_pow + y_const_pow - _Probe_lenght_pow) d_const = d z_const = z - _L3 l_const_pow = d_const * d_const + z_const * z_const l_const = _sqrt(l_const_pow) t1_const = _acos( (_L1_pow - _L2_pow + l_const_pow) / (2 * _L1 * l_const)) t2_const = _acos((_L1_pow + _L2_pow - l_const_pow) / (2 * _L1 * _L2)) t3_const = _pi - t1_const - t2_const s1 = _acos(z_const / l_const) s2 = _acos(d_const / l_const) #joint angles if (state & (1 << defs.ROBOT_CLOCKWISE)): joint_angles[0] = -_pi / 2 - _acos( _Probe_lenght / base_dist) + _atan2( y_const, x_const) #y = 71, x_const <= 0, x <= D0 joint_angles[1] = -_pi / 2 + (t1_const + s2) joint_angles[2] = -_pi + t2_const joint_angles[3] = t3_const + s1 joint_angles[4] = -joint_angles[0] - _pi else: joint_angles[0] = _acos(_Probe_lenght / base_dist) + _atan2( y_const, x_const) joint_angles[1] = _pi / 2 - (t1_const + s2) joint_angles[2] = _pi - t2_const joint_angles[3] = -t3_const - s1 joint_angles[4] = -joint_angles[0] if (state & (1 << defs.IN_STAIR) and not state & (1 << defs.HOKUYO_READING)): joint_angles[3] += IN_STAIRS_DISPLACEMENT #check if t0 and t4 angles exceeds 2*_pi if (joint_angles[0] > 2 * _pi): joint_angles[0] -= 2 * _pi elif (joint_angles[0] < -2 * _pi): joint_angles[0] += 2 * _pi # if(joint_angles[4] > 2*_pi): # joint_angles[4] -= 2*_pi # elif(joint_angles[4] < -2*_pi): # joint_angles[4] += 2*_pi joint_angles[0] += tilt_z joint_angles[1] += tilt_x joint_angles[5] = tilt_y #configure tilt if ((state & (1 << defs.ROBOT_ON_THE_LEFT)) and (state & (1 << defs.ROBOT_ANTICLOCKWISE))): if (joint_angles[0] < 0): joint_angles[0] += 2 * _pi joint_angles[0] -= _pi # print("################## 1 : joint: " + str(joint_angles[0]) + ", tilt: " + str(tilt_z)) elif ((not (state & (1 << defs.ROBOT_ON_THE_LEFT))) and (state & (1 << defs.ROBOT_CLOCKWISE))): joint_angles[0] += _pi # print("################## 2 : joint: " + str(joint_angles[0]) + ", tilt: " + str(tilt_z)) # print the joint angles and the x,y,z position of the arm (debuging) #print(joint_angles) #print("x: " + str(x) + ", y: " + str(y) + ", z: " + str(z)) _last_x = x _last_y = y _last_z = z return joint_angles, True except: #Exception as error: #(for debug, uncomment this part of code) #print(error) print("erro: posicao (", str([x, y, z]), ") fora do alcance do braco robotico") #return last valid position x = _last_x y = _last_y z = _last_z return joint_angles, False
def acos(x): if x > 1: return _acos(1) if x < -1: return _acos(-1) return _acos(x)
def acos(x): return _acos(x) if common._use_radians else _deg(_acos(x))
"""Random variable generators.
from math import acos as _acos PI = _acos(-1)
NATYP = 18 NPHB = 19 IFPERT = 20 NBPER = 21 NGPER = 22 NDPER = 23 MBPER = 24 MGPER = 25 MDPER = 26 IFBOX = 27 NMXRS = 28 IFCAP = 29 NUMEXTRA = 30 NCOPY = 31 # An alias NNB = NEXT RAD_TO_DEG = 180.0 / _pi DEG_TO_RAD = _pi / 180.0 TRUNCATED_OCTAHEDRON_ANGLE = _acos(-1 / 3) * 180 / _pi # For use in floating point comparisons TINY = 1.0e-8 SMALL = 1.0e-4 TINY_DIGITS = int(_log10(TINY) + 0.5) SMALL_DIGITS = int(_log10(SMALL) + 0.5) # For I/O DEFAULT_ENCODING = 'UTF-8'