Designing a Real-Time Robot Controller using ...

CSci 5980 - Real-Time Systems
Final Project Report
Designing a Real-Time PUMA Controller Using Simulink and QNX
IntroductionModifying Matlab and SimulinkModifying QNXBuilding a Simple ModelGenerating CodeCompiling and RunningThe PUMA w/STG Board LibraryFinal ModelResultsConclusionsFuture WorkLatest News and Release NotesAcknowledgements
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.
File Matlab Version Contains
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.
File Matlab Version Contains
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
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.
After running the model and double-clicking on the scope, you should see the plot to the right.

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:

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
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
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.
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.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 theFuture 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.

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.

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 theFuture Work section.

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
SeeLatest News and Releases for what's been fixed and/or modified
Minor Additions Removing the Digital Clock from the Trajectory Generator. This can be accomplished within code and would make a more readable and easily assembled model. Adding another output to the Trajectory Generator. This would be a boolean signal that would signal that the desired position has been reached. This output could be fed to a Stop Simulation block. This would enable models to finish when necessary and not after a command-line specified time. Joint Angle Limitations -- Currently, the Trajectory Generator does not check for violations of the joint limits. In the current test environment, this is not a problem. But to make a more robust system, this is obviously a desired feature.
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. 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. 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. 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. 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
20 Sep 2006 Removed the digital clock from the trajectory generator. The block now keeps track of the time increments internally. In it's place I'ved added an enable input. Only on a non-zero input to enable, the block generates a trajectory. This can be hooked to a status read or simply a constant. I've also added an output which is zero until the trajectory has completed. This can have several uses: it can daisy-chain several trajectory blocks together or be the input to a Stop Model block. Added a linear trajectory option to the trajectory generator. The input is now an Mx8 matrix with the following conventions: [Q1 Q2 Q3 Q4 Q5 Q6 T V] where:
Q1-Q6: ending joint angles for the next point T: type; 0=joint space;1=linear;2=wait V: average angular velocity, unless T=2, then this specifies a pause time between the preceeding and succeeding points. To specify more than one point, the matrix will look like [Q1-Q6 T V;Q1-Q6 T V;...]'. It is VERY IMPORTANT to have the transpose of the matrix. This is a feature that should be obsoleted in the next version.
Added a calibration routine to the beginning of the trajectory generator. Unfortunately, this is only specific to the PUMA560 and the pot tables are specific to one in my lab. I'll be adding an option to generate your own pot table soon. The latest version can now be compiled and used with either the Trident Robotics or Servo-To-Go interface boards.
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 theOpenQNX forums. Much graditude is owed to the experts on this on-line forum who were able to answer my many and varied newbie questions.