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

Modying Matlab and Simulink

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 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. 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 the Future Work section. Note that although the torque profiles are noisy the positions were clearly not affected.

Conclusions

Future Work

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

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

• 20 Sep 2006
  1. 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.
  2. 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.
  3. 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.
  4. The latest version can now be compiled and used with either the Trident Robotics or Servo-To-Go interface boards.

Acknowledgements