def _measure(p1, p2, p3):
""" Return hypotenuse, ratio and theta for a trio of ordered points.
"""
ha, hb = _hypot(p3.x - p1.x, p3.y - p1.y), _hypot(p2.x - p1.x, p2.y - p1.y)
va, vb = Vector(p1, p2), Vector(p1, p3)
theta = _atan2(va.y, va.x)
x = vb.x * _cos(-theta) - vb.y * _sin(-theta)
y = vb.x * _sin(-theta) + vb.y * _cos(-theta)
ratio = ha / hb
theta = _atan2(y, x)
return hb, ratio, theta
def bset_get_vertex_coordinates(bset_data, scale=1):
"""
Given a basic set return the list of vertices at the corners.
:param bset_data: The basic set to get the vertices from
"""
# Get the vertices.
vertices = []
bset_data.compute_vertices().foreach_vertex(vertices.append)
f = lambda x: _vertex_get_coordinates(x, scale)
vertices = list(map(f, vertices))
if len(vertices) <= 1:
return vertices
# Sort the vertices in clockwise order.
#
# We select a 'center' point that lies within the convex hull of the
# vertices. We then sort all points according to the direction (given as an
# angle in radiens) they lie in respect to the center point.
from math import atan2 as _atan2
center = ((vertices[0][0] + vertices[1][0]) / 2.0,
(vertices[0][1] + vertices[1][1]) / 2.0)
f = lambda x: _atan2(x[0] - center[0], x[1] - center[1])
vertices = sorted(vertices, key=f)
return vertices
def bset_get_vertex_coordinates(bset_data, scale=1):
"""
Given a basic set return the list of vertices at the corners.
:param bset_data: The basic set to get the vertices from
"""
# Get the vertices.
vertices = []
bset_data.compute_vertices().foreach_vertex(vertices.append)
f = lambda x: _vertex_get_coordinates(x, scale)
vertices = list(map(f, vertices))
if len(vertices) <= 1:
return vertices
# Sort the vertices in clockwise order.
#
# We select a 'center' point that lies within the convex hull of the
# vertices. We then sort all points according to the direction (given as an
# angle in radiens) they lie in respect to the center point.
from math import atan2 as _atan2
center = ((vertices[0][0] + vertices[1][0]) / 2.0,
(vertices[0][1] + vertices[1][1]) / 2.0)
f = lambda x: _atan2(x[0]-center[0], x[1]-center[1])
vertices = sorted(vertices, key=f)
return vertices
def euler_angles(Q):
w, (x, y, z) = Q
phi = _atan2(2.*(w*x+y*z), 1.-2.*(x*x+y*y))
theta = _asin(2*(w*y-x*z))
psi = _atan2(2.*(w*z+x*y), 1.-2.*(y*y+z*z))
return phi, theta, psi
def theta_u(Q):
w, v = Q
norm = _v.norm(v)
theta = 2. * _atan2(norm, w)
u = _v.mul(1. / norm, v)
return theta, u
def atan2(y, x):
return _atan2(y, x) if common._use_radians else _deg(_atan2(x))
def _phi(self, k_y, k_z):
"""Azimuthal angle.
Part of coordinate transformation from k-space to (theta, phi)-space.
"""
return _atan2(k_y, -k_z)
def atan2(y, x):
return _atan2(y, x) if common._use_radians else _deg(_atan2(x))
def Phi(py, px):
"""Calculate phi of a particle."""
phi = _atan2(py, px)
if( phi < 0 ):
phi += 2 * _pi
return phi
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 labelLine(line,x,label=None,align=True,**kwargs):
"""Places a label on a line plot and orients it to run parallel with the line.
Given a matplotlib Line instance and x-coordinate, places `label` at the x-coord
on the given line and orientates it parallel to the line.
Author: http://stackoverflow.com/questions/16992038/inline-labels-in-matplotlib
Arguments:
line: Matplotlib Line instance
x: x-coordinate on the line at which the label will be positioned.
label (str): The label to be added to the line.
align (bool): whether or not to align the label parallel to the line
"""
ax = line.get_axes()
xdata = line.get_xdata()
ydata = line.get_ydata()
if (x < xdata[0]) or (x > xdata[-1]):
print('x label location is outside data range!')
return
#Find corresponding y co-ordinate and angle of the
ip = 1
for i in range(len(xdata)):
if x < xdata[i]:
ip = i
break
y = ydata[ip-1] + (ydata[ip]-ydata[ip-1])*(x-xdata[ip-1])/(xdata[ip]-xdata[ip-1])
if not label:
label = line.get_label()
if align:
#Compute the slope
dx = xdata[ip] - xdata[ip-1]
dy = ydata[ip] - ydata[ip-1]
ang = _degrees(_atan2(dy,dx))
#Transform to screen co-ordinates
pt = _np.array([x,y]).reshape((1,2))
trans_angle = ax.transData.transform_angles(_np.array((ang,)),pt)[0]
else:
trans_angle = 0
#Set a bunch of keyword arguments
if 'color' not in kwargs:
kwargs['color'] = line.get_color()
if ('horizontalalignment' not in kwargs) and ('ha' not in kwargs):
kwargs['ha'] = 'center'
if ('verticalalignment' not in kwargs) and ('va' not in kwargs):
kwargs['va'] = 'center'
if 'backgroundcolor' not in kwargs:
kwargs['backgroundcolor'] = ax.get_axis_bgcolor()
if 'clip_on' not in kwargs:
kwargs['clip_on'] = True
if 'zorder' not in kwargs:
kwargs['zorder'] = 2.5
ax.text(x,y,label,rotation=trans_angle,**kwargs)
def blobs2features(blobs, min_hypot=0, min_theta=-pi, max_theta=pi, min_ratio=0, max_ratio=1):
""" Generate a stream of features conforming to limits.
Yields 5-element tuples: indexes for three blobs followed by feature ratio, theta.
"""
count = len(blobs)
# one-dimensional arrays of simple positions
xs = _array([blob.x for blob in blobs], dtype=float)
ys = _array([blob.y for blob in blobs], dtype=float)
#
# two-dimensional arrays of component distances between each blob
# dx = b.x - a.x, dy = b.y - a.y
#
xs_ = repeat(reshape(xs, (1, count)), count, 0)
ys_ = repeat(reshape(ys, (1, count)), count, 0)
dxs, dys = transpose(xs_) - xs_, transpose(ys_) - ys_
#
# two-dimensional array of distances between each blob
# distance = sqrt(dx^2 + dy^2)
#
distances = nsqrt(dxs ** 2 + dys ** 2)
#
# Make a list of eligible eligible blob pairs
#
hypoteni = distances.copy()
hypoteni[distances < min_hypot] = 0
hypot_nonzero = nonzero(hypoteni)
## Prepend separation distance, longest-to-shortest
#blobs_sorted = [(distances[i,j], i, j) for (i, j) in zip(*hypot_nonzero)]
# Prepend combined pixel size, largest-to-smallest
blobs_sorted = [(blobs[i].size + blobs[j].size, i, j) for (i, j) in zip(*hypot_nonzero)]
blobs_sorted.sort(reverse=True)
#
# check each hypotenuse for an eligible third point
#
for (row, (sort_value, i, j)) in enumerate(blobs_sorted):
#
# vector theta for hypotenuse (i, j)
#
ij_theta = _atan2(dys[i,j], dxs[i,j])
#
# rotate each blob[k] around blob[i] by -theta, to get a hypotenuse-relative theta for (i, k)
#
ik_xs = dxs[i,:] * _cos(-ij_theta) - dys[i,:] * _sin(-ij_theta)
ik_ys = dxs[i,:] * _sin(-ij_theta) + dys[i,:] * _cos(-ij_theta)
ik_thetas = arctan2(ik_ys, ik_xs)
ik_thetas = [(blobs[k].size, k, theta) for (k, theta) in enumerate(ik_thetas)]
ik_thetas.sort(reverse=True)
#
# check each blob[k] for correct distance ratio
#
for (size, k, theta) in ik_thetas:
ratio = distances[i,k] / distances[i,j]
if theta < min_theta or max_theta < theta:
continue
if ratio < min_ratio or max_ratio < ratio:
continue
if i == j or i == k or j == k:
continue
yield (i, j, k, ratio, theta)
#
# Set up some fake blobs.
#
blobs = [Point(1, 1), Point(2, 5), Point(3, 2)]
count = len(blobs)
a, b, c = blobs[0], blobs[1], blobs[2]
dab = _sqrt((b.x - a.x) ** 2 + (b.y - a.y) ** 2)
dbc = _sqrt((c.x - b.x) ** 2 + (c.y - b.y) ** 2)
dac = _sqrt((c.x - a.x) ** 2 + (c.y - a.y) ** 2)
ratios = {(0, 1, 2): dac/dab, (1, 0, 2): dbc/dab, (2, 1, 0): dac/dbc}
tab = _atan2(b.y - a.y, b.x - a.x)
tba = _atan2(a.y - b.y, a.x - b.x)
tac = _atan2(c.y - a.y, c.x - a.x)
tca = _atan2(a.y - c.y, a.x - c.x)
tbc = _atan2(c.y - b.y, c.x - b.x)
tcb = _atan2(b.y - c.y, b.x - c.x)
thetas = {
(0, 1, 2): _atan2((c.x - a.x) * _sin(-tab) + (c.y - a.y) * _cos(-tab),
(c.x - a.x) * _cos(-tab) - (c.y - a.y) * _sin(-tab)),
(1, 0, 2): _atan2((c.x - b.x) * _sin(-tba) + (c.y - b.y) * _cos(-tba),
(c.x - b.x) * _cos(-tba) - (c.y - b.y) * _sin(-tba)),
(0, 2, 1): _atan2((b.x - a.x) * _sin(-tac) + (b.y - a.y) * _cos(-tac),
(b.x - a.x) * _cos(-tac) - (b.y - a.y) * _sin(-tac)),
def Phi(self):
"""Return Phi of the particle."""
return _atan2(self._y, self._x)
def theta_u(Q): w, v = Q norm = _v.norm(v) theta = 2.*_atan2(norm, w) u = _v.mul(1./norm, v) return theta, u
def labelLine(line,x,label=None,align=True,**kwargs):
"""Places a label on a line plot and orients it to run parallel with the line.
Given a matplotlib Line instance and x-coordinate, places `label` at the x-coord
on the given line and orientates it parallel to the line.
Author: http://stackoverflow.com/questions/16992038/inline-labels-in-matplotlib
Arguments:
line: Matplotlib Line instance
x: x-coordinate on the line at which the label will be positioned.
label (str): The label to be added to the line.
align (bool): whether or not to align the label parallel to the line
"""
ax = line.get_axes()
xdata = line.get_xdata()
ydata = line.get_ydata()
if (x < xdata[0]) or (x > xdata[-1]):
print('x label location is outside data range!')
return
#Find corresponding y co-ordinate and angle of the
ip = 1
for i in range(len(xdata)):
if x < xdata[i]:
ip = i
break
y = ydata[ip-1] + (ydata[ip]-ydata[ip-1])*(x-xdata[ip-1])/(xdata[ip]-xdata[ip-1])
if not label:
label = line.get_label()
if align:
#Compute the slope
dx = xdata[ip] - xdata[ip-1]
dy = ydata[ip] - ydata[ip-1]
ang = _degrees(_atan2(dy,dx))
#Transform to screen co-ordinates
pt = _np.array([x,y]).reshape((1,2))
trans_angle = ax.transData.transform_angles(_np.array((ang,)),pt)[0]
else:
trans_angle = 0
#Set a bunch of keyword arguments
if 'color' not in kwargs:
kwargs['color'] = line.get_color()
if ('horizontalalignment' not in kwargs) and ('ha' not in kwargs):
kwargs['ha'] = 'center'
if ('verticalalignment' not in kwargs) and ('va' not in kwargs):
kwargs['va'] = 'center'
if 'backgroundcolor' not in kwargs:
kwargs['backgroundcolor'] = ax.get_axis_bgcolor()
if 'clip_on' not in kwargs:
kwargs['clip_on'] = True
if 'zorder' not in kwargs:
kwargs['zorder'] = 2.5
ax.text(x,y,label,rotation=trans_angle,**kwargs)