CS 3630

Inverse Kinematics:
Planar Arms
Assigning Link Frames
𝑥2 is collinear with the
origin of Frame 1
𝑥2
𝜃2 is the angle from 𝑥1 to 𝑥2

𝑦0
• 𝑥1 is collinear with the origin of Frame 0
𝑥1

𝑥0 𝜃1 is the angle from 𝑥0 to 𝑥1

• Frame 𝑛 is the end-effector frame. It can be attached to link 𝑛 in any manner that is
convenient.
• In this case, 𝑛 = 2, so Frame 2 is the end-effector frame.
The Forward Kinematic Map
𝑥2 is collinear with the
origin of Frame 1
𝑥2
𝜃2 is the angle from 𝑥1 to 𝑥2

𝑦0
• 𝑥1 is collinear with the origin of Frame 0
𝑥1

𝑥0 𝜃1 is the angle from 𝑥0 to 𝑥1

Once we have coordinate frames for each link:

• Determine 𝑇𝑖𝑖−1 for adjacent links as a function of 𝑞𝑖
• The forward kinematic map is given by: 𝑇𝑛0 (𝑞1 … 𝑞𝑛 ) = 𝑇10 (𝑞1 ) … 𝑇𝑛𝑛−1 (𝑞𝑛 )
The Forward Kinematic Map
• The forward kinematic map gives the position and orientation of the
end-effector frame as a function of the joint variables:
𝑥2
𝑎2
𝑇𝑛0 = 𝐹(𝑞1 , … , 𝑞𝑛 ) 𝜃2
𝑎1
𝑥1
𝜃1
• For the two-link planar arm, we have 𝑥0

cos 𝜃1 − sin 𝜃1 𝑎1 cos 𝜃1 cos 𝜃2 − sin 𝜃2 𝑎2 cos 𝜃2

𝑇20 = sin 𝜃1 cos 𝜃1 𝑎1 sin 𝜃1 sin 𝜃2 cos 𝜃2 𝑎2 sin 𝜃2
0 0 1 0 0 1

cos(𝜃1 +𝜃2 ) − sin(𝜃1 +𝜃2 ) 𝑎1 cos 𝜃1 + 𝑎2 cos(𝜃1 +𝜃2 )

= sin(𝜃1 +𝜃2 ) cos(𝜃1 +𝜃2 ) 𝑎1 sin 𝜃1 + 𝑎2 sin(𝜃1 +𝜃2 )
0 0 1
Inverse Kinematics
The General Inverse Kinematics Problem:
Given the forward kinematic map: 𝑇𝑛0 = 𝐹(𝑞1 , … , 𝑞𝑛 )
Solve for 𝑞1 , … , 𝑞𝑛 to achieve a desired 𝑇 𝑑
i.e., find 𝑞1𝑑 , … , 𝑞𝑛𝑑 such that 𝐹(𝑞1𝑑 , … , 𝑞𝑛𝑑 ) = 𝑇 𝑑

Why is this difficult?

• In general, 𝐹(𝑞1 , … , 𝑞𝑛 ) will be nonlinear. Solving nonlinear equations is hard.
• Further, for a general 𝐹(𝑞1 , … , 𝑞𝑛 ) we don’t know
• Does a solution to 𝐹 𝑞1 , … , 𝑞𝑛 = T d exist?
• If a solution exists, is it unique?
The Inverse Kinematic Solution
For the two-link arm, typically the goal is to place the end-effector at a desired location.
• Denote the coordinates of the origin of frame 2 by 𝑜2,𝑥 , 𝑜2,𝑦 .
• Solve for 𝜃1 and 𝜃2 such that
𝑜2,𝑥 = 𝑎1 cos 𝜃1 + 𝑎2 cos(𝜃1 +𝜃2 )
𝑜2,𝑦 = 𝑎1 sin 𝜃1 + 𝑎2 sin(𝜃1 +𝜃2 )
• Recall that 𝑎1 and 𝑎2 are constants defined by the mechanical structure of the arm.
• This is a nonlinear set of equations in 𝜃1 and 𝜃2 --- and nonlinear equations can be
very difficult to solve!

cos(𝜃1 +𝜃2 ) − sin(𝜃1 +𝜃2 ) 𝑎1 cos 𝜃1 + 𝑎2 cos(𝜃1 +𝜃2 ) ∗ ∗ 𝑜2,𝑥

𝑇20 = sin(𝜃1 +𝜃2 ) cos(𝜃1 +𝜃2 ) 𝑎1 sin 𝜃1 + 𝑎2 sin(𝜃1 +𝜃2 ) = ∗ ∗ 𝑜2,𝑦
0 0 1 0 0 1
NOTE:
• We don’t care about the orientation for this problem.
• In fact, we can’t choose the orientation if we also choose 𝑜2,𝑥 , 𝑜2,𝑦 .
Geometric Methods (closed-form solns)
• For some manipulators, it is possible to use fairly
simple trigonometry to solve the inverse kinematics
problem.
• Any two adjacent links are coplanar (any two
intersecting lines are coplanar).
• The origins of frames 𝑖 − 1, 𝑖, and 𝑖 + 1 define a
triangle. 𝑥2

• Simple, trigonometry in the plane might just get the

job done!
𝑦0
𝑥1

𝑥0
Solving for 𝜃2 Denote the coordinates of the
origin of frame 2 by 𝑜2,𝑥 , 𝑜2,𝑦 .
𝑥2
2 2
The Law of Cosines:
𝑟2 = 𝑜2,𝑥 + 𝑜2,𝑦 𝑟 2 = 𝑎12 + 𝑎22 − 2𝑎1 𝑎2 cos 𝛼

Define:
𝑦0 𝜃2 = 𝜋 − 𝛼
𝑎12 + 𝑎22 − 𝑟 2
𝛼 𝐷≝ = cos 𝛼
𝑥1 2𝑎1 𝑎2

𝑥0 Then sin 𝛼 = ± 1 − 𝐷2

Finally,
 With this set of equations, we can solve for 𝜃2 2
± 1 − 𝐷
using simple solutions to closed-form equations. 𝛼 = tan−1
 We never need to solve a nonlinear system! 𝐷
What about existence and uniqueness?
−1 ± 1−𝐷2
Does a solution always exist for 𝛼 = tan ?
𝐷
No solution exists if 𝐷2 > 1:
2 2 2 2
𝑎1 + 𝑎2 − 𝑟
𝐷2 = ≤1
2𝑎1 𝑎2

𝑎12 + 𝑎22 − 𝑟 2 ≤ ±2𝑎1 𝑎2

𝑎12 ± 2𝑎1 𝑎2 + 𝑎22 ≤ 𝑟 2

𝑎1 ± 𝑎2 2 ≤ 𝑟2

∣ 𝒂𝟏 ± 𝒂𝟐 ∣ ≤ 𝒓
In this case, a solution exists!
What about existence and uniqueness?
−1 ± 1−𝐷 2
Is the solution unique for 𝛼 = tan ?
𝐷
Clearly, the solution is not unique, since we may choose
either square root!
−1 − 1−𝐷2
The second solution uses 𝛼 = tan which results
𝐷
in an “elbow UP” configuration.
𝑥2
𝑥1
Elbow UP
NOTE: when ∣ 𝒂𝟏 ± 𝒂𝟐 ∣= 𝒓
the two solutions “collapse” 𝑦0
into a single solution.
𝑥1

𝑥0 Elbow DOWN
Degenerate Solutions
𝑦0 Arm fully extended
𝑟

𝑥0 𝑥1 𝑥2
𝑎1 + 𝑎2 = 𝑟

𝑦0
𝑟
Arm “folds back” on itself
𝑥2
𝑥0 𝑥1

𝑎1 − 𝑎2 = 𝑟
Solving for 𝜃1 Denote the coordinates of the
origin of frame 2 by 𝑜2,𝑥 , 𝑜2,𝑦 .
𝑥2
2
𝑟 2 = 𝑜2,𝑥 2
+ 𝑜2,𝑦 𝜃1 = 𝛽 − 𝛾

𝑎2 −1
𝑜2,𝑦
𝛽= tan
𝑦0 𝑜2,𝑥
𝛾 𝑥1
𝜃1 𝛽 The Law of Cosines, this time for 𝛾:
𝑥0
𝑎22 = 𝑎12 + 𝑟 2 − 2𝑎1 𝑟 cos 𝛾
 With this set of equations, we can solve for 𝜃1
using simple solutions to closed-form equations.
 We never need to solve a nonlinear system!
Elbow up is left as an exercise for you!
Position and Orientation
Suppose we wish to position the end effector frame at a specific position, and with a specific
orientation.
• We can parameterize the end effector frame by (𝑿𝒆 , 𝒀𝒆 , 𝝓)
• (𝑿𝒆 , 𝒀𝒆 ) give the position of the origin of the frame
• 𝝓 gives the orientation of the frame:

cos 𝜙 − sin 𝜙 𝑋𝑒
𝑇20 = sin 𝜙 cos 𝜙 𝑌𝑒
0 0 1
• We can’t do this with a two-link arm.
• Intuitively, we have three inputs (𝜃1 and 𝜃2 ) and three outputs.
• Our solution to the two-link arm shows that once we choose 𝜃1 and 𝜃2 the orientation of
the end-effector frame is fully determined.
Add another link!
 𝑥3
Three-Link Planar Arm 𝑦3 𝜙

𝐶123 −𝑆123 𝑎1 𝐶1 + 𝑎2 𝐶12 + 𝑎3 𝐶123 (𝑋𝑒 , 𝑌𝑒 )

𝑇20 = 𝑆123 𝐶123 𝑎1 𝑆1 + 𝑎2 𝑆12 + 𝑎3 𝑆123
0 0 1
cos 𝜙 − sin 𝜙 𝑋𝑒
𝑇30 = sin 𝜙 cos 𝜙 𝑌𝑒
0 0 1
𝐶123 = cos 𝜃1 + 𝜃2 + 𝜃3 , etc.

• Given (𝑿𝒆 , 𝒀𝒆 , 𝝓), we know everything about

the position and orientation of link 3!
• After all, link 3 is a rigid body, with a rigidly
attached Frame 3.
• If we know the position and orientation of
Frame 3, we know everything about Link 3!
 𝑥3
Three-Link Planar Arm 𝑦3 𝜙

Using simple trigonometry we obtain (𝑋𝑒 , 𝑌𝑒 )

𝑜2,𝑥 + 𝑎3 cos 𝜙 = 𝑋𝑒
𝑎3
𝑜2,𝑦 + 𝑎3 sin 𝜙 = 𝑌𝑒

𝜙
(𝑜2,𝑥 , 𝑜2,𝑦 )

And from this we immediately conclude

𝑜2,𝑥 = 𝑋𝑒 − 𝑎3 cos 𝜙
𝑜2,𝑦 = 𝑦𝑒 − 𝑎3 sin 𝜙
And now, it’s a 2-link 𝑥3
𝑦3
arm problem 𝜙

(𝑋𝑒 , 𝑌𝑒 )

𝑜2,𝑥 = 𝑋𝑒 − 𝑎3 cos 𝜙
𝑜2,𝑦 = 𝑦𝑒 − 𝑎3 sin 𝜙

As before, there are 0, 1, or two

solutions (elbow down is shown).
Prismatic Joint no link offset
• Define Frames 0 and 1 as before: 𝑥1 is collinear
Joint 2 is prismatic.
with the origin of Frame 0.
• Define Frame 2 such that 𝑥2 is collinear with 𝑥1
𝑎2
𝑥2
𝑦0

𝑥1
𝜃1
𝑥0
𝜽𝟐 = 𝟎 since 𝒙𝟏 and 𝒙𝟐 axes are parallel

1 0 0 1 0 𝑎2 1 0 𝑎2 1 0 𝑎2
𝑇21 = 0 1 0 0 1 0 = 0 1 0 𝑇21 = 0 1 0
0 0 1 0 0 1 0 0 1 0 0 1
Prismatic Joints
• Define Frames 0 and 1 as before: 𝑥1 is collinear
Joint 2 is prismatic.
with the origin of Frame 0.
• Define Frame 2 such that 𝑥2 is collinear with 𝑥1
𝑎2
𝑥2
𝑦0

𝑥1
𝜃1
𝑥0
Inverse Kinematic Solution
As before, let (𝑜2,𝑥 , 𝑜2,𝑦 ) denote the coordinates of the origin of Frame 2.

Since 𝜃2 = 0, the orientation of the end-

𝑎2
effector frame is completely determined by 𝜃1 : 𝑥2
𝑦0
−1
𝑜2,𝑦 (𝑜2,𝑥 , 𝑜2,𝑦 )
𝜃1 = tan
𝑜2,𝑥 𝑥1
𝜃1
𝑥0

Once we have a solution for 𝜃1 , we can directly solve the forward

kinematic equations for 𝑎2 :
𝑜2,𝑥 − 𝑎1 cos 𝜃1
(𝑎1 +𝑎2 ) cos 𝜃1 = 𝑜2,𝑥 ⇒ 𝑎2 =
cos 𝜃1
Three-link RPR Arm 𝑥3

𝑎2
𝜃3
𝑦0 Joint 2 is prismatic.
𝑥2

𝑥1
𝑥0

cos 𝜃1 − sin 𝜃1 𝑎1 cos 𝜃1 1 0 𝑎2 cos 𝜃3 − sin 𝜃3 𝑎3 cos 𝜃3

𝑇30 = sin 𝜃1 cos 𝜃1 𝑎1 sin 𝜃1 0 1 0 sin 𝜃3 cos 𝜃3 𝑎3 sin 𝜃3
0 0 1 0 0 1 0 0 1

cos(𝜃1 +𝜃3 ) − sin(𝜃1 +𝜃3 ) (𝑎1 + 𝑎2 ) cos 𝜃1 + 𝑎3 cos(𝜃1 +𝜃3 )

= sin(𝜃1 +𝜃3 ) cos(𝜃1 +𝜃3 ) (𝑎1 + 𝑎2 ) sin 𝜃1 + 𝑎3 sin(𝜃1 +𝜃3 )
0 0 1
Inverse Kinematic Solution 𝑥3
(𝑋𝑒 , 𝑌𝑒 )

𝑎2
𝝓
𝑦0 𝑥2
(𝑜2,𝑥 , 𝑜2,𝑦 )
𝑥1
𝑥0

As with the three-link RRR arm,

• Parameterize the end effector frame by (𝑿𝒆 , 𝒀𝒆 , 𝝓)
• Use 𝝓 and 𝒂𝟑 to solve for (𝒐𝟐,𝒙 , 𝒐𝟐,𝒚 )
• Solve for 𝜽𝟏 and 𝒂𝟐 using the two-link RP solution given above.
• 𝜽𝟑 = 𝝓 − 𝜽𝟏
Other Kinds of Robots
So far, we’ve looked only at simple planar arms with revolute joints.
Life becomes more complicated if
• We have prismatic joints with “link offsets”
• Robots are not planar (e.g., anthropomorphic arms)
• Robots have more joints than end-effector degrees of freedom (e.g.,
if a planar arm has four joint). Such robots are said to be redundant.

We’ll need to find other ways to solve the inverse kinematics for such
robots.
Let’s see an example…
Prismatic Joint with link offset
Joint 2 is prismatic.

𝑙2
𝑎2
𝑥2
𝑦0

𝑥1
𝜃1
𝑥0

1 0 0 1 0 𝑎2 1 0 𝑎2
𝑇21 = 0 1 0 0 1 𝑙2 = 0 1 𝑙2
0 0 1 0 0 1 0 0 1
Prismatic Joint with link offset

𝑙2 1 0 𝑎2
𝑎2
𝑥2 𝑇21 = 0 1 𝑙2
𝑦0
0 0 1
𝑥1
𝜃1
𝑥0 cos 𝜃1 − sin 𝜃1 𝑎1 cos 𝜃1 1 0 𝑎2
𝑇20 = sin 𝜃1 cos 𝜃1 𝑎1 sin 𝜃1 0 1 𝑙2
0 0 1 0 0 1

cos 𝜃1 − sin 𝜃1 (𝑎1 +𝑎2 ) cos 𝜃1 − 𝑙2 sin 𝜃1

= sin 𝜃1 cos 𝜃1 (𝑎1 +𝑎2 ) sin 𝜃1 + 𝑙2 cos 𝜃1
0 0 1
 𝑥3
Three-link RPR Example
The geometric IKS solution promises to be
painful, and this is still a simple planar arm.
𝜃3
𝑎2
𝑥2
𝑦0

𝑥1
𝜃1
𝑥0
cos 𝜃1 − sin 𝜃1 (𝑎1 +𝑎2 ) cos 𝜃1 − 𝑙2 sin 𝜃1 cos 𝜃3 − sin 𝜃3 𝑎3 cos 𝜃3
𝑇30 = sin 𝜃1 cos 𝜃1 (𝑎1 +𝑎2 ) sin 𝜃1 + 𝑙2 cos 𝜃1 sin 𝜃3 cos 𝜃3 𝑎3 sin 𝜃3
0 0 1 0 0 1

𝐶13 −𝑆13 (𝑎1 +𝑎2 ) 𝐶1 − 𝑙2 𝑆1 +𝑎3 𝐶13

= 𝑆13 𝐶13 (𝑎1 +𝑎2 ) 𝑆1 + 𝑙2 𝐶1 + 𝑎3 𝑆13
0 0 1
More General Robot Arms
Imagine finding the geometric IKS solutions for
more complicated arms in 3D work spaces…
Numerical Methods for Inverse Kinematics
• Denote the forward kinematic map by 𝐹 𝑞 .
• 𝐹 𝑞 is a vector-valued function, each component gives one coordinate of the end-effector pose. If the end
effector has 𝑚 degrees of freedom and the robot has 𝑛 joints, then

𝑓1 (𝑞)
𝐹 𝑞1 , … , 𝑞𝑛 = ⋮
𝑓𝑚 (𝑞)
• For the two-link arm, we would have
𝑥(𝜃1 , 𝜃2 )
𝐹 𝜃1 , 𝜃2 =
𝑦(𝜃1 , 𝜃2 )
• Denote by 𝑥 𝑑 the desired value of the end-effector pose.
• For the two-link arm we would have
𝑑
𝑜2,𝑥
𝑥𝑑 =
𝑑
𝑜2,𝑦

• The inverse kinematic solution is the vector 𝒒𝒅 such that 𝑭 𝒒𝒅 = 𝒙𝒅 .

Iterative Methods
Because we cannot solve 𝑭 𝒒𝒅 = 𝒙𝒅 for 𝒒𝒅 (nonlinear equation with no closed-form solution), we take
an iterative approach.

• Generate a sequence of values 𝒒𝟎 , 𝒒𝟏 , 𝒒𝟐 … until we find some 𝒒𝑵 such that 𝑭 𝒒𝑵 is sufficiently close
to 𝒙𝒅 , i.e.,

𝑭 𝒒𝑵 − 𝒙𝒅 < 𝝐

• In general, at each iteration, we compute 𝒒𝒊+𝟏 by

𝒒𝒊+𝟏 = 𝒒𝒊 + 𝜹𝒒

• The only trick is to determine a good value for 𝜹𝒒 at each iteration.

Gradient Descent
• In general, for gradient descent methods, the goal is to minimize the value of some function 𝐿(𝑞)
by choosing the updates according to 𝜹𝒒 = −𝛁𝐋 𝐪𝐢 where the gradient is w.r.t. 𝒒.

• To solve the inverse kinematics problem, we wish to minimize the error between 𝑭 𝒒 and 𝒙𝒅 .
• Let’s define 𝐿(𝑞) in terms of the squared error:

1 𝑇
𝐿 𝑞 = 𝐹 𝑞 − 𝑥𝑑 𝐹 𝑞 − 𝑥𝑑
2
• For this problem, we define the iteration as

𝒒𝒊+𝟏 = 𝒒𝒊 − 𝜶𝒊 𝛁𝐋(𝐪𝐢 )

• We can use the scalar 𝜶𝒊 to determine the magnitude of the step size.

• But what exactly is 𝛁𝐋(𝐪𝐢 )

Calculating the Error Gradient (𝑛, 𝑚 = 2)
1 𝑑
𝑓1 𝑞 − 𝑥1
𝐿 𝑞 = (𝑓1 𝑞 − 𝑥1𝑑 ) (𝑓2 𝑞 − 𝑥2𝑑 ) 𝜕𝐿
2 𝑓2 𝑞 − 𝑥2𝑑
𝜕𝑞1
𝛻𝐿 𝑞 =
1 1 2
𝜕𝐿
= (𝑓1 𝑞 − 𝑥1 ) + 𝑓2 𝑞 − 𝑥2𝑑
𝑑 2
2 2 𝜕𝑞2

𝜕 𝜕 1 2 2 𝜕 𝜕 1
𝐿 𝑞 = 𝑓1 𝑞 − 𝑥1𝑑 + 𝑓2 𝑞 − 𝑥2𝑑 𝐿 𝑞 = 𝑓1 𝑞 − 𝑑 2
𝑥1 + 𝑓2 𝑞 − 𝑑 2
𝑥2
𝜕𝑞1 𝜕𝑞1 2 𝜕𝑞2 𝜕𝑞2 2

𝜕𝑓1 𝜕𝑓2 𝜕𝑓1 𝜕𝑓2

= (𝑓 𝑞 − 𝑥1𝑑 ) + (𝑓 𝑞 − 𝑥2𝑑 ) = (𝑓 𝑞 − 𝑥1𝑑 ) + (𝑓 𝑞 − 𝑥2𝑑 )
𝜕𝑞1 1 𝜕𝑞1 2 𝜕𝑞2 1 𝜕𝑞2 2

Stack these to obtain the gradient.

Calculating the Gradient (cont)
𝜕𝐿 𝜕𝑓1 𝑑
𝜕𝑓2
(𝑓1 𝑞 − 𝑥1 ) + (𝑓2 𝑞 − 𝑥2𝑑 ) We can write this as a matrix equation
𝜕𝑞1 𝜕𝑞1 𝜕𝑞1
𝛻𝐿 𝑞 = =
𝜕𝐿 𝜕𝑓1 𝑑
𝜕𝑓2
(𝑓1 𝑞 − 𝑥1 ) + (𝑓2 𝑞 − 𝑥2𝑑 )
𝜕𝑞2 𝜕𝑞2 𝜕𝑞2
𝜕𝑓1 𝜕𝑓2
𝜕𝑓1 𝜕𝑓2 𝜕𝑞1 𝜕𝑞1
And we recognize that = 𝐽𝑇 (𝑞), the arm
𝜕𝑞1 𝜕𝑞1 𝑓1 𝑞 − 𝑥1𝑑 𝜕𝑓1 𝜕𝑓2
= 𝜕𝑞2 𝜕𝑞2
𝜕𝑓1 𝜕𝑓2 𝑓2 𝑞 − 𝑥2𝑑
Jacobian,
𝜕𝑞2 𝜕𝑞2 𝑓1 𝑞 − 𝑥1𝑑
and that = (𝑓 𝑞 − 𝑥 𝑑 )!
𝑓2 𝑞 − 𝑥2𝑑
= 𝑱𝑻 𝒒 (𝒇 𝒒 − 𝒙𝒅 )

And our gradient descent update becomes

𝒒𝒊+𝟏 = 𝑞𝑖 − 𝛼𝑖 𝛻L qi = 𝒒𝒊 + 𝜶𝒊 𝑱𝑻 𝒒𝒊 (𝒙𝒅 − 𝒇 𝒒𝒊 )
Inverse Jacobian Method
Let’s take a look at the Taylor series expansion for the forward kinematic map
around the i-th iteratate:

𝐹 𝑞 𝑖 + 𝛿𝑞 𝑖 = 𝐹 𝑞 𝑖 + 𝐽 𝑞 𝑖 𝛿𝑞 𝑖 + ℎ. 𝑜. 𝑡.

Here, h.o.t. refers to higher order terms (these go to zero quickly as 𝛿𝑞 𝑖 → 0).
If we neglect the higher order terms, the ideal choice for 𝛿𝑞 𝑖 would be
𝛿𝑞 𝑖 = 𝑞 𝑑 − 𝑞 𝑖
in which case we would step to the goal configuration in a single step!
In this case,
𝐹 𝑞 𝑖 + 𝛿𝑞 𝑖 − 𝐹 𝑞 𝑖 ≈ 𝐽 𝑞 𝑖 𝛿𝑞 𝑖
or
𝑥 𝑑 − 𝐹 𝑞 𝑖 ≈ 𝐽 𝑞 𝑖 𝛿𝑞𝑖
and this leads to the update Remember – we don’t know 𝒒𝒅 .
𝜹𝒒𝒊 = 𝑱−𝟏 𝒒𝒊 (𝒙𝒅 − 𝑭 𝒒𝒊 ) Happily, 𝒒𝒅 does not appear on the r.h.s!
What about redundant arms…
• For the two-link arm, we can position the end-effector origin anywhere in the
arm’s workspace: two inputs (𝜃1 , 𝜃2 ) and two “outputs” (𝑋𝑒 , 𝑌𝑒 ).
• For the three-link arm, we can position the end-effector origin anywhere in the
arm’s workspace, and we can choose the orientation of the frame: three inputs
(𝜃1 , 𝜃2 , 𝜃3 ) and three “outputs” (𝑋𝑒 , 𝑌𝑒 , 𝜙).
• Suppose we had a four-link arm?
• Infinitely may ways to achieve a desired end-effector configuration (𝑋𝑒 , 𝑌𝑒 , 𝜙).
The case for 𝑛 > 𝑚
In this case, there are “extra” joints:

𝐹 𝑞𝑖 + 𝛿𝑞𝑖 − 𝐹 𝑞𝑖 ≈ 𝐽 𝑞𝑖 𝛿𝑞𝑖
If we write this out in detail, we see

𝜕𝑓1 𝜕𝑓1
⋯
𝑓1 𝑞𝑖 + 𝛿𝑞𝑖 𝑓1 𝑞𝑖 𝜕𝑞1 𝜕𝑞𝑛 𝛿𝑞1𝑖
⋮ − ⋮ ≈ ⋱ ⋮
𝑓𝑚 𝑞𝑖 + 𝛿𝑞𝑖 𝑓𝑚 𝑞𝑖 𝜕𝑓𝑚 𝜕𝑓𝑚 𝛿𝑞𝑛𝑖
⋯
𝜕𝑞1 𝜕𝑞𝑛
Since 𝑱 is not square, we can’t invert it.

• Suppose 𝑟𝑎𝑛𝑘 𝐽 = 𝑚.
• Then 𝐽𝐽𝑇 ∈ ℝ𝑚×𝑚 and 𝑟𝑎𝑛𝑘 𝐽𝐽𝑇 = 𝑚
• Then 𝐽𝐽𝑇 −1 exists
What can we do with this…
Pseudoinverses
• Define 𝐽+ = 𝐽𝑇 𝐽𝐽𝑇 −1
• Suppose we let 𝛿𝑞 𝑖 = 𝐽+ 𝐹 𝑞 𝑖 + 𝛿𝑞 𝑖 − 𝐹 𝑞 𝑖
Then
𝐽𝛿𝑞 𝑖 = 𝐽𝐽+ 𝐹 𝑞 𝑖 + 𝛿𝑞 𝑖 − 𝐹 𝑞 𝑖 = 𝐹 𝑞 𝑖 + 𝛿𝑞 𝑖 − 𝐹 𝑞 𝑖

• In other words,
𝐽+ 𝐹 𝑞 𝑖 + 𝛿𝑞 𝑖 − 𝐹 𝑞 𝑖 = 𝛿𝑞 𝑖
is a solution to the equation
𝐹 𝑞 𝑖 + 𝛿𝑞 𝑖 − 𝐹 𝑞 𝑖 = 𝐽 𝑞 𝑖 𝛿𝑞 𝑖

• We can use this to define our update law:

𝜹𝒒𝒊 = 𝑱+ 𝑭 𝒒𝒊 + 𝜹𝒒𝒊 − 𝑭 𝒒𝒊

• 𝐽+ is called a pseudoinverse.