Open Modular Robot Control Architecture for Assembly Using the Task Frame Formalism

2.1. Motion Programming Based on Skill Primitives Using the Task Frame Formalism

The skill primitive programming interface introduced by Hasegawa et al. (Hasegawa, T. (1981)) allows the programmer to specify the desired end-effector motion by using atomic motion task primitives. These primitives are composed of a desired motion command to configure the control laws and a stop condition in order to progress to the next motion task. For application to assembly it is useful to specify the motion tasks with reference to a user-definable Cartesian coordinate system, namely the task frame (TF). For each dimension (i.e. degree of freedom) this frame provides, a different motion algorithm may be specified. This leads to an easy-to-use and intuitive description of hybrid control tasks and allows skill primitive nets as an assembly description to be generated automatically from geometrical data (Thomas, U. & Maaß, J., 2005). However, the benefits provided to the robot programmer cause high demands regarding scheduling and performance to the control architecture on the other hand. Here, the most important extension compared to arbitrary hybrid control architectures is that a skill primitive control must be capable to displace the currently operating combination of motion algorithms with a new one in any control cycle each time the stop condition of the current skill primitive yields true. Taking this into account, each motion algorithm is required to accept initial values for velocity and acceleration unequal to zero. This leads to the demand to cope with real time motion planning algorithms to be calculated online and, thus, high communication bandwidths and computational efforts.

Further demands rise due to the fact, that most assembly tasks require sophisticated task-optimized control laws to be applied. Consequently, a concept is needed to facilitate the implementation of novel motion algorithms without any expert knowledge regarding communication and scheduling issues to be required on the control engineers' or robot programmers' side. Moreover, the robot control should provide a robot-independent control core and support flawless migration of assembly-task-specific control laws from one robot kinematic to another.

2.2. Error-tolerant Assembly Example

The advantages of the task frame formalism programming concept are shown in the following. An exemplary assembly task, reduced to 2D for easier understanding, is to place a small cuboid into a matching hole as shown in Fig. 2 (a). The y-axis is velocity controlled to establish contact with the surface, measured by a hand mounted force-torque sensor. Then a force is applied in y-direction of the TF and the measured torque around the z-axis is divided by this force to acquire the distance in x-direction from the task frame to the contact point as depicted in (b). This procedure is repeated in negative x-direction to acquire the y-distance (c). With this information the task frame can be set into the contact point (d) and surface-surface contact is established by rotating around z-axis until a torque occurs around it. Now the orientation of the big cuboid and the position of the small relative to the gripper are known.

OPEN IN VIEWER

Fig. 2.

Simplified task frame formalism assembly example: Place cuboid into hole with uncertain position

Then the small cuboid is moved with control of force on the y-axis and of velocity on the x-axis until a distance of d in z-direction is covered (e). Via cos(α) and d the distance of the task frame in negative y-direction of part (f) is calculated and with that the position of the hole is known and the parts finally fit, as (g) shows. Though there have been a lot of geometrical tolerances before execution, this sequence of skill primitives turns out to be a reliable assembly strategy.

3.1 Control Engineering Aspects

The resultant top level control architecture for the execution of skill primitives is depicted in Fig. 3. The key components are the motion modules, located in the middle of this figure. They either use sensor information to calculate a trajectory like a force/torque control or a distance control would do or are able to perform their task without any environmental feedback. The correspondent algorithms operate in the task frame coordinate system and treat the d.o.f. of the task frame as if they were decoupled from each other. While this allows automatic generation of assembly sequences it is the origin of control stability issues, explained in detail in (Thomas, U. & Maaß, J., 2005). However, these issues can be overcome and practical implementations show satisfactory behavior (Kröger, T., 2004). The robot is controlled by a robot-specific subordinated drive controller with feedforward capability of velocities and accelerations. This allows the designer to apply modern model-based control algorithms like the well-known computed torque approach, as it is required by parallel kinematic robots. The subordinated drive controller needs to be designed carefully in order to provide stable conditions all over the robot's workspace with well-defined dynamic constraints, i.e. maximum jerk, acceleration and velocity. Consequently the force/torque control is put into reality by using impedance control as proposed and experimentally verified by Siciliano (Siciliano, B., 2005). In the following an example control cycle is described.

OPEN IN VIEWER

Fig. 3.

Functional overview of the control architecture for implementing skill primitives

Each time a new skill primitive is received, a new set of desired values is communicated to the motion modules. The current end-effector position, velocity and acceleration is transformed into the task frame of the current skill primitive in order to allow these modules to calculate jerk limited trajectories.

Now the control is prepared to execute the skill primitive. In every control cycle, the output values of the motion modules are composed to a complete output vector, containing the desired pose, velocities and accelerations with reference to the task frame. This composition is dependent on whether the motion algorithms have been able to calculate an appropriate result. The force controller, for example, is unable to calculate a desired trajectory while the end-effector is not in contact. An inappropriate result is replaced by the result of a programmer-specified second level algorithm (e.g. velocity module). In a next step, the current trajectory information with reference to the task frame is fed back to the motion modules in order to be incorporated during the next control cycle. This is necessary to keep temporarily displaced algorithms up to date, e.g. the force controller. Finally, the output vector is transformed into the robot's base frame to generate the desired values for the subordinated drive controller. Now the control is ready for either the next control cycle or a new skill primitive.

The robot-independent trajectory generator being the central element of this control architecture consists of two types of reusable, task-specific modules as depicted in Fig. 3. The control core together with its coordination unit interacts with the skill primitive programming interface and dynamically establishes communication paths between the core and the attached motion and sensor modules according to the motion task specified by the current skill primitive. This is possible by using the middleware MIRPA-X designed especially to meet the demands of high performance robot and motion control applications. This middleware and its features are explained in detail in section 4.

At startup the control core broadcasts a profile request command to all motion modules and they deliver their names and specific capabilities back into a prepared synchronized shared memory area. The most important properties of the motion modules are if they are able to operate in a hybrid control context and if they are suitable to be used in the task frame formalism. Using this knowledge, the control core is able to decide whether the requested module combination specified by the current skill primitive is executable or not during runtime and which transformations have to be performed during execution.

The important data elements of the interface to the motion modules are shown in Fig. 4. It passes the actual hand frame position, velocity and acceleration to the motion modules. Additionally the desired values for the control algorithm and an array of flags, which d.o.f. of the task frame should be controlled are passed. These data are followed by a list of pointers to shared memory areas, where the module can read its required sensor data that it has previously announced to be required by its profile.

OPEN IN VIEWER

Fig. 4.

Communication via shared memory interfaces

The motion module's algorithm returns an array of flags representing the d.o.f. for which a result has been calculated successfully and the calculated output position, velocity and acceleration. The required motion modules for the execution of a specific skill primitive are notified by a per-module command message. At the end of each trajectory generator execution cycle the control core determines using the success flags which modules' output values are selected to be transformed into the robot's base frame. If none of the modules has been able to calculate an appropriate result for a d.o.f., a built-in braking trajectory generator is used instead and the core goes into an error state.

The interface to the sensor modules is simpler. A sensor module puts one or more data structures as shown in Fig. 4 into a shared memory area. This structure contains a flag, indicating if the sensor module is requested to provide a sensor value and the returned sensor value itself. They are activated by a broadcast message in the beginning of a trajectory generator cycle. Woken up on the message reception they could calculate signal processing algorithms if necessary or just copy sensor information from a hardware interface into the above mentioned shared memory area. Before becoming blocked again they decrease a semaphore previously filled in by the control core with the number of requested sensor values needed by the motion modules or by the robot program. When the semaphore becomes zero the control core activates the motion modules. To maintain deterministic behavior and to become immune against failure of motion modules and sensor modules or even process termination, the control core needs to get the CPU privileged to the other modules at crucial points of time in the control cycle. This is achieved by making use of the QNX priority inheritance concept as explained in detail in section 5.1. All the mentioned shared memory areas are accessed utilizing a name service provided by the middleware as presented in section 4.

Since all robot control and path planning algorithms are located in the motion modules, a framework for the design of the motion modules can be provided, that frees the application designer from communication and synchronization issues with the control core. This is achieved by the use of an abstract class as an interface as depicted in Fig. 5. This figure shows the collaboration diagram of an user defined position planner motion module. A motion module is built easily in two steps: First, the programmer implements its algorithm by inheriting the abstract class «MotionAlgo» and overloading the methods of this interface. In a second step he inherits his motion module from the base class «MotionModule», fills in the properties of his algorithm into the profile data structure the base class provides and creates an instance of his algorithm passing a pointer to this class' object to the «MotionModule» class. Everything else is encapsuled in the base class, which operates the interface to the algorithm.

OPEN IN VIEWER

Fig. 5.

Modular task frame formalism trajectory generator

MIRPA-X internally uses QNX message passing as the basic mechanism for realizing synchronous and asynchronous communication services. Application processes providing control level services are regarded as servers, while service requesters are regarded as clients. According to these roles, servers block on the reception of specific queries and instructions. As soon as they unblock by message reception, data interpretation and calculation is performed and in general concluded with an acknowledging reply to the requesting client.

All message-based communication is performed and controlled by the key component of MIRPA-X, the object server. It is an independent thread within the system that evaluates inquiries issued by clients, and forwards them to the respective servers without data interpretation. The forwarding strategy is based on a name service specifying application services (e.g. «EVAL|NEW|SKILL|PRIMITIVE»). Servers are thus identified using a representation for the service name based on a hash mechanism. This kind of name service facilitates a transparent and flexible way of implementing modular and even redundant application services without reference to the actual server and server location providing the service.

In order to register a service for application within the control system, the respective server issues a configuration request, containing the desired service name. Depending on the service type to be registered, the object server checks for collisions with previous registrations, and grants the registration request. When the system has been set up completely (all services are registered), the application client might now issue inquiries. There are two different types of inquiries: COMMANDs and REQUESTs. While a command is a non-blocking message type, a request forces the issuing client to wait until an answer from a responsible server arrives.

4.2 Shared Memory and Synchronization

Alongside the message-based communication, MIRPA-X provides a communication mechanism based on shared memory usage. As for messages, the shared memory mechanism uses the MIRPA-X name service which provides modularity and transparency for the application processes (clients and servers likewise). The usage of shared memory facilitates a high-speed, non-blocking data transfer between application processes, without the object server being involved in the actual communication task. After registration, application processes directly read and write fast inter-process data using memory pointers provided by MIRPA-X. For each shared memory region they act as publishers and subscribers, respectively.

In the case of multiple application processes accessing the same shared memory region, especially for complex control systems, data integrity has to be ensured at any time. As shared memory access is neither blocking nor synchronized by itself, MIRPA-X provides mutex and condition variable mechanisms using the same name service as for messages and shared memory.

4.3 Hard-Real-Time Process Scheduling

The real-time operating system QNX provides round-robin and FIFO scheduling at different priority levels, chosen by the user application. In the proposed control architecture (see section 3.2) the task frame formalism is used, realizing a discrete control core. The processes within this control core are to operate at a determined, constant cycle time, in order to realize machine-dependent control functions. For this reason, the middleware MIRPA-X facilitates application process operation at different priority levels and operation layers. The division of application processes into hard-real-time and soft-real-time is done regarding the necessity for the application processes to operate in a strictly serial and deterministic manner within a high-speed (currently up to 8kHz) cycle frequency. Hard-real-time processes always run at high priority, without the possibility of rescheduling.

In order to provide these two layers of operation for hard- and soft-real-time, MIRPA-X provides a scheduler by using QNX scheduling combined with a message-based token mechanism. With respect to the individual application process priority setup and possible dynamic priority changes the processes achieve and release the token, which permits user functionalities to operate without interruption at high priority level.

4.4 Operating MIRPA-X

When starting up MIRPA-X, an initial set of hard-realtime application processes is set up according to an initially desired sequence of operation defined in an initialization file. This set of real-time processes can be modified and adjusted at any time of operation (e.g. by the coordination unit of the control architecture). At configuration time, application clients and servers register desired services and shared memory regions together with the necessary synchronization mechanisms (mutex and condition variables); hard-real-time processes start and wait for the token to be transferred.

At run-time, the hard-real-time processes operate sequentially on high-priority level, triggered by an external synchronization pulse (e.g. from the underlying communication system) independent of message-based activities of lower level application processes. The token is passed on to the next hard-real-time application process. The sequence of operation can be flexibly adjusted at run-time by user-defined conditions. Because there are never two hard-real-time processes running at the same time (overlap), data integrity of the shared memory regions in use is certain, if there are only hard-real-time application processes accessing the specific shared memory regions. When all these processes have finished their operation, the remaining time within the cycle is used by the message-based soft-real-time application processes to communicate and calculate their values using message passing or shared memory. As soon as another cycle start (CST) arrives within the scheduler, the cyclic sequence of operation for hard-real-time application processes restarts.

5.1 Real Time Scheduling

The control architecture's scheduling is organized in three layers, an overview is given in Fig. 10. The bottom layer with the highest scheduling priorities provides a hard-real-time environment by making use of MIRPA-X's token passing based scheduler. First, the machine monitoring process gets the token and becomes blocked until a hardware interrupt from the built-in IEEE1394-PCI field bus adapter card is caught. By an adjustable prescaler ratio the token is released and passed by the middleware to the field bus communication process. After the necessary field bus data transfers this process releases the token and sequentially the subordinated drive controller and the machine monitoring again get the token. This leads to extremely low jitter (see section 6) in phase (in terms of control engineering) and execution time. From this the subordinated drive controller benefits significantly compared to other middleware approaches like e.g. OSACA, RT-CORBA and the traditional MIRPA as mentioned above.

On adjustable numbers of interrupt occurrences additionally a QNX pulse is fired to the next control level, temporarily pushing the pulse receiver process to the same priority level as the sender. With this concept, called priority inheritance, the next scheduling architecture level is synchronized and the control core becomes ready to be run, the next time the bottom layer becomes blocked waiting for the hardware interrupt. This pulse is needed twice: The first time when a new trajectory generator cycle should start and, consequently, all motion and sensor modules have to become notified of their tasks and the second time, to get the CPU again, in case of one or more attached modules (see section 3.2) failed. The scheduling is also deterministic here, running freely by message passing and becoming cyclically synchronized by pulses fired from the bottom layer.

The monitoring and coordination unit is the interface to the top layer of this control architecture. It implements error handling and accepts the skill primitives from the robot program interpreter. In the top layer there are also the GUI server for the user interaction and to visualize data from the control and the other operating services like network devices, hardware resource managers, etc. This layer uses an event driven non-cyclic scheduling principle. Modules in this layer get the remaining CPU time that is not consumed by the lower levels' processes.

5.2 Software in the Loop

Because the two upper layers of the control architecture, as depicted in Fig. 7, are independent of the particular robot, the control's behavior can be simulated with the control core itself. Thus, the initial setup of this control architecture can be done very conveniently. The field bus module and the subordinated drive controller can be put into operation independently from the upper layers. Therefore, troubleshooting can be done very efficiently. The same is true for the upper layers: The robot program and all associated motion modules can be put into operation without the necessity to move the robot or even the robot to be existent. With a simulation environment, providing visualization of the robot and an environment model providing simulated sensor values, the whole control core can be put in the simulation loop. This means, that the control designer is able to simulate the control with the control itself. Thus, there are no risks, that timing and communication requirements are not met in reality later on.

OPEN IN VIEWER

Fig. 7.

Scheduling concept featuring three layers with different real time demands

OPEN IN VIEWER

Fig. 8.

Simulation environment

Fig. 7 shows the simulation environment for the HEXA parallel robot, used to check a sequence of robot moves if the robot is able to perform them without running into a direct kinematic singularity as indicated by the singularity index visible next to the graphics output.

7.1 Distributed Algorithms

In the future, the control architecture is planned to be distributed on several computers in order to execute more complex and computational demanding motion algorithms. The strategy to achieve this is to build a distributed version of our middleware, so communication between two processes on different computers would become transparent. The field bus applied to the HEXA based on an IEEE1394 physical layer offers very good preconditions in both communication bandwidth and synchronization of several computers to successfully follow this approach.

The schedulability analysis of the control algorithms for the HEXA parallel robot shows that most benefits from distribution can be achieved, when the motion modules' and the very demanding machine monitoring algorithms are distributed to several computers.

The distributed machine monitoring algorithms would run in a per-computer token passing cycle on each computer, becoming synchronized by every new field bus communication cycle.

To distribute the motion modules the stub/skeleton principle seems to be attractive. A skeleton of the motion module could be scheduled easily in the middle layer of the control architecture as introduced in Fig. 7, maintaining the presented immunity against modules' failures: When a skeleton module's algorithm is requested by the control core it could connect autonomously to its stub process on a remote computer and stay blocked until a reply message from the stub process has arrived. This approach also opens the way to advanced concepts like load balancing and runtime optimization as discussed in the next section.

7.2 Runtime Scheduling Optimization

Due to the algorithmic behavior of complex motion modules, significant jitter in the execution time is observed. This is caused e.g. by iterative numerical approaches or algorithms where nonlinear systems of equations have to be solved during runtime. It is planned to implement a dynamic scheduling optimization strategy into the control core as clarified by the example given in Fig. 10. With the technique of jitter guards, storing the number of CPU clock cycles at the start and the end of an algorithm, the control core is able to identify modules which execution time tends to jitter. In this example the algorithm 2 puts the algorithm 3 into danger not to be able to finish its calculations in time. Identified by the jitter guard, this algorithm would be a good candidate to run parallel to algorithm 1 and 2 on an extra computer. The span of the jitter guards could be parameterized by an estimated execution time filled into the algorithms profile information.

OPEN IN VIEWER

Fig. 10.

Example runtime optimization criterion

Moreover, extension of the motion modules' profile information regarding the modules' demands for execution frequencies is planned. This will result in an optimum scheduling for each particular combination of motion modules according to the present skill primitive. Thus, the motion algorithms have to become capable of being executed at different sample times.