-
Notifications
You must be signed in to change notification settings - Fork 12
/
Copy pathdavinci_notes
217 lines (146 loc) · 6.71 KB
/
davinci_notes
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
214
215
216
davinci frames in rviz/urdf:
one_tool_tip_link:
origin at gripper tip,
z-axis points parallel to jaws,
y-axis is dir of jaw opening
parent: one_tool_wrist_sca_shaft_link:
one_tool_wrist_sca_shaft_link:
z-axis is along gripper-jaw rotation joint;
y-axis lies along gripper jaws (parallel to jaws)
origin is on gripper-jaw rotation axis
parent: one_tool_wrist_sca_link
one_tool_wrist_sca_link:
z-axis is along wrist tilt
x-axis points towards (and intersects) jaw-rotation axis (z-axis of child frame above)
origin is centered on wrist-tilt axis, between bearings
parent: one_tool_wrist_shaft_link
one_tool_wrist_shaft_link:
origin coincident w/ child frame above
z-axis points out, coincident with shaft centerline
y-axis is coincident w/ child z-axis (wrist bend axis) at shaft-rotation = 0
parent: one_tool_wrist_link
tree:
world
one_psm_base_link
one_outer_yaw_link
one_outer_pitch_link
one_tool_main_link
one_tool_wrist_link
one_tool_wrist_shaft_link
one_tool_wrist_sca_link
one_tool_wrist_sca_shaft_link
one_tool_tip_link
one_outer_yaw_link is at trocar constraint point
one_outer_pitch_link is also at trocar constraint point
one_tool_main_link is back at motors location on prismatic joint, moving with tool
one_psm_base_link is stationary, w/
origin at constraint point,
z-axis "up" and
y-axis colinear w/ first joint (yaw= tilt) axis, pointing AWAY from base
child: one_outer_yaw_link
one_outer_yaw_link:
origin coincident w/ one_psm_base_link at constraint point
z-axis points along yaw tilt axis of jnt1, w/ z pointing TOWARDS base
at jnt1-ang=0, z-axis is UP
child: one_outer_pitch_link
one_outer_pitch_link:
origin coincident w/ constraint point
z-axis is axis of rotation, positive to robot's LEFT,
is coincident with y-axis of parent frame
at jnt2-ang = 0, y-axis of one_outer_pitch_link is coincident
w/ x-axis of one_outer_yaw_link, which points along tool centerline TOWARDS motors
child: one_tool_main_link
one_tool_main_link:
origin is at motors on tool;
z-axis points along tool centerline, from motors towards gripper;
x-axis points back towards base (y-axis is to robot's left)
child is one_tool_wrist_link
one_tool_wrist_link:
origin is at wrist-bend axis
this frame moves w/ tool-shaft rotation
frame z-axis is parallel to parent from, colinear w/ tool centerline, pointing towards gripper
frame is aligned w/ parent at zero angle of tool spin (q4)
child is: one_tool_wrist_shaft_link
one_tool_wrist_shaft_link: looks identical to one_tool_wrist_link
THESE ARE RELATED BY A FIXED JOINT--COINCIDENT!
child: one_tool_wrist_sca_link
one_tool_wrist_sca_link:
moves w/ wrist bend, q5
origin coincident w/ parent
z-axis along wrist-bend joint
x-axis points towards gripper tips (at gripper jaws q6 = 0)
-----------------------
set all jnts to 0;
transform:
"one_tool_wrist_sca_shaft_link", "one_tool_tip_link"
affine linear (R):
0 -1 4.89653e-12
4.89653e-12 4.89653e-12 1
-1 0 4.89653e-12
origin: 0 0.0102 0
offset is 0.0102;
--------------------
frame one_tool_wrist_sca_shaft_link w/rt one_tool_wrist_link
affine linear (R):
3.67322e-06 -7.3464e-06 1
1 3.67322e-06 -3.67319e-06
-3.67319e-06 1 7.34641e-06
origin: -3.34262e-08 1.73472e-18 0.0091
offset is 0.0091 (from gripper-jaw axis to wrist-bend axis)
----
frame one_psm_base_link to one_tool_tip_link
affine linear (R):
-7.3464e-06 1 -3.67328e-06
1 7.34645e-06 1.46928e-05
1.46928e-05 -3.67317e-06 -1
origin: 1.54862e-06 1.35547e-07 -0.0037
at all jnts=0, trocar constraint origin is between gripper tip and gripper-jaw axis,
----IK...
REFERENCE EVERYTHING TO one_psm_base_link:
this frame has:
origin at pivot point
z "up"
x to robot's right,
y forward
start w/ gripper jaws closed--> "one_tool_tip_link" has z-axis pointing out, parallel to jaws;
--> origin of jaw-bend axis is at O_tip - z_tip*0.0102
...plan: given gripper-tip frame, can compute origin of gripper-jaw axis frame;
frame: one_tool_wrist_sca_shaft_link:
has origin on gripper-jaw axis
has z-axis along gripper-jaw axis
rotates w/ jaw rotation
given tool-tip frame (w/rt one_psm_base_link) can find frame one_tool_wrist_sca_shaft_link (as above)
this frame moves w/ q5 (gripper jaws rotation).
given frame one_tool_wrist_sca_shaft_link, find frame one_tool_wrist_sca_link
one_tool_wrist_sca_link moves w/ wrist bend
has z-axis colinear w/ wrist-bend axis
has origin on wrist-bend axis
has x-axis pointing towards gripper-jaw rotation axis
Given desired gripper frame, and given gripper-jaw rotation angle, find frame frame one_tool_wrist_sca_link
ref frame: one_psm_base_link,
when jaw-bend (call it q6) is q6 = 0, then gripper jaws are straight out; y-axis of frame one_tool_wrist_sca_shaft_link
is parallel to tool shaft, i.e. is coincident w/ x-axis of frame
Then frame one_tool_wrist_sca_shaft_link (distal frame) aligns w/ one_tool_wrist_sca_link,
axes of distal frame w/rt proximal frame = R_{one_tool_wrist_sca_shaft_link/one_tool_wrist_sca_link}
R(q6=0) = [0 1 0
0 0 1
1 0 0]
w/ q6!=0:
R(q6) = [sin(q6) cos(q6) 0
0 0 1
cos(q6) -sin(q6) 0]
== R_{6/5}
GIVEN R_one_tool_wrist_sca_shaft_link/one_psm_base_link,
FIND R_{one_tool_wrist_sca_link/one_psm_base_link}
call these R_{6/5}, R_{6/0}, R_{5/0}, where R_{6/5} depends on q1 through q6
R_{6/0} = R_{6/5} * R_{5/0}
==>R_{5/0} = R_{6/5}'*R_{6/0}
want to back up distance a6 along axis x_5 (first column of R_{5/0} to get to origin of frame 5 (w/rt 0)
O_{5/0} = O_{6/0} - a6*xvec_{5/0}
AND
R_{5/0} = R_{6/5}'*R_{6/0}
therefore, frame 5 wrt 0 follows from given frame 6 wrt 0 and angle q6.
NEXT,
given frame 5, compute frame 4 (one_tool_wrist_shaft_link) as a fnc of wrist bend, q5
xxx
one_tool_wrist_shaft_link: does NOT move w/rt wrist bend; good frame to use upstream of wrist bend;