Kinematics Inverse Kinematic & Differential Kinematicsglobex.coe.pku.edu.cn › file › upload ›...

Kinematics – Inverse Kinematic

& Differential Kinematics

Centre for Robotics Research – School of Natural and Mathematical Sciences – King’s College London

Robotic Endovascular Interventions – Hamlyn Workshop 23 June 20152 Peking University Globex 20182

Introduction – Inverse and Forward Kinematics

Inverse

Forward


Introduction – Inverse Kinematics


Introduction – Inverse Kinematics

Robot Kinematic Control

Inverse kinematic

Actuation

Orientation θ, position pdesired force

Joint variables( ሶθ, ሶd)


Analytical Inverse kinematic

• The inverse kinematics problem consists of the determination of the joint variables

corresponding to a given end-effector position and orientation.

• Analytical inverse kinematic equation can be derived from forward kinematic equa

tion using trigonometric identities

Trigonometric identities

𝑠𝑖𝑛2(𝜃1)+𝑐𝑜𝑠2 𝜃1 = 1

𝑡𝑎𝑛2 𝜃1 + 1 = 𝑠𝑒𝑐2 𝜃1

𝑐𝑜𝑡2 𝜃1 + 1 = 𝑐𝑠𝑐2 𝜃1

sin(𝜃1 ± 𝜃2) = sin 𝜃1 cos(𝜃2) ± cos 𝜃1 sin(𝜃2)

cos(𝜃1 ± 𝜃2) = 𝑐𝑜𝑠 𝜃1 cos(𝜃2) ∓ sin 𝜃1 sin(𝜃2)

sin(2𝜃1) = 2sin 𝜃1 𝑐𝑜𝑠 𝜃1

cos(2𝜃1) = 𝑐𝑜𝑠2 𝜃1 -𝑠𝑖𝑛2 𝜃1

= 2𝑐𝑜𝑠2 𝜃1 − 1

= 1 − 2𝑠𝑖𝑛2 𝜃1


Analytical Inverse kinematic – 2 Links arm

Two-link Arm

𝑃 =𝑝𝑥𝑝𝑦

=𝑎1𝑐1 + 𝑎2𝑐12𝑎1𝑠1 + 𝑎2𝑠12

Inverse Kinematic

Forward Kinematic

𝜃1 = 𝑓 𝑝𝑥, 𝑝𝑦𝜃2 = 𝑔 𝑝𝑥, 𝑝𝑦




=𝑎1𝑐1 + 𝑎2𝑐12𝑎1𝑠1 + 𝑎2𝑠12

𝑝𝑥 = 𝑎1𝑐1 + 𝑎2𝑐12𝑝𝑦 = 𝑎1𝑠1 + 𝑎2𝑆12

𝑝𝑥2 = 𝑎1

2𝑐12 + 𝑎2

2𝑐122 + 2𝑎1𝑎2𝑐1𝑐12 (1)

𝑝𝑦2 = 𝑎1

2𝑠12 + 𝑎2

2𝑠122 + 2𝑎1𝑎2𝑠1𝑠12 (2)

(1)+(2) and apply Trigonometric equation

𝑝𝑥2 + 𝑝𝑦

2 = 𝑎12 + 𝑎2

2 + 2𝑎1𝑎2 cos −𝜃2

cos(𝜃2) =𝑝𝑥2 + 𝑝𝑦

2 − 𝑎12 − 𝑎2

2

2𝑎1𝑎2

Squaring and summing of the equations



cos(𝜃2) =𝑝𝑥2 + 𝑝𝑦

2 − 𝑎12 − 𝑎2

2

2𝑎1𝑎2

sin(𝜃2) = ± 1 − cos(𝜃2)

Two possible solutions

𝜃2 = 𝑎𝑡𝑎𝑛2(𝑠𝑖𝑛𝜃2, 𝑐𝑜𝑠𝜃2)




=𝑎1𝑐1 + 𝑎2𝑐12𝑎1𝑠1 + 𝑎2𝑠12

𝑝𝑥 = 𝑎1𝑐1 + 𝑎2𝑐12𝑝𝑦 = 𝑎1𝑠1 + 𝑎2𝑆12

𝑝𝑥 = 𝑎1𝑐1 + 𝑎2𝑐1𝑐2 − 𝑎2𝑠1𝑠2

𝑝𝑦 = 𝑎1𝑠1 + 𝑎2𝑠1𝑐2 + 𝑎2𝑐1𝑠2

𝑝𝑥𝑐1 = 𝑎1𝑐12 + 𝑎2𝑐1

2𝑐2 − 𝑎2𝑐1𝑠1𝑠2

𝑝𝑦𝑠1 = 𝑎1𝑠12 + 𝑎2𝑠1

2𝑐2 + 𝑎2𝑐1𝑠1𝑠2

𝑝𝑥𝑠1 = 𝑎1𝑐1𝑠1 + 𝑎2𝑐1𝑐2𝑠1 − 𝑎2𝑠12𝑠2

𝑝𝑦𝑐1 = 𝑎1𝑠1𝑐1 + 𝑎2𝑠1𝑐2𝑐1 + 𝑎2𝑐12𝑠2

𝑝𝑥𝑐1 + 𝑝𝑦𝑠1 = 𝑎1 + 𝑎2𝑐2 𝑝y𝑐1 − 𝑝𝑥𝑠1 = 𝑎2𝑠2

(𝑝𝑥2+𝑝𝑦

2 )𝑐1= 𝑝𝑥(𝑎1 + 𝑎2𝑐2) + 𝑝𝑦𝑎2𝑠2



𝑝𝑥𝑐1 + 𝑝𝑦𝑠1 = 𝑎1 + 𝑎2𝑐2 (1)

𝑝y𝑐1 − 𝑝𝑥𝑠1 = 𝑎2𝑠2 (2)

(𝑝𝑥2+𝑝𝑦

2 )𝑐1= 𝑝𝑥(𝑎1 + 𝑎2𝑐2) + 𝑝𝑦𝑎2𝑠2

Multiply px on both sides of eq.1 + multiply py on both sides of eq.2

cos(𝜃1) =(𝑝𝑥(𝑎1 + 𝑎2𝑐2) + 𝑝𝑦𝑎2𝑠2)


2



𝑝𝑥𝑐1 + 𝑝𝑦𝑠1 = 𝑎1 + 𝑎2𝑐2 (1)

𝑝y𝑐1 − 𝑝𝑥𝑠1 = 𝑎2𝑠2 (2)

(𝑝𝑥2+𝑝𝑦

2 )𝑠1= 𝑝𝑦(𝑎1 + 𝑎2𝑐2) − 𝑝𝑥𝑎2𝑠2

Multiply py on both sides of eq.1 - multiply px on both sides of eq.2

sin(𝜃1) =𝑝𝑦(𝑎1 + 𝑎2𝑐2) − 𝑝𝑥𝑎2𝑠2)


2


𝜃1 = 𝑎𝑡𝑎𝑛2(𝑠𝑖𝑛𝜃1, 𝑐𝑜𝑠𝜃1)

unique solution


cos(𝜃1) =(𝑝𝑥(𝑎1 + 𝑎2𝑐2) + 𝑝𝑦𝑎2𝑠2)


2

sin(𝜃1) =𝑝𝑦(𝑎1 + 𝑎2𝑐2) − 𝑝𝑥𝑎2𝑠2)


2


Two-link Arm


=𝑎1𝑐1 + 𝑎2𝑐12𝑎1𝑠1 + 𝑎2𝑠12

Inverse Kinematic

Forward Kinematic

𝜃2 = cos−1𝑝𝑥2 + 𝑝𝑦

2 − 𝑎12 − 𝑎2

2

2𝑎1𝑎2

𝜃1 = cos−1𝑝𝑥(𝑎1 + 𝑎2𝑐2) + 𝑝𝑦𝑎2𝑠2


2



Analytical Inverse kinematic – 3 Links Arm

A first algebraic solution technique is

illustrated below.

Using the forward kinematic equation of

two-link arm.

which describe the position of point W


Analytical Inverse kinematic– 3 Links Arm

Squaring and summing of the equations



Hence, the angle 𝜃2 can be computed as



Substituting 𝜃2 into equations for 𝑃𝑤𝑥, 𝑎𝑛𝑑 𝑃𝑤𝑦

to find solutions of unknown 𝑠1 and 𝑐1

In analogy to the above, it is


Analytical Inverse kinematic Simulation– 3 Links Arm

𝜃1, 𝜃2 𝑎𝑛𝑑 𝜃3 = 0 𝑡𝑜 𝜋 Forward Kinematic

𝑇𝑜𝑜𝑙 𝑜𝑟𝑖𝑒𝑛𝑡𝑎𝑡𝑖𝑜𝑛 (∅)

𝜃1 + 𝜃2 + 𝜃3 = ∅

3𝜋



• Given joint angles

𝜃1, 𝜃2 𝑎𝑛𝑑 𝜃3 = 0 𝑡𝑜 𝜋

Tool position(𝑝𝑥, 𝑝𝑦)

𝑙1 𝑙2 𝑙3

Forward Kinematic




Tool position(𝑝𝑥, 𝑝𝑦)

Inverse

Kinematic

𝑇𝑜𝑜𝑙 𝑜𝑟𝑖𝑒𝑛𝑡𝑎𝑡𝑖𝑜𝑛 (∅)

Thus, if inverse kinematic is correct, given 𝜃 = output 𝜃 of IK

Given 𝜃 FK 𝑝𝑥, 𝑝𝑦 𝑎𝑛𝑑 ∅ IK Output 𝜃

=


Programming exercise in class


IKexercise.zip

inverse_kinematic_analytical_q


Analytical Inverse kinematic Simulation


Given 𝜃 Output 𝜃

𝜃1

𝜃2 𝜃3

𝜃1

𝜃2 𝜃3

=


Analytical Inverse kinematic problem

• The equations to solve are in general nonlinear, and thus it is not always

possible to find a closed-form solution.

• Multiple solutions may exist.

• Infinite solutions may exist, e.g., in the case of a kinematic redundant

manipulator.

• There might be no admissible solutions, in view of the manipulator kinematic

structure.

• Accurate and most time efficient IK


Derivative

Differential Kinematics : Math essentials

h= Δx

f(x)


Partial derivative


A slice of the graph above showing the

function in the xz -plane at y =1



Cross product


Differential Kinematic

square matrix(𝑟 × 𝑛)

• Differential kinematics gives the relationship between the joint velocities and the corresponding end-effector linear and angular velocity.

• This mapping is described by a Jacobian(J) matrix, which depends on the manipulatorconfiguration.

End-effector velocity(𝑟 × 1)

Joint velocity(𝑛 × 1)

Geometric Jacobian matrix

The Jacobian matrix (J) can be partitioned into the (3 × 1) column vectors 𝐽𝑃𝑖 and 𝐽𝑂𝑖 as





• This mapping is described by a Jacobian(J) matrix, which depends on the manipulator configuration.



Analytical Jacobian matrix




The Jacobian matrix J is one of the most important tools for finding singularities, a

nalyzing redundancy, determining inverse kinematics equation, and describing velocity and force manipulability ellipsoids.


• This mapping is described by a Jacobian(J) matrix, which depends on the manipulator configuration.




Differential Kinematic – Geometric Jacobian matrix

• If Joint i is prismatic, translation velocity can be expressed as

and then

• If Joint i is revolute, the translational velocity can be expressed as

𝑤ℎ𝑒𝑟𝑒, 𝜔 𝑖𝑠 𝑎𝑛𝑔𝑙𝑢𝑎𝑙𝑟 𝑣𝑒𝑙𝑐𝑖𝑡𝑦, and𝑟 𝑖𝑠 𝑝𝑜𝑠𝑖𝑡𝑖𝑜𝑛 𝑜𝑓 𝑓𝑟𝑎𝑚𝑒 i respect to frame i − 1

and then



Pe

Pi-1

P0

P1

𝜃𝑖

𝜃2

𝜃1

assuming the rest joints are locked



For position components in the Jacobian matrix(𝐽𝑃𝑖)

• For the contribution to the linear velocity, the time derivative of 𝒑𝒆(𝒒)can be written as

• ሶ𝑝𝑒 can be obtained as the sum of the terms ሶ𝑞𝑖𝐽𝑃𝑖

• Each term represents the contribution of the velocity of single Joint i to the end-effector linear velocity when all the other joints are still.

• Therefore, by distinguishing the case of a prismatic joint (𝑞𝑖 = 𝑑𝑖) from the case of a revolute joint (𝑞𝑖 = 𝜃), it is:



ሶ𝑞1ሶ𝑞2⋮ሶ𝑞𝑛

ሶ𝑝𝑒 = 𝑱𝑃1 𝑱𝑃2 … 𝑱𝑃𝑛

For position components in the Jacobian matrix(𝐽𝑃𝑖)



For orientation components in the Jacobian matrix(𝐽0𝑖)

• If Joint i is prismatic,

• If Joint i is revolute



For orientation components in the Jacobian matrix(𝐽0𝑖)

• For the contribution to the angular velocity

ሶ𝑞1ሶ𝑞2⋮ሶ𝑞𝑛

𝜔𝑒 = 𝑱𝑂1 𝑱𝑂2 … 𝑱𝑂𝑛



• The Jacobian matrix (J) can be partitioned into the (3 × 1) column vectors 𝐽𝑃𝑖 and 𝐽𝑂𝑖 as

For a prismatic joint

For a revolute joint

=

=



• The Jacobian matrix (J) can be partitioned into the (3 × 1) column vectors 𝐽𝑃𝑖 and 𝐽𝑂𝑖 as

• 𝑧𝑖−1 is given by the rotation of z-axis unit vector

• 𝑃𝑒 is given by the position vector in the transformation matrix 𝑇𝑒0

𝑃𝑒 = 𝑇10(𝑞1)⋯𝑇𝑛

𝑛−1(𝑞𝑛)𝑃𝑜

• 𝑃𝑖−1 is given by the position vector in the transformation matrix 𝑇𝑖−10

𝑃𝑖−1 = 𝑇10(𝑞1)⋯𝑇𝑛−1

𝑛−2(𝑞𝑖−1)𝑃𝑜



• 𝐸𝑥𝑎𝑚𝑝𝑙𝑒(𝑇ℎ𝑟𝑒𝑒 − 𝑙𝑖𝑛𝑘 𝑝𝑙𝑎𝑛𝑎𝑟 𝑎𝑟𝑚, 𝑙𝑖𝑛𝑘𝑠 𝑙𝑒𝑛𝑔𝑡ℎ 𝑎1, 𝑎2, 𝑎3)

𝐼𝑛 𝑡ℎ𝑖𝑠 𝑐𝑎𝑠𝑒, 𝑡ℎ𝑒 𝐽𝑎𝑐𝑜𝑏𝑖𝑎𝑛 𝑖𝑠

• 𝑃0, 𝑃1, 𝑃2 and 𝑃3 𝑎𝑟𝑒 𝑎𝑠

𝑃0

𝑃1

𝑃2

𝑃3



• 𝐸𝑥𝑎𝑚𝑝𝑙𝑒(𝑇ℎ𝑟𝑒𝑒 − 𝑙𝑖𝑛𝑘 𝑝𝑙𝑎𝑛𝑎𝑟 𝑎𝑟𝑚)

• 𝑧0, 𝑧1, and 𝑧2 𝑎𝑟𝑒 𝑎𝑠

• 𝑧0 × (𝑃3 − 𝑃0)• 𝑍 ×= 𝑍 ×

𝑧0 × (𝑃3 − 𝑃0)

=0 −1 01 0 00 0 0

𝑎1𝑐1 + 𝑎2𝑐12 + 𝑎3𝑐123𝑎1𝑠1 + 𝑎2𝑠12 + 𝑎3𝑠123

0

=−𝑎1𝑠1 − 𝑎2𝑠12 − 𝑎3𝑠123𝑎1𝑐1 + 𝑎2𝑐12 + 𝑎3𝑐123

0

𝑃0

𝑃1

𝑃2

𝑃3




• 𝑇ℎ𝑢𝑠, 𝑡ℎ𝑒 𝑗𝑎𝑐𝑜𝑏𝑖𝑎𝑛 𝑚𝑎𝑡𝑟𝑖𝑥 𝑜𝑓 𝑡ℎ𝑟𝑒𝑒 − 𝑙𝑖𝑛𝑘 𝑎𝑟𝑚 𝑖𝑠

JO1

JP1

𝑃0

𝑃1

𝑃2

𝑃3


𝑃0

𝑃1

𝑃2

𝑃3



• 𝑧0, 𝑧1, and 𝑧2 𝑎𝑟𝑒 𝑎𝑠

• 𝑇ℎ𝑢𝑠, 𝑡ℎ𝑒 𝑗𝑎𝑐𝑜𝑏𝑖𝑎𝑛 𝑚𝑎𝑡𝑟𝑖𝑥 𝑜𝑓 𝑡ℎ𝑟𝑒𝑒 − 𝑙𝑖𝑛𝑘 𝑎𝑟𝑚 𝑖𝑠

• 𝐼𝑓 𝑜𝑟𝑖𝑒𝑛𝑡𝑎𝑡𝑖𝑜𝑛 𝑖𝑠 𝑛𝑜𝑡 𝑐𝑜𝑛𝑠𝑖𝑑𝑒𝑟𝑒𝑑, 𝑡ℎ𝑒 𝐽 𝑖𝑠


Differential Kinematic – Analytical Jacobian matrix

Analytical Jacobian matrix is derived via differentiation of the forward kinematics to the

actuator vector

• 𝐿𝑒𝑡′𝑠 ሶ𝑃e 𝑖𝑠 𝑡ℎ𝑒 𝑡𝑖𝑚𝑒 𝑑𝑒𝑟𝑖𝑣𝑎𝑡𝑖𝑣𝑒 𝑜𝑓 𝑡ℎ𝑒 𝑡𝑟𝑎𝑛𝑠𝑙𝑎𝑡𝑖𝑜𝑛𝑎𝑙 𝑜𝑓 𝑡ℎ𝑒 𝑒𝑛𝑑 𝑒𝑓𝑓𝑒𝑐𝑡 𝑓𝑟𝑎𝑚𝑒.

• 𝐿𝑒𝑡′𝑠 𝜙e 𝑖𝑠 𝑡ℎ𝑒 𝑜𝑟𝑖𝑒𝑛𝑡𝑎𝑡𝑖𝑜𝑛, ሶ𝜙𝑒 𝜔𝑒 𝑖𝑠 𝑡ℎ𝑒 𝑎𝑛𝑔𝑢𝑙𝑎𝑟 𝑣𝑒𝑙𝑜𝑐𝑖𝑡𝑦.

• 𝑇ℎ𝑢𝑠.

ሶ𝜙𝑒 =𝜕𝜙e

𝜕𝒒ሶ𝒒 = 𝑱𝝓 𝒒 ሶ𝒒

𝜕𝒙e𝜕𝒒



• 𝐴𝑛𝑎𝑙𝑦𝑡𝑖𝑐𝑎𝑙 𝐽𝑎𝑐𝑜𝑏𝑖𝑎𝑛 − 𝑡ℎ𝑟𝑒𝑒 𝑙𝑖𝑛𝑘 𝑎𝑟𝑚

• 𝐴𝑛𝑙𝑎𝑦𝑡𝑖𝑐𝑎𝑙 𝑗𝑎𝑐𝑜𝑏𝑖𝑎𝑛 𝑚𝑎𝑡𝑟𝑖𝑥 𝑓𝑜𝑟𝑚 𝑓𝑜𝑟𝑡ℎ𝑟𝑒𝑒 𝑙𝑖𝑛𝑘 𝑎𝑟𝑚 𝑖𝑠

𝐽𝐴 =

𝜕𝑝𝑥𝜕𝜃1



𝜕𝑝𝑦

𝜕𝜃1

𝜕𝑝𝑦

𝜕𝜃2

𝜕𝑝𝑦

𝜕𝜃3𝜕∅

𝜕𝜃1

𝜕∅

𝜕𝜃2

𝜕∅

𝜕𝜃3

Translation

Orientation




𝑝𝑥 = 𝑎1𝑐1 + 𝑎2𝑐12 + 𝑎3𝑐123𝑝𝑦 = 𝑎1𝑠1 + 𝑎2𝑠12 + 𝑎3𝑠123𝜙 = 𝜃1 + 𝜃2 + 𝜃3

• 𝐹𝑜𝑟𝑤𝑎𝑟𝑑 𝑘𝑖𝑛𝑒𝑚𝑎𝑡𝑖𝑐

𝐽𝐴 =




𝜕𝑝𝑦

𝜕𝜃1

𝜕𝑝𝑦

𝜕𝜃2

𝜕𝑝𝑦

𝜕𝜃3𝜕∅

𝜕𝜃1

𝜕∅

𝜕𝜃2

𝜕∅

𝜕𝜃3

Translation

Orientation




𝑝𝑥 = 𝑎1𝑐1 + 𝑎2𝑐12 + 𝑎3𝑐123


= −𝑎1𝑠1 −𝑎2 𝑠12 −𝑎3 𝑠123


= −𝑎2𝑠12 −𝑎3 𝑠123


= −𝑎3𝑠123

• Let’s differentiate with respect to 𝜃1, 𝜃2 𝑎𝑛𝑑 𝜃3




• Differentiate respect to 𝜃1, 𝜃2 𝑎𝑛𝑑 𝜃3

𝑝𝑦 = 𝑎1𝑠1 + 𝑎2𝑠12 + 𝑎3𝑠123

𝜕𝑝𝑦

𝜕𝜃1= 𝑎1𝑐1 +𝑎2 𝑐12 +𝑎3 𝑐123

𝜕𝑝𝑦

𝜕𝜃2= 𝑎2𝑐12 +𝑎3 𝑐123

𝜕𝑝𝑦

𝜕𝜃3= 𝑎3𝑐123

𝜕𝜙

𝜕𝜃1=

𝜕𝜙

𝜕𝜃2=

𝜕𝜙

𝜕𝜃3= 1



• Into the analytical Jacobian matrix

𝐽𝐴 =




𝜕𝑝𝑦

𝜕𝜃1

𝜕𝑝𝑦

𝜕𝜃2

𝜕𝑝𝑦

𝜕𝜃3𝜕∅

𝜕𝜃1

𝜕∅

𝜕𝜃2

𝜕∅

𝜕𝜃3

=−𝑎1𝑠1 −𝑎2 𝑠12 −𝑎3 𝑠123 −𝑎2𝑠12 −𝑎3 𝑠123 −𝑎3𝑠123𝑎1𝑐1 +𝑎2 𝑐12 +𝑎3 𝑐123 𝑎2𝑐12 +𝑎3 𝑐123 𝑎3𝑐123

1 1 1



• Compare to geometric Jacobian matrix

𝐽𝐴 =−𝑎1𝑠1 −𝑎2 𝑠12 −𝑎3 𝑠123 −𝑎2𝑠12 −𝑎3 𝑠123 −𝑎3𝑠123𝑎1𝑐1 +𝑎2 𝑐12 +𝑎3 𝑐123 𝑎2𝑐12 +𝑎3 𝑐123 𝑎3𝑐123

1 1 1

Analytical Jacobian

Geometric Jacobian

• Same Jacobian matrix, but different approaching in this case


Differential Inverse Kinematic

• A highly nonlinear relationship between joint space variable and orientation space

variable causes non-solution or redundant of the closed form solution for inverse

kinematic.

• A linear mapping between the joint velocity space and the operational velocity space

in the differential kinematic equation tackles the inverse kinematics problem.

• Differential kinematic so far is as

square matrix (𝑛 × 𝑛)



square matrix(𝑛 × 𝑛)

J must to be invertible, i.e. J is square and det (J) ≠ 0

Desired joint velocity ( ሶ𝑞) depending on desired position and orientation of the end effector can be obtained via simple inversion of the Jacobian matrix

In matlab: inv(A) provide A-1



For a small interval of time t, ሶ𝑞, 𝑣𝑒 can be assumed as constant thus

ሶ𝒒𝑡 = 𝑱−1(𝒒)𝒗𝒆𝑡

𝛿𝒒 = 𝑱−1 𝒒 𝛿𝒙𝒆

Thus

where 𝛿𝒙𝒆=𝛿𝒑𝒆𝛿𝝓𝒆



𝛿𝒒 = 𝑱−1 𝒒 𝛿𝒙𝒆 where 𝛿𝒙𝒆=𝛿𝒑𝒆𝛿𝝓𝒆

For 3-link planar arm case

[0,0]

xe=[px, py, ɸ]

xd=[pxd, pyd, ɸd]

* 𝛿𝒙𝒆 = 𝒙𝒅 − 𝒙𝒆




[0,0]

xe=[px, py, ɸ]

xd=[pxd, pyd, ɸd]

*

𝛿𝒙𝒆 = 𝒙𝒅 − 𝒙𝒆

𝛿𝜃1𝛿𝜃2𝛿𝜃3

=−𝑎1𝑠1 −𝑎2 𝑠12 −𝑎3 𝑠123 −𝑎2𝑠12 −𝑎3 𝑠123 −𝑎3𝑠123𝑎1𝑐1 +𝑎2 𝑐12 +𝑎3 𝑐123 𝑎2𝑐12 +𝑎3 𝑐123 𝑎3𝑐123

1 1 1

−1 𝛿𝑝𝑥𝛿𝑝𝑦𝛿𝜙





Inputs for differential inverse kinematic

changes of tool position and orientation

(𝛿𝑝𝑥 , 𝛿𝑝𝑦 , 𝛿𝜙)current actuator space (𝜃1, 𝜃2 𝜃3)




Output for differential inverse kinematic

current actuator space (𝜃1, 𝜃2 𝑎𝑛𝑑 𝜃3)

changes in actuator space (𝛿𝜃1, 𝛿𝜃2 𝛿𝜃3)

Therefore actuator space after the adjustment: 𝒒𝑎𝑑𝑗𝑢𝑠𝑡𝑒𝑑 = 𝛿𝒒 + 𝒒𝑐𝑢𝑟𝑟𝑒𝑛𝑡


(𝛿𝑝𝑥 , 𝛿𝑝𝑦 , 𝛿𝜙)




• Given joint inputs are 𝜃1, 𝜃2 𝑎𝑛𝑑 𝜃3 = 0 𝑡𝑜 𝜋• Increment of 𝜃1, 𝜃2 𝑎𝑛𝑑 𝜃3 is 0.0157

• Inputs for differential inverse kinematic

Differential Inverse kinematic

current actuator space (𝜃1, 𝜃2 𝑎𝑛𝑑 𝜃3)



(𝛿𝑝𝑥 , 𝛿𝑝𝑦 , 𝛿𝜙)


Programming exercise

Inverse_kinematic_fullrank_exercise_q.m



Differential vs. Analytical Inverse Kinematic

• From analytical inverse kinematics • Using differential inverse kinematic


Analytical Inverse kinematic problem

• Given 𝜃1, 𝜃2 𝑎𝑛𝑑 𝜃3 = 0 𝑡𝑜 𝜋 • Outputs of 𝜃1, 𝜃2 𝑎𝑛𝑑 𝜃3

=


Error of analytical inverse kinematic

Analytical Inverse Kinematics


Inverse kinematic error for differential approach

• Given 𝜃1, 𝜃2 𝑎𝑛𝑑 𝜃3 = 0 𝑡𝑜 𝜋 • 𝜃1, 𝜃2 𝜃3 from IK

=

• Differential inverse kinematic result has small error.

• Error compensation could be applied.


Inverse kinematic error for differential approach

Differential Inverse Kinematics

Kinematics Inverse Kinematic & Differential Kinematicsglobex.coe.pku.edu.cn › file › upload ›...

Documents

Transcript of Kinematics Inverse Kinematic & Differential Kinematicsglobex.coe.pku.edu.cn › file › upload ›...