Differential Kinematics
Differential Kinematics
Elizabeth Sklar
Department of Informatics
King’s College London
12 November 2018
(version 1.0)
1 / 35
Outline
Differential Kinematics
2 / 35
Differential Kinematics
We have talked about:
I Forward Kinematics—figuring out the end point of a robot
arm given its starting point and angle for each link
I Inverse Kinematics—figuring out the angles necessary for each
link, given a starting and ending point for a robot arm
I Mostly we have talked about this in the context of figuring out
what positions to end up in, but not about how to get to these
positions.
I So now we’ll talk about how to get there.
3 / 35
Differential Kinematics, p2
I One way to program robots to move is through trial and error.
I We have talked about this kind of motion for mobile robots
with path planning.
I With robot arms, it is more common to solve the problem
mathematically, using Kinematics.
I So you can think of:
I Forward Kinematics as figuring out how a robot will move if its
motors work in a given way.
I Inverse Kinematics as figuring out how to move the motors to
get the robot to do what we want.
4 / 35
Differential Kinematics, p3
Here is a mathematical way to represent the problem.
Given:
I n joints:
θ1 . . . θn joint angles
I k end effectors:
−→s = (s1 . . . sk)T end effector starting positions
−→
t = (t1 . . . tk)
T end effector target positions
I Each position is a coordinate in space, e.g., (x , y) or (x , y , z).
I ei = ti − si desired change in position of end effector i
I The Inverse Kinematics problem:
Find values for all θj ’s such that ti = si (θ) ∀ i
I There may be multiple solutions (if there is a solution at all).
5 / 35
Differential Kinematics, p4
I There are many ways to solve this problem.
I We will look at one incremental way.
I The advantage of an incremental approach is that it can also
be used to choreograph the movement of the arm.
I There are multiple incremental approaches, such as:
I Gradient Descent — start with an initial guess, and
incrementally change values to reduce error
I Jacobian — we’ll look at this one here
6 / 35
Jacobian
I The Jacobian approach is based on a matrix of partial
derivatives, where each partial indicates the influence on the
end effector position (pi ) by the joint angle (θj).
I The Jacobian matrix J is defined as:
J(θ) =
[
∂pi
∂θj
]
i ,j
where ∂pi
∂θj
is the partial derivative of pi with respect to θj
I We assume that J is an m × n matrix of partial derivatives:
J(θ) =
∂px
∂θ0
∂px
∂θ1
. . .
∂px
∂θn
∂py
∂θ0
∂py
∂θ1
. . .
∂py
∂θn
∂ω
∂θ0
∂ω
∂θ1
. . . ∂ω
∂θn
(m = 3 in the case of a 2-DOF arm)
7 / 35
Maths Review
I Before we go further, let’s have a quick review of Derivatives…
8 / 35
Derivatives
I A derivative is a “measure of the rate of change of one
quantity with respect to another.” [MW, p47]
I The slope of a line is a derivative: change in x with respect to
change in y .
I A velocity is another derivative: change in distance with
respect to time.
I Definition:
The derivative y = f (x) at x0 is written f ′(x0):
f ′(x0) =
∆y
∆x
=
f (x0 + ∆x)− f (x0)
∆x
9 / 35
Partial Derivatives
I The partial derivatives “of a function of several variables are its
ordinary derivatives with respect to each variable separately.”
[MW, p686]
I When we take the partial derivative of a function with two
variables, we imagine one of the variables is a constant and
compute the partial with respect to the other variable.
I For example, given:
f (x) = x2 + xy + y2
the partial derivative of f (x) with respect to x is:
∂f (x)
∂x
= 2x + y
I We pretend that y is a constant and compute the derivative of
the function f (x) with respect to x .
10 / 35
Partial Derivatives, p2
I Below (a) is an illustration of the function
z = f (x , y) = x2 + xy + y2.
I The red line highlighted in (a), shows the value of the function
at the point y = 1.
I The red line is shown within the xz plane in (b), illustrating
the partial derivative of z with respect to x : ∂z
∂x
= 2x + y .
(a) (b)
11 / 35
Differential Kinematics and the Jacobian Method for
computing Inverse Kinematics
I Differential Kinematics gives the relationship between the joint
velocities (how the joints change) and the corresponding end
effector and its angular velocity
νe =
[
ṗe
ωe
]
= Jq̇
where:
I νe is the angular velocity vector of the end effector
I ṗe is the position velocity (change in position) of the end
effector
I ωe is the angular velocity (change in rotation) of the end
effector
I J is a square matrix representing the Jacobian—the partial
derivatives of joint positions with respect to the angular
velocity of the joints
I q̇ is a vector of joint velocities
12 / 35
Jacobian, p2
I For the contribution to the linear velocity (translation), we
have:
ṗe =
n∑
i=1
JPi q̇i
I We compute ṗe as the sum of the terms JPi ∗ q̇i , where JP is
the Jacobian matrix containing the partial derivatives of the
position components with respect to q̇ (joint velocities).
I Each term represents the contribution of the velocity of a
single joint (i) to the end effector, as if all the other joints
were stationary.
ṗe = [JP1JP2 . . . JPn]
q̇1
…
q̇2
q̇n
13 / 35
Jacobian, p3
I For the contribution to the angular velocity (rotation), we
have:
ω̇e =
n∑
i=1
Jθi q̇i
where ωi is the rotation (orientation) of the i-th joint.
I We compute ṗe as the sum of the terms Jθi ∗ q̇i , where Jθ is
the Jacobian matrix containing the partial derivatives of the
orientation components (joint angles) with respect to q̇.
I Each term represents the contribution of the velocity of a
single joint angle (i) to the end effector, as if all the other
joints were stationary.
I Thus:
ω̇e = [Jθ1Jθ2 . . . Jθn]
q̇1
…
q̇2
q̇n
14 / 35
Jacobian, p4
I We put these together in the Jacobian matrix:
J =
position components
orientation components
= [ JP
Jθ
]
I and use it to compute the change in end effector position:
νe =
[
ṗe
ωe
]
=
[
JP1 JP2 . . . JPn
Jθ1 Jθ2 . . . Jθn
]
q̇1
q̇2
…
q̇n
15 / 35
Quick review: Cross Product
(Imagine A and B lie in a plane that is flat on a table; then n is sticking
straight up in the air, like a pencil perpendicular to the table top.)
I The cross product between two vectors, A and B , gives you a
vector that is perpendicular to both of them, or the normal, n.
I The cross product can be calculated as:
A× B = |A| |B| sin(θ) n
where: A and B are our vectors,
θ is the angle between them,
and n is the unit vector perpendicular to both A and B
16 / 35
Quick review: Cross Product, p2
I The cross product, when A and B intersect at the origin (0, 0)
is computed as follows:
C = A× B =
AxAy
Az
×
BxBy
Bz
=
[
0 −Az Ay
Az 0 −Ax
−Ay Ax 0
] [
Bx
By
Bz
]
=
[
(0 ∗ Bx ) + (−Az ∗ By ) + (Ay ∗ Bz )
(Az ∗ Bx ) + (0 ∗ By ) + (−Ax ∗ Bz )
(−Ay ∗ Bx ) + (Ax ∗ By ) + (0 ∗ Bz )
]
=
−Az ∗ By + Ay ∗ BzAz ∗ Bx − Ax ∗ Bz
−Ay ∗ Bx + Ax ∗ By
17 / 35
Jacobian: Example 3-Link Arm
I For example, for a 3-link arm, J can be written as:
J =
[
JP
JR
]
=
∂px
∂θ1
∂px
∂θ2
∂px
∂θ3
∂py
∂θ1
∂py
∂θ2
∂py
∂θ3
∂ω
∂θ1
∂ω
∂θ2
∂ω
∂θ3
where:
I (px , py ) represent the position of the end effector
I ω represents the rotation of the end effector
I θi represents the angle at each of the 3 joints
I and
JP =
∂px
∂θ1
∂px
∂θ2
∂px
∂θ3
∂py
∂θ1
∂py
∂θ2
∂py
∂θ3
JR = [ ∂ω∂θ1 ∂ω∂θ2 ∂ω∂θ3 ]
18 / 35
Jacobian: Example 3-Link Arm, p2
P0 =
00
0
P1 =
L1 cos(ω1)L1 sin(ω1)
0
19 / 35
Jacobian: Example 3-Link Arm, p3
P2 =
L1 cos(ω1) + L2 cos(ω1 + ω2)L1 sin(ω1) + L2 sin(ω1 + ω2)
0
20 / 35
Jacobian: Example 3-Link Arm, p4
P3 =
L1 cos(ω1) + L2 cos(ω1 + ω2) + L3 cos(ω1 + ω2 + ω3)L1 sin(ω1) + L2 sin(ω1 + ω2) + L3 sin(ω1 + ω2 + ω3)
0
21 / 35
Jacobian: Example 3-Link Arm, p5
I if we want to compute the position of the end effector, P3, we
can use the Jacobian matrix to do that.
I Given:
P3x = L1 cos(ω1) + L2 cos(ω1 + ω2) + L3 cos(ω1 + ω2 + ω3)
the partial derivatives of P3x with respect to each ωi are:
∂P3x
∂ω1
= −L1 sin(ω1)− L2 sin(ω1 + ω2)− L3 sin(ω1 + ω2 + ω3)
∂P3x
∂ω2
= −L2 sin(ω1 + ω2)− L3 sin(ω1 + ω2 + ω3)
∂P3x
∂ω3
= −L3 sin(ω1 + ω2 + ω3)
22 / 35
Jacobian: Example 3-Link Arm, p6
I and for P3y :
P3y = L1 sin(ω1) + L2 sin(ω1 + ω2) + L3 sin(ω1 + ω2 + ω3)
the partial derivatives of P3y with respect to each ωi are:
∂P3y
∂ω1
= L1 cos(ω1) + L2 cos(ω1 + ω2) + L3 cos(ω1 + ω2 + ω3)
∂P3y
∂ω2
= L2 cos(ω1 + ω2) + L3 cos(ω1 + ω2 + ω3)
∂P3y
∂ω3
= L3 cos(ω1 + ω2 + ω3)
23 / 35
Jacobian: Example 3-Link Arm, p7
I and for ωtotal = ω1 + ω2 + ω3:
∂ωtotal
∂ω1
= 1 ∂ωtotal
∂ω2
= 1 ∂ωtotal
∂ω3
= 1
24 / 35
Jacobian: Example 3-Link Arm, p8
I We can put these all together into the Jacobian matrix as
follows:
J =
∂P3x
∂ω1
∂P3x
∂ω2
∂P3x
∂ω3
∂P3y
∂ω1
∂P3y
∂ω2
∂P3y
∂ω3
∂ωtotal
∂ω1
∂ωtotal
∂ω2
∂ωtotal
∂ω3
I Substitute for each of the partial derivatives the appropriate
values from the previous slides
I Then we can use this matrix to determine how the end effector
moves when each of the joint angles change—Differential
Kinematics !!
25 / 35
Jacobian: Example 3-Link Arm, p9
I Remember where we started today:
Differential Kinematics gives the relationship between the joint
velocities (how the joints change) and the corresponding end
effector and its angular velocity
νe =
[
ṗe
ωe
]
= Jq̇ =
[
JP1 JP2 . . . JPn
Jθ1 Jθ2 . . . Jθn
]
q̇1
q̇2
…
q̇n
where:
I νe is the angular velocity vector of the end effector
I ṗe is the position velocity (change in position) of the end
effector
I ωe is the angular velocity (change in rotation) of the end
effector
I q̇ is a vector of joint velocities
26 / 35
Jacobian: Example 3-Link Arm, p10
I What if we know νe and we want to know q̇?
I i.e., we want to determine what joint velocities are required to
obtain a desired position and orientation of the end effector?
I This can be obtained by inverting the Jacobian matrix like this:
q̇ = J−1νe
I So if we know J:
then we first compute its inverse: J−1
and then we multiply the inverse by the angular velocity vector
of the end effector (νe)
→ and that will tell us how much each joint angle changes
27 / 35
Jacobian: Example Code
float [] solveJacobian( px , py , pw ,w1,w2,w3, ṗx , ṗy , ˙pw ) {
float new_angles [] = { 0.0, 0.0, 0.0 };
float νe [] = { px − ṗx , py − ṗy , pw − ˙pw }; // velocity of pe
float J[][] = new float [3][3]; // Jacobian
J[0][0] = −L1 ∗ sin(w1)− L2 ∗ sin(w1 + w2)− L3 ∗ sin(w1 + w2 + w3);
J[0][1] = −L2 ∗ sin(w1 + w2)− L3 ∗ sin(w1 + w2 + w3);
J[0][2] = −L3 ∗ sin(w1 + w2 + w3);
J[1][0] = L1 ∗ cos(w1) + L2 ∗ cos(w1 + w2) + L3 ∗ cos(w1 + w2 + w3);
J[1][1] = L2 ∗ cos(w1 + w2) + L3 ∗ cos(w1 + w2 + w3);
J[1][2] = L3 ∗ cos(w1 + w2 + w3);
J[2][0] = 1;
J[2][1] = 1;
J[2][2] = 1;
float Jinv [][] = inv3( J ); // compute J−1
float q̇[] = multiply3x1( Jinv , νe ); // angle changes
new_angles [0] = w1 + q̇[0];
new_angles [1] = w2 + q̇[1];
new_angles [2] = w3 + q̇[2];
return( new_angles );
} // end of solveJacobian ()
28 / 35
Summary
I That’s all!
I Okay it’s a bit complicated
I But you should understand it better after completing
Coursework 2 part 2
I Coursework 2 is due in 2 weeks (27th November)
I Look at starter and helper code uploaded on KEATS
29 / 35
Derivatives: Some useful rules
I sum rule:
d
dx
(u + v) =
du
dx
+
dv
dx
I product rule:
d
dx
(u ∗ v) =
du
dx
v + u
dv
dx
I constant multiple rule:
d
dx
A ∗ u = A ∗
du
dx
30 / 35
Derivatives: Some useful rules, p2
I power rule:
d
dx
(xn) = n ∗ x (x−1)
I power of a function rule:
d
dx
un = n ∗ un−1
du
dx
I quotient rule:
d
dx
(u
v
)
=
du
dx
v − u dv
dx
v2
I reciprocal rule:
d
dx
(
1
u
)
= −
1
u2
du
dx
31 / 35
Derivatives: Some useful rules, p3
I chain rule:
dy
dx
=
dy
du
·
du
dx
if y is a function of u and u is a function of x
32 / 35
Derivatives: Some useful rules, p4
I quadratic function rule:
d(ax2 + bx + c)
dx
= 2ax + b
I linear function rule:
d(bx + c)
dx
= b
d(c)
dx
= 0
33 / 35
Derivatives: Some useful rules, p5
I The change in value of sin(x) with respect to x is cos(x):
d sin(x)
dx
= cos(x)
I The change in value of cos(x) with respect to x is −sin(x):
d cos(x)
dx
= −sin(x)
34 / 35
References
SNS Introduction to Autonomous Mobile Robots, by Roland
Siegwart, Illah R. Nourbakhsh and Davide Scaramuzza (2011),
MIT Press, chapter 3.
MW Calculus, by Jerrold Marsden and Alan Weinstein (1980), The
Benjamin/Cummings Publishing Company, Inc.
35 / 35
Differential Kinematics