inverse kinematics solutions_lec_5
DESCRIPTION
inverse kinematics solution lecture five. very helpfulTRANSCRIPT
![Page 1: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/1.jpg)
Inverse Kinematics –IKS Solutions
ME 4135 – Robotics and ControlsR.R. Lindeke, Ph.D., Fall 2011
![Page 2: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/2.jpg)
FKS vs. IKS
• In FKS we built a tool for finding end frame geometry from Given Joint data:
• In IKS we need Joint models from given End Point POSE Geometry:
Joint Space
Cartesian Space
Joint Space
Cartesian Space
![Page 3: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/3.jpg)
So this IKS is ‘Nasty’ – it is a so-call Hard Mathematical Problem
• It a more difficult problem because:– The Equation set is “Over-Specified”
• 12 equations in 6 unknowns
– Space may be “Under-Specified”• for example ‘Planer devices’ with more joints than 2
– The Solution set can contain Redundancies • Thus they exhibit Multiple solutions
– The Solution Sets may be un-defined• Unreachable in 1 or many joints
![Page 4: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/4.jpg)
But the IKS in Critical in Robotics
• Builds Workspace limits and controller maps
• Allows “Off-Line Programming” solutions
• Thus, compares Workspace capabilities with Programming desirability to assure that execution is feasible
• Aids in Workplace design and operational simulations
![Page 5: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/5.jpg)
Doing a Pure IKS solution: the R Manipulator
X0
Y0
Z0
Z1
X1
Y1 Y2
X2
Z2
R Frame Skeleton
![Page 6: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/6.jpg)
LP Table and Ai’s
1
1 0 1 0
1 0 1 0
0 1 0 0
0 0 0 1
S C
C SA
Frames Link Var d l S C S C
0 → 1 1 R + 90 0 0 90 1 0 C1 -S1
1 → 2 2 P 0 d2 + cl2 0 0 1 0 1 0
22 2
1 0 0 0
0 1 0 0
0 0 1
0 0 0 1
Ad cl
![Page 7: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/7.jpg)
FKS is A1*A2:
2 2
1 0 1 0 1 0 0 0
1 0 1 0 0 1 0 0
0 1 0 0 0 0 1
0 0 0 1 0 0 0 1
S C
C S
d cl
2 2
2 2
1 0 1 1 ( )
1 0 1 1 ( )
0 1 0 0
0 0 0 1
S C C d cl
C S S d cl
![Page 8: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/8.jpg)
Forming The IKS:
2 2
2 2
1 0 1 1 ( )
1 0 1 1 ( )
0 1 0 0
0 0 0 1
S C C d cl
C S S d cl
0 0 0 1
x x x x
y y y y
z z z z
n s a d
n s a d
n s a d
![Page 9: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/9.jpg)
Forming The IKS:• Examining the two sides of this FKS equation
– n, s, a and d are given in an inverse sense – since we desire to put the manipulator on a particular target
– But we will build the general solution only• Term (1, 4) & (2,4) both side allow us to find an equation for :
• Select (1,4): C1*(d2+cl2) = dx
• Select (2,4): S1*(d2+cl2) = dy
• Form a ratio of Term2,4 by Term1,4 to build an equation for Tan() and then :
• S1/C1 = dy/ dx
• Tan = dy/dx
• = Atan2(dx, dy)
![Page 10: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/10.jpg)
Forming The IKS:
• After is found, back substitute and solve for d2:
• Choose term(1,4): C1*(d2+cl2) = dx
• Isolating d2: d2 = [dx/C1] - cl2
![Page 11: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/11.jpg)
Alternative Method – doing a pure inverse approach
• Form [A1]-1 then pre-multiply both side by this ‘inverse’
• Leads to: A2 = A1-1*T0
ngiven
2 2
1 0 0 0 1 1 0 0
0 1 0 0 0 0 1 0
0 0 1 1 1 0 0
0 0 0 1 0 0 0 1 0 0 0 1
x x x x
y y y y
z z z z
S C n s a d
n s a d
d cl C S n s a d
![Page 12: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/12.jpg)
After Simplifying the RHS:
1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1
0 0 0 1
x y x y x y x y
z z z z
x y x y x y x y
S n C n S s C s S a C a S d C d
n s a d
C n S n C s S s C a S a C d S d
2 2
1 0 0 0
0 1 0 0
0 0 1
0 0 0 1
d cl
![Page 13: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/13.jpg)
Solving:
• Selecting and Equating terms (1,4)• 0 = -S1*dx + C1*dy
• Solving: S1*dx = C1*dy
• Tan() = (S1/C1) = (dy/dx)
• = Atan2(dx, dy)
• Selecting and Equating terms (3,4) -- after back substituting solution – and note, after the above step, is known as an angle
• d2 + cl2 = C1*dx + S1*dy
• d2 = C1*dx + S1*dy - cl2
d2 = Cos[Atan2(dx, dy)]*dx + Sin[Atan2(dx, dy)]*dy –cl2
![Page 14: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/14.jpg)
Performing IKS For Industrial Robots:
• First lets consider the previously defined Spherical Wrist simplification– All Wrist joint Z’s intersect at a point– The n Frame is offset from this intersection as a
distance dn along the a vector of the desired solution (3rd column of desired orientation sub-matrix)
– This is as expected by the DH Algorithm
![Page 15: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/15.jpg)
Performing IKS
• We can now separate the effects of the ARM joints – Joints 1 to 3 in a full function manipulator (without
redundant joints)– They function to position the spherical wrist at a target
POSITION related to the desired target POSE
• Arm Joints are separated from the WRIST Joints– Joints 4 to 6 in a full functioning spherical wrist– Wrist Joints function as a primary tool to ORIENT the end
frame as required by the desired target POSE
![Page 16: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/16.jpg)
Performing IKS: Focus on Positioning
• We will define a point (the WRIST CENTER) as:– Pc = [Px, Py, Pz]
– Here we define Pc = dtarget - dn*a• Px = dtarget,x - dn*ax
• Py = dtarget,y - dn*ay
• Pz = dtarget,z - dn*az
![Page 17: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/17.jpg)
Focusing on the ARM Manipulators in terms of Pc:
• Prismatic:• q1 = Pz (its along Z0!) – cl1
• q2 = Px or Py - cl2
• q3 = Py or Px - cl3
• Cylindrical:• 1 = Atan2(Px, Py)
• d2 = Pz – cl2
• d3 = Px*C1 – cl3 or +(Px2 + Py
2).5 – cl3
![Page 18: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/18.jpg)
Focusing on the ARM Manipulators in terms of Pc:
• Spherical:• 1 = Atan2(Px, Py)• 2 = Atan2( (Px
2 + Py2).5 , Pz)
• D3 = (Px2 + Py
2 + Pz2).5 – cl3
![Page 19: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/19.jpg)
Focusing on the ARM Manipulators in terms of Pc:
• Articulating:• 1 = Atan2(Px, Py)• 3 = Atan2(D, (1 – D2).5)
– Where D =
• 2 = - – is: Atan2((Px2 + Py2).5, Pz)
– is:
2 2 2 2 22 3
2 32x y zP P P l l
l l
2 2 2 2 2 22 3 2 3
tan 2(cos ,sin )
tan 2 , 2 1x y z
A
A P P P l l l l D
![Page 20: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/20.jpg)
One Further Complication:
• This is called the d2 offset problem
• A d2 offset is a problem that states that the n frame has a non-zero offset along the Y0 axis as observed with all joints at home, in the solution of the 0Tn
• This leads to two solutions for 1 the So-Called Shoulder Left and Shoulder Right solutions
![Page 21: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/21.jpg)
Defining the d2 Offset issue
X0, X1
Y0, Z1
Z0 d2
The ARMThe Wrist
Ypc
Xpc
Zpc
Here: ‘The ARM’ might be a prismatic joint as in the Stanford Arm or it might be l2 & l3 links in an Articulating Arm and rotates out of plane
A d2 offset means that there are two places where 1 can be placed to touch a given point (and note, when 1 is at Home, the wrist center is not on the X0 axis!)
![Page 22: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/22.jpg)
Lets look at this Device “From the Top”
Pc'(Px, Py)
X0
Y0
Z1
Z1'
X1
d2
d2
a2'
a3'
R'
X1'
L2’
L3’
11
X1
![Page 23: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/23.jpg)
Solving For 1
• We will have a Choice (of two) poses for :
1 1 1
1
.52 2 22 2
1
1 tan2( , )
tan 2 ,
pc pc
pc pc
A X Y
A X Y d d
2 1 1
.52 2 22 2
1 180
180 tan2 ,
tan 2 ,
pc pc
pc pc
A X Y d d
A X Y
![Page 24: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/24.jpg)
In this so-called “Hard Arm”
• We have two 1’s
• These lead to two 2’s (Spherical)
• Or to four 2’s and 3’s in the Articulating Arm • Shoulder Right Elbow Up & Down• Shoulder Left Elbow Up & Down
![Page 25: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/25.jpg)
The Orientation Model
• Evolves from:
• Separates Arm Joint and Wrist Joint Contribution to Target (given) orientation
3 60 3 givenR R R
![Page 26: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/26.jpg)
Focusing on Orientation Issues
• Lets begin by considering Euler Angles (they are a model that is almost identical to a full functioning Spherical Wrist):
• Form Product:– Rz1*Ry2*Rz3
– This becomes R36
1
2
3
cos sin 0
sin cos 0
0 0 1
cos 0 sin
0 1 0
sin 0 cos
cos sin 0
sin cos 0
0 0 1
z
y
z
R
R
R
![Page 27: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/27.jpg)
Euler Wrist Simplified:C C C S S C C S S C C S
S C C C S S C S C C S S
S C S S C
And this matrix is equal to a U matrix prepared by multiplying the inverse of the ARM joint orientation matrices inverse and the Desired (given) target orientation
130
x x x
y y y
z z z given
n s a
R n s a
n s a
NOTE: R0
3 is Manipulator dependent!
![Page 28: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/28.jpg)
Simplifying the RHS: (our so-called U Matrix)
30
130
11 12 13
21 22 23
31 32 33
11 21 31
12 22 32
13 23 33
(this is a transpose!)
R R R
R R R R
R R R
R R R
R R R R
R R R
![Page 29: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/29.jpg)
Continuing:
11 12 13
21 22 23
31 32 33
11 21 31 11 21 31 11 21 31
12 22 32 12 22 32 12 22 32
13 23 33 13 23 33 13 23 33
x y z x y z x y z
x y z x y z x y z
x y z x y z x y z
U U U
U U U
U U U
n R n R n R s R s R s R a R a R a R
n R n R n R s R s R s R a R a R a R
n R n R n R s R s R s R a R a R a R
![Page 30: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/30.jpg)
Finally:C C C S S C C S S C C S
S C C C S S C S C C S S
S C S S C
11 12 13
21 22 23
31 32 33
U U U
U U U
U U U
![Page 31: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/31.jpg)
Solving for Individual Orientation Angles (1st ):
• Selecting (3,3)→ C = U33
• With C we “know” S = (1-C2).5
• Hence: = Atan2(U33, (1-U332).5
• NOTE: 2 solution for !
![Page 32: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/32.jpg)
Re-examining the Matrices:
• To solve for : Select terms: (1,3) & (2,3) CS = U13
SS = U23
Dividing the 2nd by the 1st: S /C = U23/U13
so Tan() = U23/U13
leading to: = Atan2(U13, U23)
![Page 33: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/33.jpg)
Continuing our Solution:
• To solve for : Select terms: (3,1) & (3,2) -SC = U31
SS = U32
so Tan() = U32/-U31 (note the ‘minus’ sign track the appropriate term!)
And Thus = Atan2(-U31, U32)
![Page 34: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/34.jpg)
Summarizing:
• = Atan2(U33, (1-U332).5
• = Atan2(U13, U23)
• = Atan2(-U31, U32)
![Page 35: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/35.jpg)
Let’s examine a Spherical Wrist:
Z3
X3
Y3
Z4
X4
Y4
Z5
X5
Y5
Z6
X6
Y6
![Page 36: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/36.jpg)
IKSing the Spherical WristFrames Link Var d l
3 → 4 4 R 4 0 0 -90
4 → 5 5 R 5 0 0 +90
5 → 6 6 R 6 d6 0 0
4
4 0 4 5 0 5 6 6 0
4 0 4 ; 5 5 0 5 ; 6 6 6 0
0 1 0 0 1 0 0 0 1
C S C S C S
R S C R S C R S C
![Page 37: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/37.jpg)
Writing The Solution:
11 12 13
21 22 23
31 32 33
4 0 4 5 0 5 6 6 0
4 0 4 5 0 5 6 6 0
0 1 0 0 1 0 0 0 1
C S C S C S
S C S C S C
U U U
U U U
U U U
![Page 38: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/38.jpg)
Lets See (By Pure Inverse) Technique:
11 12 13
21 22 23
31 32 33
5 0 5 6 6 0
5 0 5 6 6 0
0 1 0 0 0 1
4 4 0
0 0 1
4 4 0
C S C S
S C S C
C S U U U
U U U
S C U U U
![Page 39: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/39.jpg)
Simplifying
11 21 12 22 13 23
31 32 33
21 11 22 12 23 13
5 6 5 6 5
5 6 5 6 5
6 6 0
4 4 4 4 4 4
4 4 4 4 4 4
C C C S S
S C S S C
S C
C U S U C U S U C U S U
U U U
C U S U C U S U C U S U
![Page 40: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/40.jpg)
Solving:
• After Examination here let’s select (3,3) both sides:
• 0 = C4U23 – S4U13
• S4U13 = C4U23
• Tan(4) = S4/C4 = U23/U13
• 4 = Atan2(U13, U23)
• With the given and back-substituted values (from the arm joints) we have a value for 4
the RHS is completely known
![Page 41: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/41.jpg)
Solving for 5 & 6
• For 5: Select (1,3) & (2,3) terms• S5 = C4U13 + S4U23
• C5 = U33
• Tan(5) = S5/C5 = (C4U13 + S4U23)/U33
• 5 = Atan2(U33, C4U13 + S4U23)
• For 6: Select (3,1) & (3, 2)• S6 = C4U21 – S4U11
• C6 = C4U22 – S4U12
• Tan(6) = S6/C6 = ([C4U21 – S4U11],[C4U22 – S4U12])
• 6 = Atan2 ([C4U21 – S4U11], [C4U22 – S4U12])
![Page 42: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/42.jpg)
Summarizing:
• 4 = Atan2(U13, U23)
• 5 = Atan2(U33, C4U13 + S4U23)
• 6 = Atan2 ([C4U21 – S4U11], [C4U22 – S4U12])
![Page 43: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/43.jpg)
Lets Try One:
• Cylindrical Robot w/ Spherical Wrist• Given a Target matrix (it’s an IKS after all!)• The d3 “constant” is 400mm; the d6 offset (call
it the ‘Hand Span’) is 150 mm.• 1 = Atan2((dx – ax*150),(dy-ay*150))
• d2 = (dz – az*150)
• d3 = ((dx – ax*150)2,(dy-ay*150)2).5 - 400
![Page 44: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/44.jpg)
The Frame Skeleton:
X
ZF0
F1 Z
X
X
F2 Z
Z
X
F6
F2.5
Z
X
F4
XF3
Z
F5
Z
X
X
Z
Note “Dummy” Frame to account for Orientation problem with Spherical Wrist
![Page 45: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/45.jpg)
Solving for U:
1 1 0 0 0 1 0 0 1 0 1 0
1 1 0 1 0 0 1 0 0 1 0 0
0 0 1 0 1 0 0 1 0 0 0 1
x x x
y y y
z z z
C S n o a
U S C n o a
n o a
NOTE: We needed a “Dummy Frame” to account for the Orientation issue at the end of the Arm
![Page 46: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/46.jpg)
Simplifying:
11 12 13
21 22 23
31 32 33
1 1 1 1 1 1
1 1 1 1 1 1x z x z x z
x z x z x z
y y y
U U U C n S n C s S s C a S a
U U U S n C n S s C s S a C a
U U U n o a
![Page 47: Inverse Kinematics Solutions_Lec_5](https://reader036.vdocuments.us/reader036/viewer/2022082204/55cf9135550346f57b8b89c0/html5/thumbnails/47.jpg)
Subbing Uij’s Into Spherical Wrist Joint Models:
• 4 = Atan2(U13, U23)= Atan2((C1ax + S1az), (S1ax-C1az))
• 5 = Atan2(U33, C4U13 + S4U23)= Atan2{ay, [C4(C1ax+S1az) + S4 (S1ax-C1az)]}
• 6 = Atan2 ([C4U21 - S4U11], [C4U22 - S4U12]) = Atan2{[C4(S1nx-C1nz) - S4(C1nx+S1nz)],
[C4(S1sx-C1sz) - S4(C1sx+S1sz)]}