A Simulink-based robotic toolkit for simulation and control of the PUMA 560 robot manipulator

Abstract

A Simulink robotic toolkit (SRTK) for the Puma 560 robot
manipulator is developed on the MATLAB/Simulink-based platform. Through
the use of the real-time Linux target and the real-time Windows target,
the SRTK can be executed on the Linux or Win32-based operating systems
in real-time. Moreover, the graphical user-friendly nature of Simulink
allows the SRTK to be a flexible tool that can easily be customized to
fit the specific needs of the user, that is, based on the layered
approach of the SRTK, the user can perform operations such as
calibration, joint control, Cartesian control, Cartesian PD control,
impedance control, some trajectory generation tasks, and real-time
simulation of the Puma 560 through a user-friendly MATLAB-based
graphical user interface without writing any code. The SRTK allows a
researcher to use the Puma 560 without the burden of the external issues
related to the control, interface, and software issues, while providing
for the flexibility for easily modifying components for increased
functionality

Full text

Page 1

A Simulink Based Robotic Toolkit for Simulation and Control of the
Puma 560 Robot Manipulator*

W. E. Dixon1, D. Moses2, I. D. Walker2, and D. M. Dawson2
1Robotics and Process Systems Division, Oak Ridge National Laboratory, P.O. Box 2008, Oak Ridge, TN 37831-6305
2Department of Electrical and Computer Engineering, Clemson University, Clemson, SC 29634-0915



E-mail: dixonwe@ornl.gov, Telephone: (865) 574-9025


Keywords: Robotic Toolkit, Real-Time Control, Simulink, Graphical User Interface




"The submitted manuscript has been authored
by a contractor of the U.S. Government
under contract No. DE-AC05-
96OR22464. Accordingly,
Government retains a nonexclusive, royalty-
free license to publish or reproduce the
published form of this contribution, or allow
others to do so, for U.S. Government
purposes."
the U.S.











Submitted to the IEEE/RSJ International Conference on Intelligent Robots and Systems, Oct. 29-
Nov. 3, 2001, Maui, Hawaii


* This research was performed in part by a Eugene P. Wigner Fellow and staff member at the Oak
Ridge National Laboratory, managed by UT-Battelle, LLC, for the U.S. Department of Energy
under contract DE-AC05-00OR22725 and is supported in part by the U.S. NSF Grants DMI-
9457967, DMI-9813213, EPS-9630167, ONR Grant N00014-99-1-0589, a DOC Grant, and an
ARO Automotive Center Grant.

Page 2

A Simulink-Based Robotic Toolkit for Simulation and Control of the
PUMA 560 Robot Manipulator∗
W. E. Dixon†, D. Moses‡, I. D. Walker‡, and D. M. Dawson‡
†Robotics and Process Systems Division, Oak Ridge National Laboratory, P.O. Box 2008-6305, Oak Ridge, TN 37831
‡Department of Electrical and Computer Engineering, Clemson University, Clemson, SC 29634
dixonwe@ornl.gov, dmoses, ianw, ddawson@ece.clemson.edu
Abstract
In this paper, a Simulink Robotic Toolkit (SRTK) for
the Puma 560 robot manipulator is developed on the
MATLAB/SIMULINK-based platform. Through the use of
the Real-Time Linux Target and the Real-Time Windows Tar-
get, the SRTK can be executed on the Linux or Win32-based
operating systems (e.g., Windows 95/98/NT) in real-time.
Moreover, the graphical user-friendly nature of Simulink al-
lows the SRTK to be a flexible tool that can easily be cus-
tomized to fit the specific needs of the user. That is, based on
the layered approach of the SRTK, the user can perform op-
erations such as calibration, joint control, Cartesian control,
Cartesian PD control, impedance control, some trajectory
generation tasks, and real-time simulation of the Puma 560
through a user-friendly MATLAB-based graphical user inter-
face (GUI) without writing any code. One of the key features
of the toolkit is that users can easily incorporate additional
functionality and hardware through the simple block diagram
interface that Simulink provides. Moreover, the MATLAB-
based GUI can also easily be modified to allow the user to
exploit the added functionality with the GUI by using straight-
forward MATLAB script files.
researcher to use the Puma 560 without the burden of the
external issues related to the control, interface, and software
issues, while providing for the flexibility for easily modifying
components for increased functionality.
Hence, the SRTK allows a
1Introduction
The field of robotics is highly diversified and multidiscipli-
nary. For example, a few of the present robotics research ef-
forts are directed at trajectory generation, redundancy resolu-
tion, actuator-level control, sensor development and applica-
tion (e.g., visual servoing), and biologically inspired kinematic
design. The diversified nature of robotics presents a roadblock
to many researchers, because to utilize a robotic system, the
researcher must first address a variety of issues such as: hard-
ware interfacing, actuator-level control implementation, soft-
ware development, and user-interface development before the
issues that are of interest to the researcher can be addressed.
One avenue that robotics researchers have pursued to over-
come this obstacle is the development of a software platform
that provides the user with basic operating capabilities of the
robotic system. To this end, software developers have uti-
lized: 1) robot control languages, 2) high-level programming
languages like C/C++, 3) graphical control environments, and
4) robotic libraries for common programming languages.
Robot control languages provide a set of commands for im-
plementing control applications on the robotic system; how-
∗This research was performed in part by a Eugene P. Wigner
Fellow and staff member at the Oak Ridge National Laboratory,
managed by UT-Battelle, LLC, for the U.S. Department of Energy
under contract DE-AC05-00OR22725 and is supported in part by
the U.S. NSF Grants DMI-9457967, DMI-9813213, EPS-9630167,
ONR Grant N00014-99-1-0589, a DOC Grant, and an ARO Auto-
motive Center Grant.
ever, they are typically provided by the vendors of the robotic
system and exploit proprietary hardware such as special pur-
pose processors. Due to the fact that these languages are pro-
vided by the vendor and exploit propriety hardware, the users
ability to incorporate additional functionality and/or hard-
ware components is very limited. Due to the limited scope
of robot control languages, some researchers have been mo-
tivated to develop a new software platform from scratch us-
ing high-level programming languages such as C. Clearly, this
approach allows a developer to implement a custom system
that fits specific needs, however as described earlier, the de-
veloper must address issues that are not related to the specific
research task. In addition, the development of the custom
software platform is very time consuming and error-prone and
requires a high level of skill. Since the development of a cus-
tom software platform is such an extensive task, some software
libraries have been developed to facilitate robotic applications
(e.g., RCCL [1] and the robot control library ARCL [2]). How-
ever, the size and complexity of these libraries make them very
difficult to modify (see [3] for a discussion related to the ob-
stacles that were faced using RCCL and ARCL to develop a
robotic manipulator system for decommissioning tasks).
One of the limiting factors of the robotic platform concepts
described above is that to incorporate new functionality or
new hardware, a user must modify the source code. To over-
come this problem, object-oriented approaches and robot con-
trol libraries have been developed (see [4], [5], [6], and the ref-
erences within). One of the most recent object-oriented con-
cepts is given in [3]. In [3], Loffler et al. describe the structure
and benefits of the QMotor Robotic Toolkit (QRTK), which
is “a set of C++ libraries and programs that follow object-
oriented concepts to ensure code reuse, modularity, scalability,
and an intuitive code structure” that executes on the QNX4
and QNX6 operating systems. However, one of the potential
drawbacks of the QRTK is that the user is required to write
and debug code written in C or C++ to implement control
algorithms, and if the user wants to incorporate additional
functionality, the user is required to write additional object-
oriented code. In [7], Ge et al. describe the open-architecture
platform OpenRob for model building, control design, and
simulations for robot manipulators on Win32 operating sys-
tems (e.g., Windows 95/98/NT); however, OpenRob users are
required to write custom dynamic link libraries (DLLs) to de-
velop customized robot modules, the construction of the GUI
is hidden and would require an experienced user to modify
the GUI to add more flexibility, and although the authors of
[7] claim real-time control under Win32 operating systems, no
description is provided regarding how real-time performance
is achieved.
In contrast to the previous platforms, several researchers
have elected to develop robotic applications based on the
MATLAB-based platform.Specifically, in [8], Chen and
Naughton proposed a MATLAB/Simulink-based control sys-
tem platform for the Linux operating system as a means for
students to design, simulate, and implement various control
laws on a DC motor without requiring the students to: 1)
program the controller using languages such as C or C++, 2)
switch back and forth between platforms for design, simula-
tion, and implementation tasks, 3) and spend time debugging
1

Page 3

the control code. Unfortunately, as stated in [8], the control is
not implemented in real-time due to the fact that MATLAB-
MEX functions were utilized. In [9], Pires describes the de-
velopment of a MATLAB-based toolbox called MATROB-
COM that can be utilized to control some industrial robot-
ics and automation equipment in Win32 operating systems.
Unfortunately, since MATROBCOM also relies on the use of
MATLAB MEX functions, the toolbox cannot be executed in
real-time. In contrast, the Simulink Robotic Toolkit (SRTK)
that is described in this paper is developed on the MATLAB
and Simulink platforms in conjunction with the Real-Time
Linux Target (RTLT) [10] or the Real-Time Windows Tar-
get (RTWT) [11], and hence, the SRTK can be executed in
real-time1in Linux and Win32 operating systems. Specifi-
cally, the SRTK for the Puma 560 arm is set of MATLAB
script files (m-files), MATLAB-toolboxes, and Simulink block-
diagrams that can be used for real-time simulation, control,
and data-logging for post-runtime analysis of the Puma 560
robot manipulator. The driving philosophy behind the SRTK
is that a user-friendly MATLAB/Simulink-based environment
enabled by a MATLAB-based Graphical User Interface (GUI)
allows users with various backgrounds to exploit the function-
ality of MATLAB and Simulink to perform robotics-based
research on the Puma 560. That is, the efforts of a broad
class of researchers are enabled by the fact that the SRTK
exploits the modular nature of Simulink block-diagrams and
the functionality that the wide range of MATLAB toolboxes
provides. For example, a researcher with expertise in control
theory can simply remove the control block from the SRTK
Simulink block-diagram and substitute a user-developed con-
trol block-diagram for the new control algorithm. Likewise,
researchers investigating problems related to robot vision or
trajectory generation could easily modify the Simulink block-
diagram to test the specific research concepts without having
to devote time and energy to address issues that are not re-
lated to the research effort (e.g., interfacing with the Puma 560
hardware, developing an actuator-level control algorithm). Al-
though there are many elements of the SRTK that are generic
to any robot manipulator (e.g., trajectory generation), the
SRTK is robot specific due to the requirement for robot spe-
cific elements (e.g., calibration routines, hardware interface
elements). Extending the SRTK to other robots is not a triv-
ial task since either the robot would have to be truly open
architecture (i.e, open access to all the hardware components
as in the Barrett Whole Arm Manipulator (WAM)) or consid-
erable back engineering efforts of the robot would be required.
In fact, the current development of the SRTK for the PUMA
560 was greatly facilitated by the extensive back engineering
efforts by Voyle [17]. Since the WAM is a truly open architec-
ture robot, collaborative efforts with Barrett are in progress
regarding extending the SRTK to support the WAM.
The structure of the SRTK is based on a layered approach,
in which the top-most layer is the GUI. The GUI enables a
user to exploit the functionality of the SRTK without requiring
the user to work with the Simulink block-diagram; however,
based on the layered structure of the SRTK, a user can eas-
ily integrate additional hardware, incorporate additional func-
tionality, or modify the algorithm of an existing function (e.g.,
replace the SRTK trajectory generator with a user-developed
trajectory generator) by simply modifying the various layers
of the Simulink block-diagram. In some instances, the SRTK
GUI may need to be modified to reflect the additional user
defined functionality. Since the GUI is constructed using sim-
ple MATLAB script files, the user can create their own GUI
or simply modify the SRTK GUI as desired. To provide an
overview of the SRTK, this paper is organized as follows. In
Section 2, a description of the underlying Simulink-block dia-
gram is provided. In Section 3, the different operating modes
of the SRTK are described. In Section 4, the real-time Puma
simulator is described. In Section 5, a brief overview of the
1Hard real-time (i.e., predictable latency) is guaranteed us-
ing RTLT, however, it is not clear that it is guaranteed us-
ing MATLAB’s Real-Time Workshop (RTW) and the RTWT;
although, RTW is advertised as providing real-time perfor-
mance, to the best of our knowledge, no measures of latency
have been published that would indicate hard real-time is
achieved.
functionality of the SRTK GUI is given. Concluding remarks
are presented in Section 6.
2 Description
Block-Diagram
of theSimulink
The underlying Simulink block-diagram of the SRTK is orga-
nized to facilitate the addition or removal of different oper-
ating modes and/or auxiliary hardware components. As de-
scribed below, the Simulink block-diagram can be divided into
three main sections including: the Input Section, the Compu-
tation Section, and the Output Section. This structure allows
the user to either target the basic Input/Output (I/O) op-
erations with the Puma 560 through the Input and Output
sections, or target the modification of the functionality and
execution of the different operating modes that the SRTK pro-
vides through the Computation section.
Input Section: The Input Section of the Simulink block-
diagram provides the input interface between the Puma 560
and the SRTK. That is, every signal that is read into the SRTK
from the Puma 560 and the GUI such as the encoder values,
the potentiometer values, the desired joint angles, and the
desired Cartesian positions are located in this section along
with input signals from the GUI, such as the desired end-
effector position. If a user does not incorporate any additional
hardware with the Puma 560, the Input Section will not need
to be modified unless the user wants to place additional scopes
on the diagram to observe the input signals. If a user adds
additional hardware to the Puma 560, then the user would be
required to include new inputs in this section. In addition to
the input signals from the Puma 560, the Input Section has
also incorporated selector switches, which link the GUI to the
Simulink block-diagram. That is, the selector switches provide
a means for a user to execute the different operating modes of
the SRTK from the GUI.
Computation Section: The Computation Section incorpo-
rates the mathematical details for the various modes of op-
eration for the Puma as separately labeled subsets (masked
blocks). Each of the operating modes is encapsulated in a
separate subsystem that is either enabled or disabled as de-
sired using the selector switches that are accessed through the
GUI. One of the advantages of the SRTK is that the user-
friendly GUI, which is utilized to control all the functions of
the SRTK, is constructed using standard MATLAB functions,
and hence, if a user desires to incorporate additional function-
ality in the SRTK, the user can simply modify the existing
Simulink block-diagram and then either create a new GUI or
modify the existing GUI to incorporate the additional func-
tionality.
Output Section: The Output Section of the diagram pro-
vides the output interface between the Puma 560 and the
SRTK. If a user does not incorporate any additional hard-
ware with the Puma 560, the Output Section will not need to
be modified unless the user wants to place additional scopes
on the diagram to observe the output signals. If a user adds
additional hardware to the Puma 560, then the user would be
required to include new outputs in this section. In addition
to the signals output to the Puma 560, the Output Section
also incorporates the Puma 560 dynamic model to facilitate
real-time simulation of the Puma without activating the hard-
ware. The Output Section also has embedded safety features
that enable the SRTK to avoid commanding excessively high
voltage or torque values to the Puma 560. A termination block
is also included in this section for Windows users to allow the
SRTK to disable the arm-power at the end of the real-time
simulation mode.
3Operating Modes
3.1Calibration Mode
The calibration mode is the most vital mode of the SRTK be-
cause it calibrates the Puma 560 hardware. If the Puma 560
is not calibrated correctly, the other modes of the SRTK will
2

Page 4

not function correctly because the information received from
the manipulator sensors will be invalid.
modes of the SRTK GUI are not enabled until the Puma has
been successfully calibrated. To calibrate the Puma 560, the
user simply presses the Calibrate Puma button of the GUI.
When the Calibrate Puma button is pressed, the SRTK ac-
quires voltage readings (that relate to the joint angle) from
the potentiometers that are incorporated with each joint of
the manipulator. To filter the noise that is inherent to the
potentiometer voltage signals, the average of 100 readings is
taken, and hence, a 100 millisecond delay is incurred since
the SRTK is operated at 1kHz. Based on the filtered voltage
readings from the potentiometers, denoted by PV ∈ R, the
encoder values of each joint, denoted by EVestimated∈ R, are
set using the following expression:
In fact, the other
EVestimated = PS ∗ PV + PI
(1)
where PS ∈ R and PI ∈ R are parameters that represent
the “potentiometer slope” and the “potentiometer intercept”
that must be determined by the user for the specific Puma 560
(see [12] for information regarding these parameters). Based
on the rough estimates of the joint positions, the SRTK en-
ables a proportional derivative (PD) joint controller to move
each joint of the Puma to an index pulse of the joint encoder.
The motion of the manipulator is always towards the “Ready
Position” because the “Ready Position” locates each joint at
its midpoint, and hence, the possibility of reaching a joint
limit during calibration is eliminated. When the encoder in-
dex pulse is detected, the encoder readings at that point are
recorded as EVatIndex ∈ R. The correct encoder count at
the nearest index pulse, denoted by EVatNIndex ∈ R, is then
determined by the SRTK as follows:
µ·double(EVatIndex− eFirst)
+0.5])eDelta + eFirst
EVatNIndex
=floor
eDelta
(2)
where floor() is a function that rounds the argument down
to the nearest integer, double() is a function that converts
the argument to a real value, and eFirst, eDelta ∈ R are
constant parameters that depend on the user’s specific Puma
560 and denote the encoder count measured at the first index
pulse from the joint limit and the number of encoder counts
between two index pulses, respectively (see [12] for specific
information regarding these parameters). After the SRTK de-
termines the correct encoder count at the index pulse, the
manipulator moves back to the initial position. To determine
the calibrated position of the encoder (and hence the joint an-
gle) at the present position, denoted by EVcalibrated∈ R, the
following equation is utilized:
EVcalibrated= EVatNIndex+ EVpresent− EVatIndex
where EVpresent∈ R is utilized to denote the current position
instead of EVestimated since the manipulator may not have
moved back to exactly the same encoder position. The correct
encoder values are set in the SRTK when the Calibration Done
button of the GUI is pressed.
(3)
Remark 1 Due to the similarity in the construction, the re-
quired calibration parameters given in [12] can be used to cali-
brate Puma 560 manipulators. That is, given the information
in [12], a user is not required to use parameter identification
techniques.
3.2 Zero Gravity Mode
Often times, a user may want to teach specific joint positions
to the manipulator by simply positioning the joints by hand.
This task is difficult due to the weight of the Puma 560 links.
That is, once the mechanical joint brakes are released, the
user has to support the weight of the entire manipulator while
attempting to accurately position each of the joints. To facili-
tate this operation, the zero gravity mode of the SRTK applies
a gravity compensation torque to each joint allowing the con-
troller to absorb the burden of supporting the manipulator
links and allowing the user to simply move each joint to the
desired location. This goal is achieved through the SRTK by
applying a gravity compensation torque to each joint of the
manipulator as shown below
τ = G(q)
(4)
where τ ∈ R6denotes the control torque input, G(q) ∈ R6
denotes the gravity effects acting on the manipulator, and
q(t) ∈ R6denotes the joint angles. Once the user positions
the Puma in a desired configuration, the joint positions can
be stored for later use by other operating modes.
3.3Joint Control Mode
This mode is the one of the most basic modes for controlling
the Puma 560.The user-defined inputs for this mode are
the final desired joint positions and a movement scale factor.
Given the initial position and the user-defined final position
of the manipulator, the maximum of all the joint errors is
calculated as follows:
deltamax
=
=
max(|qfinal(i) − qinitial(i)|)
1,2,...6
(5)
∀i
where deltamax ∈ R denotes the maximum joint error, and
qfinal, qinitial ∈ R6denote the desired joint position and the
initial joint position, respectively.
joint error and the user-defined movement scale parameter,
denoted by MS ∈ R, the time scale for the desired trajectory
is calculated from the following equation:
Based on the maximum
¯ t =
t
MS (deltamax)
(6)
where¯t ∈ R represents a scaled time signal. Once the time
scale has been determined, the position, velocity, and acceler-
ation trajectories are developed from the following equations:
sdelta = qfinal− qinitial
qd= sdelta¡6¯t5− 15¯t4+ 10¯t3¢+ qinitial
¨ qd = sdelta¡120¯t3− 180¯t2+ 60¯t¢
velocity, and acceleration trajectories that are generated by
the SRTK.
After the desired trajectory has been formulated by the
SRTK, the difference between the actual and desired position
and velocity of the manipulator joints are utilized in conjunc-
tion with a gravity compensation term and the desired ac-
celeration trajectory to construct the following torque control
input:
(7)
(8)
˙ qd= sdelta¡30¯t4− 60¯t3+ 30¯t2¢
where qd(¯t), ˙ qd(¯t), ¨ qd(¯t) ∈ R6denote the desired position,
(9)
(10)
τ = Kp(qd− q) + Kd(˙ qd− ˙ q) + ¨ qd+ G(q)
where Kp, Kd ∈ R are constant control gains and τ(q) ∈ R6
represents the control torque input. One of the advantages of
this mode is that the PD control law does not contain singu-
larities since the control is performed at the joint level. How-
ever, the safety measures will disable the arm power and the
mechanical locks will hold the manipulator at the terminated
position if the controller attempts to force the Puma 560 in
a configuration that violates a joint limit. At the end of the
motion, the joints are maintained at the desired positions.
(11)
3.4 Cartesian Control Mode
The Cartesian control mode enables the user to select desired
final Cartesian coordinates for the manipulator rather than
selecting the desired final joint configurations as in the joint
control mode; however, for simplicity, in this mode and the
subsequent Cartesian PD and impedance control modes, the
roll, pitch, and yaw of the end-effector are held fixed and only
the position of the end-effector in the XY Z Cartesian plane
is controlled. Based on the initial and final Cartesian position
of the manipulator, the SRTK generates a desired trajectory
3

Page 5

(in a similar manner as given in (5)-(10)) in the Cartesian
Space that is scaled by the user defined movement scale factor,
and a PD control law is applied to make the Puma follow
the trajectory. Specifically, to force the Puma manipulator to
follow the desired trajectory generated by the SRTK, a PD
control law is applied to the first three joints of the Puma 560
as follows [13]:
τ(XY Z) = Kp(XY Zd− XY Z)+Kd
τ(q) = JTτ(XY Z) + G(q)
where Kp, Kd ∈ R are constant control gains, τ(XY Z) ∈
R3denotes the torque control input in the Cartesian space,
τ(q) ∈ R6denotes the torque control input in the joint space,
JT(t) ∈ R6×3represents the transpose of the Jacobian for
the first three joints, G(q) ∈ R6represents the gravity effects,
XY Zd(t) ∈ R3denotes the desired Cartesian position of the
end-effector, and XY Z(t) ∈ R3denotes the actual Cartesian
position of the end-effector that is calculated on-line from the
forward kinematics of the robot as shown below:
³
X˙Y Zd− X˙Y Z
´
(12)
(13)
XY Z = f(q)
(14)
where f(q) ∈ R3represents the forward kinematics for the first
three joints of the Puma 560 calculated using the Denavit-
Hartenberg parameters according to Lee [14].
When the Puma manipulator is controlled in this mode,
potential singularities may arise from the Jacobian. When
the manipulator approaches these singularities, the generated
torques will be very high and undesirable and unpredictable
motion of the Puma may result. Safety measures implemented
in the Output Section of the Simulink block-diagram will
stop the motion of the manipulator, unless the unpredictable
torque values remain within the joint torque saturation lim-
its. In addition to singularities, extremely high torque values
may result due to excessively aggressive trajectories. That
is, to follow a desired Cartesian trajectory that requires fast
movements, excessive joint torques may result.
3.5Cartesian PD Control Mode
The Cartesian PD control mode is similar to the Cartesian
control mode since both modes allow the user to select the
final position of the end-effector in the Cartesian space. In
both modes, the SRTK generates a desired trajectory from
the present position to the user defined final Cartesian position
where the trajectory is generated in a similar manner as in (5)-
(10). One of the differences in the Cartesian PD control mode
is that the inverse kinematics of the Puma 560 are calculated
on-line to relate the Cartesian space desired trajectory to the
joint space. Specifically, once the Cartesian trajectory has
been calculated, the inverse kinematics of the Puma 560 are
calculated on-line to relate XY Zd(t) to the desired trajectory
in the joint space; however, since there are 8 possible solutions
to the inverse kinematics problem, the SRTK computes the
inverse kinematics for each of the possible configurations as
shown below
qconfig(i)= f−1(XY Zd)∀i = 1,2,...8
(15)
where qconfig(i) ∈ R6denotes the i − th inverse kinematics
solution of the 8 configurations. The SRTK then selects the
joint angles for the configuration that results in the smallest
change from the current position as follows:
qconfiguration3 {¯¯qpresent− qconfig(i)
= min
j=1
∀i
That is, the inverse kinematics for each of the 8 different con-
figurations of the manipulator is determined, and the config-
uration that results in the smallest change from the present
position (measured by the sum of the absolute value of the
difference in the j − th element of the vectors) is selected as
the current configuration. To calculate the inverse kinematics,
qPaul
=
¯¯
(16)
Ã
6 P
¯¯qpresent(j) − qconfig(i)(j)¯¯
!)
=1,2,...8.
Denavit-Hartenberg parameters based on the angle definitions
according to Paul given in [14] are used; however, the joint
angles required by the control are defined according to the an-
gle definitions by Armstrong given in [14]. Hence, the SRTK
Simulink block-diagram converts the angles from the conven-
tion used by Paul in [14] to the convention used by Armstrong
in [14].
Once the desired trajectory has been related to the joint
space, the following PD controller is used to force the manip-
ulator to follow the desired trajectory:
τ = Kp(qd− q) + Kd(˙ qd− ˙ q) + ¨ qd+ G(q)
where Kp, Kd ∈ R are constant control gains, τ(q) ∈ R6rep-
resents the joint control torque input, G(q) ∈ R6represents
the gravity effects, and qd(¯t), ˙ qd(¯t), ¨ qd(¯t) ∈ R6denote the de-
sired position, velocity, and acceleration trajectories (defined
in Armstrong’s notation [14]) that are generated by the SRTK.
When the manipulator is controlled in this mode, potential
singularities may arise due to singularities in the desired tra-
jectory that is obtained by the inverse kinematics of the Puma
560. However a marked advantage of this control mode over
the Cartesian control mode is that since the control is not ac-
tually calculated in the Cartesian space, high torques will not
be generated when the Puma is close to a singularity.
(17)
3.6Impedance Control Mode
The impedance control mode is very similar to the Cartesian
control mode, however, this mode provides for the additional
capability of force feedback provided an auxiliary force/torque
sensor is incorporated with the Puma 560. In the absence of
force feedback (e.g., if no forces are present or if there is no
auxiliary force/torque sensor present), a desired trajectory is
generated by the SRTK in a similar manner as given in (5)-(10)
that provides a smooth transition from the initial Cartesian
position of the end-effector to the user-defined final Cartesian
position. In the absence of force feedback, the same controller
given in the Cartesian control mode is utilized to move the
manipulator along the desired trajectory. The advantage of
the impedance mode is that if forces are exerted on the end-
effector, the desired trajectory of the manipulator is modified
to reduce the forces.
Since the desired trajectory is generated in the base coordi-
nate system of the manipulator, a rotation matrix is utilized
to translate the forces measured at the end-effector to the base
coordinate system. The rotation matrix is calculated online
from the following equation:

fz
nz
f∗=

fx
fy

=


nx
ny
ox
oy
oz
ax
ay
az




Cfx
Cfy
Cfz


(18)
where Cfx(t), Cfy(t), Cfz(t) ∈ R represent the forces acting
on the manipulator along the X, Y , and Z Cartesian coor-
dinate axis.Note that it is assumed that the force-torque
sensor is mounted so that it aligns with the robot X, Y , and
Z directions when the robot is in the “ready” position so that
the desired trajectory will change appropriately based on the
force feedback information. Once the forces have been related
to the base coordinate system of the manipulator, a filtering
technique [13] is employed to offset the original desired trajec-
tory as shown below
XY Z
0
d= XY Zd− L−1
½
f∗
As2+ Bs + K
¾
(19)
where XY Z
XY Zd(t) ∈ R denotes the desired trajectory in the absence
of external forces, f∗(t) is defined in equation (18), L−1{·}
denotes the inverse Laplace transform operator, s denotes the
Laplacian variable, and A, B, K ∈ R are positive constant
filter parameters. The velocity profile of the desired trajectory
is generated using the following equation:
0
d(t) ∈ R denotes the modified desired trajectory,
X˙Y Z
0
d= X˙Y Zd− L−1
½
sf∗
As2+ Bs + K
¾
(20)
4