Передняя кинематика и динамика 3D робота (UR5) руки с симпати

Question

Я использую sympy 1.4 для кинематики и динамики робота ur5. Функция express в sympy, похоже, возвращает неправильный ответ. Общая цель состоит в том, чтобы получить матрицы масс, кориолиса, центростремительных и гравитационных сил в качестве символических выражений. В прилагаемом коде я пробую манипулятор 3R

В прикрепленном коде я ожидаю, что вектор положения фрейма J3 будет 0,707106781186548 * Base.i + 2.70710678118655 * Base.j относительно B, где в качестве выражения, возвращаемого функцией express, есть отрицательный j. Любая идея, где я делаю ошибку. Или есть лучший способ преобразовать представление Денавита-Хартенберга, чтобы получить системы координат соединений?

edit 1 : Я считаю, что соглашение о знаках, используемое в sympy, прямо противоположно тому, что я узнал. Например, для вращения оси z матрица вращения B по отношению к A определяется в sympy как

 [cos(a) sin(a) 0;
 -sin(a) cos(a) 0;
    0    0   1] 

Есть ли способ перейти к соглашению о знаках, где R_z =

 [cos(a) -sin(a) 0;
  sin(a) cos(a) 0;
    0     0    1]

без использования функции транспонирования всегда?

from sympy import *
from sympy.physics.mechanics import *
from sympy.physics.vector import ReferenceFrame, Vector
from sympy.physics.vector import time_derivative
from sympy.tensor.array import Array
# Planar 3R manipulator (minimal code)
# DH representation

a = Array([0, 1, 2])
d = Array([0.0, 0.0, 0.0])
alpha = Array([0.0, 0.0, 0.0])
# q1, q2, q3 = dynamicsymbols('q1:4')
# q = [q1, q2, q3]
q = [np.pi/4, np.pi/4, 0.0,]

x_p = a[1]*cos(q[0]) + a[2]*cos(q[0] + q[1])
y_p = a[1]*sin(q[0]) + a[2]*sin(q[0] + q[1])

print 'x_p, y_p:', x_p, y_p


def transformationMatrix():
    q_i = Symbol("q_i")
    alpha_i = Symbol("alpha_i")
    a_i = Symbol("a_i")
    d_i = Symbol("d_i")
    T = Matrix([[cos(q_i), -sin(q_i), 0, a_i],
                [sin(q_i) * cos(alpha_i), cos(q_i) * cos(alpha_i), -sin(alpha_i), -sin(alpha_i) * d_i],
                [sin(q_i) * sin(alpha_i), cos(q_i) * sin(alpha_i), cos(alpha_i), cos(alpha_i) * d_i],
                [0, 0, 0, 1]])
    return T


T = transformationMatrix()
q_i = Symbol("q_i")
alpha_i = Symbol("alpha_i")
a_i = Symbol("a_i")
d_i = Symbol("d_i")

T01 = T.subs(alpha_i, alpha[0]).subs(a_i, a[0]).subs(d_i, d[0]).subs(q_i, q[0])
T12 = T.subs(alpha_i, alpha[1]).subs(a_i, a[1]).subs(d_i, d[1]).subs(q_i, q[1])
T23 = T.subs(alpha_i, alpha[2]).subs(a_i, a[2]).subs(d_i, d[2]).subs(q_i, q[2])

T02 = T01*T12
T03 = T02*T23


B = CoordSys3D('Base')  # Base (0) reference frame
J1 = CoordSys3D('Joint1', location=T01[0, 3]*B.i  + T01[1, 3]*B.j  + T01[2, 3]*B.k,  rotation_matrix=T01[0:3, 0:3], parent=B)
J2 = CoordSys3D('Joint2', location=T12[0, 3]*J1.i + T12[1, 3]*J1.j + T12[2, 3]*J1.k, rotation_matrix=T12[0:3, 0:3], parent=J1)
J3 = CoordSys3D('Joint3', location=T23[0, 3]*J2.i + T23[1, 3]*J2.j + T23[2, 3]*J2.k, rotation_matrix=T23[0:3, 0:3], parent=J2)

express(J3.position_wrt(B), B)

ожидаемый результат: производит 0.707106781186548*Base.i + 2.70710678118655*Base.j фактический результат: производит 0.707106781186548*Base.i + (-2.70710678118655)*Base.j