CSci 5980 - Real-Time Systems

Final Project Report


Designing a Real-Time PUMA Controller Using Simulink and QNX

  1. Introduction
  2. Modifying Matlab and Simulink
  3. Modifying QNX
  4. Building a Simple Model
  5. Generating Code
  6. Compiling and Running
  7. The PUMA w/STG Board Library
  8. Final Model
  9. Results
  10. Conclusions
  11. Future Work
  12. Latest News and Release Notes
  13. Acknowledgements

Introduction

Prior efforts to create real-time control software for the PUMA robots have been limited to DOS or using third-party software, such as Control Shell, in a non-real-time environment. These solutions, while viable, have been constrained and sometimes unmanageable. This project is an effort to show the feasibility of creating real-time control systems using Mathworks' Simulink, a widely available dynamic system simulator, and a non-commercial version of QNX, one of the leading real-time operating systems in both academics and industry.

This project will demonstrate the flexibility of using Simulink, along with Mathworks Real-Time Workshop, to create models of control systems that can be easily created and debugged. Additionally, the target system can interface with the PUMA using either a Trident Robotics or Servo-To-Go card.

The benchmark for success of this project will be to create a model in Simulink, generate code, and compile cleanly on a QNX target machine. The model will specify a displacement for all six joints of the PUMA robot and command the robot to follow a smooth trajectory. This will be done for systems employing both a Servo-To-Go and Trident Robotics interface board.

Modying Matlab and Simulink

In order to use Matlab and Simulink to create models which can be used to generate code for a QNX target system, we need to install additional files to the development system, which is assumed to be Windows-based. We need to add several things to this system to make it generate compilable code for our target system. The table below has some compressed tar files which are specific to the version of Matlab you will be using. The files include the Simulink libraries, target makefiles and other configuration files. All of these files should be installed at the top level of Matlab or MATLAB_ROOT.
If you are using the version for Matlab6, you will need to add MATLAB_ROOT/toolbox/pumastg to the search path. This can be accomplished using Matlab’s addpath command or from Path under the File menu. If the Simulink Library Browser is now opened, we should be able to see PUMA w/STG Board Library.

FileMatlab VersionContains
puma_sim_v01.tar.gz Matlab 6 Libraries and makefiles for Servo-To-Go board
puma_sim_v10b.tar.gz Matlab 7 Libraries and makefiles for Servo-To-Go and Trident Robotics board
puma_sim_v20.tar.gz Matlab 7 Libraries and makefiles for Servo-To-Go and Trident Robotics board
Additional changes to trajectory generator as well as support for Trident Robotics Board.

Modifying QNX

There is very little configuration that needs to be done after installation to compile code. QNX comes with all necessary compilers, libraries, and associated files to create a real-time program. However, Matlab requires a number of header and source files for its model-based simulations to be compiled. The following directories must be copied in full from the development system to the target system:

MATLAB_ROOT/rtw/c/libsrc
MATLAB_ROOT/rtw/c/src
MATLAB_ROOT/rtw/c/tools
MATLAB_ROOT/simulink/include
MATLAB_ROOT/extern/include

This can either be done manually or by downloading a compressed tar file the table below. Make sure to get the right one for your version of Matlab. It is recommended that MATLAB_ROOT on the target QNX system be selected to be /matlab as this is the default in the makefiles. In addition to these directories, the compressed tar file from the previous section must also be added to the QNX target, ideally in the same location.

FileMatlab VersionContains
puma_ml6.tar.gz Matlab 6 Source and header files from Matlab 6
puma_ml7.tar.gz Matlab 7 Source and header files from Matlab 7

Building a Model

Simulink Model with Sine Wave and Scope Once these modifications have been made, we can now build a model. Simulink provides a number of generic building blocks that can be used to build very simple models. Let’s start with something simple by connecting a Sine Wave to a Scope. The Sine Wave block can be found in the Sources Library and the Scope can be found in the Sinks Library. The model should look like the model to the right.


Simulink Model with Sine Wave and Scope Output After running the model and double-clicking on the scope, you should see the plot to the right.


Simulink Model with Sine Wave, Scope, and External Output
If we eventually want to be able to dump the output to a file, we can add an output block to our model as seen in the figure at left.

Generating Code

Once we have built a model, we need to make a few more modifications before we can generate code. From the Tools menu, select Real-Time Workshop->Options. If you hit the Browse... button, the following window will appear:

Operating System Selection Menu

To generate code, make sure to select the QNX Neutrino Real-Time Target and click OK. Go to the Solver tab, make sure to select Fixed-Step under Solver Options and make sure that the Fixed Step Size is the same as the smallest task time of any of the blocks in your model. Now, go back to the Real-Time Workshop tab and click on the Build button. The code will be written to a directory with the name model_name_qnx_rtw. Copy the entire directory to your QNX target. Note that if your development system is a Windows environment, you may need to use a utility such as dos2unix to strip your files of extra carriage returns.

Compiling and Running

Provided that everything is in its proper place, we should be able to invoke the make command. Note that the automatically generated makefile file is named model_name.mk. The compiled binary will be placed in parent directory and is invoked as make model_name.mk. The flag -tf T will run the model to final time T, or infinity if T is inf. If the model runs properly, any output from output blocks as well as the time steps will be placed in a Matlab file called model_name.mat.

The PUMA w/STG Board Library

PUMA w/STG Board Library There are several blocks which can be used to build a model. Here is a brief description:
  • Initialization -- This block must be present in all models. This sets up the board to be used a an interface for the robot. Board address must be specified.
  • Power On -- This block sends a command to the controller to turn on power to the robot at startup. Turns power off on shutdown.
  • Zero Encoders -- Resets all encoders to zero. Runs at startup and shutdown.
  • Log -- Writes all incoming input to a specified file. Sample time and file name can be specified by double-clicking on the block.
  • Trajectory Generation -- Given input joint displacements and a clock signal, the block will output a smooth 3rd order trajectory. Sample time and average speed can be specified.
  • PD Controller -- Given the desired position and the current position, it will output a set of torque values. Sample time and the P and D gains can be specified.
  • Float Read, Float Write, Short Write, and Long Read -- Reads or writes values of a certain type to/from offset option. Sample time and channel are other options. See the figure below for example.

Input Parameters to Model Blocks

Options for Input/Output Blocks Model blocks each have specific parameters that can be tailored for your model. For example, the Float Read block expects a write option, a channel, and a sample time while the Log also requires a file name in addition to sample time.

All blocks except Initialization, Power On, and Zero Encoders need a sample time input(the sample time of these blocks is hard-coded to 1 second. We can do this because they run once at start-up and once at shutdown and nowhere else). The Initialization block only requires the address of the board. The other two require no additional parameters.

Options for Input/Output Blocks The rest of the blocks have additional parameters beyond the sample time. All of the I/O Blocks require an option setting and channel selection. The option setting helps specify what register to access and the channel denotes the joint. The Trajectory Generator needs an value for the average angular velocity of a joint. This is an important setting as a too high setting can damage the PUMA. The PD Controller needs a vector for both the proportional and derivative gains.

Final Model


With the blocks shown from the library, it should be very easy to create a model which has some degree of control over the PUMA robot. The model below will smoothly move the end manipulator to a specified position.

In this model, the three blocks on the left hand side are run before anything else. Board Initialization and Power On would generally be required of any model using a board interface. For this specific model, the Zero Encoders block is also included because a calibration block has yet to be written. Final Simulink Model The model gets the desired end position, which is a vector of radians, from the Constant block. This is fed into the Trajectory Generation block, which generates a third-order polynomial trajectory and outputs a point from that curve at every time step. Since the Trajectory Generation block and PD Controller run at different frequencies, a Transition block is needed. Transition blocks are needed everywhere to connect two blocks with different sample times. The PD Controller sends the torques to a Float Write block, which sends them to the PUMA controller via the interface board, and to a Log File block, which writes the output to a user-specified text file.

The PD Controller also gets input from a Float Read block, which reads the encoder positions of the PUMA. Finally, the current position of the PUMA and the current trajectory position are sent to another Log File block, which can then be used for analysis.

Note that the model has several different colors for different blocks and connecting lines. These different colors denote different sample times. In this model, the colors correspond to these times:
  • Light Blue - 1 second
  • Dark Blue - 0.02 seconds
  • Green - 0.01 seconds
  • Red - 0.001 seconds
Note that all sample times must be a multiple of the smallest sample time. If this is not the case, Matlab will generate an error message when you try to compile. Provided everything is connected, we can now compile.

Results

This model was built, the resulting code and other files transferred to a QNX system, and then cleanly compiled. The QNX system was a 433MHz PentiumIII processor running QNX6.3NC. This system also had a Servo-To-Go rev. XX board which was connected to a Unimate II controller which was directly connected to a PUMA 560.

The resulting executable assumes the PUMA is in the up position. This corresponds to a triple of [0 pi/2 -pi/2] for the joint angles for the first three joints. Obviously, having to set the robot to a set position before starting is a shortcoming, but the addition of a few key components, described later in the Future Work section, would easily overcome this requirement.

With the arm in the up position, the position vector input to the model was [1.75 -pi/2 pi/2] and the model ran for ten seconds. The first joint was set to 1.75 to ensure that joint 1 was moving just slightly beyond the pi/2 position. In addition, the average velocity for the Trajectory Generation block was set to 0.25 radians/sec. Values for the PD Controller were set according to the standard for a PUMA560 arm with a 1kHz controller frequency. The following graphs are the data from the Log blocks of the previous model.
Position vs. Time Graph
Both the desired and actual joint positions for the first three joints are shown in the graph to the left. The generated trajectory is shown in blue while the actual positions are shown in green. The controller follows the generated trajectory so well that it is barely possible to notice the two colors. The below graph shows a close-up of the graph for joint 1 as the robot nears its final position. One can see that the difference between desired and actual is less than 0.01 radians.

Close-up of Graph

Note first that the joints all reach their final positions at the same time. Also, they all finish at 7 seconds, which is is expected from:

time_final = max_angle / avg_speed = 1.75 rads / 0.25 rads/sec = 7 secs

The controller continues to run until the default time of 10 seconds is reached. A simple modification to quit when reaching the desired position is discussed in the Future Work section.
Torque vs. Time Graph


Note that although the torque profiles are noisy the positions were clearly not affected.

Conclusions

I have developed and demonstrated that it is possible and easy to use Mathworks Simulink to create model based control software for robotic systems. Additionaly, these models can be used with Mathworks Real-Time Workshop to produce code the, with some slight modifications, can be compiled and run on a real-time platform. In this case, the chosed platform was QNX although this could easily be extended to anything from RTLinux or VxWorks.

Using the developed models, I was able to create a position controller for a PUMA560 robot and ran the resulting executable on a QNX platform. From the shown graphs it was demonstrated that robot performed as expected.

Future Work


See Latest News and Releases for what's been fixed and/or modified

  1. Minor Additions
  2. Linear Trajectory Generation -- This would extend the current capabilities of the system. Some projects require linear motion as opposed to smooth joint trajectories. As the kinematics have already been solved this is a (nearly) trivial addition.
  3. Calibration -- Code exists for a calibration routine. It is (wishfully) simply a matter of porting to code to a Matlab S-function. However, it would be mostly useless since at this time it could not be attached to another model.
  4. Teleoperation -- Previous work done with ControlShell and Linux incorporated a CiS Dimension6 trackball into the control loop for manual trajectory generation. Currently, the Dimension6 is not only obsolete but broken, although the concept is straightforward and, again, simply a matter of building the appropriate S-Function.
  5. State Flow for more complex models -- In order to make this a truly useful project, there needs to be a way to link models that accomplish different tasks, e.g. calibration and smooth motion. This can be created using Mathwork's extensio to Simulink, Stateflow. Stateflow allows for the creation of event-based models rather than time-based models. Stateflow models should allow for calibration routines, motion models, and user input to be seamlessly integrated.
  6. Extension to TRC006 -- In addition to Servo-To-Go, Trident Research and Robotics makes another ISA bus interface for the PUMA. Current attempts to modify the existing code to work with the TRC006 have been unsuccesful. But for a complete solution to using Simulink with QNX for PUMA control, this extension is necessary.

Latest News and Releases

Acknowledgements

  • Dr. Richard Voyles. Provided the initial idea and motivation for this project. I only hope I repaid the help by creating something useful for others.
  • Bruce Thomason of CyberMetrix. CyberMetrix uses Simulink to develop code that is compiled and run on QNX. Mr. Thomason was able to answer many of my initial questions regarding creating real-time code from Matlab.
  • Steve Goetz. Steve is duplicating this work with the exception of the RTOS; he is developing for RTLinux. Steve helped me overcome the learning curve of Simlulink and get going quicker than I should have.
  • Members of the OpenQNX forums. Much graditude is owed to the experts on this on-line forum who were able to answer my many and varied newbie questions.