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.
"""
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):
"""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.
"""
# 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):
"""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 <= 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 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):
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.
def acos(x):
return _acos(x) if common._use_radians else _deg(_acos(x))
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'
