-
Notifications
You must be signed in to change notification settings - Fork 0
/
IK.py
140 lines (113 loc) · 4.83 KB
/
IK.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
''' Simple IK model for the dVRK in OpenRave
dVRK is modelled as a ball joint having 2 DOF and an extensible arm of 1 DOF
3 DOF: Theta (th) and Azimuth (az) and extension length (ext)
th spans the XY plane and az spans the XZ plane
Axis: X - towards you, Y - right, Z - upwards
Reference is taken from the X axis with all angles th and az as 0
User specified cartesian coordinates of end effector
Joint angles and extended length is automatically calculated
If ext is beyond extension limits (extlimits), then the resultant ext' will be bounded
th limits are defined by [th_low, th_high]. az limits are defined by [az_low, az_high]
User parameters:
[X,Y,Z] - End coordinates in cartesian space
'''
import math
import numpy as np
class dVRK_IK_simple:
def __init__(self):
self.thLimits = [-math.pi/2, math.pi/2]
self.azLimits = [-math.pi/2, math.pi/2]
self.extLimits = [-10, 10] # We can either retract the arm by 10 or extend the arm by 10
self.length_offset = 20
def get_joint_DOF(self, endEffector):
# Returns the joint DOF for each joint indices in the sequence [th, az, ext]
# the endEffector type can either be a single list of a nested list to handle multiple way point specifications
# Returns a nested list of [[th, az, ext] ... ] DOF
# Assume that the arm starts off at (1,0,0) always
basePos = np.array([1,0,0]);
joint_DOF = []
if any(isinstance(i,float) for i in endEffector):
# only 1 end effector position given
endEffector = [endEffector]
elif any(isinstance(i,list) for i in endEffector):
# multiply end effector positions given
pass
for j in endEffector:
desiredPos = np.array(j);
ext = np.linalg.norm(desiredPos) - self.length_offset # Gets the desired extension
[th, az] = self.__checkSingularity(basePos, desiredPos)
joint_DOF.append(self.__getLlimits([th, az, ext]))
return joint_DOF # Returns final DOF as nested list
def get_endEffector_fromDOF(self, joint_DOF):
# Returns end effector pose from DOF input in seqeunce [X, Y, Z]
# the joint_DOF type can either be a single list of a nested list to handle multiple way point specifications
# Returns a nested list of [[X, Y, Z] ... ] cartesian coordinate
# Based on the relationship x^2 + y^2 + z^2 = L
# y/x = tan(th)
# z/x = -tan(az)
# rtype = [X, Y, Z] as list
from math import tan, cos, sin
endEffector = []
if any(isinstance(i,float) for i in joint_DOF):
joint_DOF = [joint_DOF]
elif any(isinstance(i,list) for i in joint_DOF):
# multiply joint DOF specified
pass
endEffector = []
for j in joint_DOF:
th, az, ext = self.__getLlimits(j)
L = ext + self.length_offset
ratio = L**2 / (1 + tan(th)**2 + tan(az)**2)
proj_len_XY = math.sqrt(ratio * (1 + (tan(th))**2))
proj_len_XZ = math.sqrt(ratio * (1 + (tan(az))**2))
X = cos(th) * proj_len_XY; Y = sin(th) * proj_len_XY; Z = sin(az) * proj_len_XZ
endEffector.append([X,Y,Z])
return endEffector
def __getLlimits(self, joint_DOF):
# Enforces that all DOF are constraint within the limits defined
th, az, ext = [i for i in joint_DOF]
ext = min(max(ext, self.extLimits[0]), self.extLimits[1])
th = min(max(th, self.thLimits[0]), self.thLimits[1])
az = min(max(az, self.azLimits[0]), self.azLimits[1])
return [th, az, ext]
def __checkSingularity(self, basePos, desiredPos):
projXY = np.array([desiredPos[0], desiredPos[1], 0]) # Vector projection on XY plane
projXZ = np.array([desiredPos[0], 0, desiredPos[2]]) # Vector projection on XZ plane
if np.linalg.norm(projXZ) == 0:
# Case of singularity on XY plane
az = 0
else:
normXZ = projXZ /np.linalg.norm(projXZ)
az0 = math.acos(np.dot(basePos, normXZ)) # Gets azimuth value
az = self.__checkDir(basePos, normXZ)*az0
if np.linalg.norm(projXY) == 0:
# cartesianse of singularity on XZ plane
th = 0
else:
normXY = projXY /np.linalg.norm(projXY)
th0 = math.acos(np.dot(basePos, normXY)) # Gets theta value
th = self.__checkDir(basePos, normXY)*th0
return [th, az]
def __checkDir(self, basePos, normPos):
# Checks the direction of turn to be +ve or -ve
# Turn in the +ve Z axis is +ve
# Turn in the -ve Z axis is -ve
# Cross product is done wrt to basePos first
vect = np.cross(basePos, normPos)
positiveCases = [np.array([0,-1,0]), np.array([0,0,1])]
negativeCases = [np.array([0,1,0]), np.array([0,0,-1])]
if np.linalg.norm(vect) == 0:
return 1 # Catch case where vector is 0
else:
vect = vect /np.linalg.norm(vect) # Normalizes vector
if vect.tolist() in [i.tolist() for i in positiveCases]:
return 1
elif vect.tolist() in [ii.tolist() for ii in negativeCases]:
return -1
if __name__ == "__main__":
IK = dVRK_IK_simple()
DOF = IK.get_joint_DOF([[10,20,0],[30,30,0]])
endEff = IK.get_endEffector_fromDOF(DOF)
joint = IK.get_joint_DOF(endEff)
print(joint)
print(endEff)