MRROC++ Based Controller of a Dual Arm Robot System ... - CiteSeerX

MRROC++ Based Controller of a Dual Arm Robot System Manipulating a Rubik’s Cube Cezary Zieliski1 , Tomasz Winiarski1 , Wojciech Szynkiewicz1 , Maciej Staniak1 , Witold Czajewski2 , Tomasz Kornuta1 1

Institute of Control and Computation Engineering 2 Institute of Control and Industrial Electronics Grant no 4 T11A 003 25

Report no 06–10 Warsaw, June 2007

Warsaw University of Technology Institute of Control and Computation Engineering ul. Nowowiejska 15/19 00–665 Warsaw, Poland tel./fax. (48) 22-8253719

Contents 1 Introduction

3

2 Tasks requiring force control

7

2.1

Force control implementation in MRROC++ . . . . . . . . . . . . . . . . . . .

7

2.2

Experimental setup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

8

2.3

Copying drawings . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

8

2.3.1

Task formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . .

8

2.3.2

Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

8

2.3.3

Experimental results . . . . . . . . . . . . . . . . . . . . . . . . . . 10

2.3.4

Multi-robot extension of the task . . . . . . . . . . . . . . . . . . . 12

2.4

Contour following and turning a crank . . . . . . . . . . . . . . . . . . . . 13 2.4.1

Tasks formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

2.4.2

Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

2.4.3


3 Calibration of a dual arm system 3.1

3.2

16

Gathering of data required for the dual arm system calibration . . . . . . . 17 3.1.1

Task formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

3.1.2

Possible solutions of the task . . . . . . . . . . . . . . . . . . . . . . 18

3.1.3

Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

Calibration computations . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23 3.2.1

Description of the reference coordinate frames . . . . . . . . . . . . 23

3.2.2

General objective function . . . . . . . . . . . . . . . . . . . . . . . 24

3.2.3

Relationships between the coordinate frames . . . . . . . . . . . . . 25

3.2.4

Improvement of the robot kinematic model parameters . . . . . . . 27

3.2.5

Computations of the local correction matrix . . . . . . . . . . . . . 28

3.2.6

Relationship between the two manipulator base coordinate frames . 32

4 Dual arm robot system solving a Rubik’s cube puzzle 4.1

34

Localization of the cube . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35 4.1.1

Task formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35 1

4.2

4.3

4.4

4.5

4.6

4.7

4.1.2

Theoretical considerations required for the solution of the task . . . 35

4.1.3

Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36

4.1.4


Acquiring the cube – visual servoing . . . . . . . . . . . . . . . . . . . . . 44 4.2.1


4.2.2


4.2.3

Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47

4.2.4


Identification of the cube’s state . . . . . . . . . . . . . . . . . . . . . . . . 49 4.3.1


4.3.2


4.3.3

Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52

4.3.4


Solution to the Rubik’s cube puzzle . . . . . . . . . . . . . . . . . . . . . . 55 4.4.1


4.4.2


4.4.3

Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58

4.4.4


Manipulation of the cube . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61 4.5.1


4.5.2


4.5.3

Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63

4.5.4


Utilization of position–force control . . . . . . . . . . . . . . . . . . . . . . 64 4.6.1

Getting direct contact with the cube . . . . . . . . . . . . . . . . . 65

4.6.2

Cube grasping . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67

4.6.3

Face turning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68

High level task algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68

5 Conclusions

70

Bibliography

74

2

Chapter 1 Introduction Service robots, unlike their industrial counterparts, operate in unstructured environments and in close cooperation with human beings. Thus, service robots must rely heavily on sensing and reasoning, while industrial robots rely mainly on precision of movement in a well known and structured environment. As the service robot will function in human environment, it must be well adjusted to such surroundings, hence, it must be two-handed, capable of self-propulsion, have a sense of sight and touch. It should be capable of reacting to unexpected situations quickly, but also should be able to reason logically and formulate plans of actions leading to the execution of the task at hand. The main research problem is control of this type of equipment and integration of its numerous and diverse subsystems. The hard real-time aspect of such systems aggravates the problem. Service robots will be assisting humans in their daily chores, thus they will have to operate in human oriented environment. This environment, as it is adjusted to human capabilities, imposes certain constraints on the design of those devices. They must posses: • two-handed manipulation capability, • the ability to use vision data for the purpose of grasping immobile and moving objects (i.e., visual servoing), • the capacity for using tactile and force stimulus in manipulation (i.e., hybrid positionforce control), • the capability of both reasoning and quick reaction to the incoming stimulus from the environment (i.e., deliberative and behavioural control), • multi-modal communication capabilities (i.e., communication through speech and gesture), • the ability to propel themselves and navigate without collisions in the environment, • navigation and self-localization capabilities in semi-structured and unstructured environments. All of the enumerated capabilities require detailed investigations and improvement of today’s control methods (including adaptive control algorithms improving the speed and accuracy of manipulator control). The research presented in this volume concentrated on all of the above issues, but the last two. However, the most important aspect of this research was the adequate integration of all of those system components into an economically viable prototype. The software implementation effort of this project relied on the 3

MRROC++ robot programming framework (library of robot oriented software modules, a programming pattern and tools for creating new modules). MRROC++ was designed by using the transition function based formalism [25, 26, 27, 28, 31, 29, 30]. This formalism deals with systems consisting of multiple embodied agents, influencing the environment through effectors, gathering information from the environment through sensors and communicating with other agents through communication channels. The realization of the project significantly enhanced the capabilities of MRROC++. To produce a service robot a multitude of research and technical problems have to be solved. To find out how well they have been solved a benchmark task must be provided. This task must have all of ingredients that are at the focus of the above outlined research. The authors of this report proposed the solution of the Rubik’s cube puzzle as such a benchmark, because it requires of the system: • two handed manipulation to efficiently turn the faces of the cube, • visual capabilities, including locating the cube, visual servo-control to get hold of it, and identification of its initial state, • position-force control to avoid jamming of the cube while rotating the faces, • fusion of deliberative and behavioural control to work out the plan of motions solving the puzzle and to adapt quickly to sudden changes in the environment (e.g., jamming), • the ability to recognize spoken commands and to synthesize replies and queries, to initiate the task at the right moment and to get human help when the situation surmounts the capabilities of the system (e.g., the intentions of the human are not clear). The equipment at the disposal of authors of the paper enables the presentation of all of the above capabilities, but the last one – we do not have a mobile platform capable of moving two industrial robots that mimic the two hands. The system is composed of: • two 5 dof (degree-of-freedom) IRp-6 robots (one mounted on a track – this produced a 6 dof robot), • two electric grippers equipped with diverse sensors (those are shape grippers simplifying Rubik’s cube manipulation), • three cameras (one stand-alone and two mounted inside the electric grippers), • two force/torque sensors mounted in the wrists of the robots, • a network of PC computers connected by Ethernet, • QNX Neutrino ver.6.3 real-time operating system (RTOS) – this is the foundation supporting all of the processes and threads of the control system. • MRROC++ based software supervised by the QNX RTOS – this are the building blocks out of which a specific controller is constructed. This software contains the code executing a particular user’s task.

4

In different experiments the robot kinematic structure differed. In some single-robot experiments the IRp-6 robot mounted on a track was used. For the double-arm experiments the two IRp-6 robots were modified. Each of them obtained an extra degree-of-freedom in the wrist. The track was not used then, so the robots had a full 6 dof capability, thus any position and orientation of the manipulated object could be attained, save for the workspace constraints. Two force/torque sensors (hardware sensors) were installed in the wrists of the two robots. Both are ATI gamma 65/5 with component numbers: FT3084 and FT6284 (from Schunk reseller). The measurement vector contains three linear coordinates (forces) and three angular (torque) coordinates. The measurement range is: ±65 N for two force components (Fx , Fy ), ±130 N for the third force component (Fz ) and ±5 N m for the three torque components (Mx , My , Mz ). The effective resolution is 0.05 N for Fx and Fy , 0.1 N for Fz and 0.003 N m for: Mx , My and Mz . The effective measurement sampling period is 2 ms (for gathering the complete vector of force and torque components).

Figure 1.1: The electric gripper with parallel jaw motion and an in-hand camera The two electric grippers (fig. 1.1, [32]) are capable of purely parallel jaw (finger) motion. Each finger is instrumented with force transducers which detect the presence of contacts with the grasped object. Moreover, each gripper has an eye-in-hand miniature CCD color camera. Additionally, a global vision system with fixed-mount SVSVistek SVS084 CCD color camera and Leonardo Digital Video Processor from Arvoo for fast image acquisition and realtime processing of the incoming data is used. The

Figure 1.2: Calibration of one of the robots by using the POLMAN-6L [15, 16] passive parallel manipulator above equipment has been used for executing diverse benchmark tasks leading to a system capable of working as a full blown service robot. The more significant tasks, in order of their appearance, have been executed by the following systems: 5

Figure 1.3: Two-handed Rubik’s cube manipulation • a single-robot system following an unknown contour, • a single-robot system turning a crank, • a single-robot system capable of recording drawings produced by an operator and reproducing them, • a two-robot system, where one robot records drawings, and the other one or both reproduce them, • a three-effector system in which one robot records the drawing, but it is reproduced by two 5 dof robots supplemented by a conveyor moving the paper forward and backwards in the missing direction, • a system consisting of each of the robots separately and a measuring unit, capable of calibrating the kinematic models of the robots and establishing the relative positions of the two robots (fig. 1.2), • a two-robot system solving the Rubik’s cube puzzle (fig. 1.3). All of those systems required position-force control. The last one, moreover, required visual servoing and pattern recognition. This report describes those applications in more detail.

6

Chapter 2 Tasks requiring force control 2.1

Force control implementation in MRROC++ SEMAPHORE PULSE

MP UI

MESSAGE

UI_ COMM

INTERRUPT SHARED MEMORY

UI_SRP UI_ MASTER

VSP

(receives messages from other processes)

EDP

ECP

VSP_ MASTER

EDP_ MASTER

EDP_ TRANS

EDP_ VSP_I

EDP_ READER

EDP_ SERVO

EDP_ FORCE

Manipulator motors

VSP_ CREAD

Force/torque transducer

Figure 2.1: A MRROC++ based position-force control system structure with inter-thread communication diagram MRROC++ programing framework is suitable for producing controllers executing diverse tasks, e.g. requiring pure position or position-force control. The detailed structure of a single robot MRROC++ based system for applications requiring force measurements is presented in fig. 2.1. This structure was used for copying drawings [23], contour following and turning the crack applications [22] presented further on in this report. The multi-robot extension of the copying drawings task [21] requires a similar structure of the controller, but more processes are needed. All tasks use the same EDP position-force controller and communication structures between the ECP and the EDP, but differ in the task dependent part of MRROC++, i.e., kernels of the MP, ECP and VSP processes. 7

2.2

Experimental setup

Computer of operator and coordinator processes (UI i MP)

manipulator 1 computer controlling robot 1 ECP_1 VSP_1 EDP_1

F/T sensor

gripper

F/T sensor controller

F/T sensor interface

Single Robot System

Multi Robot System (2 manipulators and conveyor)

ethernet computer controlling robot 2 ECP_2 VSP_2 EDP_2 and conveyor ECP_t EDP_t robot interface

manipulator 2

F/T sensor

conveyor

robot interface

robot hardware controller

gripper robot hardware controller

F/T sensor interface

Figure 2.2: Experimental setup The experimental setup (fig. 2.2) consists of two modified IRb-6 manipulators with additional active degree of freedom and force/torque sensors, conveyor, PC computers connected by an Ethernet network supervised by the QNX real-time operating system. The executed task determines not only the software structure, but also the number of robots used and the tools mounted on the manipulators.

2.3 2.3.1

Copying drawings Task formulation

Drawing by human–robot cooperative system has attracted the attention of other researchers [19]. In our investigation the force sensor is used to manually guide the robot holding a pen through the motions producing a drawing and then to make either a single robot (fig. 2.3) or a multi-robot (fig. 2.9) system to reproduce this drawing. The force sensor is mounted on the last link of the manipulator, i.e., its wrist.

2.3.2

Implementation

The structure of a single robot system capable of teaching-in and reproducing drawings is presented in figures 2.1, 2.2. The high level task algorithm (fig. 2.4a) is implemented in the ECP process. At first, an operator makes the decision whether the new drawing will be taught-in or loaded from a file. The teach-in process is conducted by an operator leading a single robot arm and thus producing the original of the drawing. The reproduction phase is done automatically by the system.

8

Figure 2.3: Copying drawings – the reproduction phase The preparation for drawing starts when an operator catches the pen mounted on the wrist of the manipulator (the pen must be mounted vertically in relation to the paper on which the drawing will be created). Then the operator starts to move the pen, and thus the manipulator, by exerting the force on the pen or the force sensor directly. The manipulator is compliant in linear directions, but its orientation is rigid, thus it is maintained constant. The next phase is the teach-in process itself (recording of the drawing). It starts when the pen touches the paper. Now the manipulator is moved in the same way, but the motion trajectory is recorded by memorizing the location of the pen tip on the surface of the paper (X-Y coordinates), at constant intervals of time (20 ms). Additionally the information from the VSP process is appended, to label the locations at which the pen tip is jerked off the paper (fig. 2.4b). The jerk is detected by measuring the force in the vertical direction – if it exceeds a certain threshold the control system treats this as a purposeful jerk, which should be recorded as an intension to rise the pen tip over the surface of the paper. The VSP monitors the state of the drawing process. To do so it contains a finite state automaton fig. 2.4b that monitors the current state of the pen. Drawing starts in the Above paper state. Arc A arc is activated when a downward jerk is detected. The controller state changes to Lowering. The B arc is activated when the impact is detected and automaton changes its state to Impact. Then the C arc is activated and the Paper surface state occurs. Next, the D arc is associated with an upward jerk and the automaton changes its state to the Lift-off state. Then in the teach-in phase the system switches immediately to the Above paper state (arc E). Thus by traversing the states of this automaton the VSP is able to tell the ECP to record whether the pen tip currently is on the paper surface or above it, and so how should it behave in the drawing reproduction phase. The second phase of the task execution consists in the reproduction of the memorized drawing. Here, the operator moves the compliant manipulator to the location, in which the memorized drawing has to be reproduced. In this location the pen should be above the paper surface. Then the system starts the automatic reproduction process. First, the pen is moved down (the Lowering state) – the force is set to to a negative value. Once the impact is detected the vertical motion stops only the horizontal motion over the surface of the paper is induced (the Paper surface state). The automaton is in this state until the pen tip reaches the location in which an upward jerk was recorded in the teach-in phase. There pen is lifted-off the paper. The trajectory that the pen tip traverses on the paper surface is an accurate copy of the memorized trajectory. However, in the vertical direction the EDP controller tries to exert a constant force. In the reproduction phase

9

a

b

Start

Start

Above paper

Teach-in

Load drawing / teach-in drawing?

A

Load

Loading of drawing from file

Lowering

Teach-in process

B Reproduce? YES

NO

Stop

Impact NO

C

Save drawing?

Reproduction

YES

Paper surface Drawing saving

D

NO

Exit?

Lift-off

YES

E

Stop

Figure 2.4: Drawing application: a - the flow chart of the application execution high-level algorithm, b - finite state automaton graph governing the actions of the VSP process the motions executed in the Above paper and Lift-off states are fully position controlled, whilst in the other state hybrid position-force control is utilized. The Above paper motions are executed in a horizontal plane (the Z coordinate is kept constant, even if it varied during the teach-in phase). The force sensor plays a dual role. On the one hand, it is engaged in the continuous limb control, thus it is treated as a proprioceptor, and on the other hand it detects events occurring in the environment, thus it behaves as an exteroceptor. The latter behavior requires the addition of a VSP to the system.

2.3.3

Experimental results b

a 0.88 Z [m]

0.88 Z[m]

0.875

0.875

0.87

0.87

0.865 0.865

-0.02

0.05

-0.03

0.045 0.04

-0.04 0.035 Y[m] 0.03

Y [m] -0.05 0.76

0.765

0.77

0.775

0.78

X [m] 0.79 0.785

0.755

0.76

0.765

0.77

0.775

X [m]

0.78

Figure 2.5: Three dimensional motion trajectory during copying drawings: a – the teachin phase, b – the reproduction phase Fig. 2.5 present the three dimensional trajectories of the end-effector motion during teachin and reproduction of the six feathers of an arrow drawn by the operator (fig. 2.3). A visible difference between the graphs is the way the pen moves up and down. The operator makes unconstrained moves, hence the trajectory above the paper is uneven in 10

the vertical direction. The reproduction algorithm produces exact vertical motions, thus the trajectories above the paper are horizontal. This is evident in the graph in fig. 2.6. a

b

0.89

0.88 Z [m]

Z [m] 0.885

3 0.875

0.88 2 4

3 0.875 0.87

2

4

0.87 *1

1

* 0.865

1.36

1.38

1.4

1.42 step

1.44

1.46

0.865

[nr] 1.48

1.8

1.82

1.84

1.86

5

x 10

1.88 step

1.9

1.92

1.94 [nr] 1.96 5

x 10

Figure 2.6: The Z coordinate of the pen tip pose during copying drawings: a – the teach-in phase, b – the reproduction phase a

b

250 [u] 200

100 [u]

* The Z force coordinate

The Z force coordinate

150 100

*

80

50 0

1

2

3

4

-50

60

40

20

-100 -150

0

-200 1

-250

1.36

1.38

1.4

1.42 step

1.44

1.46

-20

[nr] 1.48 5 x 10

1.8

2, 3, 4 1.82

1.84

1.86

1.88 step

1.9

1.92

1.94 [nr] 1.96 5

x 10

Figure 2.7: Force applied in the vertical direction during copying drawings: a – the teach-in phase, b – the reproduction phase There are four segments of the trajectory marked as: 1, 2, 3 and 4 in the graphs in figures 2.6 and 2.7. All the four segments occur while drawing each feather of an arrow: 1 – motion on the paper surface, 2 – pen tip lift-off, 3 – motion above the paper, 4 - lowering of the pen tip. The symbol ,,∗” draws attention to the fragment of the plot representing the impact caused by the pen tip hitting the surface of the paper. During the whole of the teach-in phase and in the segment a of the reproduction phase, the controller is commanded to reach the vertical force of 20u, i.e. 1N , however, in the segments: 2, 3 and 4 of the reproduction phase, the motion is purely position controlled. Experiments show that the applied algorithms are robust enough to execute the whole task correctly, even with the force loop of the position-force control simplified to the utmost extent (proportional control) with only viscous component considered.

11

UI SRP

MP

VSP_1

ECP_1

EDP_1

robot 1 motors and encoders

ECP_2

ECP_t

EDP_t

F/T sensor

conveyor motor and encoder

VSP_2

EDP_2

robot 2 motors and encoders

F/T sensor

Figure 2.8: Structure of a MRROC++ based system executing the task of reproducing drawings in which the teach-in phase is conducted by one robot and the drawing reproduction phase by another one

2.3.4

Multi-robot extension of the task

The experimental system presented in fig. 2.8 consists of two robots. One of them reproduces drawings, while the other one is used to teach them in (figs. 2.9, 2.2). This system was built as an intermediate step in the implementation of a dual arm robot system solving a Rubik’s cube puzzle. We started with single robot tasks and gradually shifted out attention to ever more complex multi-robot tasks. The structure of a multi-robot drawing system is very similar to the structure of the Rubik’s cube puzzle solving system (however the former does not require vision, whereas the latter does). Hence, we assumed that if the multi-robot drawing will work the cube puzzle solver should work too (it subsequently turned out that the assumption had been correct).

Figure 2.9: Copying drawings in multi robot system The high level task algorithms of single and multi-robot versions of the drawing application are very similar, however, in the multi-robot version the algorithm is implemented in the MP process and the ECP processes are transparent. The teach-in phase is almost identical in both cases – it is conducted in cooperation between the operator and a single robot. The main difference lies in the reproduction phase. In multi-robot application the reproduction of the memorized drawing is done simultaneously by two arms. Before the reproduction itself starts each of the arms needs to be moved to the appropriate initial location (the pens have to be above the paper surfaces). In this phase both arms are compliant. Then the automatic reproduction process starts. The pens move down until impact is detected (this signals that the surface of paper has been reached). There are two VSP processes needed. 12

After both pens strike the surface of paper the system starts to draw two pictures with the same speed and of the same size. The drawing is reproduced by each of the robots in exactly the same manner and in the same fashion as performed by the single robot system. In the two robot version the robots are mutually synchronized before each new line segment is reproduced, thus the two drawings will appear approximately at the same time. The multi-robot drawing task was executed in still another mode. The motion of the manipulators in the Y direction was substituted by the motion of a conveyor on which the two drawing papers were located. So instead of moving the robots in the Y direction the conveyor shifted the paper in that direction. Hence, the system consisted of three motion inducing devices, each capable of independent motion. Thus a three effector MRROC++ based system was created. The tests demonstrated that the so extended system also works correctly.

2.4 2.4.1

Contour following and turning a crank Tasks formulation

The tasks of following of an unknown contour (fig. 2.10a) and rotating a crank (fig. 2.10b) are quite similar. In both cases the manipulator end effector is simply a rod held vertically downwards. In following of an unknown contour the rod has an attached bearing placed in a horizontal plane. The task starts with an operator moving the rod (the manipulator is force controlled and thus compliant) to to a location in which the bearing contacts the surface of the object. Then the robot starts to move the rod along the object surface in the clockwise direction. In the task requiring the rotation of the crank there is no bearing attached to the rod end. An operator inserts the rod end into a hole in the crank. Once the rod is released by the operator the robot starts to turn the crank. b

a

Figure 2.10: a – Following an unknown contour, b – Rotating a crank

2.4.2

Implementation

Both tasks use the same control algorithm, thus the EDP processes and the ECP processes are exactly the same. In both cases the end-effector moves in the horizontal plane. The direction of the force command in each macrostep results from the previously measured real direction of the force being applied to the surface of the object. In the case of contour 13

following this was the surface of an iron block (fig. 2.10a) and in the case of moving the crank it was the internal surface of the hole in the crank, into which the end-effector was inserted (fig. 2.10b). In the orthogonal direction the end-effector was position controlled to reach a desired velocity. In both tasks the force sensor is treated as a proprioreceptor, thus its readings are utilized only by the EDP directly and the ECP indirectly (as obtained from the EDP). VSP has been used only to send configure command to EDP force sensor.

2.4.3

Experimental results b

a

80

0.19 Y [m]

D E

70

force applied to the block surface

0.18

[u]

A

0.17 0.16 0.15 0.14 0.13 0.12

60

50

40 C

D E

30 A 20

0.11 10

0.1

C

0.09 0.78

B

B

0.79

0.8

0.81

0.82

0.83

0.84

0.85

0 1.15

1.2

1.25

1.3

[nr] 1.35

step

0.86 X [m] 0.87

5

x 10

Figure 2.11: a – An iron block detected while following an unknown contour (the internal side of an iron ring), b – Force applied to the block surface while following an unknown contour

b

a 0.26 Y [m] 0.258

F

0.256 B 0.254 0.252

E

A

0.25 0.248 0.246 C 0.244 0.242 0.82

D 0.83

0.84

0.85

0.86

0.87

X [m] 0.88

force applied to the inner surface of the hole in the crank

Fig. 2.11a presents the cross-section of the block as detected during following its contour, while fig. 2.11b presents the normal force applied to the block surface. It is visible that the block is rough on the sides corresponding to the AB and CD trajectory segments, and it is smooth on the side corresponding to the trajectory segment BC. The smoothness of the surface is related to the amplitude of force oscillations. Rough surfaces cause oscillations of about 60u peak to peak, and the smooth surface causes much lower oscillations of about 10u (fig. 2.11b). The controller was commanded to exert the force of 50u – 2.5N . The recorded contour of block edges (point B and C in fig. 2.11a) is rounded because the tip of the rod had an attached bearing, installed to eliminate the influence of friction on the end-effector motion. 120 [u] 100

80

60

40

F

A B D

20 C

E 0 1.57

1.58

1.59

1.6

1.61

1.62

1.63

step

1.64

1.65

1.66

1.67 [nr] 1.68 5

x 10

Figure 2.12: a – The end-effector trajectory in the XY plane while rotating the crank, b – The force applied to the inner surface of the hole in the crank

14

Fig. 2.12a presents the end-effector trajectory in the horizontal plane while rotating the crank. Fig. 2.12b presents the force applied to the inner surface of the round hole in the crank during the same motion. At first the crank moves counterclockwise – segment AB, then the operator pulls the end-effector to the opposite side of the hole – segment BC, and subsequently the crank starts to move clockwise – segment CD. The next, second intervention of the operator (segment DE) causes the crank to move counterclockwise again – segment EF. In the fig. 2.12b the operator interventions are visible as force jerks. The algorithms of following a contour and rotating a crank are exactly the same, so in both cases the controller is commanded to reach the force of 50u – 2, 5N . Small oscillations visible in the segments AB, CD and EF of the trajectory (fig. 2.12a) result from the roughness of the surfaces of the crank hole and the inserted rod.

15

Chapter 3 Calibration of a dual arm system The calibration of a robotic system is a procedure consisting of two calibration stages: • gathering the measurements, • data processing. Herein we shall present a calibration procedure for a system consisting of two or more robots, however we shall concentrate on a dual arm system. The data processing stage uses the measurements collected during the measurement gathering stage in computations, which result in the improvement of: • the kinematic model of each robot, • the knowledge about the relative location of the robots. Without the adequate kinematic models of the manipulators the control system has invalid information about the poses of the end effectors. And without exact information about the locations of the robot bases in relation to the global coordination system it is almost impossible to perform tasks requiring robot cooperation – unless for that purpose extra information is gathered by the exteroceptors. The aim of calibrating the robot kinematics, i.e., the robot kinematic model, is the correction of the dependencies between the joint coordinates, derived from the motor encoder measurements, and the task coordinates, represented by the pose of the coordinate frame affixed to the manipulator end-effector. Methods of calibrating the robot kinematics can be divided into two general groups, depending on their scope: • correction of the kinematic model parameters, what impacts the improvement of positioning precision of the end-effector in whole workspace (global improvement), • calculation of the local correction matrix, what improves the end-effector positioning precision only in a relatively small region in which the measurements are collected (local improvement).

16

3.1

Gathering of data required for the dual arm system calibration

The considered dual arm system consists of two IRp-6 manipulators. Currently the most complex task that it performs is solving the Rubik’s cube puzzle. To execute this task each of the manipulators has to have 6 degrees of freedom. As IRp-6 robots have only 5 degrees of freedom, they had to be modified. An extra joint providing the missing degree of freedom had to be appended to each of the manipulators. Thus, the robots can work as originally designed or they can work as 6 degrees of freedom devices, and one of the robots, as it is mounted on a track, can also work as a 6 or 7 degrees of freedom device. In each case the kinematic model of those devices changes. Moreover, in a multi-robot system the relative location of the bases of the manipulators is also relevant to the control tasks. Thus a procedure for identifying the parameters of those models had to be formulated and implemented. That is why the calibration of the dual arm system was of interest to us.

3.1.1

Task formulation

For the purposes of calibration a new parallel manipulator was constructed (fig. 3.1) [15, 16]. POLMAN 6L is a passive device, containing two triangularly-shaped platforms connected by six digital scales, which enable the measurement of all of the six pose components (position and orientation) of a robot end-effector attached rigidly to the movable triangular platform.

a

b

Figure 3.1: The Stewart platform based parallel manipulator POLMAN 6L: a – kinematic model, b – virtual model created using the ADAMS software The immobile platform of the measurement device is fixed between the two manipulators, in such a way that its movable platform is located within their common work space. Measurements are collected independently for each of the robots – during this process the end-effector of the manipulator subjected to the calibration procedure is permanently connected to the movable platform of the measurement unit (fig. 3.2). According to the classification of calibration data acquisition presented in [4], this type of calibration belongs to the class of kinematic loop methods, and in fact unifies both subclasses of those methods: • Open-loop methods – an external metrology system is used to measure the poses of the freely moving end-effector, • Closed-loop methods – the end-effector is attached to the ground, thus a mobile closed kinematic chain is formed. 17

Figure 3.2: Connection between the POLMAN’s movable platform and the end-effector of the IRp-6 manipulator

During the measurement gathering phase of the calibration procedure the arm, and thus the measurement unit, are shifted to many locations in the work space. One measurement consists, in fact, of the following two vectors: • the calibrated robot joint coordinates, computed from the readings of the motor encoders, • the readings of the digital scales contained in the measurement unit. During the calibration procedure those two vectors are used in the computation of the poses of the manipulator end-effector and the movable platform of the measurement unit. As the relationship between those quantities is constant, and known, the two quantities can be compared, and the differences resulting in all locations are minimized in such a way that the deviation from that constant is kept at a minimum. It is worth noting that the same measurements were used for all of the types of the above described calibrations.

3.1.2

Possible solutions of the task

Many problems had been solved before and during the implementation of the calibration task. The first problem was related to the process of retrieving the measurements from the digital scales. As the initiation of measurement of one digital scale takes about 200 ms, the VSP process was divided into seven threads: • six threads are related to each of the six digital scales, • one thread, is responsible for the communication with the other processes and coordination of the other threads.

18

This solution reduced the time required by the measurement gathering process from 1200 ms (this is the time needed to sequentially read the scales) to 250 ms (for the concurrent solution). During the initiation and the act of each reading the measurement unit must remain motionless. This caused the division of the process of collection of measurements into two stages, where each stage is represented by a separate process: • process which enables the user to create an acceptable trajectory, • process which reproduces this trajectory by moving from one point to another along that trajectory – pausing in each of the selected intermediate locations for a time long enough for the reading to become stable. The method of creating an acceptable trajectory was still another problem to be solved. The acceptable trajectory is a trajectory, in which all intermediate locations (not only those selected as the ones in which the measurements will be gathered) belong to the work spaces of the POLMAN 6L and the calibrated robot. It was decided to use the force sensor to detect situations, when the robot tries to make an illegal move, which, in fact, could possibly damage the measurement device or the calibrated robot itself. The force sensor was installed in the wrist of the robot. When a excessive force was detected, the motion was immediately stopped and the control system waited for the operator’s command. The same security mechanism was implemented in the trajectory reproduction process. The first thought was to use the force sensor as a joystick, but this solution required the presence of two operators during the stage of collecting measurements – one holding the joystick and the other operating the keyboard, thus this idea was quickly abandoned. Hence, currently the operator causes the motion of the robot by using the keyboard. After the implementation of those two processes a new problem appeared. The construction of the measurement unit and its interface to the calibrated robot caused the latter to produce excessively large forces to move the device from one position to another (this was especially due to the internal friction of the passively moving elements of the measurement unit). Sometimes an acceptable motion caused an out-of-range force sensor readings. As a result, both the trajectory creation and trajectory reproduction became quite annoying, because the operator had to manually unlock the blocked system many times during the experiment. There were two possible solutions of this problem: • to get rid of the force sensor, but then no security mechanism would be available, • completely change the data acquisition process. We chose the latter. To realize this concept the hardware of the robot controller had to be modified in such a way that it would be possible to read the robot motor encoders even when the power supply was cut off from the motors. This enabled the IRp-6 robots to be used as passive (backdriven) manipulators. In this approach, after starting the measurement process, the user manually moves the robot end-effector from one location to another and the system automatically verifies, whether the measurements are acquired properly. The verification is based on the comparison of the robot pose at the moment of the initiation of the measurement and after the measurement has been taken. If both are the same the control system informs the user about this fact by a beep.

19

3.1.3

Implementation

The general structure of a single robot MRROC++ based system for the purpose of gathering the calibration data is presented in fig. 3.3.

Figure 3.3: Structure of the MRROC++ controller used for the calibration data acquisition For the purpose of calibration the following modifications to the MRROC++ software were necessary: • a new window had to be added to the UI process, • a new ECP process for the IRp-6 robot mounted on the track had to be created, • a new ECP process for the IRp-6 robot with a stationary base had to be created, • a new VSP process for the POLMAN 6L had to be created (it should be noted that this passive manipulator is treated as an exteroceptor). Moreover, the following already existing processes were used in this task: • the EDP driver for the IRp-6 robot mounted on the track, • the EDP driver for the IRp-6 robot with a stationary base, • the MP process. Cooperation between all of the processes is shown in fig. 3.4. After creation of the EDP, the operator starts the MP process, which creates the necessary ECP. The ECP process establishes communication with the previously created EDP ˙ and creates the VSPAfter those actions, it sends a command to the UI, which creates and presents the required control window (fig. 3.5) and establishes a new (direct) communication channel with the ECP. This solution has two advantages: • enables the use of the existing MP process without any modifications, • speeds up the communication between the UI and the ECP. When the communication is established, the system is ready and waiting for the operator’s commands – when a button is pressed, the UI sends an adequate command to the ECP. 20

UI

MP

ECP

VSP

EDP

Operator Start EDP Process Creation Start MP Process Creation

Button Pressing

Communication Establishment

Process Creation

Control Window Creation

Communication Establishment Process Creation

Command Sending

Figure 3.4: Cooperation between the MRROC++ processes

The modifications of the UI process Because the user has many options at his/her disposal, each depending on the task execution state, a new window was created (fig. 3.5) to display them. When the user presses a button, an adequate command is sent to the ECP process. Besides that a function associated with the window timer retrieves the robot positions and sensor readings every 500 ms. Implementation of the VSP process The subdivision the VSP process into the kernel and the shell is analogous to the division of all other MRROC++ processes. The tasks of the frozen spot (process shell) of the VSP process are: • creation of the communication channels, • reaction to the commands sent by the higher layer processes, • handling of the errors that occurred, • alarming other processes about the detected errors, • dispatching the aggregated sensor readings to the requesting process. There are three types of the VSP shells, depending on the way they communicate with other MRROC++ processes: • noninteractive – the VSP repeatedly collects and aggregates measurements, regardless of whether they are needed by other processes or not, 21

Figure 3.5: New window appearing in the UI process

• interactive – the VSP collects new measurements only when an explicit reading initiation request arrives (this introduces a delay in the execution of the requesting process, because it has to wait for the measurements to be made), • interactive without delay – the VSP collects new measurements only when an explicit reading initiation request arrives, but it sends an acknowledgement before the execution of the requested task. Obviously the reading is not available at this moment, but the requesting process does not have to wait for the reading to become available.

Figure 3.6: Threads of the VSP gathering measurements from the six digital scales

As explained in section 3.1.2, the VSP process was divided into seven threads (fig. 3.6). For the purpose of gathering measurements from the POLMAN 6L measurement unit, the interactive VSP shell was selected. The Coordination thread was the main process thread, created by the shell. The kernel of the VSP process consists of the vsp digital scales sensor class, derived from the vsp sensor abstract class. Its methods are responsible for the creation and coordination of the threads gathering readings from the digital scales.

22

Implementation of the ECP processes The ECP processes for both robots are almost identical and both were divided into two threads (fig. 3.7). The Communication thread is responsible for receiving commands from the UI or MP processes, while the Automatic Measurement Gathering thread repeatedly retrieves current robot positions from the EDP (the EDP uses the proprioceptors, i.e., motor encoders, for that purpose) and readings obtained by the VSP process from the digital scales. Only measurements classified as correct are appended to the calibration data.

Figure 3.7: Threads of the ECP automatically acquiring robot positions and digital scales measurements

3.2

Calibration computations

The data processing stage in its computations takes into account the measurements collected during the measurement gathering stage. The order of calculations is as follows [12]: 1. Calibration of the first robot: (a) Computations of the relationships between the base coordinate systems of the robot and POLMAN 6L measurement unit. (b) Improvement of the robot kinematic model parameters. (c) Computations of the local correction matrix. 2. Calibration of the second robot – the calculations are similar to those conducted for the first robot. 3. Computation of the location of the bases of both robots in the global coordinate system.

3.2.1

Description of the reference coordinate frames

Reference coordinate frames affixed to the selected elements of the experimental setup are presented in fig. 3.8.

23

Figure 3.8: Reference coordinate frames used in the calibration computations

The Cartesian coordinate frame πR is assigned to the base of the manipulator. The pose of the coordinate frame πK is affixed to the manipulator end-effector. The position and orientation of frame πK in relation to frame πR change during the motion of the manipulator and are described by the position vector k and orientation matrix K respectively. The coordinate frame πB is affixed to the base of the measurement unit and the coordinate frame πM – to its movable platform. The position and orientation of πM in relation to πB , represented by the position vector m and orientation matrix M, change during the movement of the manipulator fixed to the measurement unit. The position and orientation of πB in relation to πR remain unchanged during the whole measurement gathering stage and are represented by the position vector d and orientation matrix D respectively. The vector s and matrix S, which represent the position and orientation of πK in relation to πM , also remain invariant during the acquisition of measurements.

3.2.2

General objective function

During the measurement phase, at every instant ti , the readings from the manipulator motor encoders and POLMAN’s digital scales are acquired. The ki vector and Ki can be computed on the base of the ith motor encoders readings by solving the manipulator forward kinematics. The position and orientation of the end-effector computed in that way are called the computed configuration. Analogically, on the base of the ith digital scales readings, the mi vector and Mi matrix can be computed, thus for every instant ti (every measurement point) the following approximations can be formulated: (3.1) ki ≈ d + D(mi + Mi · s)

(3.2) Ki ≈ D· Mi · S 24

The position and orientation of the end-effector, computed on the base of the formulas presented on the right side of approximations, are called the measured configuration. The vector approximations, equivalent to the matrix approximation (3.2), are: Ki · ε ≈ D· Mi · S· ε (3.3) Ki · ζ ≈ D· Mi · S· ζ Ki · η ≈ D· Mi · S· η where ε =

1 0 0

T

,ζ=

0 1 0

T

,η=

0 0 1

T

.

The general objective function is defined as a sum of distances squared. The distances are taken between the computed configuration and the measured configuration for all measured points: Pn T W = i=1 [(d + D(mi + Mi · s) − ki ) (d + D(mi + Mi · s) − ki ) T + c· ((D· Mi · S· ε − Ki · ε) (D· Mi · S· ε − Ki · ε) (3.4) + (D· Mi · S· ζ − Ki · ζ)T (D· Mi · S· ζ − Ki · ζ) + (D· Mi · S· η − Ki · η)T (D· Mi · S· η − Ki · η))] where n is the number of points in the measurement set, whereas c is a positive invariant integer used for leveling off the impact from the (3.3) and (3.1), i.e., a certain weight. Because the trace of any 3-by-3 square matrix A is defined to be the sum of the elements on the main diagonal tr(A) = εT · A· ε + ζ T · A· ζ + η T · A· η, the equation (3.4) was rewritten in the following, more compact, form: Pn T W = i=1 [(d + D(mi + Mi · s) − ki ) (d + D(mi + Mi · s) − ki ) (3.5) T + c· tr((D· Mi · S − Ki ) (D· Mi · S − Ki ))] The above presented objective function is used (in this or analogical form) for rating the solutions found by all calibration computations, performed during the calibration of a dual arm system. This enables the comparison of end-effector positioning improvement.

3.2.3

Relationships between the coordinate frames

Improvement of the kinematic model parameters of a given manipulator with the use of the external measurement unit requires the precise knowledge of: • the vector d and matrix D, describing the position and orientation of the measurement unit base reference coordinate frame in relation to the manipulator base coordinate frame, • pose of the manipulator end-effector coordinates frame in relation to the movable platform coordinates frame, described by the vector s and matrix S. Retrieving proper data can cause problems, because precise measurements are difficult, and sometimes even impossible, for example, manipulator base coordinates frame is located inside its body, at a point of abstract axis intersection. This can be solved with the use of the measurements, gathered for the purposes of kinematic model parameter improvement. Working on the assumption that even though the final parameters are not yet known, the poses of the end-effectors, estimated by the currently used parameters, from the computed and measured configurations, should be relatively close. The idea is to calculate the d, s vectors and the D, S matrices by minimizing the objective function W (3.5) with the assumption that the kinematic parameters are invariant. In the next 25

calculations stage, those results are used as invariants, and the kinematic model parameters are computed. Those two stages of computations can be performed one after another many times. Thus, in the first stage we are looking for such d, D, s and S, for which the left and right hand sides of the approximations (3.1) and (3.3), for all measured points, are as equal as possible. The D and S direction cosine matrices can be presented as a function of three angles – in computations we used the Cardano-Bryant angles representation. The vectors d and s have three elements each. Therefore we must find twelve scalar parameters: T d= x y z D = Rx (α)Ry (β)Rz (γ) T (3.6) s= u v w S = Rx (ϕ)Ry (Θ)Rz (ψ)    1 0 0 cos(ξ) where Rx (ξ) = 0 cos(ξ) −sin(ξ), Ry (ξ) =  0 0 sin(ξ) cos(ξ) −sin(ξ)  cos(ξ) −sin(ξ) 0 sin(ξ) cos(ξ) 0 are rotation matrices about x, y and z 0 0 1

 0 sin(ξ) 1 0  and Rz (ξ) = 0 cos(ξ) axes respectively.

Those parameters together form the q vector: (3.7) q = x y z | α β γ | u v w | ϕ Θ ψ The optimal solution was found numerically, with the use of the Newton-Raphson method, which required computations of the analytic partial derivatives for all q elements [24]. This method gave an observable acceleration in relation to the standard Matlab procedures. Experimental results for the IRp-6 manipulator with a stationary base The position and orientation of the measurement units coordinate frame in relation to T the manipulator coordinate frame are described by the dPB = x y z vector and the P DB = Rx (α)Ry (β)Rz (γ) direction cosine matrix. Results of calculations based on the 240 measurement points are: x y z α β γ

= = = = = =

394.999780826192 −661.058697428226 −488.317608020605 −0.012323890232 −0.001832001605 −3.092658452353

[mm] [mm] [mm] [rad] [rad] [rad]

T u v w The results for the dM vector and DM P = P = Rx (ϕ)Ry (Θ)Rz (ψ) direction cosine matrix, describing the pose of the end-effector coordinate frame of the manipulator on a stationary base, in relation to the movable platform coordinate frame, are sa follow: u v w ϕ Θ ψ

= = = = = =

213.057645075103 119.776630751384 235.512975381513 −1.603775284526 1.029150276051 −0.762586058544

[mm] [mm] [mm] [rad] [rad] [rad] 26

Experimental results for the IRp-6 manipulator mounted on a track T The dTBj = x y z vector and the DTBj = Rx (α)Ry (β)Rz (γ) direction cosine matrix, describe the position and orientation of the measurement unit base coordinates frame in relation to the manipulator base coordinates frame. On the base of 174 measurement points, the following results were computed: x y z α β γ

= = = = = =

307.67564746717 1432.50490711458 −512.55997632767 −0.01326261117 −0.01566293252 −3.12496262919


Relationship between the coordinate frames of the end-effector of the manipulator mounted T u v w vector on the track and the movable platform is described by the dM Tj = M and the DT j = Rx (ϕ)Ry (Θ)Rz (ψ) matrix: u v w ϕ Θ ψ

3.2.4

= = = = = =

501.79910159756 286.76519620296 247.60683267846 1.50579629891 −1.05564718573 2.10034841033


Improvement of the robot kinematic model parameters

Because each IRp-6 robot kinematic model in fact contains more than forty parameters, preliminary calculations, which had to pick out the most important ones out of them, were performed. The kinematic model parameters, selected for improvement, and which were used to minimize the objective function W (3.5) include: • lengths of the manipulator links, • geometric synchronization poses (home positions), • conversion rates between the motors shaft positions and joint coordinates. Standard Matlab procedure fminunc was used. Experimental results for the IRp-6 manipulator with a stationary base For the IRp-6 manipulator with a stationary base 19 kinematic model parameters were improved. They are presented in the table 3.1. For the initial parameters, the value of the objective function W 3.5 was equal to 4923. For the parameters computed during the optimization process, the value was reduced to 4654, thus the improvement of accuracy is 5.4%.

27

Parameter Value Description Initial Computed Link lengths A2 lower arm 0.45 0.4596227984 A3 upper arm 0.67 0.6729483414 D5 wrist 0.19 0.1902767544 Geometric synchronization poses geom pos 1 column -6.0 -7.7597363338 geom pos 2 lower arm -6.596 -8.7536674638 geom pos 3 upper arm -3.879 -7.5354711738 geom pos 4 wrist T 153.31 153.1366146170 geom pos 5 wrist V 316.659 309.5910028992 geom pos 6 new dof 790.0 796.0265815184 Conversion rates T10 0.0 0.0111433944 T50 0.0 0.0903181306 T60 0.0 0.0209247043 SL123 77895.25 78379.4388318017 MI2 60902.55 60059.3311246322 MI3 -44100.0 -42863.8177016819 NI2 -29346.68 -32453.0983030526 NI3 -51240.0 -53294.476236785 L02 220.3374 211.8107610445 L03 183.8348 187.6529392466 Name

Unit [m] [m] [m] [rad] [rad] [rad] [rad] [rad] [rad] [-] [-] [-] [-] [-] [-] [-] [-] [-] [-]

Table 3.1: Results of the kinematic model parameters optimization for the IRp-6 manipulator with a stationary base Experimental results for the IRp-6 manipulator mounted on a track For the IRp-6 manipulator mounted on the track 20 kinematic model parameters were improved (3.2). The value of the objective function W (3.5) for the initial parameters was equal to 14739. For the parameters computed during the optimization procedure, the value was reduced to 13313, thus the resulting improvement of accuracy was 9.7%.

3.2.5

Computations of the local correction matrix

The idea of local correction is based on the use of specially constructed matrix, which is added to the existing kinematic model to improve the end-effector positioning precision only in a relatively small region, in which the measurements were collected. The computed configuration in the ith measurement point can be rewritten in the form of an eight-element vector xi , in which three first elements describe positions of the πK coordinate frame in relation to πR coordinate frame, and the remaining describe their relative orientations: T x i = x1 x2 . . . x 8 T (3.8) yico zico h0 sin(αico ) h0 cos(αico ) hβico h0 sin(γico ) h0 cos(γico ) = xco i Because the α and γ angles are from the range h−π, πi, where the −π and π are not distinguished, thus we decided to represent those angles as four variables: sin(α), cos(α), 28

Parameter Value Description Initial Computed Segments lengths A2 lower arm 0.45 0.4647408378 A3 upper arm 0.67 0.6748259397 D5 wrist 0.19 0.1967400282 Geometric synchronization poses geom pos 1 track 0 -0.0116959770 geom pos 2 column -6.0 -7.1850003579 geom pos 3 lower arm -6.596 -23.7332554780 geom pos 4 upper arm -3.879 -4.0064593397 geom pos 5 wrist T 153.31 153.6764262556 geom pos 6 wrist V 316.659 356.0929062166 geom pos 7 new dof 790.0 791.4409465411 Conversion rates T10 0.0 0.0049894761 T50 0.0 0.0008975454 T60 0.0 0.0056050066 SL123 77895.25 79744.6384643955 MI2 60902.55 65788.6345921417 MI3 -44100.0 -44612.7793970869 NI2 -29346.68 -22524.8842052068 NI3 -51240.0 -52925.2204852239 L02 220.3374 223.9765458056 L03 183.8348 189.1123019913 Name

Unit [m] [m] [m] [m] [rad] [rad] [rad] [rad] [rad] [rad] [-] [-] [-] [-] [-] [-] [-] [-] [-] [-]

Table 3.2: Results of the kinematic model parameters optimization for the IRp-6 manipulator mounted on a track √ sin(γ) and cos(γ). The positive invariants h and h0 = h are positive integers used for leveling (weighting) impacts of the positions and orientations errors. Likewise, the measured configuration for the ith measurement can be rewritten as eightelement vector yi : T yi = y 1 y 2 . . . y 8 T (3.9) yime zime h0 sin(αime ) h0 cos(αime ) hβime h0 sin(γime ) h0 cos(γime ) = xme i As a result of the local correction in the ith measurement point, an eight-element vector zi is computed, called the improved configuration: T zi = z 1 z 2 . . . z 8 T (3.10) yiim ziim h0 sin(αiim ) h0 cos(αiim ) hβiim h0 sin(γiim ) h0 cos(γiim ) = xim i Each jth, (j = 1, . . . , 8), element of the vector zi can be represented as a linear combination of the xi vector elements: (3.11)

zji

j

=v +

8 X

uj,k xki

k=1

where the uj,k and vj coefficients as invariant for all n zi vectors (for all measurements taken). 29

Herein the objective function is defined as follows: j

(3.12) W =

n X

zji

−

2 yij

=

i=1

n X

j

v +

i=1

8 X

!2 uj,k xki

−

yij

k=1

This measure of the differences between the computed and measured configurations is analogical to the global objective function defined as (3.5). The necessary condition of finding of the objective function (3.12) extremum is that all partial derivatives of the function W j with respect to the uj,m and vj variables must be equal to zero: ! n 8 X X ∂W j (3.13) =2 vj + uj,k xki − yij xm i = 0, m = 1, . . . , 8 ∂uj,m i=1 k=1 n

8

X X ∂W j j = 2 v + (3.14) uj,k xki − yij j ∂v i=1 k=1

! =0

The equations (3.13) and (3.14) can be transformed into: ! n n n 8 X X X X k j m xm x + v x = yij xm (3.15) uj,k i i i i , m = 1, . . . , 8

(3.16)

k=1

i=1

8 X

n X

j,k

u

i=1

! xki

j

+v n=

i=1

k=1

n X

i=1

yij

i=1

Those both equations can be rewritten as one matrix equation: (3.17)  Pn

i=1

x1i x1i

Pn

i=1

x1i x2i . . .

Pn

i=1

x1i x8i

Pn

i=1

x1i



    Pn Pn Pn Pn 2  2 8 2 1 2 2   i=1 xi xi i=1 xi   i=1 xi xi i=1 xi xi . . .       .. .. . . . .. .. ..   . .       Pn P P P n n n 8 2 8 8 8   i=1 x8i x1i x x . . . x x x i=1 i i i=1 i i i=1 i      Pn P P n n 1 2 8 ... n i=1 xi i=1 xi i=1 xi

uj,1 uj,2 uj,3 uj,4 uj,5 uj,6 uj,7 uj,8 vj

 Pn j 1 i=1 yi xi P n j 2   i=1 yi xi P   n j 3   i=1 yi xi   P n j 4   i=1 yi xi   P n j 5 = i=1 yi xi   P    n y j x6   Pi=1 i i   n y j x7   Pi=1 i i   n y j x8 i i Pi=1 n j i=1 yi 

              

Solving that set of linear equations results in the values of the uj,m and vj coefficients. Eight such sets can be formulated (for each j = 1, . . . , 8). It is worth noting that uj,m and vj can be computed only when the measurement set contains more than eight points (n > 8). Attaching the local correction matrix to the kinematic model Herein we present the method of attaching the local correction matrix both to the forward and inverse kinematics. 30

In the forward kinematics, the x vector (3.8) (result of the standard forward kinematics computations based on the motor encoder readings) is used for the calculations of the corrected z vector as follows:        z1 u1,1 u1,2 . . . u1,8 x1 v1  z2   u2,1 u2,2 . . . u2,8   x1   v 1         (3.18) z =  ..  = Ux + V =  .. .. ..   ..  +  ..  ..  .   .     . . . . .  8 8,1 8,2 8,8 8 z u u ... u x v8 Having this vector computed, the improved pose of the manipulator end-effector can be calculated easily: xim y im z im (3.19) αim β im γ im

= z1 = z2 = z3 = atan2(z 4 , z 5 ) 6 = zh = atan2(z 7 , z 8 )

Using the inverse kinematics, and assuming the z vector to be known (improved pose of the end-effector), the x vector is computed: T (3.20) x = x1 x2 . . . x8 = U−1 (z − V) Those values are used subsequently in the calculations of the unimproved (original) pose of the end effector: xorig y orig z orig (3.21) αorig β orig γ orig

= x1 = x2 = x3 = atan2(x4 , x5 ) 6 = xh = atan2(x7 , x8 )

which are used as input for the standard inverse kinematics. Experimental results for the IRp-6 manipulator with a stationary base For the IRp-6 manipulator with a stationary base the following values were computed: U  = 1.00123527237913                

... −0.00037105583095 ... −0.00122742082482 ... −0.00253743914919 ... −0.01861588449429 ... −0.00048343519677 ... 0.02117035415738 ... −0.01984244805701 ...

V h =

0.00188135318844 0.00372210726033 0.99938488069085 −0.00499632036303 −0.00039707983306 −0.00757550328245 0.00504473053142 −0.00586800653622 0.01701840738223 −0.05038417986926 −0.00379138474349 1.04382468803499 −0.03687413581076 0.10327365191154 0.03556774024293 −0.09565189208996

−6.263354010007 ...

0.943899840117 59.629429066554

0.00113276382791 −0.03860948988074 −0.00240349546448 0.00491110250914 1.00048112420428 0.03836462009622 0.00211612009187 −0.01226508203035 −0.00380075246217 0.39608213572882 −0.01526799808678 0.19883191634835 −0.01304622643943 1.17875445807113 0.00225394424412 0.12725479557776

22.590522404760 −23.465010764543

31

0.02573730756473 −0.03475932913568 −0.01124199707010 0.00207545902958 0.04498283088878 0.02938455577714 1.07723327661961 −0.00024093303637 −0.19390852788752 0.45498455492213 0.16005574671181 0.11140847982097 0.02973571139614 0.10486353232000 0.00149379899653 1.19715784401706

20.745128936134 119.022426949581

0.00279295091246

...



0.00450577580455

...

−0.00269076946100

...

−0.01296843860476

...

1.07195110524703

...

−0.01129987005987

...

0.03283105057943

...

−0.03662393829970

...

               

121.320437533315

...

i

The value of the objective function W (3.5) for the initial parameters was equal to 4923. The improvement of the kinematic model parameters reduced this value to 4654, and the use of the local correction matrix decreases it further to 3564. In the measured fragment of the workspace the total increase in accuracy is thus equal to 27.6%. Experimental results for the IRp-6 manipulator mounted on a track As a result of the computations for the IRp-6 manipulator mounted on a track the following values were obtained: U  = 0.98863152869936                

... −0.02296355832038 ... −0.00291729671382 ... 0.00058762405443 ... −0.01036692711327 ... −0.00799404226927 ... −0.01047115346649 ... 0.01156999985433 ...

V h =

26.13558453368023 ...

−0.04554003435648 −0.01715989288080 0.98592220292414 0.02226557178642 −0.00229327750147 −0.00623792109828 0.00043759289332 0.00423641257356 −0.00159006637621 −0.02747709067297 −0.00008628657253 0.97377774326748 −0.03898799899165 −0.02163246067902 0.02773871022473 0.01074487735887

9.28858725633472 1.21834001829848

0.00259961465669 −0.10370987442889 −0.00410497369220 −0.03967996340634 1.00132153420898 −0.07802813432863 −0.00409956915568 0.00804703421136 0.00984175302942 −0.00649369353654 −0.00620942405903 −0.07102386602628 −0.00180345275537 0.89866944025084 −0.00127044264064 0.12067471316300

−17.78251973679289 55.91479880944826

0.02184334199876 −0.06757338190846 −0.07751214667587 −0.03756061253080 −0.01599834269632 −0.09686495562346 0.98078417818851 0.01291533587755 −0.09210788062914 0.01215738121095 −0.03617431768907 −0.05919488334985 −0.19351309470403 −0.13344336591990 0.15384236099726 1.16695884201317

5.72523145587184 −21.58633431594353

0.00243396028763

...



−0.00806134279742

...

−0.01082287540279

...

−0.00102612607773

...

0.97311365684075

...

−0.01945653548424

...

−0.01324115876334

...

0.00539145551955

...

               

32.11857718021201

...

i

For the initial parameters, the value of the objective function W (3.5) was equal to 14739. This value was decreased to 13313 through the improvement of the kinematic model parameters, and further to 7511 through the use of local correction matrix, thus the total improvement of accuracy was equal to 49%.

3.2.6

Relationship between the two manipulator base coordinate frames

The positions and orientations of the measurement unit base coordinate frame in relation to the base coordinate frames of both manipulators was computed with the use of optimization process described by (3.2.3). The position and orientation of πB in relation to πT j is: T dTBj = xTBj yBT j zBT j (3.22) T = 292.90 1447.59 −503.06 [mm]

(3.23)

Tj DTBj = Rx (αB )· Ry (βBT j )· Rz (γBT j ) = Rx (0.0050)· Ry (−0.0067)· Rz (−3.1015)

On the other hand the position and orientation of πB in relation to πP is: T dPB = xPB yBP zBP T (3.24) = 395.53 −666.85 −489.24 [mm] 32

Figure 3.9: Reference coordinate frames of the measurement unit and both manipulators

(3.25)

P DPB = Rx (αB )· Ry (βBP )· Rz (γBP ) = Rx (−0.0130)· Ry (−0.0026)· Rz (−3.0956)

Having computed those values enables us to calculate the relationship between πP and πT j coordinate frames as follows:   −101.66 (3.26) dTP j = dTBj − DTBj · (DPB )T · dPB =  2107.85  [mm] −3.48 

(3.27) DTP j

 0.9963 −0.0861 −0.0036 0.9961 −0.0179  = DTBj · (DPB )T =  0.0860 0.0051 0.0175 0.9998

33

Chapter 4 Dual arm robot system solving a Rubik’s cube puzzle Rubik’s cube combinatorial puzzle was invented by Ern˝ o Rubik of Hungary in 1974. It consists of two distinct components: the core and the outer cubes. The shape of the core can be envisaged as a central cube with six attached octagons, one on each face. Each octagon is attached to allow free rotation in either direction. Attached to this core piece are the outer cubes. The standard 3 × 3 × 3 version of the Rubik’s cube consists of 27 sub-cubes, or cubies, with different coloured stickers on each of the exposed sides of the sub-cubes. It is divided into three 3 × 3 × 1 layers, or slices, along three mutually perpendicular axes, such that each layer can be rotated a quarter turn or a half turn clockwise or counterclockwise with respect to the rest of the cube. The slices divide each face of the cube into nine smaller squares: called tiles or facelets. These are termed elementary moves. The slices also divide the cube into 33 = 27 pieces, or sub-cubes, 26 of which are visible. In its goal state each of the six faces has its own colour. Each visible piece has one, two or three facelets. The set of facelets within each cubie has a unique set of colours. This makes it distinct from the others. This preserves the orientation of the piece as it changes position as the layers are rotated. As such the cube is a pure permutation puzzle since any state can be described as some permutation of its pieces. The center pieces (with a single tile in the center of each face) and the core in the very middle are stationary, they do not change their position (they only rotate about their centers) relative to each other. Therefore, only twenty pieces actually move, the eight corner pieces with three tiles, and twelve edge pieces with two tiles. Since the seven pieces always stay in the same place no matter what moves we make, we can uniquely describe any Rubik’s cube by the position and orientation of these twenty sub-cubes. The corner pieces can be rotated any of the three ways, and the edge pieces can be twisted either of the two ways. Thus, the corner pieces have three orientations each, and the edge pieces have two orientations each. There are eight corner-pieces, so there are 38 possible corner orientations, but some of those orientations will be unattainable. The orientation of one of the corner cubies must be determined by the rest (we divide the total number of corner orientations by 3), then there are 37 = 2187 possible corner configurations. Given that there are two orientations for twelve edge pieces, we have that there are 212 possible edge cubie orientations, but again, some of these states will be unattainable. The total number of edge flips is even. Also, on the Rubik’s cube there is a constraint, due to the permutation parity. The parity of the permutation of all twenty pieces (corners and edges) must be even for the cube position to be solvable. Therefore, we have 211 = 2048 different edge pieces orientations. There are 8! = 40320 positions we can place all eight 34

of the corner cubies, and there are 12! = 479001600 different positions to place the edge cubies. Therefore, the total state space for solving a scrambled Rubik’s cube is sized at (38−1 · 8!) · (212−1 · 12!)/2 = 43, 252, 003, 274, 489, 856, 000 ≈ 4.3 × 1019 . This is the number of legal states that are attainable by some sequence of elementary moves. The total number of possible positions attainable by taking apart and re-assembling it is twelve times this number. Obviously, this number of states is prohibitively large for any sort of a brute force search technique, which is why specialized algorithms are needed to solve the Rubik’s cube puzzle.

4.1 4.1.1

Localization of the cube Task formulation

Robotic manipulation of an object requires that this object must be grasped first. If vision is used as the robot’s primary source of information about the environment, the object of interest must be identified in the image and subsequently localized in 3D space with respect to the manipulator. Localization of an object in the image is based on matching its certain previously defined features such as: shapes, colors, edges, texture etc. and filtering out the excess of visual information. Once these features are found, it is possible to calculate the 3D pose of the object with respect to the camera coordinate system and subsequently the manipulator coordinate system (this is accomplished with a simple hand-eye transformation).

4.1.2

Theoretical considerations required for the solution of the task

Finding a known object in a visual scene is a relatively simple task. It requires matching a set of previously defined features of the object with the features observed in an image. The choice of features and the matching algorithm is arbitrary and it depends primarily on the specification of the object and it will not be discussed here. This task becomes much more difficult if we want not only to localize the object in the scene, but also to find its 3D pose (localization and orientation) in relation to the camera. It has been shown in [5] that if at least four points on the object can be distinguished and their coordinates in the object frame are known, it is possible to calculate the object’s pose in the camera frame (and thus with respect to the manipulator, provided the hand-to-eye transformation is known). This, however, implies a slightly different approach to image recognition. Simply finding matching features is not sufficient. We must find such features that their relative position is known to us (it must be measured beforehand). It means that we must pick a set of features that can be easily detected and discerned, so that we know which particular feature is which and where it is located on the object. The choice of features depends on the nature of the object and must be carefully selected, so as not to induce ambiguity. For example, a white object with numerous randomly distributed colorful dots is relatively easy to find in the image, but deciding which dots to use to build a local coordinate system might be very hard, if not impossible. Good features are such that are unique or regularly spaced on the object and, what is not to be forgotten, can be easily measured (we must know their 3D location in the object coordinate frame).

35

4.1.3

Implementation

In our experiments we used a Rubik’s cube as an object to be identified, localized and manipulated. It should be noted that although it is a cube it is very metamorphic. The pattern of tiles (i.e., colors) on each face of the cube, from our point of view, is random, due to the enormous number of permutations of the puzzle. Thus, the features we are looking for are the colored tiles on each of the cube’s faces (4.1), rather than the whole faces of the cube.

Figure 4.1: The Rubik’s cube used in the experiments Although there are 6 × 9 = 54 tiles, they are regularly distributed. However, only 27 of them at the most can be visible at any one time. Usually one of the faces is best visible, i.e., it dominates the image. Sometimes we say that this is the dominating face. Thus, it is not very difficult to find four corner tiles of the face and use their centroids to localize the cube in the 3D space (the size of the cube is known a priori, so the depth information can be estimated relatively easily). Since geometry, color and distribution of the features of the cube is known, a color image segmentation routine is used, which finds all adjacent quadrilateral and convex, practically parallelogramic-shaped objects of almost homogeneous color. All other shapes with (or without) matching colors are disregarded. Region segmentation of color images is a crucial step in many vision applications. During that process individual pixels of an image are classified into one of a finite number of color classes and pixels belonging to the same class are grouped together for further high level processing. The most common solutions to this problem are: linear color thresholding, nearest neighbor classification, color space thresholding and probabilistic methods [1]. These approaches enable either precise (or human-like) color segmentation at low speed or real-time processing with relatively poor accuracy (in comparison with human performance). In our case, however, both accuracy and speed are vital, for the cube is to be identified and followed by a robot manipulator in real time. An interesting solution to this problem was proposed in [1]. We decided to use this method for feature extraction, however, since the original method does not allow for color variations and defines colors as constant rectangular blocks in the color space, it often fails to properly separate neighboring regions of the same color. We modified the original method so that it is far more robust to subtle color changes. Our approach is equivalent to applying the original method several times, but without the most computationally intensive part of the algorithm (color 36

classification), which yields a significant performance gain. Several color spaces are in use in color vision applications, however the most popular are such three dimensional spaces where chrominance is coded in two of the dimensions and intensity is coded in the third (HSV, HSI, YUV etc.). These transformations minimize the most significant correlations between colors and the resulting space can be partitioned into separate hyperrectangles (cuboids) for all of the color classes by constant value thresholding in each dimension. Thus a color class is defined by a set of six thresholds: a lower and a higher value for each dimension. We used a modified version of the popular HSV color space. In our case hue is downsampled to 256 samples for easier byte representation, as explained later. It is also rotated clockwise by a value of 30o so that red and orange colors (or their hue to be precise) lie in the range of 10-31 instead of a conjunction of two ranges 246-255 and 0-11 (thus we have one hyperrectangle less to deal with). In order to classify a pixel, six comparisons are necessary (in the most naive approach) for each color class. In the case of many color classes the number of comparisons grows rapidly. Obviously, it can be reduced by applying some decision tree optimization, but still it requires multiple comparisons per pixel which is time consuming, especially for high resolution images. The implementation proposed in [1] uses a boolean valued decomposition of the multidimensional threshold, which is stored in arrays (usually only three dimensions are used). There are as many array elements as there are values of each color components (usually 256). Therefore class membership of a pixel can be computed as a bitwise AND operation of the elements of each array (that results in two AND operations for a three dimensional color space). Consider the following example, that illustrates the above described concept. Suppose the HSV color space is discretized to 10 levels in each dimension (instead of 256 as in real application). Let ’red’ color be defined as: H[] = 1,1,1,0,0,0,0,0,0,0; S[] = 0,0,0,0,0,1,1,1,1,1; V[] = 0,0,0,0,0,0,0,1,1,1; Thus, in order to check if a pixel with HSV values of 2,5,8 respectively belongs to the class ’red’ one must evaluate a simple expression H[2] AND S[5] AND V[8]. In our case the result is 1, which means that this color belongs to the class ’red’. Another color, say ’blue’, might be defined as follows: H[] = 0,0,0,0,0,0,0,1,1,1; S[] = 0,0,0,0,0,1,1,1,1,1; V[] = 0,0,0,0,1,1,1,1,1,1; Instead of dealing with a set of separate arrays for each color, one may combine them using bitwise positioning, where the first bit represents ’red’ and the second bit represents ’blue’: H[] = 10,10,10,00,00,00,00,01,01,01; S[] = 00,00,00,00,00,11,11,11,11,11; V[] = 00,00,00,00,01,01,01,11,11,11; When 32-bit integers are used, the set of arrays becomes as follows: H[] = 2,2,2,0,0,0,0,1,1,1; S[] = 0,0,0,0,0,3,3,3,3,3; V[] = 0,0,0,0,1,1,1,3,3,3;

37

The result of H[2] AND S[5] AND V[8] equals 2 (or 10 bitwise), which means that this particular color is ’red’ but not ’blue’, while the result of H[7] AND S[6] AND V[5] equals 1 (or 01 bitwise), which indicates that this color is ’blue’ but not ’red’. This ability to determine pixels’ membership of multiple classes simultaneously is the most important advantage of the algorithm proposed in [1]. In the described implementation each array element is a 32-bit integer. Hence, it is possible to evaluate membership to 32 color classes with just two AND operations. This is an enormous performance gain in comparison to classical approaches where several conditions must be checked for each pixel. After the color classification and before region merging with an efficient tree-based union-find with path compression method the classified image is run length encoded (RLE compression stores a series of identical values in just two fields: length and value). This operation can speed connected components analysis in many robotic applications where images contain relatively large areas of pixels of the same class (large objects of the same color). However, in the case of noisy images or many little objects the RLE compression might have the opposite effect (many single values will still be stored in two fields instead of one). The undisputable advantage of the described algorithm is its fast performance and linear scalability with the number of pixels and color space dimensions. The drawback of the method lies in the fact that the thresholds defining color classes are constant and the method is unable to cope with certain dynamic images. Consider the following example illustrating the problem. Suppose we have an image with a number of adjacent, but separate, red objects. Some of the objects are dark, some are bright. That should not be a problem for a color space like HSV or YUV, but the actual results may differ from expected. If we define a color class to span over both bright and dark reds, dark red objects will be properly segmented, but some bright red objects might be segmented into one big area. This is mainly due to color bleeding effect that causes pixels separating bright areas to acquire their color to some extent. If we decide to divide the color class into two subclasses, one for dark reds and the other for bright reds, the segmentation result could be correct. However, there might always be red objects of varying intensity such that some of their pixels belong to one class and some to the other. As a result such objects will not be correctly segmented. See figure 4.2 for illustration. It is possible to select correct thresholds for one image or even for a number of images, provided the lighting conditions and observed objects do not change too much. However, in most cases, especially in dynamic robotic applications, these conditions cannot be met and there will always be some regions that are segmented incorrectly. This can lead to misinterpretation of the observed scene and can severely influence the robustness of the system. In the next stage of processing the pose of the cube is calculated from the image features that have been obtained by the above described method. ∗ Let G rG = [G xGi , G yGi , G zGi ]T be the ith distinct point of the cube with respect to i the goal frame (in the task space). The goal frame is associated with cube that has ∗ ∗ to be grasped. Let G r∗ = [G rG , ..., G rG ] be a matrix containing k such points. The 1 k points can be expressed with respect to the camera frame C r∗ = C TG G r∗ . The pinehole ∗ projection coordinates of G rG are [ai , bi ]T , where ai = C xGi / C zGi and bi = C yGi / C zGi . i The distorted point coordinates (due to the shape of the lens) are [xi , yi ]T , where:   xi = ai 1 + kc1 (a2i + b2i ) + kc2 (a2i + b2i )2 + 2 kc3 ai bi + kc4 (3 a2i + b2i ) (4.1)  yi = bi 1 + kc1 (a2i + b2i ) + kc2 (a2i + b2i )2 + kc3 (a2i + 3 b2i ) + 2 kc4 ai bi

The kc1 and kc2 are parameters of the lens associated with radial distortion. The kc3 and kc4 are parameters of the lens associated with tangential distortion. The resulting pixel

38

Figure 4.2: Original image (top) and two incorrect segmentation results obtained with one color class (left) and two color classes (right) for the red color. Boundaries of segmented regions are marked white. Small regions are not depicted as they are treated as noise. Notice merging of segments in the left image caused by color bleeding. Although all the segments in the right image are disjoint, the dark regions are not detected correctly as the bottom threshold for dark reds was set too high coordinate vector fi = [pi , qi ]T (i.e., image features): pi = λx xi + cx (4.2) qi = λy yi + cy where λx and λy is the focal length scaled by the width and hight of the light-sensitive element (the CCD matrix); [cx , cy ]T is the translation of the lens center with respect to the CCD matrix center. The four centers of the corner tiles (in the image space) on the dominating face represent the image features: [f1 , ..., f4 ]. The four centers of the corner tiles in the task space ∗ ∗ [G rG , ...G rG ], with respect to the center of the cube, formh the 3D model of the 1 4 i cube. Knowing: kc1 ÷ kc4 , λx , λy , cx and cy we can compute: A = [a1 , b1 ]T , ..., [a4 , b4 ]T . (4.3) A =

C

TG∗ G r∗

where C TG∗ is the homography matrix (homogeus transformation between the goal and the camera frame plus the projective transformation), which can be computed as: (4.4)

C

TG∗ = A G r∗+

where G r∗+ is a pseudo-inverse of the model matrix G r∗ . Normalization of the homography matrix C TG∗ makes possible the elaboration of the homogeneous transformation between the goal and the camera frame C TG . Since (4.1) is non-linear, C TG is not calculated accurately, but an iterative algorithm approximates more accurately C TG in a few steps. 39

4.1.4

Experimental results

Our first solution to the problem of detecting tiles in the image was based on adaptive thresholding. The thresholds defining the color classes were dynamically adjusted according to local line histograms (or intensity histogram of pixels lying in a line) calculated in a region of interest (ROI) after initial (coarse) segmentation. This enabled us to modify the thresholds based on peaks and valleys in a number of line histograms traversing the ROI. Although this method is quite successful, it is far too slow for real-time applications requiring full resolution image analysis. Another solution which we tested involved multiple classes for a single color. Since some of the classes are overlapping (to avoid problems at class boundary mentioned in the previous section) the segmentation routine must be run at least two times (overlapping classes had to be processed separately to avoid ambiguity). This time the processing speed was close to real-time (for two passes) and the segmentation results were satisfactory, but still not as good as we expected. We found that using three to five subclasses for a single color gives good results, but sometimes leads to partitioning of regions with color gradients. Such a situation can occur quite often in real life situations (colors are never solid) especially around edges. This affects mainly small objects, where the interior would belong to one class and the exterior to another, while human segmentation clearly indicates a single color class. This lead us to an idea of gradually ”inflating” of the color classes for one color. Instead of segmenting all of the color classes at once and merging the brighter and the darker segments together (which is not so obvious) we apply the algorithm a few times, each time lowering the lower intensity threshold. For that purpose we define the colors and their shades in a file in the following way (this example shows only two colors, each having five shades): [colors] [g1] (255,0,0) 1 brightest red (255,0,0) 2 (255,0,0) 3 (255,0,0) 4 (255,0,0) 5 darkest red [g2] (0,255,0) 6 brightest green (0,255,0) 7 (0,255,0) 8 (0,255,0) 9 (0,255,0) 10 darkest green [thresholds] [g1] (10:31,211:255,211:255) (10:31,110:210,211:255) (10:31,170:255,181:210) (10:31,170:255,130:180) (10:31,90:255,100:101) 40

[g2] (135:175,61:255,211:255) (135:175,56:255,171:210) (135:175,56:255,131:170) (135:175,50:255,91:130) (135:175,50:255,51:90) The section ’colors’ defines groups of colors. Each color has its original RGB values (or this is the color we expect to find, regardless of its brightness and saturation) and class number. Below, in the ’thresholds’ section these colors are defined in the HSV space. Usually H is constant, S changes slightly and V varies from bright to dark. In this way we start with only the brightest regions segmented (only the first color from each group is used) and ”inflate” them with each step of the routine (as the darker surroundings are gradually added each time the color with lower threshold is used). We end up with an excess number of segments (usually a few segments per region) that must be filtered out. A simple filter leaving only the second largest segment within a given radius is good enough. The segmenting results are very good (see figure 4.3), however the execution time grows substantially – the entire algorithm must be repeated many times over the entire image.

Figure 4.3: Almost human-like segmentation results using our modified approach and five thresholds. Boundaries of segmented regions are marked with white lines, small regions are not depicted as they are treated as noise Looking carefully into the original algorithm we found that its first, and usually the most time consuming part, namely, color classification of all of the pixels in the image can be performed only once with minimal changes to the rest of the segmentation routine. This time, all the subclasses (thresholds) are considered simultaneously. The first pass of the algorithm is identical, as in the original method, whereas consecutive passes omit the color classification part and merge successive color subclasses during RLE encoding. Further processing is unchanged. As a result we obtain the same segmentation results as depicted in figure 4.3, but the processing time can by even 50% shorter (compare Table 4.1). Experiments show that although the final execution time is longer than for a single pass of the original method, using up to five thresholds still enables real-time (below 40 ms) performance. Moreover, since usually three thresholds are enough, there is still some time left for additional calculations between the image frames. The main disadvantage of our proposal is that the original limit of 32 different colors is further narrowed proportionally to the number of thresholds for a single color (five thresholds per color allow simultaneous segmentation of 6 different colors only, but three thresholds make it 10). That original 41

limit, however, can be easily doubled if 64-bit integers are used instead of 32-bit integers in the current version. Number of thresholds 1 2 3 4 5

Original method [ms] 16 33 49 63 81

Our method [ms] 16 22 27 33 39

Performance gain [%] 0 33 45 48 52

Table 4.1: Average execution time of the original method and our modification for different number of thresholds. The performance tests were conducted on a set of over 50 images containing a Rubik’s cube, on a 2.13 GHz PentiumM PC. Results may vary depending on the nature of images (amount of noise, number and area of the segmented regions, etc.) After color segmentation we obtain an excessive number of regions, which are filtered (see figure 4.4) according to: • area (A), • boundary length (L), • circularity (defined as C =

L2 ), 4πA

• number of vertices (each region that is not filtered out earlier is approximated with a polygon), • number of neighbors (secluded quadrilaterals are disregarded). Usually after filtration we have only the cube tiles left. It does not mean, however, that all the tiles are successfully detected. Occasionally some of the tiles are not detected and in a few situations there are some quadrilaterals found that do not belong to the cube. Obviously, we assume that there is only one cube visible and there are no objects resembling it in terms of both colors and shapes in the field of view (finding a real cube sitting on a tablecloth with Rubik’s cubes painted on it would not be an easy task). After segmentation and filtering we have to identify the cube faces in order to be able to attach a coordinate system to one of them, preferably to the corner tiles for higher accuracy. This is done by a number of simple heuristic rules such as: • central tiles have the greatest number of neighbors, • corner tiles have the least number of neighbors, • tiles on one face are approximately of the same area. Even if some of the tiles are not detected the algorithm is able to find the biggest possible square present on the cube and calculate the cube’s pose with respect to the camera. The procedure described above is successful in most cases and can be applied in real time, especially when ROI is used. The localization precision is ±1 mm in the plane perpendicular to the camera and ±5 mm along the camera axis. Naturally it depends heavily on the camera-object distance or, to be more precise, on the size of the cube 42

Figure 4.4: Detected regions matching the cube colors and consecutive filtering results (left to right, top bottom): initial segmentation, no filtering (1950 regions detected), area and boundary length filter (36 regions left), additional circularity and quadrilaterals filter (20 regions left), additional neighbors filter (18 regions left) image. The above results were obtained with a cube image of roughly 100 × 100 pixels. However, it should be remembered that the cube can be grasped by the robot only if it is in the workspace. The cubes that are too far are not interesting. The most important problems when detecting an object in motion is motion blur and interlacing effect (see figure 4.5). Although the latter can be easily removed by a number of methods (the simplest one is disregarding every second line of the image), if the former is too strong there is little that can be done, especially in real time. That is the object can still be detected, but precise localization of its features will not be possible. When it comes to deinterlacing, one must not forget that the simplest method mentioned above halves the vertical resolution. This has two effects: the system performance substantially increases (an advantage), but the localization precision decreases (a disadvantage). Obviousely there are other methods of deinterlacing that do not change the image resolution, but they are too slow to be used in our real-time system.

43

Figure 4.5: Motion blur and interlacing effect observed when an object is moving prevent correct recognition of the object

4.2 4.2.1

Acquiring the cube – visual servoing Task formulation

The task is formulated in the following way. One of the robots has to acquire, from an operator, a Rubik’s cube. When the cube is moving the gripper should approach the vicinity of the cube and track it retaining a safe distance. When the cube stops moving the gripper should be transferred to a position enabling it to catch the cube. The assumptions about the visibility of the cube, formulated in the previous section, should be fulfilled.

4.2.2


Visual servo control can be classified using several criteria. Depending whether the endeffector is in the field of view of the camera and can be recognized by the image processing subsystem we can distinguish two types of visual servo control [6]: • end-effector closed-loop servos (ECL) – here the end-effector is perceived by the imaging system, • end-effector open-loop servos (EOL) – here the end-effector is not seen by the imaging subsystem. Depending on the space in which the visual error (i.e., either the difference between the current end-effector pose and the desired one, calculated on the basis of the information contained in the image, or the distance between the current and the desired image features) is calculated we differentiate between [6]: 44

• position-based servos– PB, • image-based servos – IB. Depending on the location of the camera (or cameras) we have: • stand-alone camera servos – SAC, • eye-in-hand servos – EIH (we are not considering attaching the camera to other links of the manipulator). Since the above classifications are mutually independent, in the following text we shall use combination of abbreviations, e.g., PB-EOL-SAC. In general, control of the end-effector motion requires: • the knowledge of the relationship between the pose of the end-effector and the current configuration of the kinematic chain (joint angles), i.e., either the kinematics model or the jacobian of the manipulator, • the pose of the camera in relation to the robot base or the end-effector coordinate frame. Each of those relationships depends on some parameters. The knowledge of the exact values of those parameters is vital. The measurement procedure and the computations leading to the expression of those values is known as calibration Because there are two relationships we have two types of calibration. The calibration dealing with the robot kinematic model or the robot Jacobian is known as robot calibration, whereas the parameters of the pose of the camera in relation to the robot is named camera-robot calibration. When the camera is placed on the effector, the pose of the camera in relation to the end-effector is named camera-end-effector calibration. The following discussion will explain which structures of the visual servo control systems are immune to which types of calibration and how calibration, or rather lack of thereof, influences the quality of servo-control. PB visual servos locate the object to be grasped (that is located at G) in the image plane and then calculate its pose with respect to the global frame in the task space. It is possible to retrieve depth information by using two cameras (stereovision) or by knowing the size of the object or by using other sensors, e.g., a laser scanner. During the calculation of the object pose, knowledge about the pose of the camera (or cameras) with respect to the global frame is used, so proper robot-camera calibration is vital. IB visual servos calculate the error between the desired image feature and the measured image feature, e.g., we can use characteristic points of the object projected onto the image plane as the image features. The task error can be calculated by using an inverted image Jacobian (an error is treated as a small increment, which is proportional to the velocity, because it can be realized in some constant time period). Sometimes a mapping between the image space and the task space can include some higher derivatives (or its numerical approximations). In such cases the Jacobian can be replaced by, e.g., Hessian. A matrix of the mapping is called an interaction matrix. In general the interaction matrix can be treated as extension of the Taylor series around the observed features [13]. Drawbacks of different types of visual servoing schemes are summarized in table 4.2 (in which the presence of an appropriate matrix T denotes the necessity of a certain type of calibration, and ε represents the dependence of the error on the robot kinematic model 45

PB EOL ECL

SAC 0 TE , 0 TC , ε 0 TE

EIH 0 TE , E TC 0 TE , E TC (fE ) = const

IB SAC EIH 0 not feasible TE , E TC 0 TE not feasible

Table 4.2: Comparison of visual servo control schemes calibration). In the case of a stand-alone camera and the end-effector not being observed (PB-EOL-SAC) both the camera-robot calibration (resulting in 0 TC ) and the robot kinematic model calibration (producing 0 TE ) are necessary. If an end-effector is being observed (PB,IB-ECL-SAC) the camera-robot calibration is unnecessary, but the robot calibration still is; however, the control error does not depend on the robot calibration. Nevertheless, the robot kinematic model is still used in control of the effector. When the camera is placed on the effector and thus it is not capable of observing the effector (PB,IB-EOL-EIH), the camera-end-effector calibration E TC and the robot calibration are vital, but the control error is independent of the robot calibration. When the end-effector is being observed (PB-ECL-EIH), the camera-end-effector calibration is not necessary, because the end-effector pose with respect to the camera frame can be estimated from the obtained image. The IB-EOL-SAC visual servoing scheme is not feasible, because the end-effector is not present in the image, because EOL scheme is utilized. IB-SAC scheme requires the observation of the end-effector to retrieve its image features to compute the control error in the image space. The IB-ECL-EIH visual servoing scheme is not feasible, because an end-effector does not move with respect to the camera frame in EIH configurations, so the end-effector velocity observed in the camera frame is zero and so the image does not hold useful information to compute the velocity in the task space (pseudo inverse Jacobian). Position based visual servos (PB) are sensitive to the quality of calibration of the camera parameters (e.g., lens focal length) and image noise. Image based visual servos (IB) do not have these drawbacks and, moreover, it is also much easier to keep the image features in the field of view, especially for an eye-in-hand camera. Due to a non-linear mapping (when the image points are treated as features) between the task and the image space, small changes in the image space can produce very drastic changes in the task space or even physically inexecutable motions. For example, in the IB-EIH case, when the rotation of the object grasp position in relation to the end-effector is too large instead of the rotation of the end-effector, and thus the camera mounted in it, the end-effector retreats, thus causing the camera retreat problem [2]. This occurs in a most drastic form, when the camera is rotated exactly π rad around the optical axis in relation to the object grasp location. Here the discrepancy between the obtained and the desired image is produced by increasing the distance between the object and the camera (i.e., withdrawing the camera) instead of rotating the image. As the distance increases the image of the object shrinks, thus the discrepancy seemingly decreases. In consequence the generated trajectory of features (points) in the image space is a sequence homothetical transformations, and not rotations – it causes the camera retreat along the optical axis instead of its rotation. The solution of the problem can be the rotation control in the task space (hybrid methods) [14], a transformation of task Cartesian coordinates into cylindrical coordinates [7] or the use of other image features than points (e.g., utilization of moment methods [20] providing information about the orientation of the object). The drawback of all of these methods is a significant complication of the image Jacobian. Still another approach is to use the image Hessian instead of the Jacobian [13]. However, although the simulation results are promising, the results obtained on a real system can 46

be not as good, because of the noise affecting the approximation of the second derivative of the value of the image feature (acceleration of the image feature) obtained by double subtraction. Complication of the interaction matrix is also the result of applying this method.

4.2.3

Implementation

We chose a position based, end-effector open loop servo with stand-alone camera (PBEOL-SAC) for the Rubik’s cube puzzle solving system. The principle of its operation is described below. The centroids of four corner tiles of the dominating cube face form the image features fG . Since those points are coplanar, the pose of the cube G with respect to the camera coordinate frame C, can be estimated – in effect C TG is obtained. To do this properly the camera intrinsic parameters (e.g., focal length, principal point and optical distortion) and camera extrinsic parameters (the pose of the camera in relation to robot coordinate frame 0 TG ) must be computed beforehand, i.e., adequate calibration performed. This pose is transformed into the robot coordinate frame 0, thus 0 TG is obtained. The MP receives from the VSP the matrix 0 TG and transforms it into an indirect desired pose 0 TD which is located in the vicinity of the cube: (G TD ). As a result, even when the cube moves it can be tracked from a safe distance. The current end-effector pose 0 TE is obtained from the EDP (through the ECP) which solves the direct kinematics problem, so the control error ε (E TD ) can be computed. The macrostep generator produces a motion trajectory M using the control error. The trajectory M is a sequence of macrosteps, i.e., a sequence of poses 0 TE 0 through which the end-effector is to pass. Only the first pose of M , i.e., 0 TE 0 , is sent to the EDP. This pose is called the intermediate desired pose. The step generator within the EDP divides the macrostep defined by the intermediate desired pose into steps, each also representing a pose. Each of those poses in turn is transformed by the inverse kinematics procedure into joint coordinates Θd . Θd is used as the desired value for the regulator producing Θ u, i.e., PWM ratio. While the EDP executes the steps of the macrostep, the ECP computes a new trajectory based on the new information delivered by the VSP, what results in smooth and continuous motion. When the cube stops moving (measurements of its pose are between some arbitrarily fixed thresholds), in each macrostep the distance G xD of the indirect desired pose G TD : 

1  0 (4.5) G TD =   0 0

0 1 0 0

0 0 1 0

G

 xD 0   0  1

from the cube starts to decrease – from the initial value of 0 TD until it reaches 0 TG . The end-effector approaches the cube in such a way that the gripper jaws are parallel to the edges of the cube, so that it can be grasped stably. In trying to get hold of the cube directly (without employing the indirect desired pose) the following problem is encountered. When the translation error is being minimized faster than the rotation error the gripper jaws can hit the face of the cube. Of course, if the cube is moved during the approach stage, the matrix G TD is reinitialized causing the gripper to retreat from the cube. So grasping the cube is realized only when the cube is virtually motionless during the approach stage. This resembles the way people hand objects to others. The object being handed over is stopped prior to the other person grasping it – unless the person holding the object is trying to tease the other. Apart from this, the robot has to 47

keep proper orientation of the gripper to catch the cube. A man’s palm is much more flexible, so the mutual orientation of the hand and the object during grasping is not that important. The macrostep generator is based on a proportional controller, decoupled for each component of the pose (i.e., separately for the 3 translations and the 3 rotations). A gain for each component is set empirically to filter the image noise (the regulator works as a lowpass filter).

MP

( )-1

E

T0

.

ε

step generator

* 0

TG

C

.

0

TG

* 0

TC

TE

ECP

0

TE'

direct kinematics inverse kinematics

fG feature pose estimation extraction

EDP

Θ Θd

object image

+

Θ

ε

joint controllers

camera

object view

Θ

u

Robot

environment

VSP

Figure 4.6: PB-EOL-SAC visual servo in the MRROC++ based system solving the Rubik’s cube puzzle

4.2.4


In the experiments the error of pose estimation was measured. The noise influencing the measurements along the X and Y axes, which are parallel to the surface of the camera light-sensitive element, was about ±1 mm. The error of both the robot and the robotcamera calibration was 5 mm, when the object had been placed near the border of the camera imaging region (500 mm × 375 mm). The measured noise along the Z axis was about ±5 mm and the calibration error was about 2% (it cannot be estimated properly due to considerable noise). Rotations around the X and Y axes are subjected to noise of about ±0.04 rad, but around the Z axis of about ±0.01 rad. The calibration errors were minimal. The noise is mainly due to quantization. For example, when two consecutive measurements differ by about one pixel in the image feature space, they differ about 0.2 rad around the X or Y axis in the task space1 . Since the center of a tile is computed as an average of pixels belonging to it, a subpixel accuracy is obtained and thus the resulting noise is reduced. Figure 4.9 shows exemplary trajectories of tracking the cube moving on a conveyor along the Y axis over a distance of 350 mm. Change of the object translations along the X and Z axes and the three rotation angles are caused mainly by the measurement errors and to a lesser extent by calibration errors. Figure 4.9 shows that the calibration deviation is about 0.5%. When using the PB-SAC-EOL scheme the calibration errors are not that large to prevent the cube to be grasped. The errors due to noise can be filtered out if control gains are fixed appropriately. The total translation deviation is not higher than ±5 mm and does not cause problems if only the distance between the 1

if only the observed cube face is parallel to the XY surface, the edges of the cube are 50 pixels long and are parallel to the X and Y axes – arccos((50 − 1)/50) = 0.2

48

0

TE

E=D

E D E 0

TD

TG

TD

G D D

a)

TG

b)

G

E

E D

c)

TD D

E=D=G

TG d)

G

Figure 4.7: Visual servoing: a) approaching the indirect desired pose D, b) reaching the pose D, c) approaching the cube as a resuly of shifting the pose D towards the cube G, d) grasping the cube gripper jaws is at least 12 mm wider than the diagonal of the cube face. The rotation deviation is not higher than ±0.04 rad.

4.3 4.3.1

Identification of the cube’s state Task formulation

Proper identification of the cube’s state, i.e., building the cube model in which position and color of every tile on each face is known, is essential for solving the puzzle. Should a single tile be misidentified the correct solution cannot be reached. Therefore the task of identification of the cube’s state can be formulated as finding the color class membership (within six given classes) of all the tiles of the cube and storing it in a model used later for puzzle solution and motion generation. 49

E

z

E

y

E

x

D

y

D

z

D

x G

y

D

xG

G

z

G

x

Figure 4.8: Coordinate frames affixed to: E, D and G

0.8785

0.878

0.1 0.05 0 -0.05 -0.1 -0.15

0.26 0.258 0.256 0.254 0.252 0.25

-0.2

0.8775 100 200 time [macrostep]

rotation around Y axis [rad]

-0.402

0

-0.404 -0.406 -0.408 -0.41 0

100 200 time [macrostep]


rotation around X axis [rad]

0

rotation around Z axis [rad]

translation along Z axis [m]

translation along Y axis [m]

translation along X axis [m]

0.879

1.52 1.51 1.5 1.49 1.48 0


0


0


3.115 3.11 3.105 3.1 3.095

Figure 4.9: Trajectories of end-effector tracking a Rubik’s cube

50

-4

x 10

18 translation increment along Y axis during macrostep [m]

0.1

translation along Y axis [m]

0.05

0

-0.05

-0.1

-0.15

16 14 12 10 8 6 4 2 0 -2

-0.2 0.8775

0.878 0.8785 translation along X axis [m]

0.879

0

50


200

250

Figure 4.10: Trajectories of the end-effector tracking a Rubik’s cube

4.3.2


To improve the results of the identification of the cube’s state it is important that the cube fills a considerable portion of the image. In the case of a robot system, where the cube can be grasped and moved it is fairly easy to fulfill this requirement. Either the in-hand camera may be used, or the cube can be transferred into the vicinity of the stand-alone camera. We have used the former. As mentioned before in section 4.1.3, the color space that features good representation of the six colors of the cube (red, green, blue, orange, yellow and white) is, among others, the HSV color space, which was used in the experiments . The subspaces for all of the above colors, except for red and orange, are clearly disjoint, so it is fairly easy to identify those colors by checking their HSV components (see figure 4.11). The problem of discerning red and orange stems from the fact that their subspaces are very close to each other and may overlap under certain conditions. It depends primarily on the cube’s original colors and the lighting conditions. Dark reds can be distinguished from bright oranges by their brightness (or the V component), but in certain situations it is impossible to tell if a given color is red or orange as it might be interpreted as dark orange or bright red (see figure 4.12). In other words, the class membership of these two particular colors depends not only on their location in the color space, but also on the context in which they are observed. In the case of a Rubik’s cube we have additional information about the colors we want to identify. Namely, it is the number of red and orange tiles which is equal to 9 for each color we are looking for. Therefore, having 18 red-or-orange tiles we decide that 9 darkest tiles belong to the class ’red’ and 9 brightest tiles belong to the class ’orange’. This approach is successful regardless of the brightness of the illumination. However, if the light intensity 51

Figure 4.11: Colors of the cube in the original HSV space. Our modification uses 256 samples per axis and is shifted to the right so that red and orange colors are divided into two regions. varies during image acquisitions of consecutive faces or the cube faces are seen at different angles (what is far more likely to happen, if not considered beforehand), the illumination effect might not be consistent for all the red-or-orange tiles and the proposed approach might fail. A straightforward solution to this problem is maintaining constant lighting angle with respect to the observed face.

4.3.3

Implementation

The definition of the six fundamental colors of the cube in the modified HSV space as seen by the wrist mounted camera is given in Table 4.3. Color Hue Saturation Red 10-31 80-255 Orange 32-60 60-200 Yellow 61-85 60-255 Green 135-175 60-255 Blue 176-200 60-255 White 0-255 0-50

Value 40-210 180-255 40-255 40-255 40-255 60-255

Table 4.3: The Rubik’s cube colors and their HSV representation

52

Figure 4.12: An example of red and orange tiles as seen by our gripper-mounted camera. The tile on the left (which is the central tile of the cube face depicted on the right) can be interpreted both as bright red and dark orange, if no context information is given. Although the hue of red and orange tiles is separate, what should be sufficient to differentiate red and orange colors, they are also classified according to their brightness (or value). This increases the robustness of the system, but if the overall image brightness is lowered below a certain threshold, the orange tiles will not be found at all as they will be classified as red tiles. Therefore it is important to maintain constant lighting or use context information.

4.3.4


The color representation depicted in Table 4.3 proved satisfactory under the laboratory lighting conditions and for the particular cube used (we tested a number of different cubes). The experiments were conducted during summer midday with laboratory fluorescent lamps on and some ambient lighting entering through the windows. All the tiles were correctly identified without the need to use context information as described in section 4.1.2. If the light conditions change (at night, without ambient light or with another light source), the most imminent effect is misinterpretation of the orange color followed by yellow. This can be corrected by finding the best matching color representation with the given lighting (or color calibrating the camera) or using the context information. We did not notice other colors to be falsely identified, but we did not test the system against many different light sources. In order to reduce the influence of the lighting changes on the colors perceived by the camera, we implemented white balance normalization. In our approach we normalize the RGB components of the input image so that white areas in the image have equal R, G and B values. Normally, after image acquisition white areas are not necessarily white. They have a slight color cast and its hue and intensity depends primarily on the light source used and the surrounding objects (bright colored objects illuminate the scene with their own color). To minimize this problem we placed white markers on the gripper, just below and above the cube (see figure 4.13) and performed image normalization. Unfortunately we did not notice that the cube tiles which were very close to the white markers reflected the light on them and thus caused a minimal coloring of the markers.

53

Figure 4.13: White markers placed on the gripper for white-balance correction. Without the cube (left), the marker RGB components averaged to 205, 200 and 190 respectively, what indicates that the scene lacks some blue light. With the cube (right) the marker color is influenced by the colors of the cube and in this particular picture it averages to 215, 200 and 190 on the left hand side and 203, 200 and 198 on the right hand side. The average color of the marker is thus 209, 200 and 194, and it is different from the real color without the cube. The resulting images were more color-stable, but still some color misalignment was noticeable (see figure 4.14). This was caused by two factors. One was the in-camera auto white-balance function which could not be switched off. The other was the above mentioned color cast from the cube. The latter can be eliminated by placing a thin barrier around the white-balance marker so that the cube’s colors do not reflect light directly on the marker.

Figure 4.14: Images after white-balance correction. In order to find color classes of all the tiles of the cube, we presented each face to the camera sequentially in exactly the same geometric configuration (within the repeatibility of the robot manipulator). Thus, we knew precisely (within a couple of pixels) the locations of all the tiles. Therefore we did not have to perform the entire image processing and recognition process as described in section 4.1. Instead, we simply checked the class membership of 10 × 10 pixels squares located in the expected tile centroids. One more important consideration during this stage is the gripper-camera dynamics, which is fairly slow. It requires a couple of seconds (between 3 and 10) to adjust to the change of illumination from almost total darkness to normal conditions. This happens during manipulation, when one robot holding the cube (and thus blocking the light to the camera) gives it to the other robot. The former must wait some time before acquiring the 54

image of the cube, otherwise the colors of the cube will be corrupted and the identification rendered impossible.

4.4

Solution to the Rubik’s cube puzzle

There are many algorithms to solve a scrambled Rubik’s cube. Most of them have the advantage of being simple enough to be memorized by humans, however they will usually not give an optimal solution to the Rubik’s cube puzzle with the minimum possible number of moves. The minimum number of moves required to solve the puzzle from a given state is known as its distance from the goal state. The maximum such distance is known as the diameter of the puzzle, which can be measured using the so called Half Turn Metric (HTM) (also known as the Face Turn Metric (FTM)) in which a move consists of quarter or half twists of any of the slices of the cube. It is not known what is the maximum number of moves required to solve any instance of the Rubik’s cube puzzle. Currently there are no known cube states for which more than 20 face turns are needed to find the solution (i.e., reach the initial state in which all of the cube’s faces contain tiles of one color– i.e. fully ordered state). There is however no proof that such positions do not exist2 . An algorithm that solves a cube in the minimum number of moves is known as the God’s algorithm [8]. In other words, the God’s algorithm is the name for the technique of solving a puzzle which always uses the fewest possible moves. For the 3 × 3 × 3 version of Rubik’s cube such an algorithm has not been developed so far. Therefore the challenge is to develop an efficient algorithm to find optimal, or near-optimal, solution within practical computational and memory limitations.

4.4.1

Task formulation

In the goal state, all the facelets on each side of the cube are of the same color. The puzzle is scrambled by making a sequence of random turns, and the task is to restore the cube to its initial (goal state, ordered state). Before we describe the algorithm and the data representation, it will be useful to define some terminology concerning the elementary moves. We define any 90◦ , 180◦ or −90◦ degree twist of a face as one elementary move (basic move), then we will use HTM to measure the distance to the goal state. It should be noted that any two consecutive moves of the same face can always be accomplished by a single twist of the face. Also, two consecutive moves of the same face can cancel out each other, then we can eliminate out of the considerations reciprocal moves of the same face. Hereafter, we shall use the notation devised by David Singmaster to describe a sequence of moves [18]. This is generally referred to as Cube notation. Clockwise quarter turns 2

The heuristics supporting the claim that 20 moves will suffice, to solve the puzzle starting from any state, is as follows. There are 18 elementary moves (6 faces times three elementary moves of each face), thus the branching factor is 18. Even if we decide to get rid of the reverse move that brings the puzzle to the previous state we still have 17 moves at our disposal, thus there are 1720 states in the solution tree at depth 20. This number considerably exceeds the number of possible cube states (4.3 × 1019 ), so probably all of possible cube states are included in the tree with the root node representing the initial state (fully ordered state of the cube) and generated by applying any of the 17 legal moves to any intermediate state of the cube. This tree is 20 moves deep. Hence, traversing the tree in reverse order of moves we should be able to reach the initial state from any scrambled state of the cube within 20 moves. Obviously this is no proof of the initial statement, because we do not know how many repetition of some states are there in the tree. However, it is quite possible that all the cube states are present in the tree of this size.

55

(90◦ ) of the six outer layers, or slices, are denoted by the capital letters: (U, D, F, B, L, R), i.e., Up, Down, Front, Back, Left, and Right (Fig. 4.15). Counterclockwise quarter turns (−90◦ ) are denoted by: U −1 , D−1 , F −1 , B −1 , L−1 , R−1 , and half turns (180◦ ) are denoted by: U 2 , D2 , F 2 , B 2 , L2 , R2 . The Rubik’s cube can be represented by a graph in

U L

F

R

B

D

Figure 4.15: Description of the Rubik’s cube faces which each node represents a unique state and each edge represents a transition between two adjacent states. A state is regarded adjacent if it is one move away in the HTM, i.e., one basic move away. This gives eighteen possible adjacent states for each node since the number of elementary moves is eighteen. Since an edge connects two adjacent nodes, this gives a total of about 3.9 × 1020 edges in the graph. This kind of graph has the same structure with respect to any node, and it is known as Cayley graph [8]. The initial branching factor for this graph is equal to 18. The branching factor is the ratio of the number of nodes at a given depth d over the number of the nodes explored at a depth d + 1. Usually, we use some technique of pruning to reduce the number of duplicate nodes in the search graph. The asymptotic branching factor for the 3 × 3 × 3 Rubik’s cube is 13.34847 [11].

4.4.2


It is worth noticing that the Rubik’s cube has fascinated not only amateur solvers, but also mathematicians and computer scientists for the past three decades. While most of the research involving the group theoretical properties of the Rubik’s cube was done by mathematicians in the 1980s, computer scientists, particulary in the field of AI have continued their effort to obtain an algorithm that produces the optimal solution for any given state of the cube. The problem is not so easy, and one of the reasons for that is that, if we want to locate a single piece, most of the other pieces must be moved as well to do so. This property, known as a non-serializable sequence of subgoal states, eludes standard Artificial Intelligence (AI) learning techniques [17]. As such, the only AI techniques that have been applied involve some form of search. The approaches to the Rubik’s cube puzzle that lead to algorithms with very few moves are based on group theory and on extensive computer searches. Optimal solution To find an optimal solution we need an admissible search algorithm. In our attempt to solve the Rubik’s cube we have followed an approach proposed by Richard Korf [10]. Similar idea was developed independently by Herbert Kociemba of Germany [9]. Korf actually used IDA∗ algorithm – a variant of A∗ called IterativeDeepening-A∗ . IDA∗ combines a depth-first, iterative-deepening style of search with the use of heuristic functions as in A∗ . IDA* uses the cost function f (n) = g(n) + h(n), where g(n) is the sum of the edge costs from the initial state to node n, and h(n) is an estimate 56

of the cost of reaching a goal from node n. Each iteration is a depth-first search where a branch is pruned when it reaches a node whose total costs exceeds the cost threshold of that iteration. The cost threshold for the first iteration is the heuristic value of the initial state, and it increases in each iteration to the lowest cost of all nodes pruned on the previous iteration. It continues until a goal node is found whose cost does not exceeded the current cost threshold. If there are multiple paths to a node, IDA∗ will re-search the successors of that node (A∗ would not). The best known upper bound comes from determining the maximum length of solutions generated by best suboptimal algorithm to date, known as Two-Phase Algorithm developed by Herbert Kociemba [9]. Korf used in his algorithm a lower-bound heuristic function based on large RAM-based lookup tables, or pattern databases [3]. Pattern databases explicitly store the number of moves remaining to solve a particular subproblem from a given state of the puzzle solution. As we mentioned before, the twenty movable pieces include eight corner pieces, and twelve edge pieces. The eight corner pieces can be used as one subproblem and two sets of six of the twelve edges for the two others. The number of moves to solve each of these subproblems is stored in a database that can be indexed effectively for very quick access. The total number of different positions and orientations of the corner pieces is only 8!·33 = 88179840, and the number of moves needed to solve just the corner pieces ranges from zero to eleven. Thus, using a breadth-first search from the goal state that ignores the edge pieces, we can compute the exact number of moves needed to solve each state of the corner pieces, and store these values in a pattern database. Since this number ranges from zero to eleven, each entry requires only four bits, thus a pattern database for the corner pieces can be stored in about 42 megabytes (MB) of memory. Similarly for six of the twelve edge pieces there are 26 · 12!/6! = 42577920 unique states, and corresponding pattern database occupies about 20 MB of memory. The remaining six edge edge pieces produce another database of the same size. Thus, all databases required approximately 82 MB of memory. Given a state of a search, we use the configuration of the corner pieces to compute an index into the corner-piece lookup table, whose value gives us the number of moves (hc ) required to solve just the corner pieces. We also use each of the two subsets of six edge pieces to compute indices into the corresponding databases, giving the number of moves (hei , i = 1, 2) needed to solve each subset of six edge pieces. Given this three different heuristic values, a modified admissible heuristic is evaluated as follows: h(n) = max(hc , he1 , he2 ) The reason for such a choice of the heuristic is that every twist of the cube face moves four edge pieces and four corner pieces, and moves that contribute to the solution of pieces in one database may also contribute to the solution of the others. The expected value of the heuristic for the corner piece pattern database is 8.764, and the expected value for either of the edge piece database is 7.668 [10]. However by taking, the maximum of the three, the expected value of the heuristic increases to 8.878. IDA∗ guarantees an optimal solution if the heuristic function is admissible. IDA∗ requires memory that is linear in the maximum search depth. All the discussion above assumes that we actually want to find the shortest solution. A tree search might take a long time even with good pruning tables. Currently we obtain, within reasonable time, an optimal solution for the cube that requires less than 16 moves. If the shortest solution needs more than 16 moves we use suboptimal algorithm that produces satisfactory solution, typically 20-21 moves. Our suboptimal version of the algorithm to solve the Cube is built upon

57

the ideas developed by Herbert Kociemba in his Two-Phase Algorithm [9]. To describe this approach we have to use some elementary notions of group theory. Suboptimal solution It is well known that all possible moves of the pieces on the Rubik’s Cube form a group G0 [8]. A group can be described by its group generators. They are the smallest set of elements such that any element of the group can be written as a sequence of these generators. For the Rubik’s Cube, its set of generators are the quarter clockwise turns of each face, written as {U, D, F, B, L, R}. To denote the group generated by the these operators, one can use the notation < U, D, F, B, L, R >. Also, we can form subgroups by restricting what moves are available. If we start with a initial cube state and only use a certain subset of all moves, then we only generate a certain subgroup of all possible states. A subset H of a group G that also has all the properties of a group is called a subgroup and can be written as H ⊆ G. In the two-phase algorithm a subgroup defined as G1 =< U, D, F 2 , R2 , B 2 , L2 > is used. Positions in G1 are characterized by the following: corners cannot change orientation, their U or D facelet remains on the U or D face. Similarly, edges cannot change orientation. Moreover, the four U-D slice edges remain in the U-D slice. The algorithm works as follows. In the first phase it finds a sequence of moves that transforms a scrambled cube from G0 into a cube in G1 using IDA∗ . The distances to the intermediate subgroup G1 are used in the pruning tables. By keeping track of the maneuvers to get there and subsequently using IDA∗ again to search from that position in G1 to the goal state, a near optimal solution can be obtained by combining the two sets of manoeuvres. In the second phase the algorithm restores the initial state of the cube by using the operators of the subgroup G1 . The algorithm does not stop when a first solution is found, but continues to search for shorter solutions by carrying out phase two from suboptimal solutions of phase one. For instance, if the first solution has ten moves in phase one followed by twelve moves in phase two, the second solution could have eleven moves in phase one and only five moves in phase two. The number of moves in phase one increases and the number of moves in phase two decreases. If the length of phase two reaches zero, the solution is optimal and the algorithm stops. This algorithm is very effective. It very quickly finds solution which is not far from being optimal.

4.4.3

Implementation

Our algorithm solving the Rubik’s cube puzzle is implemented in plain C language. The whole of the solving process consists of the following stages. First, three pruning tables are computed, one for the corner pieces and two for the edge pieces, as described earlier. Then, given a state of an IDA* search, the configuration of the corner pieces is used to compute an index into the corner-piece pattern database, whose specific value yields the number of moves needed to solve just the corner pieces. Also, each of the two sets of six edge pieces is used to compute indices into the corresponding edge-piece pattern databases, giving the number of moves needed to solve each set of six edge pieces. Thus, each unique state of the set of pieces has one unique index, whose value belongs to the interval from 0 to 88179839 for the corner pieces and to the interval from 0 to 42577919 for both sets of the edge pieces. This index is an address of the cell in the corresponding pattern database. In order to describe an orientation of the pieces, each piece is labeled with a single marker as it is shown in figure 4.16. If the marker of the corner piece coincides with its marker in the solved cube it has orientation denoted as 0. But if, to match the marker, we have to rotate the corner piece by −120◦ (or +120◦ ) about the axis aligned with the vertex 58

Figure 4.16: Markers used to determine the orientation of the pieces of this piece and the vertex of the opposite corner piece, then the orientation is denoted as 1 (or 2 respectively). For example, after the F move initiated in the goal state these markers are arranged as it is shown in figure 4.17. The state of the twenty movable pieces

Figure 4.17: Distribution of the markers after the F move of the cube (i.e. the state of the Rubik’s cube) can be encoded in a twenty-element array of integers. The goal state of the cube is described in the table 4.4. Position of a piece Corners Edges Eight corner pieces Six edge pieces Six edge pieces 0 3 6 9 12 15 18 21 0 2 4 6 8 10 12 14 16 18 20 22 Table 4.4: Goal state of the Rubik’s cube encoded in a twenty-element array is described by an index of the corresponding sub-array, namely 0, . . . , 7 for the corners and 0, . . . , 12 for the edges. Thus, the configuration of the corner piece is obtained by multiplying the position by 3 and then adding its orientation. Similarly, multiplying the position od the edge piece by 2 and adding its orientation yields the configuration of the edge piece. For instance, after executing the F elementary move (90◦ turn of the F face) from the goal state we obtain a new state given in table 4.5. The pieces whose configuration has changed are printed in boldface. We use the so-called “is carried to” representation in order to describe a new state of the cube. It means that the piece is carried to a new position with a certain orientation described by the value which appears in the corresponding array cell. To decode the position of the piece one should divide the value of the specified sub-array cell by 3 for the corners or by 2 for the edges. Then the quotient describes the position of the piece and the reminder gives its orientation. For example, the corner piece in the third cell in the goal state was on its original position 2 and its orientation was 0 (6/3 = 2 reminder 0), while after the F move it was carried to the new position 5 and its orientation was 2 (17/3 = 5 reminder 2). It should be noted that the content of a single cell is always associated with the same piece. 59

Corners Eight corner pieces 0 3 17 7 11 13 18 21

Edges Six edge pieces Six edge pieces 0 2 11 6 5 17 12 14 9 18 20 22

Table 4.5: Encoded state of the Rubik’s cube after the F move applied to the goal state The simplified pseudo-code of the IDA∗ algorithm is as follows: // Iterative - Deepening - A * algorithm function IDA * ( n ) limit := h ( n ) // the overall cost of reaching the goal should be // not greater than the estimated cost of reaching // the goal from node n while not solved do limit := DFS (n , limit )

// Depth - First - Search algorithm function DFS (n , limit ) i f f ( n ) > limit then return f ( n ) i f h (n) = 0 then return solved return lowest value of DFS ( n_i , limit ) f o r all successors n_i of n

As mentioned earlier IDA∗ executes a series of independent depth-first searches, each with the cost-limit increased by the minimal value. The total cost f (n) of node n is the sum of the cost already incurred in reaching that node, g(n), and a lower bound on the estimated cost of the path to a goal state, h(n). At the start, the cost bound is equal to the heuristic estimate of reaching the goal state from the initial state. It is known that with an admissible (i.e., non-overestimating) heuristic estimate function h, IDA∗ guarantees to find an optimal solution.

4.4.4


To evaluate our implementation of the Rubik’s cube puzzle solver we generated several random instances of the cube state and then solved the puzzle. As mentioned before, in the current implementation of the Rubik’s cube solving algorithm we could obtain, within reasonable time (i.e. less then 10-20 seconds on the standard 3GHz PC), an optimal solution for the cube that requires less than 16 moves. If the shortest solution needs more than 16 moves we use sub-optimal algorithm that produces satisfactory solution, typically 20–21 moves. The solver is located in the MP. It is invoked when the current state of the cube has been obtained from the cube state identification sub-system. For instance, for the cube state shown in figure 4.18 we obtained the following sequence of moves (B 2 , R2 , U −1 , F 2 , U −1 , R2 , D−1 , L2 , B −1 , L−1 , F −1 , D, U −1 , R−1 , U −1 , L, F, D, F 2 , R) within a few seconds.

Figure 4.18: An exemplary Rubik’s cube random instance

60

4.5

Manipulation of the cube

Generally, two-handed manipulation can be classified into uncoordinated and coordinated tasks. The latter can be further subdivided into symmetric and asymmetric ones. In this particular case we are interested in a symmetric coordinated manipulation in which both hands are manipulating the same object, thus creating a closed kinematic chain. The task that the system has to execute can be divided int two phases: acquiring the cube and manipulating it.

4.5.1

Task formulation

The process of manipulation of the Rubik’s cube involves all aspects of visual servoing to the vicinity of the cube, alignment of robot arms with the cube, grasping it with the grippers, and finally rotating the adequate face of the cube. The last three actions are repeated as many times as the number of moves required to solve the scrambled cube. Here we assume that from the high-level task planning system, i.e. Rubik’s Cube solver, a sequence of the turns of the faces is obtained. The goal is to generate a proper sequence of hand movements and grasping actions for both arms.

4.5.2


Figure 4.19 presents the geometrical structure of the two-arm manipulation system, and coordinate frames Fi attached to the appropriate components of the system together with the distribution of sensors. Coordinate frames Fei , i = 1, 2 are attached to the grippers

Fc

Fc1

Fe 1

Ft 1

Fs1

Fo

Fe2

Fc2 Fs2

j

Ft 2

Fw

Figure 4.19: Coordinate frames attached to the two-handed robotic system and sensor coordinate frames Fsi and Fci attached to the force sensors and to the eyein-hand cameras respectively. Given the view of the scene, the robot should be able to recognize the cube and localize it in the robot workspace. As a result of the visual localization the position and orientation of the coordinate frame Fo attached to the cube with respect to (w.r.t) world coordinate frame Fw is computed (as described earlier). 61

To describe the task for each hand two task coordinate systems Fti , i = 1, 2 are introduced as shown in Figure 4.19. These frames are used to plan manipulation primitives such as approach trajectories for both hands, grasp and re-grasp operations, and hand movements to turn the cube faces. Homogenous transformation matrix i Tj ∈ SE(3) (where SE(3) is a special Euclidean group of a rigid body motions) is a mapping between the appropriate coordinate frames. Matrix i Tj may be interpreted as the pose of frame Fj w.r.t. frame Fi . Left-hand superscript is omitted (i.e., Tj ) when the reference frame is evident from the context, e.g. it is the world frame Fw . Pre-computed sequence of twists of the faces can be described in the Fo coordinate frame as u, ϕ) = Rot(u u, ϕ), a sequence of rotations about unit vectors xˆ o , yˆo , zˆo of its axes, i.e., o R(u π π u x ˆ y ˆ z ˆ where = o , o , o , and ϕ = − 2 , −π, 2 , or π. The desired grasp configurations w.r.t. coordinate frame Fo are described by the following matrices: • o Tt1 – to grasp a single slice (F, U, D, L, R, B) • o Tt2 – to grasp two slices simultaneously Locations of the possible contact regions on the cube are imposed by the specific shape of the gripper jaws. The shape of each of the jaws of the gripper matches the form of the corner of the cube. The cube is being grasped diagonally in such a why that either one or two layers are immobilized, where the corner pieces of one layer define the diagonal. Now we have to plan such a sequence of admissible grasps (o Tt1 , o Tt2 ) that enable each turn of the face without re-grasping: (4.6) Tt1 = To o Tt1

Tt2 = To o Tt2

The conditions of grasp feasibility are as follows: (4.7) Tt1 = Tb1 b1 Te1 (qq 1 ) e1 To o Tt1

Tt2 = Tb2 b2 Te2 (qq 2 ) e2 To o Tt2

or equivalently (4.8) Tt1 = Tb2 b2 Te2 (qq 2 ) e2 To o Tt2

Tt2 = Tb1 b1 Te1 (qq 1 ) e1 To o Tt1

where Tbi , i = 1, 2 is the homogenous transformation matrix from the world frame Fw to the robot base coordinate frame Fbi . Matrix bi Tei (qq i ) represents direct kinematics of the robot arm i, and q i , i = 1, 2 is the vector of joint coordinates of the arm i. In this case grasp stability conditions are of a geometric nature and grasp synthesis is reduced to the choice of four contact regions on the cube (two for each gripper) from the given set of contacts and computing desired poses of both grippers, i.e., Te1 and Te2 which guarantee firm grasps. In fact, grasp synthesis comes down to the proper positioning of the grippers. Therefore grasp configurations can be described in the task space as well as in the joint space. When both grippers firmly hold the cube the closed kinematic chain is established. Now the motion planning problem is complicated by the need to maintain the closed loop structure, described by the loop closure constraint. (4.9) Tb1 b1 Te1 (qq 1 ) e1 To (ϕ) − b1 Tb2 b2 Te2 (qq 2 ) e2 To = 0 However, in our case, the motion of the closed chain linkage can be described in the Fo coordinate frame as a single rotation about its axes (i.e., the elementary move of the cube’s 62

face). For the frame Fo chosen as it is shown in figure 4.19 these moves are described as follows: F

– Rot(ˆ x o , − π2 )

B

F2

– Rot(ˆ x o , −π)

B2

−1

– −1

Rot(ˆ x o , π2 )

– Rot(ˆ x o , π)

–

xo , π2 ) Rot(ˆ

B

R

–

Rot(ˆ y o , − π2 )

L

– Rot(ˆ y o , π2 )

R2

– Rot(ˆ y o , −π)

L2

– Rot(ˆ y o , π)

R−1

– Rot(ˆ y o , π2 )

L−1

– Rot(ˆ y o , − π2 )

U

– Rot(ˆ z o , − π2 )

D

– Rot(ˆ z o , π2 )

U2

– Rot(ˆ z o , −π)

D2

F

U

−1

–

Rot(ˆ z o , π2 )

D

−1

xo , − π2 ) – Rot(ˆ

– Rot(ˆ z o , π) – Rot(ˆ z o , − π2 )

These moves can be easily transformed to the motions of the grippers. However, due to kinematic calibration errors, the two robot arms cannot be position controlled while executing the turns. This would cause excessive bildup of force in a rigid closed kinematic chain due to small misalignments. Thus, at this stage the motions have to be executed in position-force control mode.

4.5.3

Implementation

The structure of the controller produced by using the MRROC++ framework for the purpose of executing the two-handed manipulation task is presented in Figure 4.20. From the point of view of the executed task MP is the coordinator of all effectors present in the system. It is responsible for trajectory generation in multi-effector systems, where the effectors cooperate tightly – as is the case in the presented system. The manipulation planning system contained in the MP transforms the solution obtained from Rubik’s cube solver into a proper sequence of manipulation primitives. In the MRROC++ framework these primitives are implemented as motion generators, which are used by the Move instructions. Therefore the MP is responsible both for producing the plan of the motions of the faces of the cube and subsequently the trajectory generation for both manipulators. This trajectory can be treated as a crude reference trajectory for both arms. At a later stage this trajectory is modified by taking into account the force readings. Each effector has two processes controlling it: Effector Control Process ECP and Effector Driver Process EDP. The first one is responsible for the execution of the user’s task dedicated to this effector (in our case the task is defined by the MP – it is defined by the reference trajectory that is to be executed by the manipulator), and the other for direct control of this effector. The EDP is responsible for direct and inverse kinematics computations as well as for both position and force servo-control. The EDP has been divided into several threads: EDP MASTER (communication with the ECP including interpretation of its commands), EDP TRANS (designation of force and position control directions), EDP SERVO (motor position servocontrol) and EDP FORCE (force measurements).

4.5.4


The overall experimental setup consists of two 6 degree of freedom (dof) modified IRp-6 robot arms, each with a parallel jaw gripper fig. 4.21. Each jaw is instrumented with 63

MRROC++

UI

MP ECP1

EDP1

ECP2

VSPGS1

VSPMPC VSP MPC VSP MPC

VSPMPF VSP MPF

EDP2

Parallel interface

Parallel interface Axis controllers

VSPGS2

Exteroceptors

Effector1

Axis controllers

Effector2

Figure 4.20: MRROC++ based controller for the two-arm system tactile sensors which detect the presence of contacts with the grasped object and optical IR proximity sensors. Moreover, each hand is equipped with a wrist-mounted ATI Gamma force-torque sensor, and an eye-in-hand miniature CMOS color camera. Additionally, a global vision system with fixed-mount SVS-Vistek SVS084 CCD color camera and Leonardo Digital Video Processor from Arvoo for fast image acquisition and realtime processing of the incoming data is used. Ocena jakosci wyznaczonych sekwencji, czasu ich wyznaczania etc. (o ile jest potrzebna)

4.6

Utilization of position–force control

During the task execution either pure position control or position–force control is used, depending on the current task execution stage. Typically those execution stages are position controlled, in which there in no simultaneous contact between the two end-effectors and the cube or between one of the end-effectors and the cube held by the operator. The stages, where such contact is present or expected to occur, are position–force controlled.

64

CCD camera tactile sensor force sensor

j CMOS camera IR sensor

Figure 4.21: Sensors used to locate and manipulate the Rubik’s cube a

b

Figure 4.22: a – Approach to face change, b – Gripper closing The high level task algorithm with the sequence of stages will be presented further on (4.7). The currently described task uses the same TFF position–force controller as the previously presented tasks requiring force control (2). There are three stages (subtasks) of the present application that use position–force control: • attaining direct contact with the cube, • grasping of the cube, • turning of a face. The EDP TFF position–force command is specified by the MP generators. In each of the above subtasks local reference frame is specified as the tool frame (4.23). Hence, the subtasks are specified by pos xyz rot xyz, force xyz torque xyz and selection vector vectors. The force/torque and position measurements originating in theEDP are taken into account by the MP generators and subsequently change the above mentioned parameters of EDP command, thus closing the feedback loop.

4.6.1

Getting direct contact with the cube

This stage occurs when one of the manipulators is currently holding the cube and the second one is approaching to gain direct contact with the other side of the cube (fig.4.22). 65

E

x

E

y

E

z

Figure 4.23: The end–effector frame a

b

c

Figure 4.24: a – approaching the face of the cube, b – closing of the gripper, c – turning the face The manipulator currently holding the cube is commanded to keep the current position, hence it is position controlled, by a TFF command with the following form (i denotes irrelevant): • selection vector – (0, 0, 0, 0, 0, 0) – pure position control, • pos xyz rot xyz – (0, 0, 0, 0, 0, 0) – null position increment. • force xyz torque xyz – (i, i, i, i, i, i). The manipulator approaching the cube is position-force controlled, i.e.: • selection vector – (0, 0, 1, 0, 0, 0) – Z axis is force controlled, others are position controlled, • pos xyz rot xyz – (0, 0, i, 0, 0, 0) – null position increment, • force xyz torque xyz – (i, i, vf, i, i, i) – the manipulator moves along the endeffector Z axis with the velocity determined by the force error between the virtual force and the measured force. 66

The virtual force is the desired force of a certain value which causes the manipulator motion (when the force control is used) – if there is no contact between the end-effector and environment. When the manipulator reaches direct contact with cube the force error is reduced, the motion stops and the subtask is finished. The higher the virtual desired force is, the higher the velocity of motion (the velocity is proportional to the force error). The same property of the virtual force was helpful in drawing – both during the teach-in and reproduction phase 2.3. The mp tff gripper approach generator generator was implemented to obtain the above results.

4.6.2

Cube grasping

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 position (in the same way as in the first subtask). The cube grasping subtask is executed after grasping manipulator attains contact with the cube (fig. 4.22b) or just after the grasping manipulator has reached the previously specified position (fig. 4.24b). Gripper closing is divided into two stages: • The grasping manipulator is compliant in one axis • The grasping manipulator is compliant in two axes The mp tff rubik grab generator generator was implemented to obtain the above results. Compliance along one axis The manipulator moves to the position where it is possible to grasp the cube. Then it 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 X-Y surface of the end-effector frame to reduce this 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, because there is some noise in the force measurements. Hence, the manipulator slowly moves. That is why in the first stage, while the gripper closes, the manipulator is compliant only along one axis (namely the Y axis) to avoid undesirable motions. The MP TFF command is as follows: • selection vector – (0, 1, 0, 0, 0, 0) – Y axis is force controlled, others are position controlled, • pos xyz rot xyz – (0, i, 0, 0, 0, 0) – null position increment, • force xyz torque xyz – (i, 0, i, i, i, i) – the manipulator is compliant along the Y axis.

67

Compliance along two axes 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 gripper still closes the jaws, until it reaches the desired distance between the jaws and finally catches the cube: • selection vector – (1, 1, 0, 0, 0, 0) – X and Y axes are force controlled, others are position controlled, • pos xyz rot xyz – (i, i, 0, 0, 0, 0) – null position increment, • force xyz torque xyz – (0, 0, i, i, i, i) – the manipulator is compliant along the X and Y axes.

4.6.3

Face turning

Face turning can be initiated only when both manipulators properly hold the cube. The kinematic chain is closed then (fig. 4.24c, fig. 4.25a). From that moment one of the manipulators keeps constant pose (in the same way as in the first subtask), and the other start turning. The motion ends when the desired turn angle is reached. • selection vector – (1, 1, 0, 0, 0, 1) – X and Y axes are force controlled and the rotation about the Z axis is torque controlled, others are position controlled, • pos xyz rot xyz – (i, i, 0, 0, 0, i) – null position increment, • force xyz torque xyz – (0, 0, i, i, i, m) – the manipulator is compliant along the X and Y axes. There is a desired torque – m set about the Z axis. The mp tff rubik face rotate generator generator was implemented to obtain the above results. a

b

Figure 4.25: a – Turning the face, b – Visual servo

4.7

High level task algorithm

The task is subdivided into the following actions: • approach op, 68

• identify colors, • communicate with windows solver, • execute manipulation sequence, • departure op, which are implemented as the methods of the mp rubik cube solver task class and are executed by the MP. To acquire the cube from the operator the controller invokes the method approach op, which uses three generators. The mp teach in generator commands the end-effector of the first manipulator to the the area where the cube is seen by the stand-alone-camera. The mp seven eye generator is the visual servo, which moves the end-effector towards the cube (section 4.2.3). The mp tff rubik grab generator clenches the jaws (section 4.6.2). mp teach in generator moves the manipulators to a position defined in the configuration (joint) space. The position is obtained off-line by moving the manipulator to it and saving the joint positions (this is often called ”teaching”). The method identify colors recognizes the state of the cube. The cube is being regrasped (face change op) letting the eye-in-hand cameras to look at each of the faces of the cube. the (initiate reading/ get reading) methods are invoked after each regrasping operation to retrieve the information about the colors of the tiles on the currently visible cube face (sections 4.3.3, 4.3.4). The sequence of regrasps has been chosen in such a way that the number of regrasps is minimal. The communication with the puzzle solver uses the HTTP protocol. At this stage some extra tags have to be appended to the text. After getting back the information about the sequence of manipulation the HTTP tags are removed and the sequence is translated to a form understood by the method execute manipulation sequence. The method communicate with windows solver sends the state of the cube to the external application and receives the sequence of regrasps that solves the puzzle. The method execute manipulation sequence executes the sequence of regrasps (face change op) and face turns that solve the puzzle (face turn op). The method face change op uses several generators. At first pure position control is applied by the mp teach in generator to transfer the gripper of the first manipulator into the vicinity of the face and the jaws are set in motion mp tff rubik grab generator. Simultaneously the gripper is being moved towards the cube using mp tff gripper approach generator (section 4.6.1). After touching the cube (detected by force monitoring) the jaws are finally clenched by the mp tff rubik grab generator (section 4.6.2). Then the jaws of the second manipulator start to open – mp tight coop generator is invoked. The first manipulator places the held cube between the jaws of the second manipulator using mp teach in generator. The method face change op also uses several generators. At first the gripper jaws of the second robot are clenched by using mp tff rubik grab generator (section 4.6.2). Then the face of the cube is rotated 90, 180 or -90 degrees (section 4.6.3). The gripper of the first manipulator is opened by utilising (mp tight coop generator). Now the gripper of the first manipulator moves away – (mp teach in generator). The method departure op gives back the cube to the operator. For that mp teach in generator is used.

69

Chapter 5 Conclusions This report summarizes some of the more significant experiments conducted with systems based on the MRROC++ robot programming framework. Those experiments have been conducted because of three reasons, i.e.: • to develop and test each component of a service robot separately, • to show that all of the so developed components can be successfully integrated into a single system performing a service task, • to produce, or rather improve and enhance, the tool for experimenting with service robot controllers, namely MRROC++. Each of the above enumerated tasks has been fulfilled successfully as a result of the work done on the project no: 4 T11A 003 25. However, this does not mean that the attained results cannot be improved. Each of the components of the system is a proof-of-theconcept element within this system, and thus requires more work to produce a faster version. Especially the vision subsystem requires a reduction in reaction time – here the computational time is critical to the performance of the system. Force sensing should be made more sensitive. The calibration procedure, improves the accuracy of the kinematic model, but the exteroceptors used, especially the force/torque sensor, is a remedy for most of the errors introduced by the relatively small inaccuracies of the parameters. Thus, although calibration is helpful it is not vital. The electric gripper designed within the scope of this project had many sensors built-in to accommodate possible sources of errors. Some of them proved unnecessary. For instance, one of the grippers had two LEDs pointing out of the fingers, while the other one had two phototransistors mounted in its fingertips. Those were meant for precisely aligning the grippers at the end of rotating a face of the cube. However, they proved unnecessary – the servos rotate the faces precisely enough. Similarly the force sensors in the fingers proved unnecessary – the gripping force did not have to be controlled. Nevertheless, in some later experiments those sensors might become necessary. The Rubik’s cube puzzle solver should be substituted by a planer executing some more useful tasks. Moreover, the theoretical considerations regarding the fusion of behavioral and deliberative [29, 30] control strategies still have to be explicated experimentally. Hence, although the main objectives of this research have been attained, there is still much room for improvement. Nevertheless, both the tool used for producing the software, i.e., MRROC++, and the integrated system, both showed that there are no significant obstacles to the design of service robots. The obtained software pattern suits its purpose very well. Outside the scope of this project some experiments were conducted on including multimodal communication facilities in the system. Both voice recognition and voice synthesis 70

subsystems were integrated with the Rubik’s cube puzzle solving system. The microphone was treated as a virtual sensor, while the vice synthesis subsystem was treated as an extra effector, so additional VSP and EDP were included in the system.

71

Bibliography [1] J. Bruce, T. Balch, and M. Veloso. Fast and inexpensive color image segmentation for interactive robots. In 2000 IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS ’00), volume 3, pages 2061–2066, October 2000. [2] F. Chaumette. Potential problems of stability and convergence in image-based and position-based visual servoing. In The Confluence of Vision and Control, number 237, pages 66–78. Springer, 1998. [3] J. C. Culberson and J. Schaeffer. Pattern databases. Computational Intelligence, 14(4):318–334, 1998. [4] J.M. Hollerbach and C.W. Wampler. The Calibration Index and Taxonomy for Robot Kinematic Calibration Methods. Intl. J. Robotics Research, 14, 1996. [5] Radu P. Horaud, Bernard Conio, Olivier Leboulleux, and Bernard Lacolle. An Analytic Solution for the Perspective 4-Point Problem. Computer Vision, Graphics and Image Processing, 1989. [6] S. A. Hutchinson, G. D. Hager, and P. I. Corke. A tutorial on visual servo control. IEEE Tran. Robotics and Automation, 12(5):651–670, October 1996. [7] M. Iwatsuki and N. Okijama. A new formulation of visual servoing based on cylindrical coordinate system. IEEE Tran. on Robotics, 21(2):266–273, April 2005. [8] D. Joyner. Lecture notes on the mathematics of the Rubik’s Cube. [9] H. Kociemba. Cube Explorer. [10] R. Korf. Finding optimal solutions to Rubik’s cube using pattern databases. In National Conf. on Artificial Intelligence (AAAI-97), pages 700–705, July 1997. [11] R. Korf, M. Reid, and S. Edelkamp. Time complexity of iterative-deepening-a*. Artificial Intelligence, 129(1–2):199–218, June 2001. [12] T. Kornuta, M. Wojtyra, K. Mianowski, and C. Zieliski. Calibration of a multirobot system (in polish). In K. Tcho, editor, 9-th National Conference on Robotics – Advances in Robotics: Robot Systems and Cooperation, volume 2, pages 97– 106. Wydawnictwa Komunikacji i cznoci, Warsaw, 2006. Kalibracja systemu wielorobotowego. [13] J.T. Lapreste and Y. Mezouar. A Hessian approach to visual servoing. In 2004 IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS 2004), volume 1, pages 998 – 1003, September 28 – October 2 2004. [14] E. Malis, F. Chaumette, and S. Boudet. 2 1/2 visual servoing. IEEE Tran. Robotics and Automation, 15(2):238–250, April 1999. 72

[15] K. Mianowski and M. Wojtyra. Stewart platform based parallel manipulator destined for the kinematics justification of two cooperated robots. In K. Kozowski, editor, Fourth International Workshop on Robot Motion and Control, RoMoCo’04, Puszczykowo, Poland, pages 115–120. June 17–20 2004. [16] K. Mianowski and M. Wojtyra. Wirtualny model manipulatora polman-6l typu platforma stewarta przeznaczonego do pomiarw. In K. Tcho, editor, VIII Krajowa Konferencja Robotyki – Postpy Robotyki: Sterowanie robotw z percepcj otoczenia, volume 1, pages 315–322. Wydawnictwa Komunikacji i cznoci, Warszawa, 2005. [17] S. Russell and P. Norvig. Artificial Intelligence: A Modern Approach. Prentice Hall, Upper Saddle River, N.J., 1995. [18] D. Singmaster. Notes of the Rubik’s Magic Cube. Enslow, 1981. [19] Tsumugiwa T., Yokogawa R., 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, pages 644–650, May 2002. [20] O. Tahri and F. Chaumette. Point-based and region-based image moment for visual servoing of planar objects. IEEE Tran. on Robotics, 21(6):1116–1127, December 2005. [21] T.Winiarski and C. Zieliski. Force control in dual arm systems (in polish). In K. Tcho, editor, 9-th National Conference on Robotics – Advances in Robotics: Robot Systems and Cooperation, volume 2, pages 267–276. Wydawnictwa Komunikacji i cznoci, Warsaw, 2006. Sterowania siowe w systemach dwuramiennych. [22] T. Winiarski and C. Zieliski. Position-force controller experimental station (in Polish). In Postpy Robotyki: Sterowanie robotw z percepcj otoczenia, volume 1, pages 85–94. Wydawnictwa Komunikacji i cznoci, June 23-25 2004. [23] T. Winiarski and C. Zieliski. Implementation of position–force control in MRROC++. In Proceedings of the 5th International Workshop on Robot Motion and Control, RoMoCo’05, Dymaczewo, Poland, pages 259–264. June, 23–25 2005. [24] M. Wojtyra. Stanowisko do kalibracji robota wyznaczanie wzajemnego usytuowania ukadw odniesienia. Instytut Techniki Lotniczej i Mechaniki Stosowanej, 2005. [25] C. Zieliski. Programming and control of multi-robot systems. In 6th International Conference on Control, Automation, Robotics and Vision, ICARCV’2000, Singapore, pages on CD–ROM. December 5–8 2000. [26] C. Zieliski. 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. [27] C. Zieliski. A unified formal description of behavioural and deliberative robotic multi-agent systems. In Proc. 7th IFAC International Symposium on Robot Control SYROCO 2003, Wrocaw, Poland, volume 2, pages 479–486. September 1–3 2003. [28] C. Zieliski. Specification of behavioural embodied agents. In K. Kozowski, editor, Fourth International Workshop on Robot Motion and Control, RoMoCo’04, Puszczykowo, Poland, pages 79–84. June 17–20 2004.

73

[29] C. Zieliski. Formal approach to the design of robot programming frameworks: the behavioural control case. Bulletin of the Polish Academy of Sciences – Technical Sciences, 53(1):57–67, March 2005. [30] C. Zieliski. Transition-function based approach to structuring robot control software. In K. Kozowski, editor, Robot Motion and Control: Recent Developments, Lecture Notes in Control and Information Sciences, Vol.335, pages 265–286. Springer Verlag, 2006. [31] C. Zieliski. Formalization of programming frameworks for multi-robot systems (in Polish). In National Conference on Robotics – Advances in Robotics: Industrial and Medical Robotic Systems, volume 2, pages 53–66. Wydawnictwa Komunikacji i cznoci, Warsaw 2005. Formalizacja opisu struktur ramowych do programowania systemw wielorobotowych. [32] C. Zieliski, W. Szynkiewicz, K. Mianowski, A. Rydzewski, and T.Winiarski. Effectors of a service robot capable of dexterous two-handed manipulation (in polish). In K. Tcho, editor, 9-th National Conference on Robotics – Advances in Robotics: Robot Systems and Cooperation, volume 2, pages 257–266. Wydawnictwa Komunikacji i cznoci, Warsaw, 2006. Efektory robota usugowego do dwurcznej manipulacji z czuciem.

74

Index acceptable trajectory, 19 adjacent states, 56 approach op, 68, 69 basic move, 55 branching factor, 56 calibration, 16 calibration stages, 16 calibration., 45 camera extrinsic parameters, 47 camera intrinsic parameters, 47 camera retreat problem, 46 camera-end-effector calibration, 45 camera-robot calibration, 45 Cayley graph, 56 CCD color camera, 5 center pieces, 34 circularity, 42 communicate with windows solver, 69 computed configuration, 24 core, 34 corner pieces, 34 correction matrix, 16 Cube notation, 55 cube state space, 35 cube’s state, 49 cubies, 34 data processing stage, 16 departure op, 69 diameter of the puzzle, 55 distance from the solution, 55 dof, 4 dominating face, 36 ECL servo, 44 edge pieces, 34 EIH servo, 45 electric gripper, 5 elementary moves, 34, 55 end-effector open-loop servos, 44 EOL servo, 44 Ern˝o Rubik, 34 execute manipulation sequence, 69 eye-in-hand servos, 45

face dominating the image, 36 Face Turn Metric, 55 face change op, 69 facelets, 34 finger, 5 force control, 7 force/torque sensors, 5 force xyz torque xyz, 65 FTM, 55 fully ordered state, 55 general objective function, 25 get reading, 69 global vision system, 5 goal distance, 55 goal frame, 38 God’s algorithm, 55 gripper, 5 group G0 , 58 group generators, 58 Half Turn Metric, 55 hardware configuration, 4 homography matrix, 39 HTM, 55 IB, 46 IB servo, 45 IB visual servos, 45 IDA∗ algorithm, 56 identify colors, 69 image based visual servos, 46 image features, 39 image-based servos, 45 improved configuration, 29 indirect desired pose, 47 initial state, 55 initiate reading, 69 interaction matrix, 45 interactive communication, 22 interactive communication without delay, 22 intermediate desired pose, 47 jaw, 5 Kociemba Herbert, 56 75

Korf Richard, 56 layers, 34 line histogram, 40 local correction matrix, 16 local reference frame, 65 measured configuration, 25 measurement gathering stage, 16 measurement unit, 17 mp rubik cube solver task class, 69 mp seven eye generator, 69 mp teach in generator, 69 mp tff gripper approach generator, 67 mp tff rubik face rotate generator, 68 mp tff rubik grab generator, 67, 69

tiles, 34 Two-Phase Algorithm, 57 visual error, 44 vsp digital scales sensor, 22 vsp sensor, 22 wrist, 8

non-serializable sequence, 56 noninteractive communication, 21 pattern databases, 57 PB, 46 PB servo, 45 PB visual servos, 45 permutation puzzle, 34 pixel color classification, 37 POLMAN 6L, 17 pos xyz rot xyz, 65 position based visual servo, 46 position-based servos, 45 position-force control, 7 puzzle diameter, 55 puzzle state space, 35 reproduction process, 9 robot calibration, 45 robot kinematics, 16 ROI, 40 RTOS, 4 Rubik Ern˝o, 34 Rubik’s cube, 34 SAC servo, 45 selection vector, 65 slices, 34 solution distance, 55 stand-alone camera servos, 45 state space, 35 sub-cubes, 34 subgroup, 58 subgroup G1 , 58 system hardware configuration, 4 teach-in process, 9 76

MRROC++ Based Controller of a Dual Arm Robot System ... - CiteSeerX

MRROC++ Based Controller of a Dual Arm Robot System ... - CiteSeerX

Suggest Documents