def calc_edge_compass_angle(edge):
north_ray = sympy.Ray2D((0, 0), (0, 1))
# 反向算进入edge，直观。
edge_ray = sympy.Ray(*edge.getShape()[:2][::-1])
angle = north_ray.closing_angle(edge_ray)
angle = (angle + 2 * sympy.pi) % (2 * sympy.pi)
angle_degrees = float(degrees(angle))
angle_radians = float(radians(degrees(angle)))
edge._angle_degrees = round(angle_degrees, 4)
edge._angle_radians = round(angle_radians, 4)
return angle_degrees, angle_radians
def convertFromRadians(expr):
newExpr = None
if trigUnit == TrigUnit.Degrees:
newExpr = mpmath.degrees(expr)
elif trigUnit == TrigUnit.Gradients:
newExpr = expr * 200 / mpmath.pi
else:
newExpr = expr
return newExpr
def calc_edge_compass_angle_no_modify(edge):
north_ray = sympy.Ray2D((0, 0), (0, 1))
# 要算所有edge，所以不要反向。
edge_ray = sympy.Ray(*edge.getShape()[:2])
angle = north_ray.closing_angle(edge_ray)
angle = (angle + 2 * sympy.pi) % (2 * sympy.pi)
angle_degrees = float(degrees(angle))
# angle_radians = float(radians(degrees(angle)))
# edge._angle_degrees = round(angle_degrees,4)
# edge._angle_radians = round(angle_radians,4)
return angle_degrees
def choose_best_q4q5q6(self, q4q5q6_solutions):
"""
"""
assert len(q4q5q6_solutions) > 0
if debug_IK == True:
rospy.loginfo("Choosing from %d solution(s)",
len(q4q5q6_solutions))
for i in xrange(0, len(q4q5q6_solutions)):
sln = q4q5q6_solutions[i]
rospy.loginfo(" sln#%d: %04.4f' %04.4f' %04.4f'", i + 1,
degrees(sln[0]), degrees(sln[1]),
degrees(sln[2]))
# TODO: Implement logic to select best solution from multiple solutions
# based on current joint states. For now just pick the first solution.
theta4 = q4q5q6_solutions[0][0]
theta5 = q4q5q6_solutions[0][1]
theta6 = q4q5q6_solutions[0][2]
return (theta4, theta5, theta6)
def clean_angles_rad_deg(angles, radians, degrees, accuracy):
x = 0
if degrees is True:
for i in angles:
angles[x] = mpmath.degrees(i)
x += 1
for i in angles:
if angles.count(i) > 1:
angles.remove(i)
angles.sort()
return angles
def aufgSchnittwinkelGeraden(f,g,lsg="c"):
alpha=mpmath.degrees(atan(abs((f.a-g.a)/(1+f.a*g.a)))).evalf(2)
#alpha=(atan(abs((f.a-g.a)/(1+f.a*g.a)))).evalf(2)
text=""
if lsg=="c":
text+="{\\bf Schnittwinkel:} \\\\ \n"
text+="\\begin{align*} \n"
text+="\\tan(\\alpha) &=" + "\\left|\\frac{m_1-m_2}{1+m_1\\cdot m_2}\\right| \\\\ \n"
text+="\\tan(\\alpha) &=" + "\\left|\\frac{(" + latex(f.a) + ")-(" + latex(g.a) + ")}{1+(" + latex(f.a) + ")(" + latex(g.a) + ")}\\right| \\\\ \n"
text+="\\text{mit CAS:} \\\\ \n"
text+="\\alpha &=" + latex((alpha)) + "^{\\circ} \n"
text+="\\end{align*} \n"
return text
def aufgSteigungGerade(f,lsg="c"):
alpha=mpmath.degrees(atan(f.a)).evalf(2)
#alpha=atan(f.a).evalf(2)
text=""
if lsg=="o":
text+="$m=" + latex(f.a) + "=" + latex((f.a*(100.0)).evalf(4)) + "\\% \\qquad \\alpha=" + latex((alpha)) + "^{\\circ}$"
return text
elif lsg=="c":
text+="{\\bf Steigung} (in Prozent):" + "\\quad $m=" + latex(f.a) + "=" + latex((f.a*(100.0)).evalf(4)) + "\\%$ \\\\ \n"
text+="{\\bf Steigungswinkel:} \\\\ \n"
text+="\\begin{align*} \n"
text+="\\tan(\\alpha) &=" + latex(f.a) + "\\\\ \n"
text+="\\text{mit CAS:} \\\\ \n"
text+="\\alpha &=" + latex((alpha)) + "^{\\circ} \n"
text+="\\end{align*} \n"
return text
def get_angle(complex_number, deg=False):
atan_angle = None
if complex_number.real == 0:
if complex_number.imag > 0:
atan_angle = atan(mpf("inf"))
else:
atan_angle = atan(mpf("-inf"))
else:
atan_angle = atan(complex_number.imag / complex_number.real)
if complex_number.real < 0:
atan_angle += pi
if deg:
angle = degrees(atan_angle)
return angle
return atan_angle
def normalized_degrees_from_radians(theta):
"""Return normalized degrees from radians, theta.
Function 'degrees' comes from mpmath."""
return normalized_degrees(degrees(theta))
'tan': ['primitive', [lambda x, y: mp.tan(x), None]],
'sinpi': ['primitive', [lambda x, y: mp.sinpi(x), None]], #sin(x * pi)
'cospi': ['primitive', [lambda x, y: mp.cospi(x), None]],
'sec': ['primitive', [lambda x, y: mp.sec(x), None]],
'csc': ['primitive', [lambda x, y: mp.csc(x), None]],
'cot': ['primitive', [lambda x, y: mp.cot(x), None]],
'asin': ['primitive', [lambda x, y: mp.asin(x), None]],
'acos': ['primitive', [lambda x, y: mp.acos(x), None]],
'atan': ['primitive', [lambda x, y: mp.atan(x), None]],
'atan2': ['primitive', [lambda x, y: mp.atan2(y[0], x), None]],
'asec': ['primitive', [lambda x, y: mp.asec(x), None]],
'acsc': ['primitive', [lambda x, y: mp.acsc(x), None]],
'acot': ['primitive', [lambda x, y: mp.acot(x), None]],
'sinc': ['primitive', [lambda x, y: mp.sinc(x), None]],
'sincpi': ['primitive', [lambda x, y: mp.sincpi(x), None]],
'degrees': ['primitive', [lambda x, y: mp.degrees(x),
None]], #radian - >degree
'radians': ['primitive', [lambda x, y: mp.radians(x),
None]], #degree - >radian
#
'exp': ['primitive', [lambda x, y: mp.exp(x), None]],
'expj': ['primitive', [lambda x, y: mp.expj(x), None]], #exp(x*i)
'expjpi': ['primitive', [lambda x, y: mp.expjpi(x), None]], #exp(x*i*pi)
'expm1': ['primitive', [lambda x, y: mp.expm1(x), None]], #exp(x)-1
'power': ['primitive', [lambda x, y: mp.power(x, y[0]), None]],
'powm1': ['primitive', [lambda x, y: mp.powm1(x, y[0]),
None]], #pow(x, y) - 1
'log': [
'primitive',
[lambda x, y: mp.log(x) if y is None else mp.log(x, y[0]), None]
],
