forked from udacity/RoboND-Kinematics-Project
/
RDHentry.py
213 lines (160 loc) · 7.43 KB
/
RDHentry.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
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
# helper libraries
from mpmath import * # for arbitraty floating point precision operations
from sympy import * # for symbolic operations
import numpy as np
from RDHmath import *
class RJointType :
NONE = 0
REVOLUTE = 1
PRISMATIC = 2
class RDHparams :
alpha_i_1 = 0
a_i_1 = 1
d_i = 2
theta_i = 3
class RDHentry( object ):
"""
Object representation of an entry in the DH table
@param {boolean[]} vfixed Whether or not the joints are fixed or not
@param {float[]} vvalues Value of the DH params for this entry
@param {float} minJointValue Minimum value of the joint for this entry
@param {float} maxJointValue Maximum value of the joint for this entry
@param {float} jointSign Sign that the joint variable takes in the dh representation
@param {float} jointOffset Offset applied to the joint variable in the dh representation
"""
def __init__( self,
vfixed, vvalues,
minJointValue, maxJointValue,
jointSign = 1.0, jointOffset = 0.0 ):
super( RDHentry, self ).__init__()
assert len( vfixed ) == 4, 'Wrong length for vfixed parameter, should be 4'
assert len( vvalues ) == 4, 'Wrong length for vvalues parameter, should be 4'
self.m_fixed = vfixed
self.m_dhparams = vvalues
self.m_jointSign = jointSign
self.m_jointOffset = jointOffset
self.m_minJointValue = minJointValue
self.m_maxJointValue = maxJointValue
self.m_rangeJointValue = maxJointValue - minJointValue
self.m_transform = np.eye( 4 )
self.m_symTransform = eye( 4 )
self.m_sym_alpha_i_1 = symbols( 'alpha_i_1' )
self.m_sym_a_i_1 = symbols( 'a_i_1' )
self.m_sym_d_i = symbols( 'd_i' )
self.m_sym_theta_i = symbols( 'theta_i' )
self.m_dhparamsSym = { self.m_sym_alpha_i_1 : self.m_dhparams[ RDHparams.alpha_i_1 ],
self.m_sym_a_i_1 : self.m_dhparams[ RDHparams.a_i_1 ],
self.m_sym_d_i : self.m_dhparams[ RDHparams.d_i ],
self.m_sym_theta_i : self.m_dhparams[ RDHparams.theta_i ] }
self.m_jointType = self._getJointType()
self._buildSymTransform()
self._updateTransform()
def setDHparamValue( self, indx, value ) :
if indx < 0 or indx > 3 :
print 'RDHentry> tried to access invalid dh param'
return
self.m_dhparams[ indx ] = value
self._updateTransform()
def getDHparamValue( self, indx ) :
if indx < 0 or indx > 3 :
print 'RDHentry> tried to access invalid dh param'
return 0.0
return self.m_dhparams[ indx ]
def setJointValue( self, value ) :
if value < self.m_minJointValue :
print 'RDHmodel> min value reached: ', self.m_minJointValue
value = self.m_minJointValue
elif value > self.m_maxJointValue :
print 'RDHmodel> max value reached: ', self.m_maxJointValue
value = self.m_maxJointValue
if self.m_jointType == RJointType.REVOLUTE :
self.m_dhparams[ RDHparams.theta_i ] = value
elif self.m_jointType == RJointType.PRISMATIC :
self.m_dhparams[ RDHparams.d_i ] = value
self._updateTransform()
def getJointValue( self ) :
if self.m_jointType == RJointType.REVOLUTE :
return self.m_dhparams[ RDHparams.theta_i ]
elif self.m_jointType == RJointType.PRISMATIC :
return self.m_dhparams[ RDHparams.d_i ]
def getDHparamsBehavior( self ) :
return self.m_fixed
def getDHparams( self ) :
return self.m_dhparams
def getMinJointValue( self ) :
return self.m_minJointValue
def getMaxJointValue( self ) :
return self.m_maxJointValue
def getRangeJointValue( self ) :
return self.m_rangeJointValue
def getTransform( self ) :
return self.m_transform
def getSymTransform( self ) :
return self.m_symTransform
def getSymTransformEvaluated( self ) :
return self.m_symTransform.evalf( subs = self.m_dhparamsSym )
"""
Return which kind of joint this entry represents
"""
def _getJointType( self ) :
if not self.m_fixed[ RDHparams.theta_i ] :
return RJointType.REVOLUTE
elif not self.m_fixed[ RDHparams.d_i ] :
return RJointType.PRISMATIC
print 'RDHentry> this joint has no degree of freedom :('
return RJointType.NONE
"""
Builds the symbolic representation of this entry's world-transform
"""
def _buildSymTransform( self ):
self.m_symTransform[0,0] = cos( self.m_sym_theta_i )
self.m_symTransform[1,0] = sin( self.m_sym_theta_i ) * cos( self.m_sym_alpha_i_1 )
self.m_symTransform[2,0] = sin( self.m_sym_theta_i ) * sin( self.m_sym_alpha_i_1 )
self.m_symTransform[3,0] = 0
self.m_symTransform[0,1] = -sin( self.m_sym_theta_i )
self.m_symTransform[1,1] = cos( self.m_sym_theta_i ) * cos( self.m_sym_alpha_i_1 )
self.m_symTransform[2,1] = cos( self.m_sym_theta_i ) * sin( self.m_sym_alpha_i_1 )
self.m_symTransform[3,1] = 0
self.m_symTransform[0,2] = 0
self.m_symTransform[1,2] = -sin( self.m_sym_alpha_i_1 )
self.m_symTransform[2,2] = cos( self.m_sym_alpha_i_1 )
self.m_symTransform[3,2] = 0
self.m_symTransform[0,3] = self.m_sym_a_i_1
self.m_symTransform[1,3] = -sin( self.m_sym_alpha_i_1 ) * self.m_sym_d_i
self.m_symTransform[2,3] = cos( self.m_sym_alpha_i_1 ) * self.m_sym_d_i
self.m_symTransform[3,3] = 1
"""
Updates both numeric and symbolic world-transforms
"""
def _updateTransform( self ) :
_alpha_i_1 = self.m_dhparams[ RDHparams.alpha_i_1 ]
_a_i_1 = self.m_dhparams[ RDHparams.a_i_1 ]
_d_i = self.m_dhparams[ RDHparams.d_i ]
_theta_i = self.m_dhparams[ RDHparams.theta_i ]
if self.m_jointType == RJointType.REVOLUTE :
_theta_i = _theta_i * self.m_jointSign + self.m_jointOffset
elif self.m_jointType == RJointType.PRISMATIC :
_d_i = _d_i * self.m_jointSign + self.m_jointOffset
self.m_dhparamsSym[ self.m_sym_alpha_i_1 ] = _alpha_i_1
self.m_dhparamsSym[ self.m_sym_a_i_1 ] = _a_i_1
self.m_dhparamsSym[ self.m_sym_d_i ] = _d_i
self.m_dhparamsSym[ self.m_sym_theta_i ] = _theta_i
# update numeric transform
self.m_transform[0,0] = np.cos( _theta_i )
self.m_transform[1,0] = np.sin( _theta_i ) * np.cos( _alpha_i_1 )
self.m_transform[2,0] = np.sin( _theta_i ) * np.sin( _alpha_i_1 )
self.m_transform[3,0] = 0
self.m_transform[0,1] = -np.sin( _theta_i )
self.m_transform[1,1] = np.cos( _theta_i ) * np.cos( _alpha_i_1 )
self.m_transform[2,1] = np.cos( _theta_i ) * np.sin( _alpha_i_1 )
self.m_transform[3,1] = 0
self.m_transform[0,2] = 0
self.m_transform[1,2] = -np.sin( _alpha_i_1 )
self.m_transform[2,2] = np.cos( _alpha_i_1 )
self.m_transform[3,2] = 0
self.m_transform[0,3] = _a_i_1
self.m_transform[1,3] = -np.sin( _alpha_i_1 ) * _d_i
self.m_transform[2,3] = np.cos( _alpha_i_1 ) * _d_i
self.m_transform[3,3] = 1
# update symbolic transform
self.m_symTransform.subs( self.m_dhparamsSym )