Robotics

QARM

Teach Pendant
for MATLAB Simulink
 © 2021 Quanser Inc., All rights reserved.

Quanser Inc.
119 Spy Court
Markham, Ontario
L3R 5H6
Canada

[email protected]
Phone: 19059403575
Fax: 19059403576
Printed in Markham, Ontario.
For more information on the solutions Quanser Inc. offers, please visit the web site at: http://www.quanser.com

This document and the software described in it are provided subject to a license agreement. Neither the
software nor this document may be used or copied except as specified under the terms of that license
agreement. Quanser Inc. grants the following rights: a) The right to reproduce the work, to incorporate the
work into one or more collections, and to reproduce the work as incorporated in the collections, b) to
create and reproduce adaptations provided reasonable steps are taken to clearly identify the changes
that were made to the original work, c) to distribute and publicly perform the work including as
incorporated in collections, and d) to distribute and publicly perform adaptations. The above rights may be
exercised in all media and formats whether now known or hereafter devised. These rights are granted
subject to and limited by the following restrictions: a) You may not exercise any of the rights granted to
You in above in any manner that is primarily intended for or directed toward commercial advantage or
private monetary compensation, and b) You must keep intact all copyright notices for the Work and
provide the name Quanser Inc. for attribution. These restrictions may not be waved without express prior
written permission of Quanser Inc.

1
QArm Concept Review
Teach Pendant

What is a Teach Pendant?

A teach pendant is a hardware interface used to first “teach” a set of discrete points to a robotic manipulator,
then initiate motion playback where the robot traverses each of the taught points. Teach pendants are
commonly used to program robots to perform pick, manipulate and place tasks in an assembly line. Since
the desired points that are taught to the robot are Cartesian coordinates of the end-effector, we require a
formulation that determines the required joint angles that result in the desired end-effector position. This
formulation is referred to as the manipulator’s inverse kinematic model. The purpose of this lab is two-fold.
First, you will determine the robot’s inverse kinematics model, then use the model to implement a software-
based teach pendant that simulates a robotic assembly process.

2
Forward Kinematic Equations
Recall from the Forward Kinematics Workspace Identification lab the forward kinematics formulation 𝑓𝐹𝑃𝐾

⃗⃗⃗ )
⃗⃗⃗ = 𝑓𝐹𝑃𝐾 ( 𝜃
𝑝
𝑝𝑥 = 𝜆2 𝑐1 𝑐2 − 𝜆3 𝑐1 𝑠23
𝑝𝑦 = 𝜆2 𝑠1 𝑐2 − 𝜆3 𝑠1 𝑠23 (1)
𝑝𝑧 = 𝜆1 − 𝜆2 𝑠2 − 𝜆3 𝑐23

where 𝑠 and 𝑐 represent the trigonometric functions sine and cosine, respectively, 𝜆1 through 𝜆3 are arm-
specific constants, ⃗⃗⃗
𝜃 is the vector representing the joint states,
𝜃1
𝜃
𝜃 = [ 2]
⃗⃗⃗ (2)
𝜃3
𝜃4

and 𝑝
⃗⃗⃗ is the position of the end-effector {4} (expressed in base frame {0})
𝑝𝑥
⃗⃗⃗ = [ 𝑦 ] = 0𝑝4
𝑝 𝑝 (3)
𝑝𝑧

Inverse Kinematic Formulation

The goal of the inverse kinematics formulation 𝑓𝐼𝑃𝐾 is to calculate the joint angles from the Cartesian
coordinates of the end-effector. In other words, we will develop the formulation

⃗⃗⃗
𝜃 = 𝑓𝐼𝑃𝐾 ( 𝑝
⃗⃗⃗ ) (4)

End-Effector Position

Begin with the equations describing the end-effector’s 𝑝𝑥 and 𝑝𝑦 .

𝑝𝑥 = 𝜆2 𝑐1 𝑐2 − 𝜆3 𝑐1 𝑠23 = 𝑐1 (𝜆2 𝑐2 − 𝜆3 𝑠23 )

(5)
𝑝𝑦 = 𝜆2 𝑠1 𝑐2 − 𝜆3 𝑠1 𝑠23 = 𝑠1 (𝜆2 𝑐2 − 𝜆3 𝑠23 )

Next, we square and then add the equations describing 𝑝𝑥 and 𝑝𝑦 , giving,

𝑝𝑥2 + 𝑝𝑦2 = 𝑐12 (𝜆2 𝑐2 − 𝜆3 𝑠23 )2 + 𝑠12 (𝜆2 𝑐2 − 𝜆3 𝑠23 )2 = (𝜆2 𝑐2 − 𝜆3 𝑠23 )2
(6)
±√𝑝𝑥2 + 𝑝𝑦2 = 𝜆2 𝑐2 − 𝜆3 𝑠23

Equations 1 and 6 provide the following set of equations,

±√𝑝𝑥2 + 𝑝𝑦2 = 𝜆2 𝑐2 − 𝜆3 𝑠23

(7)
𝜆1 − 𝑝𝑧 = 𝜆2 𝑠2 + 𝜆3 𝑐23

3
This resembles the following trigonometric equations,

𝐴 cos 𝜃2 + 𝐵 cos(𝜃2 + 𝜃3 ) + 𝐶 sin(𝜃2 + 𝜃3 ) = 𝐷

(8)
𝐴 sin 𝜃2 + 𝐵 sin(𝜃2 + 𝜃3 ) − 𝐶 cos(𝜃2 + 𝜃3 ) = 𝐻

Where the following mapping applies,

𝐴 = 𝜆2

𝐵 = 0

𝐶 = −𝜆3

(9)
𝐷 = ±√𝑝𝑥2 + 𝑝𝑦2

𝐻 = 𝜆1 − 𝑝𝑧

𝐷2 +𝐻 2 −𝐴2 −𝐶 2
𝐹 =
2𝐴

Note that there are two possible values for 𝐷. Regardless, the solution for 𝜃3 involves squaring the 𝐷 term
which clears ambiguity. The two possible solutions for 𝜃3 are,

𝐶 + √𝐶 2 + 𝐹 2
𝜃3,1 = 2 tan−1 ( ) = 2 𝑎𝑡𝑎𝑛2 (𝐶 + √𝐶 2 + 𝐹 2 , 𝐹 )
𝐹
(10)
𝐶 − √𝐶 2 + 𝐹 2
𝜃3,2 = 2 tan−1 ( ) = 2 𝑎𝑡𝑎𝑛2 (𝐶 − √𝐶 2 + 𝐹 2 , 𝐹 )
𝐹

Note that the function 𝑎𝑡𝑎𝑛2 takes the y and x components into account separately to provide a solution in
the range [−𝜋, 𝜋 ] as supposed to the 𝑎𝑡𝑎𝑛 function, which takes a single input 𝑝𝑦 /𝑝𝑥 and only provides
solutions in the range [−𝜋/2, 𝜋/2 ].

Before solving for 𝜃2 , some additional mapping is required,

𝑀 = 𝐴 + 𝐶 sin 𝜃3

𝑁 = −𝐶 cos 𝜃3
𝐷𝑀+𝐻𝑁 (11)
cos 𝜃2 =
𝑀2 +𝑁2
𝐻−𝑁 cos 𝜃2
sin 𝜃2 =
𝑀

The possible solutions for 𝜃2 are calculated using the same equation,
sin 𝜃2
𝜃2 = tan−1 ( ) = 𝑎𝑡𝑎𝑛2 (sin 𝜃2 , cos 𝜃2 ) (12)
cos 𝜃2

The pair of terms 𝑀 and 𝑁 have 2 possible values due to the two solutions for 𝜃3 . For each of these, the
cos 𝜃2 term has 2 possible values due to the 𝐷 term. This results in a total of 4 possible solutions, all found
using equation 12. Knowing 𝜃2 and 𝜃3 , return to equation 5, and take the following ratio
4
 𝑝𝑦
⁄𝜆 𝑐 − 𝜆 𝑠
tan 𝜃1 = 𝑝 2 2 3 23
(13)
𝑥
⁄𝜆 𝑐 − 𝜆 𝑠
2 2 3 23

Using the inverse tangent, we can solve for 𝜃1 .

𝑝𝑦
⁄𝜆 𝑐 − 𝜆 𝑠 𝑝 𝑝
𝜃1 = tan−1 (𝑝 2 2 3 23
) = 𝑎𝑡𝑎𝑛2 ( 𝑦⁄𝜆 𝑐 − 𝜆 𝑠 , 𝑥⁄𝜆 𝑐 − 𝜆 𝑠 ) (14)
𝑥 2 2 3 23 2 2 3 23
⁄𝜆 𝑐 − 𝜆 𝑠
2 2 3 23

Although the term 𝜆2 𝑐2 − 𝜆3 𝑠23 is common in the numerator and denominator of the ratio in Equation 14,
note that it would be unwise to simply cancel it out. The sign of this term matters and will result in a unique
𝜃1 solution for each set of 𝜃2 and 𝜃3 .

End-Effector Orientation

Although the wrist angle 𝜃4 is not calculated from the position above, an end-effector orientation may also
be provided in the form of a rotation matrix. Knowing the possible values for 𝜃1 , 𝜃2 and 𝜃3 , one can solve the
rotation matrix partially and match terms to derive 𝜃4 .

A user cannot, however, specify an arbitrary position and orientation for the end-effector. There are 6
general degrees of freedom for position and orientation, however, there are only 4 degrees of freedom in
the manipulator. Often, the orientation may simply be specified as a wrist angle 𝛾, and the solution for the
wrist joint angle is

𝜃4 = 𝛾 (15)

Summary

To summarize, the Inverse Kinematics formulation is given as follows,

2
𝑝2 + 𝑝𝑦2 + (𝜆1 − 𝑝𝑧 )2 − 𝜆22 − 𝜆23 𝑝𝑥2 + 𝑝𝑦2 + (𝜆1 − 𝑝𝑧 )2 − 𝜆22 − 𝜆23
𝜃3 = 2 𝑎𝑡𝑎𝑛2 (−𝜆3 ± √𝜆3 2 + ( 𝑥 ) , )
2 𝜆2 2 𝜆2

(2 solutions possible)

±(𝜆2 − 𝜆3 sin 𝜃3 )√𝑝𝑥2 + 𝑝𝑦2 + (𝜆3 cos 𝜃3 )(𝜆1 − 𝑝𝑧 )

𝜆1 − 𝑝𝑧 − 𝜆3 cos 𝜃3 ( )
(𝜆2 − 𝜆3 sin 𝜙3 )2 + (𝜆3 cos 𝜃3 )2 ±(𝜆2 − 𝜆3 sin 𝜃3 )√𝑝𝑥2 + 𝑝𝑦2 + (𝜆3 cos 𝜃3 )(𝜆1 − 𝑝𝑧 )
𝜃2 = 𝑎𝑡𝑎𝑛2 ,
𝑀 (𝜆2 − 𝜆3 sin 𝜃3 )2 + (𝜆3 cos 𝜃3 )2
( ) (16)
(2 solutions possible for each 𝜙3 )

𝑝𝑦
⁄𝜆 cos 𝜃 − 𝜆 sin(𝜃 + 𝜃 ) 𝑝 𝑝𝑥
𝜙1 = tan −1
(𝑝 2 2 3 2 3
) = 𝑎𝑡𝑎𝑛2 ( 𝑦⁄𝜆 cos 𝜃 − 𝜆 sin(𝜃 + 𝜃 ) , ⁄𝜆 cos 𝜃 − 𝜆 sin(𝜃 + 𝜃 ))
𝑥 2 2 3 2 3 2 2 3 2 3
⁄𝜆 cos 𝜃 − 𝜆 sin(𝜃 + 𝜃 )
2 2 3 2 3

(1 unique solution possible for each 𝜃2 and 𝜃3 )

𝜃4 = 𝛾

(1 unique solution)

5
Teach Pendant Procedure
Once the inverse kinematic model of the manipulator has been formulated, you can proceed to implement
the software-based teach pendant. The teach pendant process used in this laboratory involves two steps:
(a) learn and (b) follow. In the first step, illustrated in Figure 1.1, the robot is taught a series of end-effector
coordinates as waypoints. For every waypoint, using the inverse kinematic model, the optimal joint
configurations are determined and commanded to the robotic arm. These waypoints are also added to a
first-in-fist-out (FIDO) queue.

Figure 1.1: The “learn” procedure of the teach pendant

In the second step, illustrated in Figure 1.2, the taught waypoints are extracted from the queue and replayed
in sequence.

Figure 1.2: The “follow” procedure of the teach pendant