ch. 3: inverse kinematics ch. 4: velocity kinematics · the interventional centre recap: kinematic...
TRANSCRIPT
The Interventional Centre
Ch. 3: Inverse KinematicsCh. 4: Velocity Kinematics
The Interventional Centre
Recap: kinematic decoupling• Appropriate for systems that have an arm a wrist
– Such that the wrist joint axes are aligned at a point
• For such systems, we can split the inverse kinematics problem into two parts:
1. Inverse position kinematics: position of the wrist center2. Inverse orientation kinematics: orientation of the wrist
• First, assume 6DOF, the last three intersecting at oc
• Use the position of the wrist center to determine the first three joint angles…
( )( ) oqqo
RqqR
=
=
6106
6106
,...,,...,
The Interventional Centre
Recap: kinematic decoupling• Now, origin of tool frame, o6, is a distance d6 translated along z5 (since z5 and
z6 are collinear)– Thus, the third column of R is the direction of z6 (w/ respect to the base frame)
and we can write:
– Rearranging:
– Calling o = [ox oy oz]T, oc0 = [xc yc zc]T
+==
100
606 Rdooo o
c
−=
100
6Rdoooc
−−−
=
336
236
136
rdordordo
zyx
z
y
x
c
c
c
The Interventional Centre
Recap: kinematic decoupling• Since [xc yc zc]T are determined from the first three joint angles, our forward
kinematics expression now allows us to solve for the first three joint angles decoupled from the final three.
– Thus we now have R30
– Note that:
– To solve for the final three joint angles:
– Since the last three joints for aspherical wrist, we can use a set ofEuler angles to solve for them
36
03RRR =
( ) ( ) RRRRR T03
103
36 ==
−
The Interventional Centre
Recap: Inverse position kinematics
• Now that we have [xc yc zc]T we need to find q1, q2, q3– Solve for qi by projecting onto the xi-1, yi-1 plane, solve trig
problem– Two examples
• elbow (RRR) manipulator: 4 solutions (left-arm elbow-up, left-arm elbow-down, right-arm elbow-up, right-arm elbow-down)
• spherical (RRP) manipulator: 2 solutions (left-arm, right-arm)
The Interventional Centre
Inverse orientation kinematics
• Now that we can solve for the position of the wrist center (given kinematic decoupling), we can use the desired orientation of the end effector to solve for the last three joint angles– Finding a set of Euler angles corresponding to a desired rotation
matrix R– We want the final three joint angles that give the orientation of the
tool frame with respect to o3 (i.e. R63)
The Interventional Centre
Inverse orientation: spherical wrist• Previously, we said that the forward kinematics of the spherical
wrist were identical to a ZYZ Euler angle transformation:
−+−+−−−
==
10006556565
654546465464654
654546465464654
6543
6 dcccscsdssssccscsscccsdscsccssccssccc
AAAT
The Interventional Centre
• The inverse orientation problem reduces to finding a set of Euler angles (θ4, θ5, θ6) that satisfy:
• to solve this, take two cases:1. Both r13 and r23 are not zero (i.e. θ5 ≠ 0)… nonsingular2. θ5 = 0, thus r13 = r23 = 0… singular
• Nonsingular case– If θ5 ≠ 0, then r33 ≠ ±1 and:
−+−+−−−
=
56565
546465464654
54646546465436
ccscsssccscsscccssccssccssccc
R
−±=
−±==
233335
2335335
1,2
1
rr
rsrc
atan
,
θ
Inverse orientation: spherical wrist
The Interventional Centre
• Thus there are two values for θ5. Using the first (s5 > 0):
• Using the second value for θ5 (s5 < 0):
• Thus for the nonsingular case, there are two solutions for the inverse orientation kinematics
Inverse orientation: spherical wrist
( )( )32316
23134
,2,2
rrrr
−=
=
atanatan
θθ
( )( )32316
23134
,2,2rr
rr−=
−−=
atanatan
θθ
The Interventional Centre
• In the singular case, θ5 = 0 thus s5 = 0 and r13 = r23 = r31 = r32 = 0• Therefore, R6
3 has the form:
• So we can find the sum θ4 + θ6 as follows:
• Since we can only find the sum, there is an infinite number of solutions (singular configuration)
Inverse orientation: spherical wrist
=
−=
+−+−−−
=10000
10000
10000
2221
1211
4646
4646
64646464
6464646436 rr
rrcssc
ccsssccscsscsscc
R
( ) ( )1211211164 ,2,2 rrrr −==+ atanatanθθ
The Interventional Centre
Inverse Kinematics: general procedure
1. Find q1, q2, q3 such that the position of the wrist center is:
2. Using q1, q2, q3, determine R30
3. Find Euler angles corresponding to the rotation matrix:
−=
100
6Rdoooc
( ) ( ) RRRRR T03
103
36 ==
−
inverse positionkinematics
inverse orientationkinematics
The Interventional Centre
Example: RRR arm with spherical wrist
• For the DH parameters below, we can derive R30 from the forward
kinematics:
• We know that R63 is given as follows:
• To solve the inverse orientation kinematics:
– For a given desired R
link ai αi di θi
1 0 90 d1 θ1
2 a2 0 0 θ2
3 a3 0 0 θ3
−−
−=
02323
1231231
123123103
cscsscs
sscccR
−+−+−−−
=
56565
546465464654
54646546465436
ccscsssccscsscccssccssccssccc
R
( ) RRR T03
36 =
The Interventional Centre
Example: RRR arm with spherical wrist
• Euler angle solutions can be applied. Taking the third column of (R30)TR
• Again, if θ5 ≠ 0, we can solve for θ5:
• Finally, we can solve for the two remaining angles as follows:
• For the singular configuration (θ5 = 0), we can only find θ4 + θ6 thus it is common to arbitrarily set θ4 and solve for θ6
2311315
3323232311323154
3323232311323154
rcrscrcrssrscss
rsrcsrccsc
−=
+−−=
++=
( )
−−±−= 2
2311312311315 1,2 rcrsrcrsatanθ
( )( )2211212111116
33232323113231332323231132314
,2,2
rcrsrcrsrcrssrscrsrcsrcc
−+−=
+−−++=
atanatan
θθ
The Interventional Centre
Example: elbow manipulator with spherical wrist
• Derive complete inverse kinematics solution
• we are given H = T60 such that:
link
ai αi di θi
1 0 90 d1 θ1
2 a2 0 0 θ2
3 a3 0 0 θ3
4 0 -90 0 θ4
5 0 0 0 θ5
6 0 0 d6 θ3
=
=
333231
232221
131211
rrrrrrrrr
Rooo
o
z
y
x
,
The Interventional Centre
Example: elbow manipulator with spherical wrist
• First, we find the wrist center:
• Inverse position kinematics:
• Where d is the shoulder offset (if any) and D is given by:
−−−
=
336
236
136
rdordordo
zyx
z
y
x
c
c
c
( )( )
( )23
333321222
2
1
1,2
,2,2
,2
DD
sacaadzdyx
yx
ccc
cc
−±=
+−
−−+=
=
atan
atanatan
atan
θ
θ
θ
( )32
23
22
21
222
2 aaaadzdyxD ccc −−−+−+
=
The Interventional Centre
Example: elbow manipulator with spherical wrist
• Inverse orientation kinematics:– Now that we know θ1, θ2, θ3, we know R3
0. need to find R36:
• Solve for θ4, θ5, θ6, Euler angles:( ) RRR T0
336 =
( )( )
( )2211212111116
22311312311315
33232323113231332323231132314
,2
1,2
,2
rcrsrcrs
rcrsrcrs
rcrssrscrsrcsrcc
−+−=
−−±−=
+−−++=
atan
atan
atan
θ
θ
θ
The Interventional Centre
Example: inverse kinematics of SCARA manipulator
• We are given T40:
−−−+−−−+−+
=
=
1000100
00
10
43
12211412412412412
12211412412412412
04
ddsasassccsccscacasccssscc
oRT
link
ai αi di θi
1 a1 0 0 θ1
2 a2 180 0 θ2
3 0 0 d3 0
4 0 0 d4 θ4
The Interventional Centre
Example: inverse kinematics of SCARA manipulator
• Thus, given the form of T40, R must have the following form:
• Where α is defined as:• To solve for θ1 and θ2 we project the manipulator onto the x0-y0 plane:
• This gives two solutions for θ2:• Once θ2 is known, we can solve for θ1:
• θ4 is now give as:• Finally, it is trivial to see that d3 = oz + d4
−=
10000
αα
αα
cssc
R
( )1211421 ,2 rratan=−+= θθθα
21
22
21
22
2 2 aaaaoo
c yx −−+=
−±= 2
222 1,2 ccatanθ
( ) ( )222211 ,2,2 sacaaoo yx += atan-atanθ
( )1211214 ,2 rratan−+= θθθ
The Interventional Centre
Example: number of solutions• How many solutions to the inverse position kinematics of a planar 3-link
arm?– given a desired d=[dx dy]T, the forward kinematics can be written as:
– Therefore the inverse kinematics problem is under-constrained (two equations and three unknowns)
123312211
123312211
sasasadcacacad
y
x
++=
++=
∞ solutions is d is inside the workspace1 solution if d is on the workspace boundary0 solutions else
The Interventional Centre
Example: number of solutions• What if now we describe the desired position and orientation of the end
effector?– given a desired d=[dx dy]T, we can now call the position of o2 the ‘wrist center’.
This position is given as:
– Now we have reduced the problem to finding the joint angles that will give the desired position of the wrist center (we have done this for a 2D planar manipulator).
– Finally, θ3 is given as:
( )( )dyy
dxx
adwadw
θθ
sincos
3
3
−=
−=
∞ solutions if the wrist center is on the origin2 solutions if wrist center is inside the 2-link workspace1 solution if wrist center is on the 2-link workspace boundary0 solutions else
( )213 θθθθ +−= d
The Interventional Centre
Velocity Kinematics• Now we know how to relate the end-effector position and orientation to the
joint variables• Now we want to relate end-effector linear and angular velocities with the joint
velocities• First we will discuss angular velocities about a fixed axis• Second we discuss angular velocities about arbitrary (moving) axes• We will then introduce the Jacobian
– Instantaneous transformation between a vector in Rn representing joint velocities to a vector in R6 representing the linear and angular velocities of the end-effector
• Finally, we use the Jacobian to discuss numerous aspects of manipulators:– Singular configurations– Dynamics– Joint/end-effector forces and torques
The Interventional Centre
Angular velocity: fixed axis
• When a rigid body rotates about a fixed axis, every point moves in a circle– Let k represent the fixed axis of rotation, then the angular velocity is:
– The velocity of any point on a rigid body due to this angular velocity is:
– Where r is the vector from the axis of rotation to the point
• When a rigid body translates, all points attached to the body have the same velocity
k̂θω =
rv ×= ω
The Interventional Centre
Next class…
• Derivation of the Jacobian