Motion Generation in the MRROC++ Robot ... - Semantic Scholar

Motion Generation in the MRROC++ Robot Programming Framework∗ Cezary ZIELIŃSKI, Tomasz WINIARSKI Warsaw University of Technology, [email protected], WWW home page: http://robotics.ia.pw.edu.pl

Abstract The paper presents a formal approach to robot motion specification. This motion specification takes into account three elementary behaviors that suffice to define any robot interaction with the environment, i.e.: free motion, exerting generalized forces and transition between both of those behaviors. Those behaviors provide a foundation for general motion generation taking into account any sensors, any effectors and the capability to exchange information between embodied agents. This specification can be used both for the definition of robot tasks and implementation of robot control software, hence both of those aspects are presented in the paper. This formal approach was used for the implementation of MRROC++ robot programming framework. As an exemplary task two-handed manipulation of a Rubik’s cube is used. Extensive experimentation both with the presented formalism and MRROC++ framework showed that the imposed formal rigor eliminates many errors at the software specification phase, produces well structured control software and significantly speeds up and simplifies its implementation. Those advantages are mainly due to the fact that the proposed formal specification tool is derived from operational semantics used in computer science for the definition of programming languages, thus a close relationship between abstract definition and implementation of the control system resulted.

1

Introduction

The diversity of hardware used in robot systems, the multiplicity of their applications, and the possible approaches, both to structuring the software of their control systems and their implementation means (operating systems, programming languages and paradigms used) make the quest for the ultimate robot programming framework [26, 38] very difficult. The challenges that the designer of such software faces are thoroughly enumerated in [41]. The above mentioned quest started in mid eighties of the 20th century with the creation of robot programming libraries, e.g. RCCL [22], PasRo [7, 8], followed by later developments, e.g. Multi-RCCL [35, 36], Kali [6, 20, 21], RORC [51]. Robot programming libraries have been since extended by architectural and design patterns supplemented by particular programming language constructs called idioms [26]. All of those efforts target software reuse. Reuse obviously is more economical than writing custom software from scratch, but it is also beneficial because of increased reliability (multiple users test the ∗

The authors gratefully acknowledge the support of this work by Warsaw University of Technology Research Program and the Ministry of Science and Higher Education grant N N514 1287 33.

1

same components) and fosters standardization. Work is also done on program generators, e.g. Genom (Generator of Modules) [2, 17] which is a tool for automatic generation of the structure of the control system execution level (functional level) from the builder’s specifications of modules. A module is an entity providing services upon request. The dominating approach to the design of robot programming frameworks utilizes software engineering perspective combined with the ingenuity of the creators of the framework regarding the general structure of a robotic system. This structure is usually based on the functional components that are necessary in the domain that the framework targets in particular. As the functional components require diverse programming means and above all, must communicate with each other, a lot of effort is put into creation of universal and effective communication means between those components, e.g. ORCA [9, 10, 37], CLARAty [41], OROCOS [11, 12], Player [1, 14, 18, 19, 48]. However, it should be noted that ORCA and Player, in opposition to OROCOS and CLARAty, do not have a full real-time capability, thus they are not well suited to tasks requiring tight deadlines, such as the force control tasks. An alternative approach to the design of robot programming frameworks is followed in this work. Here a robotics view of the system dominates. The utilized software engineering, although important, is secondary in nature. In this approach a generic view of a robot system is created from the perspective of interaction of the robot with the environment and internal data processing exhibited externally as the robot’s behavior. This approach was formalized [53]. This paper focuses on a specific aspect of robot programming frameworks – motion generation, because influencing the environment through motion of the effectors is the primary reason of existence of robot systems. For this purpose they gather information from the environment through their sensors, reason to make control decisions and communicate with other robots and devices. Thus motion generation is generalized in this paper to encompass all of those aspects. The discussion treats the subject formally by introducing symbolic notation and a two level general decomposition of robot systems. The upper level treats robots as embodied agents while the lower level uses transition functions to specify the behavior of those agents. The transition function approach stems from operational semantics [39] specification of computer programming languages, however it has to be much broader, as computer programs run in well modeled environment, i.e. the computing machine, while robot control systems must deal with only partially modeled environment that robots interact with. Motion generation must take into account two fundamental behaviors of robot effectors: transition of the end–effector and interaction between the end–effector and the objects in the environment. The former requires motion control, which is already well studied [15, 45], and the latter requires some form of interaction control, which still is an active filed of research. The tasks requiring contact between the manipulator and the environment necessitate appropriate force control, especially if the model of the environment is geometrically uncertain [4]. The manufacturing process: deburring, polishing or grinding; haptic devices [24] medical surgery [49] and service robots [25] are good examples. Depending on the purpose of control, the position and force control can be divided into the following groups [16] (the names of each type of control are cited as in original works): • pure force control – a constant force is commanded, • pure position control – a desired velocity trajectory is commanded, 2

• pure impedance control – in addition to position control the mechanical impedance of the robot is regulated to avoid excessive force buildup [23], • parallel position-force control – the control law is a superposition of force and position (velocity) control laws [13, 44], • hybrid impedance control – extends the pure force control by manipulator impedance control [3]. The robot can be specified in multidimensional spaces by using the Task Frame Formalism TFF [16] or the operational space [28, 29, 30]. In our work we combine the original hybrid control approach proposed by TFF with the parallel position-force control of each axis. The presentation is structured in the following way. Section 2 introduces the general structure of robot systems and the formalism used in the subsequent sections. Section 3 focuses on the general method of motion generation in such systems. Section 4 defines elementary behaviors of effectors. Section 5 briefly describes the method of implementing robot control systems by using MRROC++. Section 6 introduces transformations required for control law formulation. Section 7 concentrates on the implementation of the ideas formulated in previous sections using the MRROC++ robot programming framework. Section 8 deals with the implementation of motion execution while section 9 presents a simple example of formal motion specification for a two-handed robot system solving a Rubik’s cube puzzle. Section 10 concludes the presentation.

2

An Embodied Agent

The structure of MRROC++ is due to formal considerations presented in [52, 53]. Those considerations have been instrumental not only in the specification of the framework, but also in its implementation [54]. MRROC++ was preceded by a single robot control framework RORC [51] and a multi-robot control framework MRROC [50] that relied on procedural programming paradigm and not on the object-oriented approach that is employed by MRROC++. Each robot or other autonomous device is treated in MRROC++ as an embodied agent, i.e., an agent possessing a physical body. A multi-robot system can be composed of j = 0, . . . , na − 1 agents aj . The internal structure of each agent aj is presented in fig. 1. Four distinct entities are distinguished: ej – effector, i.e., a device responsible for influencing the environment, Rj – exteroceptors, i.e., devices gathering the information about the state of the environment (external to the agent) – subsequently processed to produce virtual sensor readings Vj , Tj – transmission links, which are responsible for direct interchange of data between the considered agent aj and the other agents, cj – control subsystem – enforces a certain behavior of the agent aj . In this and the following text the denotations representing system components and their state are not differentiated, because they pertain to the same entity and context makes this differentiation obvious, whilst significantly reducing the number of symbols used.

3

Figure 1: General structure of an embodied agent A bundle of virtual sensor readings used by agent aj contains nvj individual virtual sensor readings: (1) Vj = hvj1 , . . . , vjnvj i Each virtual sensor vjk , k = 1, . . . , nvj , produces an aggregate reading from one or more exteroceptors. The data obtained from exteroceptors usually cannot be used directly in motion control, e.g., control of arm motion requires the grasping location of the object and not the bit-map delivered by a camera. In other cases a simple exteroceptor would not suffice to control the motion (e.g., a single touch sensor), but several such sensors deliver meaningful data. The process of extracting meaningful information for the purpose of motion control is named data aggregation and is performed by virtual sensors. Thus the kth virtual sensor reading obtained by the agent aj is formed as: vjk = fvj (cj , vcjk , Rjk )

(2)

k

where Rjk is a bundle of exteroceptor readings used for the creation of the kth virtual sensor reading, vcjk forms the sensoric memory that holds aggregated previous readings, e.g., an integral of measurements. With each virtual sensor vjk a function fvj is associk ated. It defines the prescription for creating the virtual sensor reading, thus it determines a specific method for the aggregation of data contained in the arguments of that function. Rjk = hρjk , . . . , ρjknρ i

(3)

1

where nρ is the number of exteroceptor readings ρjk , l = 1 . . . , nρ , taken into account in l the process of forming the reading of the kth virtual sensor of the agent aj . It should be noted that (2) implies that the reading of the virtual sensor depends also on cj . In this way the agent has the capability of configuring the sensor as well as delivering to the virtual sensor the relevant information about the current state of the agent (including its effector). This might be necessary in the case of computing the reading of a virtual sensor having its associated exteroceptors mounted on the effector (e.g., in-hand camera). 4

The first three of the four entities listed above as components of an agent aj (i.e., ej , Vj , Tj ) are represented in its control subsystem as images. Those images form internal models (data structures) that are either the source of information (denoted by a leading subscript x) or the sink of information (subscript y) created by the control subsystem. Diverse images (models) of the physical devices can be envisaged, thus creating different ontologies. MRROC++ provides several views of the system for the programmer to choose from. The control subsystem cj of the agent aj besides the above mentioned three entities contains its own internal data structures, thus the following components exist within it: x cej

– input image of the effector (a set of data conforming to the assumed input model of the effector in the control subsystem – it is produced by processing the input signals transmitted from the effector proprioceptors to the control subsystem, e.g., motor shaft positions, joint angles, end-effector location – they form diverse ontologies),

x cVj

– input images of the virtual sensors (current virtual sensor readings – control subsystem’s perception of the sensors and through them of the environment),

x cTj

– input of the inter-agent transmission (information obtained from other agents),

y cej

– output image of the effector (a set of data conforming to the assumed output model of the effector in the control subsystem – e.g., PWM ratios supplied to the motor drivers; thus the input and output models of the effector need not be the same – and usually are not),

y cVj

– output images of the virtual sensors (current configuration and commands controlling the virtual sensors),

y cTj

– output of the inter-agent transmission (information transmitted to the other agents),

ccj – all of the other relevant variables taking part in data processing within the agent’s control subsystem. It should be noted that the current state of the effectors is made available to the control subsystem also by reading receptors, but in this case these are proprioceptors, i.e., receptors responsible for gathering the information about the positions and orientations of limbs, as well as the internal forces between the links. The distinction between exteroceptors and proprioceptors is fundamental. They are not only responsible for collecting the information from different sources (external versus internal to the agent), but their sampling frequencies usually differ, thus they must be located in different components of the system, hence data processing feedback loops of different length result. It might seem that the communication with each of the entities (i.e., effectors, exteroceptors, other agents) is very similar, thus the control subsystem should not differentiate between them. However, there is a significant difference in the primary flow of information. In the case of an effector the primary flow is from the agent’s control subsystem to the effector, in the case of virtual sensor the primary flow is in the reverse direction, whilst in the case of communication with another agent the information flow is fundamentally bidirectional. In the case of effectors and receptors the flow is asymmetric – the primary source of information is the control subsystem in the former case and the environment in the latter. In the case of agents communicating with each other the situation is symmetric. This is why other agents are not treated simply as remote sensors or effectors. Obviously in each case there is a secondary flow of information. The agent needs the information about the state of the effector to control it, and it might want to configure the virtual sensor to obtain appropriate readings. Nevertheless, this flow of information 5

Figure 2: General structure of a MRROC++ based system (a0 – a virtual agent, aj – embodied agents, j = 1, . . . , na ) is regarded as supplementary, although still crucial to the realization of the task. The above view has a paramount influence on the general structure of the created systems. The above described structure of the agent aj is very general. One can envisage agents with certain of the listed components missing. The most useful is an agent without effectors – i.e., a virtual agent. MRROC++, in its current version, fosters the implementation of hierarchical systems with a single virtual agent coordinating many embodied agents (fig. 2), but is not limited to this structure. The virtual agent can be dormant, and thus uncoordinated operation of embodied agents is possible. A structure with several virtual agents is also possible. From the point of view of the system designer the state of the control subsystem changes at a servo sampling rate or a low multiple of that. If i denotes the current instant, the next considered instant is denoted by i + 1. This will be called a motion macrostep. The control subsystem uses: 0 i x cj

= hcicj , x ciej , x ciVj , x ciTj i

(4)

to produce: i+1 y cj

i+1 i+1 i+1 = hci+1 cj , y cej , y cVj , y cTj i

6

(5)

For that purpose it uses transition functions:        

ci+1 cj

= fccj (cicj , x ciej , x ciVj , x ciTj )

i+1 y ce j

= fcej (cicj , x ciej , x ciVj , x ciTj )

i+1   y cVj      ci+1

= fcVj (cicj , x ciej , x ciVj , x ciTj )

y Tj

(6)

= fcTj (cicj , x ciej , x ciVj , x ciTj )

This can be written down more compactly as: i+1 y cj

= fcj (x cij )

(7)

Formula (7) is a prescription for evolving the state of the system, thus it has to be treated as a program of the agent’s behavior. For any agent exhibiting useful behaviors this function would be very complex, because it describes the actions of the system throughout its existence. The complexity of this function renders impractical the representation of the program of agent’s actions as a single function. Function (7) has to be decomposed to make the specification of the agent’s program of actions comprehensible and uncomplicated. However, this implies that there will be many partial functions that need to be selected and composed to produce the program of the agent’s actions. Both selection and composition must be defined formally. Usually selection is based on predicates and composition is based on concatenation, e.g., function (7) can be decomposed sequentially, thus the partial functions must be composed by executing them one after the other. Hence, instead of a single transition function fcj , nf partial transition functions are defined: i+1 y cj

=

fcj (x cij ),

m

m = 1, . . . , nf

(8)

Variability of agents is due to the diversity of those partial transition functions and their different compositions. A predicate based selection instructions, based on x cij treated as arguments, is used: if

m

pcj (x cij ) then y ci+1 := j

m

fcj (x cij ) endif

(9)

In actual systems an endless loop must be constructed. It must contain: the input of arguments x cij , the conditional instructions (9) selecting the appropriate partial transition function for computation, and the output of values y ci+1 j . Close inspection reveals that instruction (9) has considerable redundancy built into it. In each iteration of the loop for each instruction (9) the predicate m pcj is tested and one of the partial transition functions mfcj is evaluated. Both take the same arguments x cij , thus in principle both can perform the same computations. It is more reasonable to differentiate their functions. The predicate should be responsible for major decisions taken infrequently, and the partial transition function should produce the desired behaviour in each iteration. However, this rises the question of how many iterations should be executed before the next evaluation of the predicate? Should this number be fixed or variable? Fixed number is over-constraining, variable number requires the means for defining this number. Thus each partial transition function mfcj is decomposed into two sub-functions: mfτj and mfc0j . The former is a Boolean valued function determining the terminal condition, i.e., a condition deciding when to stop the repetition of computations of the latter function, i.e., the function responsible for the behaviour of the system within each period i → i + 1. This reasoning led to the formulation of the MRROC++ motion instruction.

3

Motion Instruction

A motion instruction of each embodied agent aj requires the input of all arguments x cij , testing of the terminal condition mfτj , and if it is not true, the computation of the next 7

desired values y ci+1 j , which in turn have to be dispatched to the appropriate components of the system. This is its general form: loop // Check the terminal condition if mfτj ( x cij ) = true then break // Quit the loop endif // Compute the next control subsystem state i+1 := mfc0j ( x cij ); y cj // Transmit the results i+1 i+1 i+1 y cej  ej ; y cVj  Vj ; y cTjj 0 x cTj 0 j ; // Wait for the next iteration i := i + 1; // Determine the current state of the agent ej  x ciej ; Vj  x ciVj ; y cTj0 j  x ciTjj0 ; endloop

(10)

where  represents transition of data. The motion instruction starts with the test of the terminal condition, so it is assumed that prior to the initiation of the current motion instruction all the necessary data has been read-in by the control subsystem. Hence the motion instruction terminates with this data being read-in. At system initiation this data is also input. The satisfaction of the terminal condition of the currently executed motion instruction causes its termination and immediate invocation of the next motion instruction. It is the responsibility of the next motion instruction to compute the next control subsystem state. The terminal condition is a predicate mfτj ( x cij ) that takes as arguments x cij . The value of x cij is known at the instant i, and the computations are performed between i and i + 1. The argument x cij = hcicj , x ciej , x ciVj , x ciTj i, thus proprioceptive x ciej , exteroceptive i i x cVj , communication x cTj and internal data is fully known. Those values are used by the transition function of the next motion instruction computing both its terminal condition m fτj ( x cij ) and, if it is not satisfied, its transition function y ci+1 := mfc0j ( x cij ). Thus a new j motion instruction is initiated. Implementation of the motion instruction must assure that the transition from one motion instruction to the other, requiring the computation of two terminal conditions (one for each of those instructions) and the computation of the next control subsystem state (executed by the new motion instruction), is done within ∆i = (i + 1) − i. Usually terminal conditions are very simple predicates, thus satisfying this requirement is not difficult. The computation of the next motion step is done always, either by the current motion instruction or the next one, so the switch between the two requires only extra computation of one terminal condition. The transmitters cTj of agent aj have received a more detailed description denoting both the owner of the transmission buffer (the first right subscript after T – this is the original subscript used by the one subscript version) and the source/destination of the information (the trailing right subscript), e.g. cTjj0 is composed off the transmission buffer of agent aj receiving information from agent aj 0 : x cTjj0 , or sending information to aj 0 : y cTjj0 . Both functions mfτj and mfc0j can be bundled into one programming object called the motion generator. This object as well as the variables x cij and y ci+1 form the variable j part of the instruction, i.e., its parameters or arguments. However the general structure 8

is fixed, thus regardless of the type and number of devices the same instruction can be used. It should be noted that robot programming languages usually have instructions used for waiting for certain events to occur. Instruction (10) can be used with the transition function mfc0j keeping y ci+1 ej constant, what results in lack of motion of the effectors, i.e., pure waiting. This shows that a single motion instruction is required, although it is highly parameterized by all of the above enumerated arguments. In MRROC++ the code (10) defines the Move instruction.

4

Elementary Behaviors

Code (10), i.e. the MRROC++ Move instruction, defines the agent’s elementary behavior. Due to enormous multiplicity of possible transition functions (7) (i.e., m fc0ej , m fτj pairs) there is no limit to the definition of those behaviors. Thus the programmer has to be supported with some guidance to facilitate the creation of useful systems. The main purpose of functions m fc0j is to induce motion of the effectors. The state of the effectors is measured by proprioceptors. Thus elementary behaviors used for the creation of general behaviors (10) are based on propropceptive input (i.e., pose of the effector and the generalized force it exerts on the environment). Fortunately all elementary behaviors fall into three general categories. Those categories have been singled out by inspecting possible behaviors of the effector in very diverse tasks. The experience gained trough extensive use of MRROC++ in the creation of both industrial and service robot controllers executing considerably differing tasks [56, 57] showed that the following general behaviors are necessary: • unconstrained motion with the assumption that no contact with obstacles will be encountered – where pure motion control suffices, • contact with the environment – where pure force/torque control is used, • intermediate or transitional behavior – where initial unconstrained motion is expect to result in eventual contact, or vice versa – for this purpose some form of parallel position–force control has to be utilized (e.g., stiffness, damping or impedance control). The three enumerated elementary behaviours are used as building blocks for constructing more elaborate functions fc0j , which take into account the data obtained form virtual sensors and other agents, as presented by (10). The functions m fc0j produce values that are the arguments of elementary behaviours executed in the process of transmitting the results (execution of the  operator in code (10)). It should be noted that sometimes simultaneously one form of those behaviors is expected to occur in one spatial direction, whereas another form has to be realized in another. Motion generation facilities provided by a robot programming framework need to cater to all of the enumerated requirements. Further on the text explains how this problem has been solved in MRROC++. The user’s task has to be formulated in terms of the three elementary behaviours. Both unconstrained motion (requiring motion control) and contact behaviour (requiring pure force/torque control) are implemented to be stable when considered separately. The formulation of the task should be such that no direct switch between those two behaviours is required. A switch between those behaviours always takes place through the intermediate (transitional) behaviour. Obviously this behaviour is also implemented by a stable controller. Thus unconstrained motion control is switched to motion control of the transitional behaviour and this leads to the contact situation. From this moment contact 9

behaviour can take over. The same procedure, but in reverse order, is executed when transition from contact to unconstrained motion is necessary. As verified by numerous experiments, this method of formulating tasks enables stable implementations, as well as a framework can, thereby supporting assurance of stable behaviours in specific application.

5

Implementation of an Agent Using MRROC++

MRROC++ [54, 55] is a robot programming framework that is the basis for the design of Multi-Robot Research-Oriented Controllers. Its underlaying software is coded in C++. As such it provides a library of software modules (i.e. classes, objects, processes, threads and procedures) and design patterns according to which any multi-robot system controller can be constructed. This set of ready made modules can be extended by the user by coding extra modules in C++. The freedom of coding is, however, restricted by the general structure of the system. New modules have to conform to this general structure. Even if a single-robot controller is designed it is assumed that it can work in a multi-robot environment, so its controller really has the capability of controlling several robots. The same applies to sensors. Regardless of the fact, whether they are necessary for the execution of the user’s task, the potential for their utilization always exists in the system. A MRROC++ based control system is implemented as a set of processes (fig. 2): UI – User Interface Process – a single system configuration dependent process, MP – Master Process – a single process (representing a virtual agent) responsible for the coordination of the embodied agents present in the system, ECP – Effector Control Process – responsible for the execution of the task allotted to the effector – several of those processes can coexist (the number depends on the number of distinct effectors present in the system, i.e., the number of embodied agents – na ), EDP – Effector Driver Process – responsible for controlling the hardware associated with the effector – there are as many EDPs as there are ECPs, VSP – Virtual Sensor Process – responsible for performing data aggregation on real sensor (exteroceptor) readings and thus producing a virtual sensor reading – zero or more of those processes can be associated with MP or any of the ECPs. Some of those processes have extra threads. Any system controller is composed of those processes. There is always a virtual agent a0 implemented as MP and na embodied agents aj implemented as ECP-EDP pairs (fig. 2). The reason for the subdivision of aj is to facilitate programming – hardware and task dependent parts are disjoint. If the effector is a manipulator, the EDP is responsible for direct and inverse kinematics computations as well as for both motion and interaction servocontrol. The EDP is further divided into several threads (fig. 11): EDP_MASTER (communication with the ECP including interpretation of its commands – thus it is responsible for possible definitions of effector images), EDP_TRANS (designation of force/torque and pose control directions), EDP_SERVO (motor position servocontrol) and EDP_FORCE (force measurements – here force/torque sensors are treated as proprioceptors). The UI depends only on the number of effectors constituting the system. It is responsible for presenting the system status to the operator and enables the operator to start, pause, resume or terminate the execution of the task. 10

Both EDPs and VSPs depend on the associated hardware, whereas MP and ECPs are hardware independent, but depend on the task that is to be executed by the system. This division highly simplifies coding. When the hardware configuration is fixed the programmer with each new task modifies only the MP and the ECPs. Only when this configuration has to be changed, e.g., by introducing a new type of manipulator, a new EDP has to be appended to the system. $

'

Process shell (constant – fixed part) Contains: • inter-process communication • error handling '

$

Process kernel (modifiable – user defined part) Contains: • user’s program & &

% %

Figure 3: General structure of MRROC++ processes

The general structure of MP, ECPs and VSPs is shown in fig. 3. The shell (fixed part) of each process is composed of the code responsible for the communication with the other processes and performing error handling. The kernel (user defined part) of each of those processes is responsible for the execution of a specific task and thus is delivered by the user. In the case of MP and the ECPs it is usually composed of the Move instructions. Errors are dealt with within the whole system by exception handling, so the users need not deliver the program code responsible for that. Three classes of errors are distinguished: non-fatal, fatal and system errors. Non-fatal errors are caused by computational problems or wrong arguments of commands/replies within the IPC. Fatal errors are caused by malfunction of the effector (e.g. over-current in the motor or hitting a limit switch). System errors are caused by problems with IPC or process creation. It is assumed that MRROC++ based systems have a single coordinator (implemented as a virtual agent a0 – possibly dormant) and as many embodied agents aj as there are effectors. It is further assumed that the embodied agents communicate explicitly only with the coordinator. Obviously implicit communication through the environment or indirect communication through the coordinator between the embodied agents is possible. The MP (i.e., a0 ) is a coordinator and thus in tasks requiring continuous coordination of the effectors it generates the trajectories for them. If no coordination is required or sporadic coordination is necessary the burden of trajectory generation is imposed on each ECP. Hence Move instructions can appear both in the MP and the ECP. Their format is negligibly different (fig. 4 and fig. 5) and thus their code in both cases is represented by (10). The difference is due to the fact that the MP is a virtual agent a0 , thus without direct access to the effectors, while the ECP–EDP pairs composing an embodied agent aj influence their effector directly. The ECP Move instruction uses its motion generator to compute m fc0j and m fτj (the last argument in fig. 4). Both of those functions require the argument x cij (composed of the first three arguments in fig. 4). The evaluation of m fc0j (x cij ) results in y ci+1 j . Those values are the desired values for the components of the agent and inter-agent communication. They are dispatched to them by the code (10). 11

sensor images c x Vjnv and y cVjnv .. . sensor images x cVj2 and y cVj2 sensor images and y cVj1

effector images x cej and y cej ,

Move (

motion generator m 0 fcj and m fτj

transmission buffers x cTj0 and y cTj0

x cVj1

,

,

);

Figure 4: General format of the ECP Move instruction

sensor images x cV0nv and y cV0nv

transmission buffers x cT0nt and y cT0nt

.. .

.. .

sensor images x cV02 and y cV02

transmission buffers x cT02 and y cT02

sensor images and y cV01

transmission buffers x cT01 and y cT01

x cV01

Move (

,

motion generator m 0 fc0 and m fτ0 ,

Figure 5: General format of the MP Move instruction

12

);

At the MP level, both the information about the current state and the desired state of the effectors must be obtained through the transmission links. Hence, in fig. 5 there are no arguments directly pertaining to the effectors, but there is a list of transmission links to all of the embodied agents. Through those links the relevant information is transmitted. The functions m fc00 and m fτ0 take as arguments only x ciV0 and x ciT0j and compute y ci+1 T0j . The link from MP (a0 ) to ECP (aj ) involves: i+1 y cT0j

 x ci+1 Tj0

i+2 y cej

(11)

where represents computation of a certain transition function m fc0j – this requires time (as presented by code (10)), thus the superscript i is incremented. To simplify the considerations it was assumed that the inherently introduced delay associated with both the inter–process transmission of data and computation of transition function will be cumulated and assigned to computations, thus making the data transition delay-free. In the case of continuous coordination, when MP is responsible for motion generation, the ECP (aj ) is transparent, i.e., its motion generator does not modify the desired values that are produced by the motion generators in the MP Move instructions. Although the transparent generator’s transition function is an identity function, its computation introduces the assumed time delay for consistency, thus i is incremented. If the MP requires effector state feedback for motion generation two variants are possible. One involves true state measurement eij  x ciej |

i+1 y cTj0

{z

}

aj

 x ci+1 T0j |

i+2 y cT0j

{z

(12)

}

a0

and the other, which does not introduce the delay evident in (12), relies on state estimation based on the computed desired value. In this case either the MP saves the value that it has computed in the previous iteration: i+1 y cT0j

 ci+1 c0

(13)

or, if the ECP modifies this value, the MP has to receive the altered value from that process, i.e.: i i i+1 i+1 (14) y cT0j  x cTj0 y cTj0  x cT0j It should be noted that the presented formalism enables the assessment of the delays introduced into the control feedback loops prior to the implementation of the system – at the specification stage. This is done by studying the value of the superscript i, e.g., the full feedback loop involving MP is the concatenation of (11) and (12): i y cT0j

 x ciTj0

i+1 y cej

 ei+1 j

ei+2  x ci+2 j ej

i+3 y cTj0

 x ci+3 T0j

i+4 y cT0j

(15)

thus it needs 4 macrosteps for completion. The transition ei+1 ei+2 causes physical j j execution of the motion by the effector (EDP takes care of this), hence it needs time to do this. Actually the duration of this stage defines the duration of the macrostep in MRROC++. The assumed transition function cumulative computation time is made equal to the duration of the macrostep. In reality it is usually (has to be) shorter than that, but still all the processes are synchronised with macrostep cycle. This is the system heartbeat. The EDP divides the macrostep into steps. In majority of cases such long feedback loop as in (15) is unnecessary. Either the MP or the ECP generates the set values for the lower control layers and they rely on accurate execution of the command by the EDP for attaining the required effector goal, i.e. during interpolation the current effector pose (x ciej ) is not fed back to the ECP and subsequently to the MP. Thus only a short feedback loop requiring only one macrostep eij ei+1 j 13

suffices in this case. Motion generators of the Move instructions of the MP rely on the fact that the information transmitted from and to the ECP may contain the current and the desired state of the effectors. This enables trajectory generation for coordinated effectors. However, the data transmitted between a0 and aj can contain any information and can be used for any purpose, e.g., some agents may be used as remote sensors and a0 can perform as the distributor of the remote sensor readings. It can also be a general planner deducing the actions that the system has to take without going into the details of effector motions – those can be formulated by the aj (ECP).

6

Generalized Position and Force Transformations

Implementation of the here postulated three elementary behaviors requires the formulation of an adequate control law. This law is defined in terms of poses (position and orientations) and interaction forces and torques of the end–effector and the environment. Hence transformations used for the control law formulations have to be introduced first. Fig. 6 presents the modified IRb-6 manipulator [58] (in this case equipped to solve the Rubik’s cube puzzle) and the coordinate systems affixed to it: 0 – base, W – wrist, S – force sensor, G – gripper (its origin is located at the gripper mass center and the orientation is the same as that of frame W ), E – end effector (task frame).

20 2

Z X

Z Wrist (W)

Z Sensor (S)

Z (G) Gripper

Z End (E) Effector

Base (0)

Figure 6: Manipulator structure and the coordinate frames [40] In general the pose of frame Q in relation to frame U can be expressed by a homogeneous matrix [15, 45]: " # U U R P U Q Q (16) QT = 01×3 1 where UQ R is a 3 × 3 matrix expressing the orientation of Q with reference to U and UQ P is a 3 × 1 column vector describing the position of the origin of Q in relation to U . The relation between frames U and Q can be represented as a homogeneous matrix UQ T , or a column vector UQ r(X) , where X is the label assigned to the pose representations: X ∈ {A, E, J, M } (see tab. 1). The operator AX transforms the column vector UQ r(X) into homogeneous matrix UQ T . The operator A−1 X produces the reverse operation: AX (UQ r(X) ) =

U A−1 X (Q T ) =

U QT

U Q r(X)

(17)

In the particular case of AA , the representational singularities are avoided by assuming that if the orientation vector is approaching 03×1 then the rotation submatrix is assumed 14

to be an identity matrix I3×3 . AM is the direct kinematics problem solver and A−1 M is the inverse kinematics problem solver. The 6 × 1 column vector representing the general velocity of a frame U in relation to frame 0, where its components are expressed with respect to U is denoted as UU r˙(A) = h

U T U T U v , Uω

iT

: U U r˙(A)

U 0 (U r˙(A) )

=

U 0 0 ξ∗ U r˙(A)

=

(18)

where in the general case UQ ξ∗ is: " U Q ξ∗

=

U QR

03×3

#

03×3 U QR

(19)

This matrix is responsible only for the orientation change of six component free vectors. iT

h

The 6 × 1 general force vector UU F = UU F T , UUN T is composed of 3 × 1 force vector F and 3 × 1 torque (moment) vector N . This force is applied to the origin of frame U (left subscript) and its components are expressed in relation to frame U (left superscript). Sometimes in the case of a free vector (e.g., pose increments, velocities and forces) it is required to express them in different orientation than they were expressed initially. In that case the notation C (UU r˙(A) ) can be used where the, e.g., velocity of the frame U in relation to frame 0 is expressed in frame C. The following definitions apply to general velocity: C U (U r˙(A) )

=

C U 0 ( (U r˙(A) ))

C U 0 U ξ∗ 0 ξ∗ U r˙(A)

=

=

C 0 0 ξ∗ U r˙(A)

=

C U r˙(A)

(20)

and analogically for generalized force: C U (U F)

=

C U U ξ∗ U F

=

C UF

(21)

To simplify the notation the following abbreviation will be used: U

r˙(A) =

U U r˙(A)

=

U 0 (U r˙(A) )

(22)

and analogically for generalized force: U

F=

U UF

(23)

There exist transformations UQ ξV , UQ ξF relating general velocities/forces in one frame to those expressed in the other when the two are physically bound to each other [45]. U

r˙(A) =

U Q r˙(A) , Q ξV

U

F=

U Q F Q ξF

(24)

Both transformation UQ ξV and UQ ξF can be easily computed by extracting appropriate components from the homogeneous transformation matrix UQ T (16). " U Q ξV

=

U QR

03×3

S(UQ P ) UQR U QR

"

#

,

U Q ξF

=

U QR U S(Q P ) UQR

03×3 U QR

#

(25)

where S(P ) is the skew symmetric matrix constructed out of the components of a given vector P = [px , py , pz ]T .   0 −pz py  0 −px  S(P ) =  pz (26)  −py px 0 The skew symmetric matrix S(P ), if right-multiplied by a vector Q, yields the vector cross product P × Q. It is worth noting that C U ξ∗ =

C U ξV

=

C U ξF

when C U P = 03×1 . 15

7

EDP Commands

The interface between ECP and EDP defines how the effector is perceived by the higher layers of the control structure, i.e., it defines the internal structure of x cej and y cej . The EDP interprets and executes the commands issued by its respective ECP. It executes the i ei+1 ei+1  x ci+1 transition y ciej  eij j j ej . The execution of the transition ej requires one macrostep (i i + 1) which is subdivided into steps (denoted as ι). The list of all commands accepted by the EDP is presented in fig. 7, 8, 9, 10. There are two main commands: SET and GET. The former influences the state of the EDP and so the effector, and the latter causes the EDP to read its current status and thus also the effector pose. Sometimes, the user needs to exert simultaneous influence on the effector and to read its current state, so a SET_GET command has been defined, which causes simultaneous execution of a SET and GET command. As the execution of motion commands takes time, to retain the client-server mode of the EDP operation the ECP issues a QUERY command to obtain any feedback from the EDP, so QUERY has to follow any other command. As the majority of the robots has incremental position measuring devices, it is required that prior to task execution the robot defines its current configuration in the work-space. This is usually done only once and by moving the arm to a known base location. This is caused by the SYNCHRO command. EDP Commands

SET

SET_GET

GET

SYNCHRO

ARM

ARM

ROBOT_MODEL

ROBOT_MODEL

OUTPUTS

INPUTS

QUERY

Figure 7: Effector Driver Process commands and their main arguments (arrows with bullets represent EITHER OR, those with open circles – OR, and those without any symbol – AND; this convention is also used in figs. 8, 9, 10) The SET command can: set the arm configuration or pose, i.e. cause the robot to move to the desired pose, assure the desired configuration, or set the forces and torques to be exerted by the manipulator end-effector, set the gripper jaws distance (gap) ggd , redefine the tool affixed to the arm, initiate the force sensor calibration (force_bias), change the kinematic model, switch the servo-control algorithm of any or all of the arm motors, alter the parameters of the servo algorithm, or set the binary outputs of the robot controller (fig. 8). The GET command can read: the current configuration or pose of the arm, the forces and torques exerted by the manipulator end-effector, the current gripper jaws distance ggm and the information whether the gripper jaws are blocked gb , the currently used tool, the kinematic model and servo algorithm parameters, or the binary inputs to the robot controller. Switching of kinematic model should improve local precision of Cartesian arm motions. Modification of servo algorithms or their parameters can improve tracking ability. This switch can be performed when significant load modification is anticipated. Both the tool and the arm pose can be defined in terms of homogeneous transforms, Cartesian coordinates with orientation specified either as Euler angles or using angle and axis convention. In the case of the arm configuration, moreover, it can be specified in terms of joint angles or motor shaft angular increments. The arm pose or configuration 16

argument in the command can be regarded as an absolute or relative value. The gripper state is defined as: gs = hggm , gb i. The gb = 1 informs that gripper jaws are blocked, i.e., the gripper is holding an object, gb = 0 represents an opposite situation. Depending on the task the tool, i.e. end–effector, might change 1 . Either the gripper or some distinguished point of the held object are treated as the tool. This fact is stressed by the third column of table 1. Each SET ARM motion command is treated as an execution of a single macrostep. representation motor (M ) joint (J) xyz Euler zyz (E) xyz angle and axis (A) homogoneous matrix

absolute

relative

0 E i+1 r(M )d 0 E i+1 r(J)d 0 E i+1 r(E)d 0 E i+1 r(A)d 0 E i+1 Td

Ei E i+1 r(M )d Ei E i+1 r(J)d Ei E i+1 r(E)d Ei E i+1 r(A)d Ei E i+1 Td

tool W i+1 E r(E)d W i+1 E r(A)d W i+1 E Td

Table 1: EDP command pose and tool representations. The subscript d stands for the desired value.

ARM number_of_steps value_in_step_no MIM

POSE

TCIM

POSE velocity FORCE behavior

POSE

motors

FORCE

joints frame xyz_euler_zyz xyz_angle_axis absolute / relative gripper_jaws_distance

force_xyz_torque_xyz inertia reciprocal_of_damping

s

Figure 8: ARM motion parameters of a SET command The motion interpolation parameter µ ∈ {MIM, TCIM} decides which kind of motion interpolation will be used by the EDP. If motor interpolated motion (MIM) is commanded by the ECP, the EDP converts the representation of the desired pose into motor shaft positions. This low-level representation is subsequently used for the subdivision of the macrostep into steps, what results in motor motion interpolation. This interpolation is used near the end–effector poses, where singularities of kinematics prevent us from using TCIM interpolation. In the case of task coordinates interpolated motion (TCIM), the EDP performs the interpolation in Cartesian space supplemented by angle and axis orientation representation. In that case either the arm velocity r˙(A)d (TCIM-velocity) can be set or the arm pose (TCIM-POSE). The different approach to interpolation used by MIM and TCIM 1

This command suffices only for simple grippers for which the jaws distance can be specified. For more complex grippers, not to mention other tools (e.g., welding or paint spraying guns), either other EDP commands would have to be added, or more probably they would be controlled by a different agent, thus an extra ECP-EDP pair would be appended to the system.

17

results in a different number of coordinate transformations needed by each scheme. MIM transforms only the desired end–effector pose once for each macrostep, while TCIM does this once for each step, thus many times within the commanded macrostep. The TCIM interpolation enables parallel position–force control, hence, the additional arguments of TCIM are: desired general force – E Fd , inertia – I, reciprocal of damping – B, the definition of behavior b of the end–effector along/about each axis of frame E. All of the enumerated vectors have six components each. The I and B determine the virtual inertia and virtual reciprocal of damping along/about each of the axes of the end–effector frame E. The behavior definition b is a vector with the following possible values for each coordinate: UNGUARDED MOTION, GUARDED MOTION and CONTACT, in short b ∈ {u, g, c}. ARM

POSE POSE force_xyz_torque_xyz gripper_state

motors joints frame xyz_euler_zyz xyz_angle_axis

Figure 9: ARM motion parameters of a GET command The number of steps in the macrostep (ns ) is an additional parameter of the instruction. To produce continuous motion the set value for the next motion must be delivered prior to the termination of the previous motion. The reading of the current pose of the manipulator and the delivery of the next set value are simultaneous. To solve the problem of continuity of motion another parameter has been introduced. It defines in which step (counting from the beginning of the currently realized macrostep) should the reading be made available (at this very moment the ECP has to deliver the next motion command – this command may be produced earlier, but the EDP has to have it available at this very moment). For uninterrupted trajectory segment transition it suffices if it is one less than the number of interpolation steps. This parameter is called value_in_step_no (nQ ). If value of this parameter is one more than the the number of motion steps in the macrostep, the reading will be transmitted just after the end of interpolation. The thus obtained pose typically will not be the one that the manipulator will finally attain, because there is still an error, which the axis controllers will try to eliminate. It is important to be aware that if no new motion command is issued, the manipulator is brought to a standstill in about 100 steps after the termination of the previous motion command. ROBOT_MODEL KINEMATIC_MODEL

parameter_set_no

TOOL

frame xyz_euler_zyz xyz_angle_axis

SERVO_ALGORITHM

algorithm_no algorithm_parameter_set_no position

FORCE_TOOL force_bias

weight

Figure 10: ROBOT MODEL parameters of a SET/GET command The above described commands are the building bricks for motion generators implemented within the ECP processes. The simplest case arises when the trajectory is generated without taking into account the virtual sensor readings or inter-agent transmissions. For 18

example, a trajectory that contains via points or is composed of blended segments can be generated in the simplest case as a sequence of SET ARM MIM frame commands, if we want the trajectory to be expressed in Cartesian space. Each segment of the trajectory is defined in terms of its end pose and end velocity. The motion generator uses this data to interpolate those segments with subsegments, starting with the initial pose of the effector and finishing with the end-point of the last segment. Each interpolation subsegment is executed as a single SET ARM MIM frame command. If MIM is used the EDP uses motor interpolation within subsegments. The velocity and acceleration along this subsegment is governed by the number_of_steps parameter as each step takes 1 or 2 ms, depending on the type of the controlled manipulator, and the length of this subsegment. Smooth transition between those subsegments is ascertained by an adequate value of the parameter value_in_step_no. Usually value_in_step_no = number_of_steps-1. Thus three levels of interpolation occur. First the required trajectory is divided into segments - this is done by specifying the via points. Then each segment is divided into subsegments ensuring appropriate accelerations and velocities and finaly those subsegments (macrosteps), coded as EDP commands, are internally interpolated by dividing macrosteps into steps - this is done by the EDP. MRROC++ has readily available motion generators for different linear interpolators, spline functions and NURBS. If any commanded behaviour of the manipulator, regardless whether it is caused by purely motion control or interaction control, requires excessive torques from the motors, currently this is treated as a non-fatal error and the execution of the user task is aborted. As errors are dealt with by exception handling this standard method of treating errors can be redefined by the MRROC++ user.

8 8.1

Motion/Interaction Command Implementation Motion/Interaction Controller

Fig. 11 presents the structure of the parallel pose–force controller implemented in MRROC++. The figure contains two parts: the macrostep execution part and the step execution part. Each macrostep i is subdivided into steps ι. The controller was developed to execute elementary behaviors (sec. 4) specified individually for each coordinate of the task frame. It should be noted that the transitional behaviour between pure position control and pure force control has been introduced to make the reaction to impact of the effector on an object faster than if this behaviour would not be introduced. Switch in the behaviour is brought about by checking whether a terminal condition is fulfilled. This is done at a macrostep rate i (see code (10)). However, the implemented position-force controller, presented in Fig. 11, can react much faster – at the step rate ι. For that the controller must be primed that it should expect a collision and have the capability of accommodating it. This is implemented by the transitional behaviour. The following text describes the computations executed by the controller executing all of the postulated behaviours. 8.1.1

Initial Macrostep Computations

There are two variants of the TCIM motion command: TCIM-POSE and TCIM-velocity. In i+1 the TCIM-velocity variant the EDP_TRANS thread obtains the desired velocity E r˙(A)d from the ECP command argument. In the TCIM-POSE variant the end–effector frame is set to move by one macrostep from its current location at 0E iT to the desired one specified by 0E i+1Td (tab 2). Those computations are feasible and relatively simple for 19

variant 1

0 E

variant 2

IMC

nQ

T

T

Fa nv

r

0 Eι +1 c

0 Eι c

Dl E

i +1 y ce j

T m AM

nQ

0 Eι ( M ) m

AM−1

*

ι

E Eι

ι +1

E

r

0 Eι +1 ( M ) c

r

+

motor control

Tc

τι +1

EDP_ SERVO

StGen

Eι +1 ( A ) c

Faι

EDP_FORCE F/T conv

0 Wι

-

AA

Sι

Fmι

Manipulator

Reply

EDP_TRANS

F/T Sensor

c

i x ej

Interpret

Macrostep generation

EDP_ MASTER

Macrostep Step Figure 11: Motion/Interaction controller, where: Interpret – the ECP command interpretation, Reply – sends the status to the ECP, StGen – step generator, IMC – initial macrostep computations in EDP_TRANS thread, F/T conv – force/torque reading conversion, ∗ – homogeneous matrix multiplication, Dl – one step delay 6-DOF manipulators. representation motor (M ) joint (J) xyz Euler zyz (E) xyz angle and axis (A) homogoneous matrix

absolute relative i 0 0 AM (E i+1 r(M )d ) AM (E i r(M ) +E E i+1 r(M )d ) i AJ (0E i+1 r(J)d ) AJ (0E i r(J) +E E i+1 r(J)d ) 0 Ei AE (0E i+1 r(E)d ) E iT AE (E i+1 r(E)d ) 0 Ei AA (0E i+1 r(A)d ) E iT AA (E i+1 r(A)d ) Ei 0 0 E i+1Td E iT E i+1Td

Table 2: The desired end–effector pose (represented as

0 E i+1Td )

computation

i

Using 0E iT and 0E i+1Td the incremental frame E E i+1 Td is computed. However, this motion has i i+1 to be interpolated. The desired general velocity E r˙(A)d as defined by (22) is computed to specify the motion during interpolation: E i i+1 r˙(A)d

=

E i 0 i+1 (E r˙(A)d )

=

Ei E i+1 r(A)

∆t ns

i

E 0 −1 0 A−1 A−1 A (E i+1 Td ) A (E i T E i+1 Td ) = = ∆t ns ∆t ns

(27)

where the servo sampling period ∆t is equal to the step duration. Generally the quantities with the right superscript i + 1 are constant during the whole macrostep execution. 8.1.2

Computations for Each Step of a Macrostep

In each step the adjusted general force vector (see sec. 8.2.3) 0W ι Faι is computed with ι respect to frame E by using (21) and (24), i.e. E Faι results. When ι = nQ this reading is used as the reply data sent to the ECP in response to the QUERY command. 20

ι

i+1 i+1 In the TCIM-velocity variant, the general velocity E r˙(A)d is treated as E E r˙(A)d . For the ι i+1 ι TCIM-POSE variant, the general velocity E E r˙(A)d is computed in relation to frame E by ι i+1 using (20). The E E r˙(A)d is the standardized representation of the desired trajectory used in the following computations. This is the desired velocity of the end–effector E expressed with respect to the current end–effector frame E ι by using formula (20). ι Previous computed velocity 0W ι−1 r˙(A)c is expressed with respect to the end–effector frame ι Eι ι E to produce r˙(A)c using (20), (24). The frame E ι+1 is the desired pose for the next step (E ι+1 becomes the current end–effector pose E ι after execution of this step). Both the ι that are memorized in relation to frame 0 are expressed force 0W ι Faι and velocity 0W ι r˙(A)c ι ι with respect to frame W and need transformations (21), (20), (24) to obtain E Fa[l] and Eι ι r˙(A)c[l] . The tool can change, so the frame W was chosen as the reference frame.

In the following the right subscript part in square brackets denotes a coordinate of a vector. The vector components will be referred to by x, y, z (linear coordinates) and ax , ay , az (angular coordinates). The control law is formulated for each direction of motion separetly. The superposition of ι ι+1 control laws (31), (37), (38) results in the compound pose/force output velocity E r˙(A)c[l] for each axis:  Eι

ι+1 r˙(A)c[l]

=

B[l]



Eι



ι

i+1 ι Fd[l] −E Fa[l] +

E ι i+1 E r˙(A)d[l]



∆t + B[l] I[l]

Eι

ι r˙(A)c[l]

∆t + B[l] I[l]

(28)

The derivation of this control law is presented in section 8.1.3. The units of the control law parameters are presented in tab. 3. coordinate x, y, z ax , ay , az

b Fd r˙(A)d u, g, c [N ] [ ms ] u, g, c [N m] [ 1s ]

I B s ] [kg] [ kg s 2 [kg m ] [ kg m2 ]

Table 3: ECP command parameters and their units

The behavior definition parameter of the ECP command is specified for each coordinate and influences the above control law formulation: • UNGUARDED MOTION – where B[l] is set to zero (damping becomes infinite for the force portion of the control law, i.e. force does not cause any displacement), • CONTACT – where the desired velocity • GUARDED MOTION – the desired force The computed velocity

Eι

E ι i+1 E r˙(A)d[l]

becomes zero,

Eι

i+1 Fd[l] is set to zero.

ι+1 r˙(A)c is transformed into the desired step increment Eι E ι+1 r(A)c

=

Eι

ι+1 r˙(A)c ∆t

Eι E ι+1 r(A)c :

(29)

For of the next step computations the computed velocity is memorized as  the purpose  ι 0 W ι ι+1 Eι r˙(A)c . Then the homogeneous transformation matrix E E ι+1 Tc = AA (E ι+1 r(A)c ) is computed. The end-effector pose setpoint for the current step of the macrostep execution is described by 0E ι+1 Tc : 0 0 Eι (30) E ι+1 Tc = E ι Tc E ι+1 Tc 21

The desired motor position vector 0E ι+1 r(M )c is the solution of the inverse kinematics problem for 0E ι+1 Tc . The vector 0E ι+1 r(M )c is a part of the command sent to the motor controllers implemented in the EDP_SERVO thread. Motor controllers force the motors to move in the direction of the setpoint by delivering the vector τ ι+1 consisting of the values of PWM duty cycles. The end-effector pose that is delivered to the ECP, for the purpose of high-level feedback, can originate either directly with the encoder measurements, that are subsequently transformed by the procedure solving the direct kinematics problem (variant 1), or it is simply the rewritten set value for step nQ (variant 2). 8.1.3

Derivation of the Control Law

The control law is derived for the plant (fig. 12) containing the manipulator, its axis motor regulators and software modules responsible for direct and inverse kinematic transformation. For the purpose of this derivation a crude assumption is made that the plant is linear. This control law is implemented by the step generator StGen and it differs for each of the three elementary behaviors (UNGUARDED MOTION, CONTACT, GUARDED MOTION). Vd Fd +

εF

Vc

StGen

(G )

-

kinematic transformations, force transformations, motor control, manipulator and environment

Fa

Figure 12: The simplified controller model, where: G - transmittance The signals in fig. 12 are as follows: the desired force Fd = EFd[l] , the adjusted force measurement Fa = EFa[l] , the force error εF = EFd[l] −EFa[l] , the desired velocity Vd = Eι Eι r˙(A)c[l] . For the purpose of stability analysis E r˙(A)d[l] and finally computed velocity Vc = the signals were represented in continuous domain (s) and then for the purpose of control law formulation in discrete domain (z). Hence the StGen block can be represented in three forms: discrete control law (the way it is implemented), discrete transform G(z) and continuous transform G(s). An UNGUARDED MOTION (pure pose control) does not need force feedback, hence the control law formulation is as follows: Eι

ι+1 r˙(A)c[l] =

E ι i+1 E r˙(A)d[l]

⇒ Vc = Vd

This control law simply converts the desired velocity E ι ι+1 r˙(A)c[l] ,

E ι i+1 E r˙(A)d[l]

(31) into the computed velocity

To sustain CONTACT (pure force control) only force feedback is needed. To achieve this, in the initial experiments the control law was formulated as: Eι

ι+1 r˙(A)c[l] = B[l]



Eι

i+1 Fd[l] −

Eι



ι Fa[l] = B(Fd − Fa )

(32)

namely damping control. A similar approach (impedance based indirect force control) is presented in [47]. Unfortunately the inertial forces in the manipulator end–effector affect the force measurement [33]. If there is no estimation of these forces, because of the lack of the tool model and acceleration measurement, and the force measurement is not corrected, the resonance peak occurs for some frequencies of the input signal εF . To avoid the influence of this phenomenon a new control law was developed using frequency 22

analysis method in continuous domain and then transformed into the discrete domain. An adequately short sample period ∆t was chosen to make this transformation valid. For the simple control law (32) G(z) = B and, after transformation for the purpose of the analysis into continuous domain, G(s) = B. The identification experiments showed an approximately linear behavior of the plant. The analysis was performed and the inertial block: B[l] (33) G(s) = B[l] I[l] s + 1 was chosen to perform low pass filtering, instead of the PI controller, commonly used in force feedback. PI controller also constrains high frequency response, but the systematic force measurement error, which cannot be removed, can cause the manipulator to accelerate, in the case of commanding free motion (i.e., without contact), because this force error is integrated. The transmittance (33) can be transformed into discrete form: G(z) =

B[l] ∆t B[l] I[l] (1 − z −1 ) + ∆t

(34)

using a simple backward rectangular transformation [5]: 1 − z −1 (35) ∆t The goal was to obtain a control law simple to implement, compute and extend. Hence transformation (35) was used instead of the Tustin’s method (bilinear transformation), producing more complex results. Then, after some simple algebraic transformations, the following equation can be written. It is the base for the final control law formulation: s=

(∆t + B[l] I[l] )zVc (z) − B[l] I[l] Vc (z) = B[l] ∆t zεF (z)

(36)

and finally, after some more simple transformations and symbol substitutions as in fig. 12, the adjusted control law is obtained: Eι

ι+1 r˙(A)c[l]

=

B[l]



Eι



ι

i+1 ι Fd[l] −E Fa[l] ∆t + B[l] I[l]

Eι

ι r˙(A)c[l]

∆t + B[l] I[l]

(37)

Finally for GUARDED MOTION (pose control with desired compliance) the control law is ι i+1 analogical to (37), but the desired force E Fd[l] is set to zero and the desired velocity ι E i+1 E r˙(A)d[l] is set to some value, hence:  Eι

ι+1 r˙(A)c[l]

=



ι



ι B[l] −E Fa[l] +

E ι i+1 E r˙(A)d[l]



∆t + B[l] I[l]

∆t + B[l] I[l]

Eι

ι r˙(A)c[l]

(38)

The superposition of control laws (31), (37), (38) results in the compound pose/force ι ι+1 output velocity E r˙(A)c[l] for each axis (28).

8.2

Measurement of Force/Torque

The force sensor can be treated both as an exteroceptor and a proprioceptor. However in the following we concentrate on the latter role, hence the adjusted force measurement E Faι becomes a part of the agent’s effector image: x cej

ι

= h E Faι , •i 23

(39)

where here and in the following text • denotes irrelevant components of x cej . The EDP_FORCE thread (fig. 11) using (21), (24) transforms the raw force/torque measurement ι vector S Fmι into the vector 0W ι Faι (sec. 8.2.3) during the task execution. One of the EDP_FORCE thread’s tasks is the removal of the gravity force influence on the force/torque measurements. Hence, the tool weight w (combined with the held object weight) and gripper mass center translation W G P are taken into account. These parameters can be experimentally obtained by the procedure described in sec. 8.2.1. Sometimes the same parameters can be set based on the model of an a priori known gripper and the held object, which can be identified, e.g., by a vision subsystem. With the introduction of a new w and W G P parameters the compensation force must be computed (sec. 8.2.2) to obtain the adjusted force readings. This is commanded by the ECP, while the manipulator is at stand-still and there is no contact with the environment. 8.2.1

Gripper Mass Center Computation

b)

a)

c)

Z

X X

(G)

(W)

(S) (W)

Y

(S)

N

(G)

b [ ax ]

b [z]

Fga

Z

Y

F

(S)

F

X

Z

b g

Y

(W)

N[ca y ]

(G)

Fgc

N[ba y ]

Figure 13: Gripper mass center computation [58] Fig. 13 presents three phases of gripper mass center computation. The following four parameters are identified: the tool weight w (in Newtons) and the three coordinates of the gripper mass center translation W G P. For the purpose of the following measurements the end–effector frame E should be set as the wrist frame W . Initially the end effector is made to point downward (fig. 13 a). Then the bias operation is executed. This causes the current reading of the sensor to become 06×1 . After that the end–effector is made to point upward (fig. 13 b) thus the tool weight w can be easily computed as: W b F[z] w= (40) 2 b where WF[z] is the gravitational force reading measured by the sensor in phase b. The W W G P[y] and G P[x] coordinates are computed in the same phase: W W G P[x]

=−

N[ab y ] 2w

W

,

W G P[y]

=

N[ab x ] 2w

where WN[ab y ] is the torque about the wrist frame y axis measured in phase b and is the torque about the x axis measured in phase b.

24

(41) W

N[ab x ]

Then the manipulator moves the end–effector to a horizontal orientation, where both y and z axes are located in the horizontal plane (fig. 13 c), and W G P[z] is calculated: W W G P[z]

8.2.2

=

N[ac y ] w

W

−

N[ab y ]

(42)

2w

Compensation Force Computation

The compensation force computations are commanded by the ECP, at some instant of time ic while the manipulator is standing still, and are executed in the EDP to obtain the ic compensation force W Fric . These computations involve gripper mass center G and wrist W reference frames. The general gravity force vector applied at the gripper mass center is expressed in relation to frame 0 as: 0 ic G ic F g

= [0, 0, −w, 0, 0, 0]T

(43)

ic

The 6 × 6 W Gic ξF matrix which transforms the general force vectors acting on the gripper ic W ic mass center into the wrist coordinates W is derived from W Gic T using (24). The Gic T frame depends on the mechanical structure of the manipulator end-effector (geometric relationship between the gripper mass center and the wrist). The general compensation force (reaction to the gravity force) in sensor coordinates can be written as: ic W i c ic G i c ic G ic 0 ic W ic (44) Fr = − G Fg = − W ic ξF Gic ξF 0 ξ∗ Gic Fg During compensation force computation a new offset level is defined (bias). It causes the ic current raw force and torque reading Fm to become zero and influences the future force ic ι and torque raw readings Fm . These readings are distorted by W Fric (by the value of bias). 8.2.3

Force/Torque Measurement Transformations During Motion Execution ι

Another set of transformations is performed when each new raw reading S Fmι arrives. ι The raw force measurement S Fmι is transformed by using (24) into frame W to produce ι W Fmι . The kinematic model of a manipulator, gripper mass center parameters and sensor calibration are imprecise, hence the computational procedure works well for the orientation of the end–effector close to the orientation, in which the compensation force was W recently computed (sec. 8.2.2). The frame W S T and matrix S ξF depend on the mechanical structure of the manipulator end-effector (geometric relationship between wrist and sensor) and do not change during task execution. The adjusted force with reference to the frame W is calculated: W

ι

ι

Faι = −( W Fmι −

W

ι

Fgι −

W ic

ι

Fric ) = −( W S ι ξF

Sι

Fmι −

Wι Gι 0 ι G ι ξF 0 ξ∗ G ι Fg

−

W ic

Fric ) (45)

ι

where S Fmι is the current force measurement obtained directly from transducer interface, 0 ι T G ι Fg = [0, 0, −w, 0, 0, 0] is the current gravity force influence on the gripper mass center ic and finally W Fric is the compensation force „memorized” in the transducer interface during compensation force computation (i.e. bias executed at time instant ic ). The minus sign is used to transform the adjusted force into the manipulator end–effector reference frame instead of the external obstacle reference frame. Finally the general adjusted force acting at W is expressed with relation to frame 0 as 0 ι W ι Fa . 25

9

Exemplary Application and Its Generators

In general in the proposed approach, the control system is decomposed into distinct agents (this is the first tier of decomposition which takes into account the distribution of functions between the agents, and the volume of inter-agent communication required). An agent, in general, is responsible for control of its effector, perception of the environment for the purpose of its effector control, and inter-agent communication. It is up to the system designer to decide how the effectors or sensors are grouped. However this grouping can be specified formally and the advantages and drawbacks of each grouping can be discussed at a very early stage in the project. The behaviour of the agent is governed by its set of transition functions (forming the lower tier), which are clearly defined in terms of the input and output quantities described above. Thus the specification of the control system is two tier – the upper tier is defined by the flow of information between the agents and the lower tier is defined by the formal specification of each agent’s behaviour (influence on the environment, gathering sensor readings, production and consumption of the information for/from the other agents). In our view, in the case of complex systems, this approach enables the discussion of many possible structures of the system, in terms of well defined entities (embodied agents, transition functions). This approach was successful in the specification and implementation of many experiments that we have performed on systems having effectors and sensors (and one of them is described below in detail). Thus not only diverse tasks were specified formally by the proposed method, but also those specifications significantly facilitated the implementation of the controllers executing those tasks. This was due to the fact that the images and transition functions used in formal task specification can be directly transformed into the corresponding code expressed in a programming language (MRROC++ programming framework used in this work utilizes C++). Currently this transformation is done manually by the implementation team, however work is under way to automate this transformation. For that purpose a graphical tool is being designed. MRROC++ has been validated by extensive use in control of diverse robots executing very differing tasks. Among others the experiments have been conducted on a Rubik’s cube puzzle. Rubik’s cube puzzle has been chosen as a benchmark task, as it requires of the robot all capabilities that one would expect a service robot to have (two-handed manipulation, visual servoing, force-control, logical reasoning etc. [46]). Also all of the behaviors (see sec. 4) executed by the manipulators were used in the system. To the best of our knowledge, only a few attempts to solve the Rubik’s cube puzzle using robots have been made, e.g. [27, 34, 42]. The majority of these robots are specialized devices that had been designed by robotics students [34] or robot enthusiast [42]. The most advanced design RuBot II is a two-handed, pneumatically controlled robot with vision and speech synthesis system [42]. Also, the Kawasaki Heavy Industry Group has presented their dual-arm robot solving Rubik’s cube [27]. In the experiments the forces and torques measured by the force/torque sensor mounted in the wrist of the robot initially holding the cube were monitored. The data for the plots was gathered by the self-monitoring thread of the EDP, while the two-arm system composed of two modified IRb-6 robots (fig. 14) was manipulating the Rubik’s cube. During the task execution all of the three behaviours are used. When no simultaneous contact occurs between the two end-effectors and the cube or between one of the end-effectors and the cube held by the operator, the pure motion control is used (UNGUARDED MOTION). When such contact is present or is expected to occur, the two remaining behaviours are used. The high level task algorithm producing the required sequence of motions is based on iterative depending A* algorithm [43] with tabelarized heuristic function based on the 26

a

b

c

Figure 14: End–effectors of two robots manipulating a Rubik’s cube: a – approaching the face of the cube, b – closing of the gripper, c – turning the face approach used by Korf [32]. This algorithm finds an optimal solution to the puzzle within reasonable time if it is not more then 14 face rotations from the initial cube state. If more motions are necessary, the solution time increases significantly, hence the solution finding algorithm is switched to Kociemba’s sub optimal two phase algorithm [31]. The system consists of three agents: two embodied agents a1 , a2 (j = 1, 2) each controlling one robot arm (treated as an effector) and the system coordinator a0 (j=0). There are three subtasks in the presented application. Each subtask needs two functions m fcj , hence the left superscript m is: • m = 1, 2 – attaining direct contact with the cube, • m = 3, 4 – grasping the cube, • m = 5, 6 – turning a face. Six functions m fcj , m = 1, . . . , 6, are defined later. The three enumerated subtasks repeated as many times as it is necessary to turn a face of the cube suffice to execute any Rubik’s cube manipulation task. Thus only the transition functions for m = 1, . . . , 6 have to be defined.

9.1

General Structure of Images

To formally define the transition functions for the motions required by the task of manipulating the Rubik’s cube by two robot hands, the data structures contained within the control subsystems of the agent’s images must be defined more precisely. In general the images and buffers of an agent are shared by all of the subtasks. The state of the internal data structures ccj is represented by a structure containing nccj variables: ccj = hccj[1] , . . . , ccj[nccj ] i

(46)

Analogically input effector image x cej consists of nexj variables: x ce j

= hx cej[1] , . . . , x cej[nexj ] i 27

(47)

The input virtual sensor image x cVj contains nV xj individual sensor readings: x cVj

= hx cVj1 , . . . , x cVjn

V xj

i

(48)

where each of those readings has the following structure: = hx cVjk [1] , . . . , x cVjk [nV xjk ] i

x cVjk

(49)

Each input transmission buffer x cTjj0 consists of nT xjj 0 variables: x cTjj 0

= hx cTjj0 [1] , . . . , x cTjj0 [n

]

T xjj 0

i

(50)

The agent aj contains as many such input transmission buffers as there are direct connections with other agents aj 0 . The output effector image y cej consists of neyj variables: y ce j

= hy cej[1] , . . . , y cej[neyj ] i

(51)

The output virtual sensor image y cVj contains nV yj individual sensor commands: y cVj

= hy cVj1 , . . . , y cVjn

V yj

i

(52)

where each of those commands has the following structure: = hy cVjk [1] , . . . , y cVjk [nV yjk ] i

y cVjk

(53)

Output transmission buffer y cTjj0 consists of nT yjj0 variables: y cTjj 0

= hy cTjj0 [1] , . . . , y cTjj0 [n

T yjj 0

]

i

(54)

The agent aj contains as many such output transmission buffers as there are direct connections with other agents aj 0 . Generally each input transmission image of agent aj corresponds to the output transmission image of agent aj 0 and vice versa: x cTjj 0

− y cTj0 j ,

y cTjj 0

− x cTj0 j

(55)

Some types of agents do not have all of the images enumerated by (46) - (54). In general the lack of an image is equivalent to the respective number n being equal to 0. In the case of heterogeneous agents each one of them has a different structure of respective images.

9.2

Structure of the Images Required by the Rubik’s Cube Manipulation Task

In the following the variables, described in general by (46) - (54) are presented for all of the mentioned agents. Variables within each agent are shared by all of the previously mentioned subtasks. The input virtual sensor image x cVj is not used by the three agents: x cVj

= h•i

(56)

Thus nV x0 = nV x1 = nV x2 = 0 for each of those agents. The output images of virtual sensors y cVj are also not used, so in this example the agents do not communicate with their virtual sensors. y cVj

= h•i

Thus nV y0 = nV y1 = nV y2 = 0. Hence there is no need to define m fcVj . 28

(57)

9.2.1

Images of Agents a1 and a2

The input effector images x cej contain nexj = 5 data items, (j = 1, 2): i x cej[1] i x cej[2] i x cej[3] i x cej[4] i x cej[5]

i = ggm j = gbij i = E Faij = 0E i Tmj = i

(58)

Input transmission buffers x cTj0 of agents a1 and a2 consist of two elements each (nT xj0 = 2, j = 1, 2) the variables holding the Boolean values sent by the coordinator a0 . Those values decide whether the currently used transition function m fcj should be further employed or should be switched to another one. x cTj0[1]

– the variable used by the odd transition functions,

x cTj0[2]

– the variable used by the even transition functions.

All of the data that must be memorized is extracted from input images x cej and is stored in ccj , where nccj = 3, j = 1, 2. ccj[1] – memorizes a certain time instant, ccj[2] – memorizes the input data contained in x cej[1] , i.e., the measured gripper jaw distance, ccj[3] – memorizes the input data contained in x cej[4] , i.e., the end–effector pose. The output effector images y cej contain neyj = 10, j = 1, 2, components of the command sent to the effector. Their contents are defined by (59). The agents a1 and a2 inform the coordinator that a certain event has occurred. This is done through the output image of transmitters y cTj0 consisting of a single Boolean variable for each one of them, thus nT yj0 = 1, j = 1, 2. – Boolean variable used for notification of the coordinator that a certain event has occurred. y cTj0

9.2.2

Images of the Coordinator - Agent a0

Input transmission images x cT0j from agents a1 and a2 consist of a single variable each. Thus nT x0j = 1, j = 1, 2. All of the data that must be memorized is stored in cc0 , where ncc0 = 2. The cic0[1] , cic0[2] Boolean variables inform whether the currently used m fc0 function should be switched or not. cc0[1] – the variable used by the odd transition functions, cc0[2] – the variable used by the even transition functions. 29

Output transmission buffers y cT0j to agents a1 and a2 consist of two elements each, thus nT y0j = 2, j = 1, 2. y cTj0[1]

– the variable used by the odd transition functions,

y cTj0[2]

– the variable used by the even transition functions.

The agents a1 and a2 do not communicate with each other directly. They exchange information with the coordinator a0 . Only Boolean data is transfered between the agents.

9.3 9.3.1

Transition Functions and Terminal Conditions Transition Functions and Terminal Conditions Shared by All Subtasks

m 0 The contents of output effector images y ci+1 ej of agents a1 and a2 are produced by f cej :

m 0 f cej (x cij )

,

                                                  

i+1 y cej[1]

= mbj

i+1 y cej[2]

= mFdj

i+1 y cej[3]

= mr˙(A)dj

i+1 y cej[4]

= mIj

i+1 y cej[5]

= mBj

i+1 y cej[6]

= nsj = const

i+1 y cej[7]

= nQj = nsj − 1 = const

i+1 y cej[8] = µj = TCIM-velocity i+1 m i i+1 y cej[9] = ggdj = x cej[1] + ∆ ggdj i+1 W i+1 = const y cej[10] = E Tdj

(59)

= const

where j = 1, 2, , indicates the function definition symbol, and „const” means that the described value does not change during the whole task execution. It should be noted that the structure of functions m f 0cej , m = 1 . . . 6, are the same, but for each subtask (designated by m) the parameters of those functions differ. The tool does not change – the end–effector frame is located in the middle of the space between the gripper jaws (fig. 6). Its y axis points along the gripper jaws clamping motion and z is the rotational axis of the last manipulator degree of freedom (all frames are dextrogyrate). All of the terminal conditions m fτj have a similar structure for agents a1 and a2 , and are defined as:  i  0 for x cTj0 [l] = 0 m (60) fτj (x cij ) ,  1 for x ciTj0 [l] = 1 where j = 1, 2, l = 1 for odd m and l = 2 for even m. For the task of manipulating the Rubik’s cube the coordinator is responsible for designating the instants at which the embodied agents should switch the transition functions. Thus sporadic coordination results. It is event driven. However the switch triggering event is detected by either of the embodied agents, so first the agent that has detected the event notifies the coordinator and then the coordinator signals to both embodied agents that they should switch to other transition functions, so this is done simultaneously by both of them. 30

All of the terminal conditions m

m

fτ0 for the coordinator a0 are similar and are defined as:

fτ0 (x ci0 ) ,

 

0 for cic0[l] = 0



1 for cic0[l] = 1

(61)

where l = 1 for odd m and l = 2 for even m. The coordinator’s function m f 0 cc0 checks whether the agent aj signaled an event. It prepares the data for the coordinator’s terminal condition:

m 0

f

i cc0 (x c0 )

,

    

ci+1 c0[k] = 0

for

i x cT0j[1]

=0

ci+1 c0[k] = 1

for

i x cT0j[1]

=1

   

ci+1 c0[3−k] = 0

(62)

where k = 1 for odd m and k = 2 for even m. For m = 1, 2, 5, 6 j = 1 while for m = 3, 4 j = 2. The function m f 0 cT0 is similar to m f 0 cc0 and prepares the output buffers for agents a1 and a2 , subsequently causing the change of the function m fcj used by those agents:

m 0

f

i cT0 (x c0 )

,

 i+1    y cT0j 0 [k]  

i+1 y cT0j 0 [k]

=0 =1

     y c i+1 T0j 0 [3−k]

for

i x cT0j[1]

=0

for

i x cT0j[1]

=1

(63)

=0

where j 0 = 1, 2, k = 1 for odd m and k = 2 for even m. For m = 1, 2, 5, 6 j = 1 while for m = 3, 4 j = 2. The method of general task decomposition is as follows. At each time instant i all of the three agents use functions with the same index m – m fcj (j = 0, 1, 2). One of the embodied agents either a1 or a2 checks, using the function m f 0 cT (j = 1, 2), whether j a specified criterion is satisfied (an event occurred). This function sets the variables of the output transmission image y cTj0 to signal to the coordinator whether this criterion is satisfied or not. The coordinator checks the appropriate input image x cT0j to detect this event both within functions m f 0 cc0 and m f 0 cT0 . The function m f 0 cT0 prepares the content of the output images y cT0j to be sent to the embodied agents a1 and a2 , thus causing the change of the transition functions in both agents at the same time. The role of the function m f 0 cc0 is to prepare the internal variable in the coordinator which will enable the terminal condition m fτ0 to terminate the currently utilized function m f 0 c0 in the next macrostep (i + 1). The terminal conditions of the embodied agents a1 and a2 depend on the values obtained from the output transmission image of the coordinator (y cT0j ). These values are received by the input transmission images x cTj0 of the embodied agents. Thus sporadic coordination is implemented. In the following the transition functions essential for each subtask are presented. 9.3.2

Approaching the Cube

The robot represented by agent a2 holds still the cube and the other one (a1 ) approaches the cube to grasp it. The phase specific parameters of y ci+1 ej output image are presented in tab. 4. The contents of this image form a command that is accepted by the control hardware of the effector (in MRROC++ this is the EDP command).

31

It turns out that structurally the definitions of functions 1 f 0cej and 2 f 0cej are the same for both robots, thus: 1 0 f cej , 2 f 0cej for j = 1, 2 (64) They only differ in constant parameters they use. The structure of these functions is presented by (59), while the sets of parameter values are shown in tabular form in tab. 4. (a) Parameters of 1 f 0ce1 (robot approaching the cube) 1

b1

1

Fd1

1

1

r˙(A)d1

1

I1

(b) Parameters of 1 f 0ce2 (robot holding the cube) 1

B1

b2

1

1

Fd2

r˙(A)d2

1

I2

1

B2

x

u

-

0

-

-

x

u

-

0

-

-

y

u

-

0

-

-

y

u

-

0

-

-

z

g

-

0.01

5

0.0025

z

u

-

0

-

-

ax

u

-

0

-

-

ax

u

-

0

-

-

ay

u

-

0

-

-

ay

u

-

0

-

-

az

u

-

0

-

-

az

u

-

0

-

-

∆1 ggd1

∆1 ggd2

0

0

Table 4: Parameters for guarded motion phase (fig. 16). Here and in the following tables the dash „-” represents the irrelevant values. The function 1 f 0 ccj is an identity function retaining the previously memorized data: 1 0

f

i ccj (x cj )

,

n

i ci+1 cj[l] = ccj[l] for l = 1, 2, 3, j = 1, 2

(65)

The function 1 f 0 cT1 detects the event consisting in exceeding the desired value of the exerted force (4N in this case), and prepares the information sent to the coordinator: 1 0

f

i cT1 (x cj ) ,

  y c i+1 T10[1]  c i+1 y T10[1]

= 0 for

i x ce1[3][z]

≤4

= 1 for

i x ce1[3][z]

>4

(66)

In this phase of the execution of the subtask the coordinator uses the functions 1 f 0 cc0 (62) and 1 f 0 cT0 (63) with the parameter j = 1. In the second phase function 2 f 0 cc1 memorizes the time instant at which this phase of motion started:  i+1 i    cc1[1] = x ce1[5] for i = i0 2 0

f

i cc1 (x c1 ) ,

    

i ci+1 c1[1] = cc1[1]

for i 6= i0

i ci+1 c1[l] = cc1[l]

∀i ∧ l = 2, 3

(67)

The second embodied agent does not need to memorize any new data, hence the function 2 0 f cc2 is defined as: 2 0

f

i cc2 (x c2 )

,

n

i ci+1 c2[l] = cc2[l] for l = 1, 2, 3

32

(68)

The function 2 f 0 cT1 is used to wait for a certain duration of time defined by id to partially stabilize the force exerted on the the cube surface. Once this time elapses the coordinator is signaled that this event has occurred.

2 0

f

i cT1 (x cj ) ,

  y c i+1 T10[1]  c i+1 y T10[1]

= 0 for

i x ce1[5]

− cic1[1] < id

= 1 for

i x ce1[5]

− cic1[1] ≥ id

(69)

In this phase of the execution of the subtask the coordinator uses the functions 2 f 0 cc0 (62) and 2 f 0 cT0 (63) with the parameter j = 1. 9.3.3

Grasping the Cube

Cube grasping starts with one of the manipulators initiating the closing of the gripper jaws to catch the cube already held by the other manipulator or the operator. The other manipulator keeps constant pose (in the same way as the first manipulator in the first subtask). The cube grasping subtask is executed after the grasping manipulator has come into contact with the cube(fig. 14b). Gripper closing is divided into two stages: • The grasping manipulator is compliant in one direction (m = 3) • The grasping manipulator is compliant in two directions (i.e., in a plane) (m = 4) The grasping manipulator starts to decrease the distance between the gripper jaws. The grasping pose is only roughly known, so during gripper closing high tension can occur. Hence, force compliance is introduced in the xy plane of the end-effector frame to reduce the tension while the gripper jaw touches the cube. The problem with force compliant motions is, especially when there is no contact with the environment, that the force error is non–zero. This is caused by the noise in the force measurements and sensor calibration errors. The manipulator slowly reacts to those artificial forces. That is why in the first stage, while the gripper closes, the manipulator is compliant only along one axis (namely the y axis). This is introduced to avoid undesirable motions (tab. 5). There is no need to memorize any new data, hence the function 3 f 0 ccj is the same as 4 f 0 ccj , so both are defined as: 3 0

f

i ccj (x cj )

, 4 f 0 ccj (x cij ) ,

n

i ci+1 cj[l] = ccj[l] for l = 1, 2, 3, j = 1, 2

(70)

The function 3 f 0 cT2 checks whether the current gripper jaws distance exceeds the desired value (0.06 m) in agent a2 : 3 0

f

i cT2 (x cj )

,

  y c i+1 T20[1]  c i+1 y T20[1]

= 0 for

i x ce2[1]

> 0.06

= 1 for

i x ce2[1]

≤ 0.06

(71)

In this phase of the execution of the subtask the coordinator uses the functions 3 f 0 cc0 (62) and 3 f 0 cT0 (63) with the parameter j = 2. The second stage of gripper closing starts when the gripper reaches the desired distance between the jaws. Then both axes (x and y) become compliant, but the gripper still 33

(a) Parameters of 3 f 0ce1 3

x y z ax ay az

b1 u u u u u u

3

Fd1 -

3

r˙(A)d1 0 0 0 0 0 0

∆3 ggd1

3

I1 -

(b) Parameters of 3 f 0ce2 3

3

B1 -

x y z ax ay az

3

b2 u c u u u u

Fd2 0 -

3

r˙(A)d2 0 0 0 0 0

∆3 ggd2

0

3

I12 20 -

3

B2 0.005 -

-0.001

Table 5: Parameters for the phase in which compliance in one axis during cube grasping is necessary. (b) Parameters of 4 f 0ce2

(a) Parameters of 4 f 0ce1 4

x y z ax ay az

b1 u u u u u u

4

Fd1 -

4

∆4 ggd1

r˙(A)d1 0 0 0 0 0 0

4

I1 -

4

4

B1 -

x y z ax ay az

b2 c c u u u u

∆4 ggd2

0

4

Fd2 0 0 -

4

r˙(A)d2 0 0 0 0

4

I2 20 20 -

4

B2 0.005 0.005 -

-0.001

Table 6: Parameters for inducing compliance along two axes during cube grasping

closes the jaws, until it reaches the desired distance between the jaws and finally catches the cube (tab. 6). The function 4 f 0 cT2 checks whether the cube is finally grasped (the gripper jaws are blocked):   y c i+1 = 0 for x c i T20[1] e2[2] = 0 4 0 i (72) f cT2 (x cj ) , i  c i+1 = 1 for x ce2[2] = 1 y T20[1] This event is detected by the effector taking into account the current in the gripper motor and the dislocation of the jaws. In this phase of the execution of the subtask the coordinator uses the functions 4 f 0 cc0 (62) and 4 f 0 cT0 (63) with the parameter j = 2. 9.3.4

Face Turning

Face turning can be initiated only when both manipulators properly hold the cube. The kinematic chain is closed then (fig. 14c, fig. 15). Now the grasping robot becomes immobile and motion controlled while the robot initially holding the cube is motion controlled in two rotational directions, all three translational directions are compliant (force controlled), while the direction of the face rotation is torque controlled (tab. 7). There is no need to memorize any new data in the embodied agent a2 for this subtask,

34

(a) Parameters of 5 f 0ce1 5

b1 c c c u u c

x y z ax ay az

5

Fd1 0 0 0 5

5

r˙(A)d1 0 0 -

∆5 ggd1

(b) Parameters of 5 f 0ce2

5

I1 20 20 20 0.5

5

5

B1 0.005 0.005 0.005 0.025

x y z ax ay az

b2 u u u u u u

5

Fd2 -

5

r˙(A)d2 0 0 0 0 0 0

∆5 ggd2

0

5

I2 -

5

B2 -

0

Table 7: Parameters for contact phase during turning of the face.

hence the function 5 f 0 cc2 is the same as 6 f 0 cc2 and both are defined as: 5 0

f

6 0 i i cc2 (x c2 ) , f cc2 (x c2 ) ,

n

i ci+1 c2[l] = cc2[l] for l = 1, 2, 3

(73)

Function 5 f 0 cc1 memorizes the a1 agent’s end–effector pose:

5 0

f

i cc1 (x c1 )

,

        

i ci+1 c1[3] = x ce1[4] for i = i0 i ci+1 c1[3] = cc1[3]

for i 6= i0

i ci+1 c1[l] = cc1[l]

∀i ∧ l = 1, 2

(74)

where i0 is the time instant, when the face turn was initiated. The function 5 f 0 cT1 checks whether the desired angle of rotation has been reached: 5 0

f

i cT1 (x cj )

,

  y c i+1 T10[1]  c i+1

y T10[1]

= 0 for = 1 for

E i0 E i r(A)m1 [az ] E i0 E i r(A)m1 [az ]

i −1 i o = argϕ (A−1 x ce1[4] )) < 90 A ((cc1[3] ) o −1 i i = argϕ (A−1 x ce1[4] )) ≥ 90 A ((cc1[3] )

(75)

where argϕ is the function extracting the angle of rotation out of the angle–axis representation of a transformation represented by r(A) .

Figure 15: Face turn In this phase of the execution of the subtask the coordinator uses the functions 5 f 0 cc0 (62) and 5 f 0 cT0 (63) with the parameter j = 1. In the next phase the gripper of robot 1 is being opened (both arms are immobile) (tab. 8). 35

(a) Parameters of 6 f 0ce1 6

x y z ax ay az

b1 u u u u u u

6

Fd1 -

6

6

r˙(A)d1 0 0 0 0 0 0

∆6 ggd1

I1 -

(b) Parameters of 6 f 0ce2 6

6

B1 -

x y z ax ay az

b2 u u u u u u

6

Fd2 -

6

r˙(A)d2 0 0 0 0 0 0

∆6 ggd2

0.001

6

I2 -

6

B2 -

0

Table 8: Parameters for gripper opening after turning

The function 6 f 0 cc1 memorizes the distance between the gripper jaws of agent a1 :

6 0

f

i cc1 (x c1 )

,

        

i ci+1 c1[2] = x ce1[1] for i = i0 i ci+1 c1[2] = cc1[2]

for i 6= i0

i ci+1 c1[l] = cc1[l]

∀i ∧ l = 1, 3

(76)

where i0 is the time instant in which the gripper of robot 1 starts opening after termination of the turn. The function 6 f 0 cT1 checks whether the kinematic chain is being opened (the distance between the gripper jaws increases by a certain amount (0.02 m)): 6 0

f

i cT1 (x cj )

,

  y c i+1 T10[1]  c i+1

y T10[1]

= 0 for

i x ce1[1]

− cic1[2] < 0.02

= 1 for

i x ce1[1]

− cic1[2] ≥ 0.02

(77)

In this phase of the execution of the subtask the coordinator uses the functions 6 f 0 cc0 (62) and 6 f 0 cT0 (63) with the parameter j = 1.

9.4

Results of Experiments

Fig. 16 presents three phases of motion: free motion (until point A in the plot), impact (section A to B) and contact (beyond point B in the plot). Initially the approaching robot is commanded to execute a guarded motion in the direction toward the cube (m = 1). Then in the static case the force should stabilize at the value of 4N (beyond point B) (m = 2). The visible oscillations (after point B) occur due to controller parameters and properties of the arms and the Rubik’s cube (stiffness, damping and inertia). The high frequency oscillations are mostly caused by the inertial force disturbing the force measurement, the low frequency oscillations are mostly due to compliance of the robot arms forming the closed kinematic chain. Fig. 17 presents the three phases of task realization. Initially the robots operate separately (until point A in the plot). Once the grasping operation starts the torque mounts. The cube holding robot (robot 1) is immobile and motion controlled while the other robot (robot 2) is compliant in the plane of the cube face that is to be rotated (all other directions of translation and rotation are motion controlled) (m = 3, 4). Once the cube is grasped securely the torque stabilizes (point B). 36

[N ] B 6

fo rc e

4

2

0

A 1 2 4 2 0 0

1 2 4 4 0 0

1 2 4 6 0 0

1 2 4 8 0 0

1 2 5 0 0 0

1 2 5 2 0 0

1 2 5 4 0 0

1 2 5 6 0 0

s te p

Figure 16: Force along the cube approach direction (step ∆t = 2ms) [N m ] D 3

2

E G

to rq u e

1

F A 0

H B C

-1

4 6 4 0 0 0

4 6 5 0 0 0

4 6 6 0 0 0

4 6 7 0 0 0

4 6 8 0 0 0

4 6 9 0 0 0

4 7 0 0 0 0

s te p

Figure 17: Torque around the axis of rotation of the face of the cube (step ∆t = 2ms) The face turn subtask starts at point C, where the rapid change in torque appears when the rotation of the cube face is initiated (m = 5). This torque is set to 5 Nm, however the lowest measured value is 3 Nm. Initially the rotated face was jammed – this can be seen from the plot: first the torque absolute value mounts (until point D), then it suddenly drops and becomes relatively constant (here mainly dry and viscous friction are the cause) – until point G. At point E the friction mounted quite quickly, however the motion was sustained and then gradually the friction dropped (until point F ). Then again the friction increased. This proves that the production process of the cube is far from perfect (perhaps this could be used in cube quality control). The turn ends at point G, when the desired face rotational angle is reached. Between points G and H the gripper of robot 1 is being opened and both manipulators stand still (m = 6). It is worth to mention that time instants i0 (i0 is the initial macrostep of transition function execution) for the function 5 f 0 ccj and for the function 6 f 0 cj relate to points C and G.

37

10

Conclusions

A formal approach to robot programming based on transition functions has been proposed. This formalism enables the specification of: • the high level tasks in terms of sequences of transition functions executed by the agents working on this task, • the low level control laws, • the elementary behaviors that can be used for the implementation of any high-level tasks, • the methods of coordinating multiple agents, and last but not the least, it implies the general structure of the control software, simultaneously not overconstraining the programmer as to the exact choice of software tools used for the implementation of the control system (e.g., programming language, operating system). Using this formalism three elementary behaviors sufficient to execute a diverse set of tasks were specified and implemented. The well known Task Frame Formalism has been extended to obtain the unified parallel motion/interaction control. The resulting control structure was implemented using MRROC++ robot programing framework. It was practically tested on: a two arm system solving the Rubik’s cube puzzle [57], the edge following benchmark, drawing reproduction system [56] and a haptic two arm system. However only the first task has been presented in this paper. The tests confirmed that the controllers executing tasks specific to service robots can be specified and implemented in the proposed way. Currently work is in progress on the development of a graphical tool enabling the formulation of specifications in a more user friendly way. Besides that visual servoing structures are being included into the MRROC++ framework. As this work targets service robots, which will have to learn to improve their actions in a partially structured environment, reinforcement learning capability is being introduced into this system.

References [1] Player/Stage documentation. http://playerstage.sf.net/, 2009. [2] R. Alami, R. Chatila, S. Fleury, M. M. Ghallab, and F. Ingrand. An architecture for autonomy. Int. J. of Robotics Research, 17(4):315–337, 1998. [3] R. Anderson and M. Spong. Hybrid impedance control of robotic manipulators. Robotics and Automation, IEEE Transactions on Robotics and Automation, 4(5):549– 556, 1988. [4] M. Ang Jr. Towards pervasive robotics: Compliant motion in human environments. International Journal of Software Engineering and Knowledge Engineering, 15(2):135–145, 2005. [5] K. Åström and B. Wittenmark. Computer-controlled systems. Prentice-Hall, Inc. Upper Saddle River, NJ, USA, 1997.

38

[6] P. Backes, S. Hayati, V. Hayward, and K. Tso. The kali multi–arm robot programming and control environment. In NASA Conference on Space Telerobotics, pages 89–7, 1989. [7] C. Blume and W. Jakob. PASRO: Pascal for Robots. Springer–Verlag, 1985. [8] C. Blume and W. Jakob. Programming languages for industrial robots. 1986. [9] A. Brooks, T. Kaupp, A. Makarenko, S. Williams, and A. Orebäck. Towards component-based robotics. In IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS’05), pages 163–168, August 2005. [10] A. Brooks, T. Kaupp, A. Makarenko, S. Williams, and A. Orebäck. Orca: A component model and repository. In D. Brugali, editor, Software Engineering for Experimental Robotics, pages 231–251. Springer, 2007. [11] H. Bruyninckx. OROCOS – Open Robot Control Software. http://www.orocos.org/, 2002. [12] H. Bruyninckx. The real–time motion control core of the OROCOS project. In Proceedings of the IEEE International Conference on Robotics and Automation, Taipei, Taiwan, pages 2766–2771. IEEE, September, 14–19 2003. [13] S. Chiaverini and L. Sciavicco. The parallel approach to force/position control of roboticmanipulators. Robotics and Automation, IEEE Transactions on, 9(4):361– 373, 1993. [14] T. Collett, B. MacDonald, and B. Gerkey. Player 2.0: Toward a practical robot programming framework. In Proceedings of the Australasian Conference on Robotics and Automation (ACRA), December 5–7 2005. [15] J. J. Craig. Introduction to Robotics, Mechanics & Control. Addison–Wesley, 1986. [16] J. De Schutter, H. Bruyninckx, W. Zhu, and M. Spong. Force control: a bird’s eye view. Control Problems in Robotics and Automation, pages 1–17, 1998. [17] S. Fleury and M. Herrb. Genom user’s guide. Report, LAAS, Toulouse, December 2001. [18] B. Gerkey, R. Vaughan, and A. Howard. The Player/Stage Project: Tools for MultiRobot and Distributed Sensor Systems. In Proceedings of the International Conference on Advanced Robotics, ICAR’03, Coimbra, Portugal, pages 317–323. June 30 – July 3 2003. [19] B. Gerkey, R. Vaughan, and A. Howard. Player, user manual ver.1.5. Robotics Laboratory Stanford University, Robotics Research Laboratory University of Southern California, Autonomy Laboratory Simon Frazer University, http://playerstage.sourceforge.net/doc/Player-manual-1.5.pdf, June 2 2004. [20] V. Hayward, L. Daneshmend, and S. Hayati. An overview of KALI: A system to program and control cooperative manipulators. In K. Waldron, editor, Advanced Robotics, pages 547–558. Springer–Verlag, Berlin, 1989. [21] V. Hayward and S. Hayati. Kali: An environment for the programming and control of cooperative manipulators. In 7th American Control Conference, pages 473–478, 1988.

39

[22] V. Hayward and R. P. Paul. Robot manipulator control under unix RCCL: A robot control C library. International Journal of Robotics Research, 5(4):94–111, Winter 1986. [23] N. Hogan. Impedance control: An approach to manipulation: Part I- theory. J. DYN. SYST. MEAS. CONTROL., 107(1):1–7, 1985. [24] T. Horie, K. Tanaka, N. Abe, and H. Taki. Remote force control of robot using PHANToM haptic model and force sensor. Proceedings of the IEEE International Symposium on Assembly and Task Planning, pages 128–135, 2001. [25] S. Jeong, T. Takahashi, and E. Nakano. A safety service manipulator system: the reduction of harmful force by a controllable torque limiter. In IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), volume 1, pages 162–167. IEEE, 2004. [26] S. Kaisler. Software Paradigms. Wiley Interscience, 2005. [27] Kawasaki. Intelligent dual-arm robot system for solving Rubik’s Cube. http://www.khi.co.jp/kawasakiworld/english/video_index_e.html, 2005. [28] O. Khatib. A Unified Approach for Motion and Force Control of Robot Manipulators: The Operational Space Formulation. International Journal of Robotics and Automation, RA-3(1):43–53, February 1987. [29] O. Khatib. Inertial Properties in Robotic Manipulation: An Object-Level Framework. The International Journal of Robotics Research, 14(1):19–36, 1995. [30] O. Khatib, K. Yokoi, K. Chang, D. Ruspini, R. Holmberg, and A. Casal. Coordination and decentralized cooperation of multiple mobile manipulators. Journal of robotic systems, 13(11):755–764, 1996. [31] H. Kociemba. Cube Explorer. http://kociemba.org/cube.htm, 2009. [32] R. Korf. Sliding-tile puzzles and Rubik’s Cube in ai research. IEEE Intelligent Systems, 14(11):8–12, Nov. 1999. [33] T. Kroger, D. Kubus, and F. M. Wahl. 12D force and acceleration sensing: A helpful experience report on sensor characteristics. In IEEE International Conference on Robotics and Automation, ICRA 2008., pages 3455–3462, 2008. [34] D. Li, J. Lovell, and M. Zajac. Rubik’s Cube Solver. http://www.eecs. umich.edu/courses/eecs373/Labs/Web/W05/Rubic Cube Web/top.html, 2005. [35] J. Lloyd and V. Hayward. Real-time trajectory generation in Multi-RCCL. J. of Robotics Systems, 10(3):369–390, 1993. [36] J. Lloyd, M. Parker, and R. McClain. Extending the RCCL Programming Environment to Multiple Robots and Processors. pages 465–469, 1988. [37] A. Makarenko, A. Brooks, and T. Kaupp. Orca: Components for robotics. In IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS’06), pages 163–168, October 2006. [38] M. Markiewicz and C. de Lucena. Object oriented framework development. ACM Crossroads New York, NY, USA, 7(4):3–9, 2001.

40

[39] A. McGettrick. The Definition of Programming Languages. Cambridge University Press New York, NY, USA, 1980. [40] K. Mianowski. Analiza właściwości chwytaka specjalnego przeznaczonego dla robota usługowego. mat. Kraj. Konf. TMM, Oficyna Wyd. Uniwersytetu Zielonogórskiego, pages 185–190, 2006. [41] I. Nesnas. The CLARAty project: Coping with hardware and software heterogenity. In D. Brugali, editor, Software Engineering for Experimental Robotics, volume 30 of Springer Tracts in Advanced Robotics, pages 31–70. Springer, 2007. [42] P. Redmond. Rubot II. http://mechatrons.com/rubot/, 2009. [43] S. Russell and P. Norvig. Artificial Intelligence: A Modern Approach. Prentice Hall, Upper Saddle River, N.J., 1995. [44] B. Siciliano. Parallel force/position control of robot manipulators. Proceedings of the 7th International Symposium of Robotics Research, Springer-Verlag, London, pages 79–89, 1996. [45] M. Spong, S. Hutchinson, and M. Vidyasagar. Robotics Modeling and Control. John Wiley & Sons, New York, 2005. [46] W. Szynkiewicz, C. Zieliński, W. Czajewski, and T. Winiarski. Control Architecture for Sensor-Based Two-Handed Manipulation. In T. Zielińska and C. Zieliński, editors, CISM Courses and Lectures – 16th CISM–IFToMM Symposium on Robot Design, Dynamics and Control, RoManSy’06, June 20–24, number 487, pages 237–244, Wien, New York, 2006. Springer. [47] T. Tsumugiwa, R. Yokogawa, and K. Hara. Variable impedance control based on estimation of human arm stiffness for human-robot cooperative calligraphic task. In Proceedings of the 2002 IEEE Conference on Robotics and Automation, volume 1, pages 644–650, May 2002. [48] R. Vaughan, B. Gerkey, and A. Howard. On device abstractions for portable, reusable robot code. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, IROS’03, Las Vegas, Nevada, pages 2121–2427. October 2003. [49] N. Zemiti, G. Morel, T. Ortmaier, and N. Bonnet. Mechatronic design of a new robot for force control in minimally invasive surgery. IEEE/ASME Transactions on Mechatronics, 12(2):143–153, 2007. [50] C. Zieliński. Control of a multi-robot system. 2nd Int. Symp. Methods and Models in Automation and Robotics MMAR’95, Międzyzdroje, Poland, pages 603–608, 30 Aug.–2 Sept. 1995. [51] C. Zieliński. Robot Programming Methods. Publishing House of Warsaw University of Technology, Warsaw, 1995. [52] C. Zieliński. By How Much Should a General Purpose Programming Language be Extended to Become a Multi-Robot System Programming Language? Advanced Robotics, 15(1):71–96, 2001. [53] C. Zieliński. Transition-function based approach to structuring robot control software. In K. Kozłowski, editor, Robot Motion and Control: Recent Developments, Lecture Notes in Control and Information Sciences, Vol.335, pages 265–286. Springer Verlag, 2006. 41

[54] C. Zieliński, A. Rydzewski, and W. Szynkiewicz. Multi-robot system controllers. In Proc. of the 5th International Symposium on Methods and Models in Automation and Robotics MMAR’98, Międzyzdroje, Poland, volume 3, pages 795–800, August 25–29 1998. [55] C. Zieliński, W. Szynkiewicz, K. Mianowski, and K. Nazarczuk. Mechatronic design of open-structure multi-robot controllers. Mechatronics, 11(8):987–1000, November 2001. [56] C. Zieliński, W. Szynkiewicz, and T. Winiarski. Applications of MRROC++ robot programming framework. In K. Kozłowski, editor, Proceedings of the 5th International Workshop on Robot Motion and Control, RoMoCo’05, Dymaczewo, Poland, pages 251–257, June, 23–25 2005. [57] C. Zieliński, W. Szynkiewicz, T. Winiarski, M. Staniak, W. Czajewski, and T. Kornuta. Rubik’s cube as a benchmark validating MRROC++ as an implementation tool for service robot control systems. Industrial Robot: An International Journal, 34(5):368–375, 2007. [58] C. Zieliński, T. Winiarski, K. Mianowski, A. Rydzewski, and W. Szynkiewicz. Endeffector sensors role in service robots. In K. Kozłowski, editor, Robot Motion and Control 2007 (LNCiS) Lecture Notes in Control and Information Sciences, pages 401–413. Springer Verlag London Limited, June, 11–13 2007.

42