![Page 1: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/1.jpg)
Forward and Inverse Kinematics
CS475 / 675, Fall 2016
Siddhartha Chaudhuri
Slides from Tiffany Barnes, Bill Baxter, Serafim Batzoglou and Jean-Claude Latombe
![Page 2: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/2.jpg)
Forward Kinematics
● Incrementally manipulating each component of a flexible,jointed object to achieve an overall desired pose
● Mathematically: find the position of the end effector,given the angles of the joints and the length of eacharticulated segment
![Page 3: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/3.jpg)
Forward Kinematics Equation
![Page 4: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/4.jpg)
Forward Kinematics EquationPx1=L1×cos (Θ 1)Py1=L1×sin(Θ 1)
![Page 5: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/5.jpg)
Forward Kinematics EquationPx2=Px1+L2×cos (Θ 1+Θ 2)Py2=Py1+L2×sin (Θ 1+Θ 2)
![Page 6: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/6.jpg)
Forward Kinematics EquationPx2=L1×cos (Θ 1)+L2×cos(Θ 1+Θ 2)Py2=L1×sin (Θ 1)+L2×sin (Θ 1+Θ 2)
![Page 7: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/7.jpg)
Drawing Kinematic Chains
● Links should be drawn from the outermost link(nearest the end effector) to the innermost(nearest the root)
● The positioning of each link requires translationsand rotations from preceding links
![Page 8: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/8.jpg)
Drawing Kinematic Chains● Starting with the end effector’s object:
![Page 9: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/9.jpg)
Drawing Kinematic Chains● Starting with the end effector’s object:
● Translate base to origin
![Page 10: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/10.jpg)
Drawing Kinematic Chains● Starting with the end effector’s object:
● Translate base to origin
● Rotate by angle
![Page 11: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/11.jpg)
Drawing Kinematic Chains● Starting with the end effector’s object:
● Now translate by length of next link...
![Page 12: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/12.jpg)
Drawing Kinematic Chains● Starting with the end effector’s object:
● Now translate by length of next link…
● … and rotate the entire chain by the angle of that link
![Page 13: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/13.jpg)
Drawing Kinematic Chains● Starting with the end effector’s object:
● Translate again by the length of the next link in the chain...
![Page 14: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/14.jpg)
Drawing Kinematic Chains● Starting with the end effector’s object:
● Translate again by the length of the next link in the chain...
● … and rotate the entire chain by the angle of that link
![Page 15: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/15.jpg)
Drawing Kinematic Chains● Starting with the next link’s object:
![Page 16: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/16.jpg)
Drawing Kinematic Chains● Starting with the next link’s object:
● Translate the object’s base to the origin...
● … and rotate by the object’s angle
![Page 17: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/17.jpg)
Drawing Kinematic Chains● Starting with the next link’s object:
● Translate by the next object’s length...
![Page 18: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/18.jpg)
Drawing Kinematic Chains● Starting with the next link’s object:
● Translate by the next object’s length...
● … and rotate the entire chain by that object’s angle
![Page 19: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/19.jpg)
Drawing Kinematic Chains● So on, and so forth...
● Place the next link
![Page 20: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/20.jpg)
Drawing Kinematic Chains● So on, and so forth...
● Place the next link
● Translate
![Page 21: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/21.jpg)
Drawing Kinematic Chains● So on, and so forth, until the chain is complete
● Place the next link
● Translate
● Rotate
![Page 22: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/22.jpg)
Inverse Kinematics● Now that we know what Forward Kinematics (FK)
is, what is Inverse Kinematics (IK)?
![Page 23: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/23.jpg)
Inverse Kinematics● Now that we know what Forward Kinematics (FK)
is, what is Inverse Kinematics (IK)?
● Inverse Kinematics: Mathematically determiningthe positions and angles of joints in a flexible,jointed object, given the position and orientationof some subset of the joints (typically the endeffectors)
![Page 24: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/24.jpg)
Inverse Kinematics
Constraints
These motions areautomatically inferred
● Now that we know what Forward Kinematics (FK)is, what is Inverse Kinematics (IK)?
“UTPoser”, Yamane and Nakamura, 2003
![Page 25: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/25.jpg)
Inverse Kinematics
● What is IK used for?● Originally used in industrial robotics for assembly
plants– To get the robot to weld this point, how do I have to position
all the links in its arm?● In computer graphics, IK is typically used for character
animation– Animator manipulates a few handles (e.g. hands, feet)– The system infers the pose of the rest of the skeleton
![Page 26: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/26.jpg)
Types of IK solutions
● Closed form/analytical solution● Calculate sequence of joint angles from the root to the
effector, allowing us to determine if a solution is evenpossible
● By parametrizing the solution, we can get a range offeasible configurations
● For a two link chain the solution is (almost) unique:
Θ 2=cos−1( x
2+ y2−L12−L2
2
2L1L2 )Θ 1=tan
−1(−L2 sinΘ 2 x+(L1+L2cosΘ 2) yL2 sinΘ 2 y+(L1+L2cosΘ 2) x )
![Page 27: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/27.jpg)
Types of IK solutions
● Closed form/analytical solution● Relatively simple solution for smaller problems● However, as the chain increases in length, each new
element adds new degrees of freedom, and theproblem quickly becomes complex
![Page 28: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/28.jpg)
Types of IK solutions
● Numerical solutions● Suitable for complex linkages● Iteratively improve solution, progressing towards goal
configuration● Cyclic Coordinate Descent (this class)● Inverse Jacobian Method (next class)
![Page 29: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/29.jpg)
Cyclic Coordinate Descent● Start with a vector from the root of our effector R to the current
endpoint E● Draw a vector from R to the desired endpoint D● The inverse cosine of the dot product gives the angle between the
vectors: cosa=R⃗ D⋅R⃗ E
![Page 30: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/30.jpg)
Cyclic Coordinate Descent● Rotate our link so that RE falls on RD
![Page 31: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/31.jpg)
Cyclic Coordinate Descent● Move one link up the chain and repeat the process
![Page 32: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/32.jpg)
Cyclic Coordinate Descent● The process is repeated until the root joint is reached. Then, the
process begins all over again starting with the end effector, andcontinues until we are close enough to D for an acceptable solution.
![Page 33: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/33.jpg)
Cyclic Coordinate Descent● The process is repeated until the root joint is reached. Then, the
process begins all over again starting with the end effector, andcontinues until we are close enough to D for an acceptable solution.
![Page 34: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/34.jpg)
Cyclic Coordinate Descent● The process is repeated until the root joint is reached. Then, the
process begins all over again starting with the end effector, andcontinues until we are close enough to D for an acceptable solution.
![Page 35: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/35.jpg)
Cyclic Coordinate Descent● The process is repeated until the root joint is reached. Then, the
process begins all over again starting with the end effector, andcontinues until we are close enough to D for an acceptable solution.
![Page 36: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/36.jpg)
Cyclic Coordinate Descent● The process is repeated until the root joint is reached. Then, the
process begins all over again starting with the end effector, andcontinues until we are close enough to D for an acceptable solution.
![Page 37: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/37.jpg)
Cyclic Coordinate Descent● The process is repeated until the root joint is reached. Then, the
process begins all over again starting with the end effector, andcontinues until we are close enough to D for an acceptable solution.
![Page 38: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/38.jpg)
Cyclic Coordinate Descent● The process is repeated until the root joint is reached. Then, the
process begins all over again starting with the end effector, andcontinues until we are close enough to D for an acceptable solution.
![Page 39: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/39.jpg)
Cyclic Coordinate Descent● The process is repeated until the root joint is reached. Then, the
process begins all over again starting with the end effector, andcontinues until we are close enough to D for an acceptable solution.
![Page 40: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/40.jpg)
Cyclic Coordinate Descent● The process is repeated until the root joint is reached. Then, the
process begins all over again starting with the end effector, andcontinues until we are close enough to D for an acceptable solution.
![Page 41: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/41.jpg)
Inverse Jacobian Method● Q: n-vector of internal parameters (joint angles)● X: 6-vector defining the endpoint’s position and
orientation● … we can have other target configurations/constraints
as well, but we’ll currently stick with this for simplicity
● Assume n ≥ 6
● Forward kinematics: X = F(Q)
●
● dX = J dQ
dxi=∂ f i (Q)d q1
dq1+∂ f i (Q)d q2
dq2+…+∂ f i (Q)d qn
dqn
![Page 42: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/42.jpg)
The Jacobian
J = [∂ f 1 (Q)d q1
∂ f 1 (Q)d q2
…∂ f 1 (Q)d qn
∂ f 2 (Q)d q1
∂ f 2 (Q)d q2
…∂ f 2 (Q)d qn
⋮ ⋮ ⋱
∂ f 6 (Q)d q1
∂ f 6 (Q)d q2
…∂ f 6 (Q)d qn
]If J is square, dX = J dQ implies dQ = J – 1 dX
![Page 43: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/43.jpg)
Case where n = 6● J is a square 6x6 matrix● Problem: Given D (desired endpoint location), find
Q such that D = F(Q)
● Solution, starting at any initial configuration X0 with known parameters Q0
● Interpolate linearly between X0 and D– Produces sequence X1, X2, X3, … , Xp = D
● For i = 1, 2,…, p do– Qi = Qi – 1 + J – 1(Qi – 1)(Xi – Xi – 1)
– Reset X1 to F(Qi)
![Page 44: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/44.jpg)
Case where n > 6● J is a 6 x n matrix● Recall: we’re trying to solve the linear system
dX = J dQ
● for dQ, where dX = Xi – X
i – 1, and J is the Jacobian
evaluated at Qi – 1
● The system is under-determined!● Too few constraints (6)● Too many variables (n)
● We can’t take the inverse of J
![Page 45: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/45.jpg)
Non-square linear system
● Assume we’re trying to solve Ax = b
… but A is not square!● So we’ll add some constraints:
● If A has more rows than columns (over-determined), find x that minimizes
|| Ax – b ||● If A has more columns than rows (under-determined),
find x satisfying Ax = b that minimizes
|| x ||
![Page 46: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/46.jpg)
Enter the pseudoinverse
● If A has more columns than rows (under-determined), find x satisfying Ax = b thatminimizes || x ||
● It turns out that this solution is given by
x = A+ b
where A+ is the pseudoinverse AT(AAT) – 1 of A
(we’ll assume A is full rank so this pseudoinverse formula works)
![Page 47: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/47.jpg)
Why least-norm solution?
Let x* be the pseudoinverse solution AT(AAT) – 1 b
Proof that it is a solution:● A x* = A AT(AAT) – 1 b = (A AT)(AAT) – 1 b = I b = b
![Page 48: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/48.jpg)
Why least-norm solution?
Proof that it is least-norm:● Consider any solution x of Ax = b
● … we have A(x – x*) = b – b = 0
● (x – x*)T x* = (x – x*)T AT(AAT) – 1 b= (A(x – x*))T(AAT) – 1 b= 0T (AAT) – 1 b= 0
● … so x – x* and x* are orthogonal● Hence, ||x||2 = ||x* + x – x*||2 = ||x*||2 + ||x – x*||2 ≥ ||x*||2
![Page 49: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/49.jpg)
Derivation via Lagrange multipliers
● Problem: minimize ||x||2 = xTxsubject to Ax = b
● Solution: Introduce Lagrange multiplier λ
L(x, λ) = xTx + λT(Ax – b)● At the minimum of L, we have
∇x L = 2x + AT λ = 0 and ∇λ
L = Ax – b = 0
● From first condition, we have x = –AT λ / 2
● Substituting into second condition, λ = –2(AAT) – 1 b
● Hence, x = AT(AAT) – 1 b
Hence theminimumof L is alsoa solutionof thelinearsystem
![Page 50: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/50.jpg)
Inverse Jacobian Method
● Need to reach some target configuration D frominitial configuration X
0
● General Algorithm:● Interpolate linearly between X
0 and D
– Produces sequence X1, X
2, X
3, … , X
p = D
● For i = 1, 2,…, p do– Q
i = Q
i – 1 + J+ (Q
i – 1)(X
i – X
i – 1)
– Reset X1 to F(Q
i)
![Page 51: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/51.jpg)
Inverse Jacobian Method
● Disadvantages:● Slow to compute inverse of AAT
● Instability around singularities (J loses full rank)
● Improvement in practice:● Use the Jacobian transpose J T instead of J +
Singular direction(2 DOF → 1 DOF)
Non-singular
![Page 52: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/52.jpg)
Jacobian Transpose Method
● Replace
dQ = J + dX
with
dQ = J T dX
● Why does this work?!!!
![Page 53: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/53.jpg)
Principle of Virtual Work
● “External work = internal work”● External work = force * distance● Internal Work = torque * angle
f T dX = τ T dQ● dX = J dQ (forward kinematics)● f T J dQ = τ T dQ (substituting)
● f T J = τ T (holds for any dQ)
i.e. τ = J T f
![Page 54: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/54.jpg)
Jacobian Transpose Method● Virtual work equation:
τ = J T f
● Compare with:
dQ = J T dX
● We’re taking the distance to the goal to be a forcethat pulls our end effector
● With J +, we had an exact solution to linearizedproblem
● … now no longer
![Page 55: Forward and Inverse Kinematics - IIT Bombay · 2016. 10. 25. · Forward Kinematics Incrementally manipulating each component of a flexible, jointed object to achieve an overall desired](https://reader036.vdocuments.site/reader036/viewer/2022070223/614652838f9ff81254203103/html5/thumbnails/55.jpg)
Jacobian Transpose Method● dQ = J T dX is not exact, but has the right trend● Throw in a scaling factor h and iterate
(ΔQ)i + 1
= h J T (ΔX)i
● h can be thought of as a timestep Δt
● So we’re just solving the differential equation
Δ QΔ t
=JT Δ X
dQdt
=JT X=JT F (Q)