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] ],