-
Notifications
You must be signed in to change notification settings - Fork 2
/
robot.py
147 lines (106 loc) · 4.08 KB
/
robot.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
import torch
from se3 import matrix_exp_screw, adjoint, inv_adjoint, matrix_log_transform, twist_vector, rotation, translation
from math import degrees
def fkin_space(M, s_list, theta_list):
"""
Computes the position end effector given end_effector home position, list of screws in the space (inertial) frame
and joint angles
:param s_list: 6, J screws in the space frame
:param theta_list: J joint angles (pose of the robot)
:return: (4x4) transformation matrix for the pose of the end effector in the body frame
"""
_, J = s_list.shape
A = torch.eye(4)
for i in range(J):
A = A.matmul(matrix_exp_screw(s_list[:, i], theta_list[i]))
return A.matmul(M)
def space_to_body(M, s_list):
"""
Converts screws in the space frame to screws in the body frame
:param M: (4x4) transform to the end_effector in the space frame
:param s_list: 6, J screws in the space frame
:return: b_list: 6, J screws in the body frame
"""
return inv_adjoint(M).matmul(s_list)
def body_to_space(M, b_list):
"""
Converts screws in the body frame to screws in the space frame
:param M: (4x4) transform to end effector in the space frame
:param b_list: 6, J screws in the space frame
:return: s_list: 6, J screws in the body frame
"""
return adjoint(M).matmul(b_list)
def fkin_body(M, b_list, theta_list):
"""
Computes position and pose of end effector
:param M: home position of end effector in the space frame
:param b_list: 6, J screws in the end effector frame
:param theta_list: J joint angles
:return: end effector position in the space frame
"""
_, J = b_list.shape
E = M.clone()
for i in range(J):
E = E.matmul(matrix_exp_screw(b_list[:, i], theta_list[i]))
return E
def jacobian_space(s_list, theta_list):
"""
Computes the jacobian in the space frame for J jointed robot
:param s_list: 6, J screws in the space frame, J is the number of joints
:param theta_list: J joint positions (pose of the robot)
:return: 6, J space jacobian
"""
_, J = s_list.shape
A = torch.eye(4)
jac = []
for i in range(J):
jac += [adjoint(A).matmul(s_list[:, i])]
A = A.matmul(matrix_exp_screw(s_list[:, i], theta_list[i]))
return torch.stack(jac, dim=1)
def jacobian_body(b_list, theta_list):
"""
Computes the body jacobian for a J jointed robot
:param b_list: 6, J screws in the body frame, J is the number of joints
:param theta_list: J joint positions (pose of the robot)
:return: 6, J body jacobian
"""
_, J = b_list.shape
A = torch.eye(4)
jac = []
for i in reversed(range(J)):
jac += [adjoint(A).matmul(b_list[:, i])]
A = A.matmul(matrix_exp_screw(-b_list[:, i], theta_list[i]))
return torch.stack(list(reversed(jac)), dim=1)
class Singularity(Exception):
pass
def angular_manip_ellipsoid(jac):
return manip_ellipsoid(jac[0:3])
def translational_manip_ellipsoid(jac):
return manip_ellipsoid(jac[3:6])
def manip_ellipsoid(jac):
try:
lam, v = torch.symeig(jac.matmul(jac.T).inverse(), eigenvectors=True)
except Exception:
""" your code should handle singularities, modern robotics 5.3 """
raise Singularity('Robot pose is at singularity')
lengths = lam.sqrt()
axis = v[:, 0]
return lengths, axis
def ikin_body(b_list, M, Tsd, theta_list_0, eomg, ev, maxcount=10):
count = 0
while count < maxcount:
print([degrees(theta) for theta in theta_list_0])
Tsb = fkin_body(M, b_list, theta_list_0)
jb = jacobian_body(b_list, theta_list_0)
# compute the twist from end effector Tsb to desired position Tsd
Tbd = Tsb.inverse().matmul(Tsd)
Vb, mag = matrix_log_transform(Tbd)
Vb = twist_vector(Vb * mag)
# if twist is small, we are done
if Vb[0:3].norm() < eomg and Vb[3:6].norm() < ev:
return theta_list_0
# take a full gradient descent step
delta = jb.pinverse().matmul(Vb)
theta_list_0 += delta
count += 1
return theta_list_0