6 Simulating Robot Systems

6 Simulating Robot Systems

How can a macro or composition of performance orders be verified for proper operation in LLAMA? Performance orders will eventually be generated by G2 with no user interaction. This chapter presents (1) a simplified finite state machine representation of LLAMA, (2) a "walk-through" of a simple macro for operational verification using a proposed consistency checker, (3) a suggested graphical method of representing LLAMA commands in G2, and (4) the results of two performance orders executing in parallel. Item (4) could be used to verify any future consistency checkers.

Representing LLAMA with Finite State Machines

Portions of LLAMA can be represented using Finite State Machines (FSM). FSMs can be used for simulation purposes. Achieving a thorough representation of concurrent LLAMA operation would require a technique such as Petri Nets, which is beyond the scope of this discussion (Murata 1989, 541-580). FSMs can be accessed when system states are needed in a LLAMA command simulation: Since many LLAMA commands activate and deactivate hardware and software modules in a binary fashion, a two state FSM is appropriate for simulating this behavior. Trajectory simulation can be performed using a variation of the algorithm presented in appendix A. Sensor simulation can be performed using similar techniques. The following states illustrate how the drive and steer motion status of the robot can be represented using FSMs:

	Dr0 = Drive motor stopped (initial)
	Dr1 = Drive motor turning
	St0 = Steer motor stopped (initial)
	St1 = Steer motor turning
	ES0 = System motion ready (initial)
	ES1 = System in emergency stop
	ME0 = Motion Enabled (initial)
	ME1 = Motion Disabled
A FSM diagram of robot drive and steer motions is given in figure 6.1. Note that if the system enters state ES1, this implies that a transition to states St0 and Dr0 occurs. If the system enters state ME1, all Dr and St state transitions are disabled. In a simple example using two performance orders, we can analyze the behavior of the system based on the above states. Assume the following performance orders have been transmitted:

	P.O. #1:	{ 10 30 00 3 txabs 20 st ststop }
	P.O. #2:	{ 10 30 01 3 txabs plst 0 = if sts endif }
Before analyzing the behavior of these performance orders, the components need to be explained:

	10 30 00 3 txabs-waits until 10:30:00 am arrives, then executes next commands.
	20 st-turns head clockwise at 20 degrees per second.
	ststop-waits until head stops turning.
	plst-returns 1 if the head motor has stopped, 0 if it is still turning.
	0 = if sts endif-a head motion stop is issued if plst returned a zero.

Fig. 6.1. Finite State Machine Description of Motion. Drive and steer motions on the Denning Mobile Robot are independent.

The question is: will both performance orders complete or will one or both be temporally bound forever? The presence of "ststop" indicates that the upper performance order could conceivably wait forever for the head motion to stop since it contains no means of stopping the head. The first performance order will start at 10:30:00 AM and the second at 10:30:01 AM. We note that initially there is no motion (Dr0 Ŕ St0) in the system and that all motion is enabled (ES0 Ŕ ME0). The following events take place:

	10:30:00	Steer motor slews head at 20 degrees per second
			State change: (St0 -> St1)
	10:30:00+	Performance Order #1 waits until head motion ceases
			(Loop until St0)
	10:30:01	Performance Order #2 checks if in state St1, if yes, St0 and end
			State Change: (St1 -> St0)
	10:30:01+	Performance Order #1 ends since state St0 reached
Both performance orders complete at about the same time-one second after 10:30 AM. If an emergency stop occurred during execution of the first performance order, the first order would become temporally bound forever. Since performance orders announce when they start and stop execution, G2 could monitor the status of the performance orders and issue an "exit" instruction to the first performance order if it has exceeded its implied execution time of one second. A LLAMA simulator in G2 can use FSMís to estimate the progress of performance orders and take corrective measures if it becomes necessary.

Creating Performance Orders: The Consistency Checker

Once a LLAMA performance order has been created, it can be analyzed using a proposed G2 module that analyzes the program structure for consistency. This module will verify the following qualities of the performance order: (1) commands must have a stack consistency, implying that parameters are not lost or added unnecessarily to the stack; (2) certain instructions that are not temporally bound must not occur in sequence and certain instructions must occur in groups-the consistency checker can verify , warn, or fix most of these cases; (3) ensure that the program will do what it is expected to do under all or most circumstances. The following example illustrates some of the issues in designing a consistency checker for LLAMA performance orders.

The following scenario is presented: G2 needs to generate a performance order with a performance that will move the robot in a 180 degree arc clockwise from the robotís current position and orientation with some unspecified radius. G2 performs a search which locates no performances with the required qualities. G2 proceeds to create a performance from various skills using a performance generator. Unfortunately, the hypothetical performance generator occasionally makes errors in its creation of performances. The following is the performance created from this generator. Note that the indentation and lines are for readability but the performance is a linear sequence of operators when sent to LLAMA as a performance order.

	0 0 0 spos			(set encoders to zero degrees)
	3 5 dra 200 100 STA		(start motors at arbitrary speeds and accelerations)
		xytsave			(get position)
		3 rem			(remove unneeded data)
		0 > 			(test for completion)
	s				(stop motors)
Letís analyze the performance. State variables and stack pointer need to be initialized:

	Steer = St0
	Drive = Dr0
	E-Stop = ES0
	Motion = ME0
	stack index i = 0;
The proposed algorithm to validate the performance is as follows:

Stage 1: Validate stack requirements:

	( push(i=1)  0 ; push(i=2)  0 ; push(i=3)  0 ; x  pop(i=2) ; y  pop(i=1) )
	( t  pop(i=0) ; spos ; push(i=1)  3 ; push(i=2)  5 ; ds  pop(i=1) )
	( da  pop(i=0) ; dra ; push(i=1)  200; push(i=2)  100 ; ss  pop(i=1) )
	( sa  pop(i=0) ; STA ; begin ; xytsave ; push(i=1)  th ; push(i=2)  y )
	( push(i=3)  x ; push(i=4)  3 ; push(i=5)  3 ; num  pop(i=4) ; rem )
	( i=i-3=1 (num) ; push(i=2)  0 ; c1  pop(i=1) ; c2  pop(i=0) ; >  )
	( push(i=1)  1/0 ; test  pop(i=0) ; until ; s )
We note that i equals zero after this sequence which means that no numbers were left on the stack. Also, the stack index never dropped below zero, indicating that an attempt to remove a non-existent number from the stack never occurred. Since "xytsave" may leave either one or four numbers on the stack the previous algorithm needs to include branches for all possible stack items that an operator may return. This type of extended testing could use a search tree that would work its way through the arguments and whenever an operator may produce more than one set of values, each branch in the tree could be tested. Note that in this tree the branches fall back into the roots implying a potentially very efficient algorithm for solving this problem.

Stage 2: Validate temporal binding and groupings

( spos - temporally bound )
( dra - a temporally unbound command, valid, no other dra before )
( STA - a temporally unbound command, valid, no other STA before )
( begin - valid, first begin in sequence )
( xytsave - note: not grouped, no xyton at start of sequence )
At this point the consistency checker will automatically insert an "xyton" at the beginning of the performance.

( rem, > , ignore )
( until, valid, closes begin loop grouping)
( s, valid, resets motion state machine to original state )
( a flag alerts the system that an xytoff is needed at the end to match inserted xyton )
The system is valid in the second stage of the tests.

The final, and most difficult part of the algorithm is deciding whether the begin-until loop will be exited under all conditions. As noted in the previous section, if the motors are engaged and an emergency stop is issued, loops must be verified so they wonít hang (although this condition cannot always be guaranteed).

Stage 3:

The consistency checker determines that the performance has no means of detecting an emergency-stop condition, implying the performance might enter a suspended state. To remedy this situation, the consistency checker inserts a function that puts a "1" on the stack if the system is stalled due to an emergency stop or it places a "0" otherwise. It also adds an "or" function so that either the check for 180 degree rotation is tested or stalled motion is tested. The final, corrected algorithm is as follows:

	xyton				(initialize encoder read)
	0 0 0 spos			(set encoders to zero degrees)
	3 5 dra 200 100 STA		(engage motors)
		xytsave			(get position)
		3 rem			(remove unneeded data)
		0 > 			(test for completion)
		pldr			(test for stall)
		or			(combine results)
	s				(stop motors)
	xytoff				(disable encoder read)
Unfortunately, this algorithm will not work because the "greater-than" test is incorrect, it should be "less-than." The performance generator, which was not well developed, used the wrong symbol in the loop. The function "xytsave" puts an angle between -180 and 180 degrees on the stack. If the steer motion is positive, the angle returned will be positive until the robot has turned 180 degrees, at which point the returned angle will suddenly switch to -180 degrees and increase until it reaches zero. If the steer motion is positive, the head will have turned 180 degrees the moment the returned angle switches from +180 to -180 degrees. Thus we need only wait until the returned angle is -180 degrees or negative to complete the 180 degree turn. The "greater-than-zero" test should be "less- than-zero" if we want a negative angle. As listed above, the loop will be exited immediately since the returned angle will be positive during the first 180 degrees of motion.

This problem could be detected by sending the performance order to a simulator. If the simulator produces an output which is not what is expected, the consistency checker could request that a user fix the problem in the performance generator. Problems could be isolated by observing the simulatorís real-time trajectory output. Ideally, the expert system could also be used to store a repertoire of skills with known behaviors. When these skills are built into a performance, the final product could be verified using a consistency checker.

Visual Programming for LLAMA

LLAMA performance orders are serial entities: A performance order is received one character at a time. A graphical composition system could be used to define how such a string is created. G2 has the capability to express LLAMA commands in a graphical format. If these commands could be expressed as connected objects, each object could contain attributes that define how it operates in the LLAMA framework. Such an approach could become part of a simulator and consistency checker. This visual programming interface, along with a simulator and consistency checker could aid in the understanding of how LLAMA operates and would produce elaborate code without the burden of using a text editor.

We present a method in which LLAMA can be encoded into G2 objects in this section. If these objects include certain attributes, a visual programming system can be developed, albeit one in which a user must know FORTH and LLAMA fundamentals. Appendix B provides the attribute descriptions for all commands in the LLAMA system.


A performance order is a set of instructions sent to the robot. In G2 a performance order object could contain attributes that determine when the performance order is to execute, and when executing, its current execution status. This is very important since it identifies that LLAMA and G2 are operating in a symbiotic manner. For example, we can represent whether the robot is moving, by incorporating a logical variable in G2 that represents the two state FSM described earlier in this chapter. The performance order execution status attribute in G2 could be used to verify completion of the order or it might confirm that the order has stalled or locked-up. The execution attribute receives notice from LLAMA when the performance order has started so that G2 can know when to send another performance order.

The performances in a performance order, since they are a linear list of commands, can be reduced to macros. These macros could be defined using a definition order: a semi-permanent list in the on-board system of commands represented by a single token. The implication is that once a definition order creates a macro, which might take a minute to download, then it is available for immediate use by a performance order. If it is known that a performance order will be executed at some time and the system is idle at that instant, then the performance can be sent as a definition order. Figure 6.2 illustrates the advantage of a definition order when transmission times are restricted due to the limited baud rate of the radio link.



Fig. 6.2. Performance and Definition Order Execution Sequence. If G2 makes the false assumption that transmission time of a performance order is invariant, then if transmission time is increased, the performance order will very likely execute late. This is shown in case 1. If a performance order is sent as a definition order, there is more flexibility in the transmission time, since the bulk of the performance was sent earlier, as shown in case 2.

Once a performance order string has been received, LLAMA creates a spawned process that will execute the instructions. LLAMA returns a handle number which uniquely identifies the performance order and G2 places that number and a time stamp into the performance order objectís execution status attribute. After receiving the performance order, LLAMA attempts to compile the performance order string. If an error occurs during compilation, G2 is notified and a no-handle message is returned, indicating that the performance is not executing and the compilation failed. If all was successful the performance order will start executing. Once execution is complete, a termination message and the performance orderís handle number is returned to G2. It is essential that G2 keep track of which performance orders are active, otherwise G2 may conclude incorrectly that a performance order has completed when it may not have.

The LLAMA Primitive Object

We have seen that certain elements of LLAMA can be simulated in G2 using a FSM. Sensors and actuators could be simulated using more elaborate techniques. The simulators and FSM could be interfaced into the LLAMA primitive commands using an object representation. A command can be described in G2 through a hierarchy of G2 objects. Figure 6.3 shows a typical primitive, "movacc," with its operational qualities exposed. The upper most object, the "execution object," represents an instance of "movacc," with connection stubs to connect it to other execution objects (primitives and macros). The execution object contains the "movacc" parameters, in the form of "number" objects (push object), and the "movacc" object itself. The "number" objects precede the "movacc" object because every primitive object representation in G2 will mirror the linear flow of commands to LLAMA. The execution objects are designed such that, given the appropriate G2 rule and procedure controls, a connected sequence of these G2 macro and primitive representations can be analyzed for consistency or incorporated into a simulator.

Fig. 6.3. An Execution Object Representation of "movacc." A hierarchy of objects is used to form a complete specification for the "movacc" command.

In this connected object scheme, a "daemon" could be used to move along such a connected system and would "activate" and operate upon every appropriate object. If the selected object is "number," then the value of the object is placed ("push") onto the stack (a subclass of the list object). When the "movacc" object is activated, control descends to an object lower in the execution objectís hierarchy that contains "un-number," or "pop" objects. These objects are connected to the "movacc" interface control object. When each is activated, it pops a number from the stack and inserts it into the given attribute of the interface control object. Then once the "movacc" interface control object is activated, it calls a procedure that analyses the simulated LLAMA systemís state and modifies it appropriately. Control continues to the next execution object.

It may seem that the push and pop objects could be represented as one, simplifying the entire object representation; however, under certain circumstances instructions, such as "movacc," might obtain parameters from the stack placed there by other primitives or macros. The pop numbers are not alterable since they control the particular primitiveís interface to the simulator whereas the push numbers could be removed and the stack values implicitly obtained from previous execution objects. The following is an example of such a case:

	10 1 do I 5 5 movacc drstop loop
Although "movacc" requires three parameters, the first parameter is obtained implicitly from the stack as placed there by the "I" operator, which obtains the do loopís current index and places it onto the stack. In such an instance, the "movacc" function would no longer have three push numbers, but only two, in its execution object.

Visual Programming

In standard structured programming, a programmer will enter command lines in a text format, typically employing indentation to provide improved code readability. Indentation or some form of delimiting is generally used to set a control structure apart from code. In the proposed visual system, a different control structure can be developed if macro and primitive execution objects are placed on individual workspaces and connected together to form skills and performances. By using G2ís capability to show the workspace hierarchy, an object representation of a program can be produced.

All LLAMA commands can be composed under a visual programming system. Most of the commands are linear and lend themselves to an object representation. Conditionals and variable definitions require additional support, but still can be made to fit the visual framework. Figure 6.3 provides a basis for this design. Appendix B lists the attributes of each LLAMA command to help translate them into a visual style. A detailed explanation of visual programming is presented in Chang (Chang 1990).

Concurrent Performance Orders: Indirect Interaction, an Example

Executing concurrent performance orders is an advanced topic briefly discussed in chapter 3. It was suggested that performance orders may function together without passing any variables or control between each other. We present a working example of a complex control "program" based on two performance orders that shows the indirect interaction behavior.

We wanted to show that it was possible to create complex system behavior by using only LLAMA commands. The chosen example is a "beacon follower." A person holds an infra-red beacon in front of the robot. The robot attempts to approach the beacon but is allowed no closer than two feet. The robot must also rotate to follow any left-right motion of the moving beacon. A definition order approach will be used.

Performance order number 1 turns the head of the robot so that it always faces the beacon. Performance order number 2 moves the robot forward and uses the ultrasonic range finder to ensure that the robot stops if it gets nearer than two feet from the beacon and the person holding it. (Note that the beacon could be mounted on the other robot.) A pseudo-code version of the performance orders followed by the LLAMA code is given below (refer to the LLAMA Userís Manual for a detailed description of the commands):

P.O. #1: Turn Head to Track Beacon

1. Loop until a beacon is detected. 2. Get azimuth angle to beacon (+/-10 degrees). 3. If the distance is < 0.2 degrees, stop steer motor. 4. If distance is >= 0.2 degrees, slew motor in proper direction to center beacon.

Implementation of P.O. #1:

	: bfollow
	beon begin				(turn on beacon blackboard and begin loop)
		besave				(get beacon data)
		if				(execute if good data)
			1 >  if			(execute if found beacon)
				drop rot drop swap drop		(get angle)
				bturn		(execute motion)
				4 rem		(drop beacon data, since no beacon found)
			sts			(stop steer motor due to bad beacon data)
	poll @ until				(check "quitting" variable, continue until done)
	sts					(turn off steer motor, just in case)
	beoff ;					(turn off beacon blackboard)
	: bturn
	dup					(duplicate angle)
	abs 2 < 					(0.2 degrees, check if centered)
	if					(centered)
		plst 0 =			(check if turning)
		if				(turning)
			sts			(stop steer motor)
		drop				(drop extra angle value)
		3 / st				(use extra angle to turn at speed prop. to angle)
	endif ;
P.O. #2: Move Forward Unless Less Than 2 Feet

1. Get current head angle 2. Get three ultrasonics pointing in forward direction 3. Get minimum distance of the three sensors 4. If distance is <= 2 feet, stop drive motor 5. If distance is > 2 feet, move robot forward slowly

Implementation of P.O. #2:

	: mover
	ulon xyton				(turn on ultrasonics and position blackboard)
	begin					(start loop)
		1000 minim !			(initialize minimum distance variable)
		make3				(get the three forward ultrasonics numbers)
		getmin				(get the minimum distance of the three ultrasonics)
		20 >  if				(distance is greater than 2 feet)
			pldr if			(check if not moving)
				2 dr		(drive forward if not moving)
			pldr 0 =		(check if moving)
				drs		(stop drive motor if not moving)
		poll @
	until					(check if time to quit)
	drs					(stop drive motor)
	uloff xytoff ;				(turn off ultrasonics and position blackboard)
	: make3 GANGP				(get head angle)
	150 /					(get a number between 0-23)
	dup					(again)
	1 -					(is it 0 ?)
	0<  if					(yes)
		drop 23				(left adjacent ultrasonic number is 23)
	left !					(save left)
dup (get altered head angle) 1 + (is it 23 ?) dup 24 = if (yes) drop 0 (right adjacent ultrasonic number is 0) endif right ! (save right) center ! ; (save center)
	: getmin
	ulsave					(save 24 ultrasonic readings)
	drop					(assume all readings valid so drop count)
	24 rep 24 rep				(duplicate readings twice)
	left @ getval				(is left reading smallest?)
	right @ getval				(is right reading smallest?)
	center @ getval				(is center reading smallest?)
	minim @ ;				(get calculated minimum distance reading)
	: getval
	-1 *					(get ultrasonic number 0-23, make into -23 to 0)
	24 +					(add 24 to reverse sense)
	deep					(extract distance reading at depth = ultra number)
	dup					(save for later)
	minim @ < 				(check if smallest distance)
	if					(yes)
		minim !				(save as smallest distance)
		23 rem				(drop all remaining readings)
		24 rem				(not smaller, drop all readings)
	endif ;
To execute these performance orders, the following sequence is performed:

1. Initialize any variables

	1000 variable minim
	0 variable left
	0 variable center
	0 variable right
	0 poll !				(enable loops)
2. Send all definition orders (in this order)

	: bturn ... ;
	: bfollow ... ;
	: getval ... ;
	: getmin ... ;
	: make3 ... ;
	: mover ... ;
3. Initialize robot position

	home ststop 0 0 0 spos
4. Execute performance orders

	{ bfollow }
	{ mover }
5. Terminate when done

	1 poll !
When these performance orders are executed, the robot will wait until it finds a beacon. It will track to it and approach it. Initially, if no beacons are found but are present, a "10 st" steer command could be placed after step 4 to search for beacons; the performance order number 1 will override the steer command once a beacon is found.

Notice that the two performance orders do not share any variables (other than "poll") and rely only on the interaction of the robots sensors and effectors. Each performance order is independent of the other, but a coordinated behavior emerges from their execution. This shows the "indirect" interaction of performance orders.

Return to Thesis Index