Rotation¶
Euler angles¶
Extrinsic Rotation¶
Right multiply matrix
In [1]:
import sympy
from sympy.vector import AxisOrienter, CoordSys3D
x, y, theta = sympy.symbols("x y theta")
N = CoordSys3D("N")
M = AxisOrienter(theta, N.k).rotation_matrix(N)
M
Out[1]:
$\displaystyle \left[\begin{matrix}\cos{\left(\theta \right)} & \sin{\left(\theta \right)} & 0\\- \sin{\left(\theta \right)} & \cos{\left(\theta \right)} & 0\\0 & 0 & 1\end{matrix}\right]$
In [2]:
M.inv()
Out[2]:
$\displaystyle \left[\begin{matrix}- \frac{\sin^{2}{\left(\theta \right)}}{\cos{\left(\theta \right)}} + \frac{1}{\cos{\left(\theta \right)}} & - \sin{\left(\theta \right)} & 0\\\sin{\left(\theta \right)} & \cos{\left(\theta \right)} & 0\\0 & 0 & 1\end{matrix}\right]$
In [3]:
v = sympy.Matrix([x, y, 0]).T * M.subs(theta, sympy.pi/2)
v
Out[3]:
$\displaystyle \left[\begin{matrix}- y & x & 0\end{matrix}\right]$
In [4]:
import py3d
py3d.Transform.from_euler("xyz", [0.4, -0.2, 0])
Out[4]:
Transform([[ 0.98006658, 0. , 0.19866933, 0. ],
[-0.07736548, 0.92106099, 0.3816559 , 0. ],
[-0.18298657, -0.38941834, 0.9027011 , 0. ],
[ 0. , 0. , 0. , 1. ]])
In [5]:
import py3d
axis=py3d.axis(dashed=True)
rotated_axis=py3d.axis()@py3d.Transform.from_euler("xyz", [1,2,3])
py3d.render(axis,rotated_axis)
Out[5]:
Intrinsic Rotation¶
In [6]:
import py3d
py3d.Transform.from_euler("XYZ", [0.4, -0.2, 0])
Out[6]:
Transform([[ 0.98006658, -0.07736548, 0.18298657, 0. ],
[ 0. , 0.92106099, 0.38941834, 0. ],
[-0.19866933, -0.3816559 , 0.9027011 , 0. ],
[ 0. , 0. , 0. , 1. ]])
In [7]:
import py3d
py3d.Transform.from_rpy([1.3, 1.0, 0]) @ py3d.Transform.from_rpy([0.9, .0, 0])
Out[7]:
Transform([[ 0.54030231, 0.68032627, 0.49520661, 0. ],
[ 0. , -0.58850112, 0.8084964 , 0. ],
[ 0.84147098, -0.43683247, -0.31796851, 0. ],
[ 0. , 0. , 0. , 1. ]])
Apply a intrinsic rotation to a point
In [9]:
import py3d
p = py3d.Vector3([1, 3, 4])
py3d.label("P", p.tolist())
py3d.render(p.as_point().paint(py3d.Color(g=1)))
t = py3d.Transform.from_euler("XYZ", [0, 0, 1])
p1 = (p @ t)
a = py3d.axis()
a.xyz @= t
py3d.label("P1", p1.tolist())
py3d.render(p1.as_point().paint(py3d.Color(r=1)), a)
Out[9]:
In [10]:
p
Out[10]:
Vector3([1., 3., 4.])
Difference between extrinsic rotation and intrinsic rotation¶
In [11]:
import py3d
e_car = py3d.car()
e_car.color = py3d.Color(g=1)
e_car.xyz @= py3d.Transform.from_euler("xyz", [0.4, -0.2, 0])
py3d.label("Extrincs", [4, 1, 3], color="green")
i_car = py3d.car()
i_car.color = py3d.Color(r=1)
i_car.xyz @= py3d.Transform.from_euler("XYZ", [0.4, -0.2, 0])
py3d.label("Intrincs", [4, -1, 2], color="red")
py3d.render(e_car, i_car)
Out[11]:
RPY¶
In py3d, rpy represents roll, pitch and yaw, it is kind of intrinsic rotation
In [12]:
import py3d, numpy
data = py3d.read_img("20220917214012.jpg")
data.as_image() @ py3d.Transform.from_rpy(p=numpy.linspace(0, py3d.pi/3, 3)) @ py3d.Transform.from_translation(z=numpy.linspace(0, 300, 3))
Out[12]:
Axis angle¶
Axis angle rotation matrix¶
In [13]:
from sympy.vector import AxisOrienter, CoordSys3D
import sympy
x, y, z, theta = sympy.symbols("x y z theta")
N = CoordSys3D('N')
expr = AxisOrienter(theta, x*N.i+y*N.j+z*N.k).rotation_matrix(N)
expr.subs(x**2+y**2+z**2, 1)
Out[13]:
$\displaystyle \left[\begin{matrix}x^{2} + \left(1 - x^{2}\right) \cos{\left(\theta \right)} & - x y \cos{\left(\theta \right)} + x y + z \sin{\left(\theta \right)} & - x z \cos{\left(\theta \right)} + x z - y \sin{\left(\theta \right)}\\- x y \cos{\left(\theta \right)} + x y - z \sin{\left(\theta \right)} & y^{2} + \left(1 - y^{2}\right) \cos{\left(\theta \right)} & x \sin{\left(\theta \right)} - y z \cos{\left(\theta \right)} + y z\\- x z \cos{\left(\theta \right)} + x z + y \sin{\left(\theta \right)} & - x \sin{\left(\theta \right)} - y z \cos{\left(\theta \right)} + y z & z^{2} + \left(1 - z^{2}\right) \cos{\left(\theta \right)}\end{matrix}\right]$
In [14]:
import py3d
py3d.Transform.from_axis_angle(axis=[[0, 0, 1], [0, 0, 1]], angle=[0.6, 1])
Out[14]:
Transform([[[ 0.82533561, 0.56464247, 0. , 0. ],
[-0.56464247, 0.82533561, 0. , 0. ],
[ 0. , 0. , 1. , 0. ],
[ 0. , 0. , 0. , 1. ]],
[[ 0.54030231, 0.84147098, 0. , 0. ],
[-0.84147098, 0.54030231, 0. , 0. ],
[ 0. , 0. , 1. , 0. ],
[ 0. , 0. , 0. , 1. ]]])
Order of rotation and translation
In [15]:
import sympy
r = sympy.symbols("r:9")
dr = sympy.symbols("dr:9")
t = sympy.symbols("t:3")
R = sympy.Matrix([
[r[0], r[1], r[2], 0],
[r[3], r[4], r[5], 0],
[r[6], r[7], r[8], 0],
[0, 0, 0, 1]
])
T = sympy.Matrix([
[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, 0],
[t[0], t[1], t[2], 1]
])
R*T
Out[15]:
$\displaystyle \left[\begin{matrix}r_{0} & r_{1} & r_{2} & 0\\r_{3} & r_{4} & r_{5} & 0\\r_{6} & r_{7} & r_{8} & 0\\t_{0} & t_{1} & t_{2} & 1\end{matrix}\right]$
Get rotation matrix from two vectors
In [16]:
import py3d
py3d.Transform.from_two_vectors([1, 0, 0], [0, 0, 1]).as_rpy()
Out[16]:
Vector3([ 0. , -1.57079633, 0. ])
Rotation Vector¶
In [17]:
import py3d
r = py3d.Transform.from_rotation_vector([[1, 2, 3],[-1,0,7]])
r
Out[17]:
Transform([[[-0.69492056, -0.19200697, 0.69297817, 0. ],
[ 0.71352099, -0.30378504, 0.6313497 , 0. ],
[ 0.08929286, 0.93319235, 0.34810748, 0. ],
[ 0. , 0. , 0. , 1. ]],
[[ 0.71124095, 0.70173688, -0.04125129, 0. ],
[-0.70173688, 0.70534791, -0.10024813, 0. ],
[-0.04125129, 0.10024813, 0.99410696, 0. ],
[ 0. , 0. , 0. , 1. ]]])
In [18]:
r.as_rotation_vector()
Out[18]:
Vector3([[-0.67925191, -1.35850382, -2.03775573],
[-0.11142341, 0. , 0.77996389]])
Quaternion¶
In py3d, quaternion is in scalar-last format. Normaly, Vector4 is used to represent it.
In [20]:
import py3d
py3d.Vector4(x=0, y=0, z=0, w=1)
Out[20]:
Vector4([0., 0., 0., 1.])
In [21]:
import py3d
py3d.Transform.from_quaternion([0, 0, 0, 1])
Out[21]:
Transform([[1., 0., 0., 0.],
[0., 1., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]])
Get a quaternion from an axis angle pair, and get its rotation matrix
In [22]:
import py3d
angle_axis = py3d.Vector4(x=1, y=1, z=1, w=py3d.pi/2)
print(angle_axis)
q = angle_axis.from_axis_angle_to_quaternion()
print(q)
py3d.Transform.from_quaternion(q)
[1. 1. 1. 1.57079633] [0.40824829 0.40824829 0.40824829 0.70710678]
Out[22]:
Transform([[ 0.33333333, 0.9106836 , -0.24401694, 0. ],
[-0.24401694, 0.33333333, 0.9106836 , 0. ],
[ 0.9106836 , -0.24401694, 0.33333333, 0. ],
[ 0. , 0. , 0. , 1. ]])
In [23]:
import py3d
q = [
[0, 0, 0, 1],
[0, 0, 1, 0],
[0, 1, 0, 0],
[1, 0, 0, 0]
]
py3d.Transform.from_quaternion(q)
Out[23]:
Transform([[[ 1., 0., 0., 0.],
[ 0., 1., 0., 0.],
[ 0., 0., 1., 0.],
[ 0., 0., 0., 1.]],
[[-1., 0., 0., 0.],
[ 0., -1., 0., 0.],
[ 0., 0., 1., 0.],
[ 0., 0., 0., 1.]],
[[-1., 0., 0., 0.],
[ 0., 1., 0., 0.],
[ 0., 0., -1., 0.],
[ 0., 0., 0., 1.]],
[[ 1., 0., 0., 0.],
[ 0., -1., 0., 0.],
[ 0., 0., -1., 0.],
[ 0., 0., 0., 1.]]])
Convert quaternion to angle axis
In [25]:
import py3d
quat = py3d.Transform.from_quaternion(
[0.18257419, 0.36514837, 0.54772256, 0.73029674])
quat.as_axis_angle()
Out[25]:
Vector4([0.26726125, 0.53452248, 0.80178373, 1.50408018])
Convert RPY to quaternion¶
Return quaternion with sequence "xyzw"
In [27]:
import py3d
rpy = [
[0, 0, -py3d.pi],
[0, 0, -py3d.pi-0.01],
[0, 0, -py3d.pi/3],
[-1, 0, 0],
[-py3d.pi, 0, 1],
[0, -py3d.pi, 0]]
py3d.Transform.from_rpy(rpy).as_quaternion()
Out[27]:
Vector4([[ 0. , 0. , -1. , 0. ],
[ 0. , 0. , 0.9999875 , 0.00499998],
[ 0. , 0. , -0.5 , 0.8660254 ],
[-0.47942554, 0. , 0. , 0.87758256],
[-0.87758256, 0.47942554, 0. , 0. ],
[ 0. , -1. , 0. , 0. ]])
In [29]:
import py3d
# positions
p = [[3.24544068, -1.4610586, 1.21331756],
[3.66378017, -1.32072563, 1.89712674],
[-5.0884622, -2.48626808, 1.68773464],
[-5.47338134, -2.65634697, 2.02463642],
[1.830746, -0.8155359, 1.90245186],
[-0.7094184, -0.84719837, 1.4467056],
[-1.72178753, -0.681502, 1.17706321],
[-3.88463547, -1.20610078, 1.14303617],
[-4.527405, -3.12150274, 0.94426914],
[4.13260871, -1.71061771, 1.49295544],
[3.25896384, -1.46451182, 0.9032174],
[-3.63891521, -1.03317465, 1.11405222]]
# quaternions
q = [[0.00307048, -0.27852711, -0.24115858, 0.92965357],
[0.00955487, -0.328664, -0.25972646, 0.90798174],
[0.05519327, 0.22308439, 0.22751421, 0.94626864],
[0.05064761, 0.24596963, 0.23314524, 0.93945572],
[-0.01006987, -0.17448035, -0.11591101, 0.97776267],
[0.01230182, -0.03710485, 0.00123949, 0.99923489],
[0.02991609, 0.0383105, 0.0729396, 0.99615117],
[0.05252438, 0.12527874, 0.1242716, 0.98290538],
[-0.09333274, 0.14651227, 0.2808575, 0.94389735],
[0.00967634, -0.29085732, -0.28211318, 0.91417752],
[0.00214324, -0.25691119, -0.23230781, 0.93809655],
[0.04813863, 0.1177435, 0.11584668, 0.98508816]]
vertice = py3d.Vector3(
z=[0, 1]) @ py3d.Transform.from_quaternion(q) @ py3d.Transform.from_translation(p)
directions = vertice.as_linesegment()
directions.start.color = py3d.Color(b=1)
directions.end.color = py3d.Color(r=1)
py3d.render(directions)
Out[29]: