def getrotth(th1, dth):
r1 = Rotation(th1[0], 2) * Rotation(th1[1], 1) * Rotation(th1[2], 2)
th2 = th1 + dth
r2 = Rotation(th2[0], 2) * Rotation(th2[1], 1) * Rotation(th2[2], 2)
t1 = th1 / 180 * np.pi
t2 = dth / 180 * np.pi
# print(r1.getXYZ())
# print(r2.getXYZ())
dxyz = np.array(r2.getXYZ()) - np.array(r1.getXYZ())
return t1, t2, dxyz / 180 * np.pi
def testQuaternionRotation():
""" Test quaternino operation is same to analytical method """
rot = np.array([1 / np.sqrt(2), 1 / np.sqrt(2), 0])
theta = 40
pos = [-0.1, 0.5, np.sqrt(1 - 0.1**2 - 0.5**2)]
w = Quaternion.setRotation(theta, rot)
m = w.getRotationMat()
mat = w.T().getRotationMat()
mat[1:4, 1:4] = mat[1:4, 1:4].T
print(mat.dot(m))
print((w * Quaternion(0, *pos) * w.T()).getIJK())
th = 40 / 2 / 180 * np.pi
e = [0, *(rot * np.sin(th)), np.cos(th)]
R = np.array([[
1 - 2 * e[2]**2 - 2 * e[3]**2, 2 * (e[1] * e[2] - e[3] * e[4]),
2 * (e[1] * e[3] + e[2] * e[4])
],
[
2 * (e[1] * e[2] + e[3] * e[4]),
1 - 2 * e[1]**2 - 2 * e[3]**2,
2 * (e[2] * e[3] - e[1] * e[4])
],
[
2 * (e[1] * e[3] - e[2] * e[4]),
2 * (e[1] * e[4] + e[2] * e[3]),
1 - 2 * e[1]**2 - 2 * e[2]**2
]])
print(R)
print(Rotation(mat=R) * Translation(mat=pos))
def findTransform(r1, r2):
""" Find the translation, scaling, rotation of r1 from r2 """
# normalize
rr1 = r1 - r1.mean(axis=1)[:, None]
rr2 = r2 - r2.mean(axis=1)[:, None]
# find scale
scale = np.sqrt((rr1**2).sum(axis=0) / (rr2**2).sum(axis=0)).mean()
rr2 *= scale
# find rotation
## calc convariance matrix
cov = rr2.dot(rr1.T)
M = np.array(
[[cov[0, 0] + cov[1, 1] + cov[2, 2], 0, 0, 0],
[cov[1, 2] - cov[2, 1], cov[0, 0] - cov[1, 1] - cov[2, 2], 0, 0],
[
cov[2, 0] - cov[0, 2], cov[0, 1] + cov[1, 0],
cov[1, 1] - cov[0, 0] - cov[2, 2], 0
],
[
cov[0, 1] - cov[1, 0], cov[2, 0] + cov[0, 2],
cov[2, 1] + cov[1, 2], cov[2, 2] - cov[0, 0] - cov[1, 1]
]])
for i in range(0, 3):
for j in range(i + 1, 4):
M[i, j] = M[j, i]
## calc rotation matrix
eigen = np.linalg.eig(M)
q = Quaternion(mat=eigen[1][:, 0])
# a = q.getRotationParam()
# q = Quaternion.setRotation(a[0], a[1])
qmat = q.getRotationMat()
qmat_inv = q.T().getRotationMat()
qmat_inv[1:4, 1:4] = qmat_inv[1:4, 1:4].T
R = qmat.dot(qmat_inv)[1:, 1:]
# find translation
translate = np.mean(r1 - R.dot(r2) * scale, axis=1)
# print the parameters
print("---" * 20)
print("Eigen", eigen)
print("translation", translate)
print("scale", scale)
print("Rotation", R)
print("Rotation param", q.getRotationParam())
s = Transform()
s.mat[np.arange(3), np.arange(3)] = scale
transform = Transform(loc=Translation(mat=translate),
rot=Rotation(mat=R)) * s
print("Transform matrix", transform)
return transform
import numpy as np
from hw2 import Transform, Translation, Rotation, Base
from hw3 import Quaternion, link, symLink
import matplotlib.pyplot as plt
from simulation import Simulation
import sympy
from pprint import pprint
from gradient import gradientVariable, forwardVariable
# exit()
print("Q1")
t01 = Transform(rot=Rotation(180, 0), loc=Translation(-8, 6, 2))
tan68 = np.arctan(6 / 8) * 180 / np.pi
t13 = Transform(rot=Rotation(90 + tan68, 0) * Rotation(-90, 1),
loc=Translation(0, 6, -8))
t03 = t01 * t13
print("T01", t01)
print("T13", t13)
print("T03", t03)
t31 = t13.T()
print("T31", t31)
q = Quaternion.fromRotationMat(t31.rot)
print("Quaternion", q)
angle, raxis = q.getRotationParam()
print("Rotation axis", raxis)
print("Rotation angle", angle)
import numpy as np
import torch
import torch.nn.functional as F
from simulation import Simulation
from gradient import tensorLink, forwardVariable
from hw2 import Transform, Translation, Rotation
import matplotlib.animation as animation
import matplotlib.pyplot as plt
target = Transform(rot=Rotation(90, 0), loc=Translation(550, 0, 400)).mat
linkparam = [(0, 0, None, 352), (90, 70, None, 0), (0, 360, None, 0),
(90, 0, None, 380), (90, 0, None, 0), (90, 0, None, 65)]
sim = Simulation(linkparam, 500)
def gradientVariable(wanted, linkparam, init_angle=None):
"""
Use gradient descent to find parameters
Unknown parameters are setted to None.
Assume no two parameters exist at the same link
"""
# init tensor
n = np.sum([p[2] is None or p[3] is None for p in linkparam])
if init_angle is None:
th = torch.rand((n), requires_grad=True)
else:
th = torch.tensor(init_angle, requires_grad=True)
wanted = torch.Tensor(wanted)
optimizer = torch.optim.Adam([th], lr=0.2)
# gradient decent
def link(twist, dist, angle, offset):
T1 = Transform(rot=Rotation(twist, 0))
T2 = Transform(loc=Translation(dist, 0, 0))
T3 = Transform(rot=Rotation(angle, 2))
T4 = Transform(loc=Translation(0, 0, offset))
return T1 * T2 * T3 * T4
sympy.cos(angle), 0, 0], [0, 0, 1, offset],
[0, 0, 0, 1]])
return T1 * T2
def inv(T):
T = T.copy()
T[:3, :3] = T[:3, :3].T
T[:3, 3] = -T[:3, :3] * T[:3, 3]
return T
if __name__ == "__main__":
# Q1
print("Q1")
t = Transform(rot=Rotation(30, 2), loc=Translation(10, 0, 5)).T()
print(t)
print(t.rot * Translation(0, 2, -3))
print(t.rot * Translation(1.414, 1.414, 0))
# Q2
print("Q2")
th1, th2, th3 = 0, 0, 0
for th1, th2, th3 in [[0, 0, 0], [10, 20, 30], [90, 90, 90]]:
print(th1, th2, th3)
T1 = link(0, 0, th1, 0)
T2 = link(0, 4, th2, 0)
T3 = link(0, 3, th3, 0)
T4 = link(0, 2, 0, 0)
th1, th2, th3 = 90, 90, 90
print(T1 * T2 * T3)
return T
if __name__ == "__main__":
"""
# Q7-1
# Calculate Wanted Transform matrix
T_wt = Transform(loc=Translation(5, 10, 200))
T_bs = Transform(rot=Rotation(30, 2), loc=Translation(700, 0, 500))
T_sg = Transform(rot=Rotation(60, 2), loc=Translation(30, 30, 30))
T_bw = T_bs * T_sg * T_wt.T()
inverse_irb140(T_bw)
"""
# Q7-2
T_bw1 = Transform(rot=Rotation(90, 0), loc=Translation(550, 0, 400))
T_bw2 = Transform(rot=Rotation(90, 2) * Rotation(30, 1) * Rotation(45, 0),
loc=Translation(650, 100, 300))
print("T_bw1", T_bw1)
print("T_bw2", T_bw2)
T_w1w2 = T_bw1.T() * T_bw2
print("T_w1w2", T_w1w2)
r = T_w1w2.rot.mat
angle = np.arccos((np.trace(r) - 1) / 2)
raxis = 1 / 2 / np.sin(angle) * np.array(
[r[2, 1] - r[1, 2], r[0, 2] - r[2, 0], r[1, 0] - r[0, 1]])
print("Rotation axis", raxis)
print("Rotation angle", angle)
w = Quaternion.setRotation(angle / np.pi * 180, raxis)
def vTransform(rot, p_rel=Translation()):
""" matrix of velcoity and angular """
p = p_rel.mat
if type(rot) is not Rotation:
raise ValueError("Wrong Rotation")
s = [[0, -p[2], p[1]], [p[2], 0, -p[0]], [-p[1], p[0], 0]]
mat = np.vstack([
np.hstack([rot.mat, -rot.mat.dot(s)]),
np.hstack([np.zeros([3, 3]), rot.mat])
])
return Transform(mat=mat)
if __name__ == "__main__":
print("HW3 Q1")
r = Rotation(30, 2).T()
t = Translation(10, 0, 5)
v = Translation(0, 2, -3)
w = Translation(1.414, 1.414, 0)
T = vTransform(r, t)
p = vTranslate(v, w)
print(T * p)
print("HW4 Q1")
def getrotth(th1, dth):
r1 = Rotation(th1[0], 2) * Rotation(th1[1], 1) * Rotation(th1[2], 2)
th2 = th1 + dth
r2 = Rotation(th2[0], 2) * Rotation(th2[1], 1) * Rotation(th2[2], 2)