rubik's cube puzzle as a benchmark for service robots - CiteSeerX

RUBIK’S CUBE PUZZLE AS A BENCHMARK FOR SERVICE ROBOTS Cezary Zieli´nski, Wojciech Szynkiewicz, Tomasz Winiarski, Maciej Staniak

Warsaw University of Technology, Faculty of Electronics and Information Technology, Institute of Control and Computation Engineering, ul. Nowowiejska 15/19, 00–665 Warsaw, Poland, tel. +48 22 660 7866, fax. +48 22 825 3719 Email: [C.Zielinski,W.Szynkiewicz,T.Winiarski,M.Staniak]@ia.pw.edu.pl Abstract This paper describes Rubik’s cube manipulation as an example of highly demanding benchmark task for two-handed service robot. Vision is used both for acquiring the cube (servoing) and identifying its initial configuration. During two-handed manipulation force/torque measurements are used to prevent jamming of the cube. Rubik’s cube solver, nominal trajectory generation, visual servoing, and position-force control have been successfully integrated by the MRROC++ robot programming framework.

Index Terms Service robot, robot sensing systems, position-force control

1

RUBIK’S CUBE PUZZLE AS A BENCHMARK FOR SERVICE ROBOTS I. I NTRODUCTION Service robots operate in only partially structured environments, unlike their industrial counterparts which perform their tasks in fully structured surroundings. Moreover, service robots act 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. The environment, as it is adjusted to human capabilities, imposes certain constraints on the design of service robots. They must posses: •

Two-handed manipulation capability,

•

The ability to use vision data for the purpose of grasping immobile and moving objects,

•

The capacity for using tactile and force stimulus in manipulation,

•

The capability of both reasoning and quick reaction to the incoming stimulus from the environment,

•

Multi-modal communication capabilities, i.e., communication through speech, gesture, facial expressions,

•

The ability to propel themselves and navigate without collisions in the environment.

In other words, service robots must have similar motion, sensing and behavioural capabilities as humans, because they are going to operate in an environment that has been structured to fit human capabilities. The main research problem is control of this type of equipment and integration of its numerous and diverse subsystems. The investigation of the above problems requires a benchmark task. The authors of this paper propose 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),

2

•

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 ability to transfer over large distances in the environment.

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. However, this paper concentrates only on the first four capabilities, treating them as the most important and difficult to integrate. The software of the control system was implemented using MRROC++ [25], [26] robot programming framework. One of the challenging problems of service robotics is grasping and manipulating objects in semi-structured environments, where object properties are not known a priori and sensing is prone to errors. Many studies have been devoted to single-handed manipulation, for overview see [13]. However, two-handed manipulation has attracted less attention, especially in an uncalibrated workspace. Our research is focused on understanding the nature of two-handed cooperative manipulation and enhancing the manipulative skills of the service robot. Two cooperative hands, if properly utilized, are capable of grasping and manipulating a much larger class of objects, including long, flexible, irregularly shaped, and complex objects (e.g., Rubik’s cube). However, there are only a few works on a two-handed robotic manipulation (see for example [21], [28]). The pure pose control of manipulators in task space needs an exact and very precise model of robot and environment, especially in the situation of contact between manipulator tool and external objects. Determination of the model is typically very complicated and takes a lot of time. In addition, it is impossible to predict unexpected obstacles, hence even the best model could be incomplete. Fortunately information from force/touch sensors and cameras can influence the manipulator trajectory and system becomes immune for model inaccuracies. Control methods utilizing force sensors are still a research challenge [12]. We can distinguish two main topics in this area [20]. How to combine position and force control in one direction of movement, and how to do this for complete set of six coordinates (3 translations and 3 rotations). There are some control strategies integrating position and force control in one direction of movement. The basic ones are stiffness control and impedance/admittance control. Stiffness control provides some stiffness in the situation of contact between manipulator and environment. The impedance control [5], [10], [18] extends stiffness control by introducing

3

mechanical impedance. Distinguishing position and force directions of movement is another possibility [12], [14]. The well known and useful solution of this problem is Task Frame Formalizm (TFF) [2], [11]. In TFF position and force directions are defined separtely in the task frame. The TFF strategy can be combined with impedance control producing individual impedance associated with each direction. Position-based (PB) visual servos are sensitive to the quality of calibration of: camera parameters, camera-robot relative location and robot kinematic model [6]. Due to that imagebased (IB) visual servos are drawing the attention of scientists. However, the trajectories that are generated in the image space may not be feasible in the task space, due to highly nonlinear transformation between the two spaces [3]. The search for the solution of this problem takes into account such image features as object moments [17], transformation of coordinates into cylindrical coordinate frame [7] or enhancing the image Jacobian. The method that we propose uses PB servo and a stand alone camera for rough localization and bringing the effector to the vicinity of the object to be grasped (this does not require high quality of calibration), and then switching to IP servo using an eye-in-hand (EIH) camera configuration. As the trajectory executed by the IB servo will be short the problems with convergence of the algorithm and keeping the features within the field of view of the camera will not occur [4]. II. MRROC++ MRROC++ [25], [26] is a programming framework facilitating the implementation of single and multi-robot system controllers executing diverse tasks. Its structure was derived by formalizing the description of the general structure of multi-robot controllers [27]. Its implementation was preceded by a version for single robots (RORC [22], [24]), which was later substituted by a non-object oriented version for multi-robot systems (MRROC [23], [24]). MRROC++ is derived from C++, thus it is object oriented. It uses the facilities (e.g., processes, threads, inter process communication) provided by a real-time operating systems QNX ver.6.3. The software pattern provided by MRROC++ results in a hierarchic structure (Fig.1). The system coordinator is implemented as the Master Process MP supplemented by zero or more Virtual Sensor Process VSP. Each arm is controlled by its own control subsystem, which is composed of the Effector Control Process ECP, Effector Driver Process EDP supplemented by zero or more Virtual Sensor Process VSP. The VSPs are responsible for aggregating data from hardware sensors, and thus providing sensor readings that can be readily utilized in motion control. Sensors are classified into two categories: exteroceptors

4

and proprioceptors. The former gather the information about the state of the environment and the latter about the state of the effectors (e.g., their position and orientation). However some sensors exhibit duality in this respect (e.g., force/torque sensors mounted in the wrist of an arm), as they can provide information both about the environment and the limbs. The VSPs process the information obtained from the exteroceptors.

MRROC++

UI

System dependant layer Task dependant layer

MP ECP1

ECP2 Hardware dependant layer

EDP1

VSPECP1

VSPMPC

VSPMPF

Parallel interface

Axis controllers

Effector2

Fig. 1.

VSPECP2

EDP2

Parallel interface

Exteroceptors

Axis controllers

Effector1

General control system structure

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

5

using motion generators and initial conditions respectively, as their arguments. The code of MP contains similar Move and Wait instructions [27], but they refer to all the coordinated effectors, whereas the Move and Wait instructions within ECPs refer to single effectors. The User Interface Process UI depends only on the number of effectors constituting the system. It is responsible for presenting the system status to the operator and enables the operator to: start, pause, resume or terminate the execution of the task. It is not necessary in autonomous systems. III. E XPERIMENTAL SETUP The structure of the controller produced by using MRROC++ for the purpose of executing the benchmark task is presented in fig. 1. The overall experimental setup consists of two 6 degree of freedom (dof) modified IRp-6 robot arms, each with a parallel jaw gripper fig. 2. Each jaw is instrumented with tactile sensors which detect the presence of contacts with the grasped object. Moreover, each hand is equipped with a wrist-mounted ATI Gamma forcetorque sensor, and an eye-in-hand miniature CCD 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.

CCD camera force/torque sensor

tactile sensor CCD camera

Fig. 2.

Sensors used to locate and manipulate the Rubik’s cube

6

IV. RUBIK ’ S C UBE SOLVER The standard 3 × 3 × 3 version of Rubik’s Cube consists of 27 subcubes, and its statespace contains about 4.3 × 1019 states. This is the number of states that can be reached from any given state. However, the complete state space of such a cube consists of twelve separate subspaces with no legal moves connecting them. The challenge is to develop an efficient algorithm to find optimal, or near-optimal, solution within practical computational and memory limitations. To solve the Rubik’s Cube we use Iterative-Deepening-A∗ (IDA∗ ) algorithm [15] with heuristics based on pattern databases [8]. IDA∗ is a depth-first search that looks for increasingly longer solutions in a series of iterations, using a lower-bound heuristic to prune branches once their estimated length exceeds the current cost threshold. The cost threshold for the first iteration is the heuristic value of the initial state, and increases in each iteration to the lowest cost of all nodes pruned in the previous iteration. IDA∗ guarantees an optimal solution if the heuristic function is admissible. IDA∗ requires memory that is linear in the maximum search depth. We define any 90, 180, and 270 degree twist of a face as one move. We never twist the same face twice in a row, because the same result can be obtained with a single twist of the face. The median optimal solution appears to be 18 moves, and it is believed that any cube can be solved in no more than 20 moves [8]. 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 sub-optimal algorithm that produces satisfactory solution, typically 20–22 moves. The solver is located in the MP. It is invoked when the current state of the cube has been obtained from the global vision system. In this stage instead of VSPM P C , a special VSPM P C ′ is used for the purpose of identifying the state of each face of the cube. The cube is presented by the robot holding it to the camera, one face at a time, to avoid incorrect identification of the initial state of the cube. This would be disastrous to the solution of the Rubik’s cube problem, and thus to the execution of the task as a whole. V. T WO

HANDED MANIPULATION

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 [16]. The task that the system has to execute can be divided int two phases: acquiring the cube and manipulating it. For the execution of the first phase visual servoing [6], [9] combined with stiffness control are utilized, while in the second phase TFF (i.e., selection of directions

7

for position or generalized force control) is used. Efficient realization of TFF requires the development of sensor-based manipulation primitives which robustly perform task-relevant functions and can be easily composed into sequences or complex nets, which are solutions to more complex manipulation tasks. To describe these primitives we use TFF, which is easily implemented within the MRROC++ programming framework. The task of solving Rubik’s cube needs several sensor-based manipulation primitives such as: •

approaching the cube while avoiding collisions by using visual information from standalone camera and grasping the cube using eye-in-hand and tactile sensors mounted in the jaws together with force/torque measurements for stiffness control,

•

sensorless motions, to present the cube to the global camera for identifying the cube’s initial state,

•

turning the faces of the cube while avoiding jamming using information from force/torque and tactile sensors for implementing TFF.

The task 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 [26], [27]. VI. M OTION

CONTROL

Fig. 3 presents a more detailed structure of the motion controller. This structure covers both visually guided motions and two types of force control: stiffness control and TFF. When the end-effector is not in contact with the environment, only the cameras are used to locate the cube and guide the gripper to the grasping location G. The VSPM P C associated with the MP processes the image obtained from the camera to extract image features fG necessary for visual servo-control. It is assumed that the Rubik’s cube faces contain tiles of a’priori known colors. Fast classification in HSV color space finding connected regions is performed [1] to find the tiles in the image. The regions are filtered according to their size, circularity, number of vertices and number of neighbours. Only the regions which are quadrangles with high value of circularity (resembling squares or rhombuses rather than general rectangles or parallelograms) and having at least three neighbours (similar regions not too far apart from each other) are considered. Those regions (tiles) are grouped into faces of cube using K-mean algorithm

8

MP

ECP

E

T0

. *

H

EDP

Hardware

0

( )-1

TE

4

direct kinematics 0

macrostep M generator

step generator

TE

inverse kinematics

4d +

4

H

4

TG

C

.

Fig. 3.

TG

* 0

TC

Robot & F/T trans

F/T

VSP MPF 0

u

joint controllers

fG feature pose estimation extraction

object image

camera

object view

environment

VSP MPC

The structure of the control system

taking diagonals and areas as input vectors. For the most visible face the image coordinates of centers of the four corner tiles is calculated. They form the visual features fG . The pose of the object G with respect to the camera coordinate frame C is denoted as C

TG . This pose is transformed into the robot coordinate frame 0, thus 0 TG is obtained. C TG is

obtained using the four coplanar points. Such solution is based on two important assumptions – at least one face of the cube is well visible (without occlusion) and there is no other similar object in the field of view (FOV). As opposed to [1] we use HSV color space calculated from RGB not YUV obtained directly from the framegrabber. The HSV has better properties for color classification in our task. The HSV is precalculated off-line and stored in a lookup table (LUT), so during classification HSV values corresponding to a RGB pixel are not calculated, but are extracted from the LUT. The MP receives from the VSPM P C the 0 TG and the current end-effector pose 0 TE from the ECP which in turn receives it from the EDP computing the direct kinematics task. In consequence the control error ε (E TG ) can be computed. The macrostep generator produces a motion trajectory M using the control error. The trajectory M is a set of macrosteps, i.e., a sequence of poses 0 TE ′ through which the end-effector is to pass. Only the first pose of M , i.e., 0 TE ′ , is sent to the EDP via the ECP (ECP is transparent in those experiments, as all the trajectory generation tasks are performed in the MP, due to the manipulators tightly cooperating). The step generator within the EDP divides the macrostep into even smaller

9

steps, each representing a pose. The calculated pose 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 MP computes a

new trajectory based on the new information delivered by the VSPM P C . However, when the end-effector enters into contact with the environment, the VSPM P F produces, on the basis of measurements obtained from the force/torque sensor, an input to the macrostep generator that modifies the trajectory M . Thus the manipulator becomes compliant, reducing impact. When the cube is securely held by both hands, the vision is no longer necessary, but force measurements are of paramount importance for two-handed manipulation. At this stage TFF is used to select position and force control directions, thus avoiding jamming. In the stiffness control mode the MP process generates macrostep command every 20 ms. The command contains the end-effector pose, which is to be reached in 20 ms. The pose is calculated on the basis of both force measurement (from VSPM P F process) and vision processing (VSPM P C process). The VSPM P C process generates the new end effector velocity every 40 ms and VSPM P F process does the same every 20 ms. The time periods of virtual sensors differ hence there is an extra parameter needed to communicate with VSP processes, which defines the time that new information from a specific VSP will be ready. The resultant end effector velocity takes into account the information from both virtual sensors. The endeffector velocity is transformed into the end effector pose, which is to be reached in a specified time. In position/force control MP process generates a macrostep command every 20 ms. In this case the macrostep command contains not only the end effector pose in a certain direction in a specific reference frame, but also information about force/torque set point in the same frame, but in the orthogonal directions. The force/torque measurements are utilized in the EDP process. In this case force/torque sensor is used as a proprioceptor, thus a VSP is not necessary to execute TFF. The EDP does it on its own. The internal time period of position force controller in EDP is 2 ms. The detailed implementation is presented in [19]. VII. C ONCLUSION The paper presents the Rubik’s cube puzzle solution as a task requiring nearly all skills that are essential in a service robot. It concentrates on both several necessary subsystems (i.e., visual servoing, visual recognition and localisation, position-force control, logical solution of the puzzle and trajectory generation for the two-handed system) and their integration into a

10

coherent system. The problem of significantly differing data rates produced by the subsystems has been solved within the framework of MRROC++. Currently the system is undergoing intensive testing. R EFERENCES [1] J. Bruce, T. Balch, and M. Veloso. Fast and inexpensive color image segmentation for interactive robots. In Proc. of the IEEE/RSJ Int. Conference on Intelligent Robots and Systems (IROS’00), volume 3, pages 2061–2066, 2000. [2] H. Bruyninckx and J. De Schutter. Specification of force-controlled actions in the task frame formalism: A synthesis. IEEE Transactions on Robotics and Automation, 12(4):581–589, August 1996. [3] F. Chaumette. Potential problems of stability and convergence in image-based and position-based visual servoing. In Workshop on Vision and Control, Block Island, USA, June 1997. [4] G. Chesi, K. Hashimoto, D. Prattichizzo, and A. Vicino. A tutorial on visual servo control. IEEE Transactions on Robotics, 20(5):908–914, Oct 2004. [5] S. Huang and J.M Schimmels. Sufficient conditions used in admittance selection for force- guided assembly of polygonal parts. IEEE Transactions on Robotics and Automation, 19(4):737–742, August 2003. [6] S. A. Hutchinson, G. D. Hager, and P. I. Corke. A tutorial on visual servo control. IEEE Trans. on Robotics and Automation, 12(5):651–670, Oct 1996. [7] M. Iwatsuki and N. Okijama. A new formulation of visual servoing based on cylindrical coordinate system. IEEE Transactions on Robotics, 21(2):266–273, Apr 2005. [8] R. Korf. Finding optimal solutions to rubik’s cube using pattern databases. In Proceedings of the National Conference on Artificial Intelligence (AAAI-97), pages 700–705. July 1997. [9] D. Kragic and H. I. Christensen. Robust visual servoing. Int. J. of Robotics Research, 22(10-11):923–939, 2003. [10] Z. Lu and A.A. Goldenberg. Robust impedance control and force regulation: theory and experiments. Int. Journal of Robotics Research, 14(4):225–254, June 1995. [11] M. Mason. Compliance and force control for computer controlled manipulators. IEEE Transactions on Systems, Man, and Cybernetics, (11):418–432, 1981. [12] C. Natale. Interaction control of robot manipulators, six degrees of freedom tasks. springer tracts in advanced robotics. 3, 2003. [13] A.M. Okamura, N. Smaby, and M.R. Cutkosky. An overview of dexterous manipulation. In Proceedings of the IEEE International Conference on Robotics and Automation, pages 255–262, April 2000. [14] S.R. Pandian and S. Kawamura. Hybrid force/position control for robot manipulators based on a d-type learning law. Robotica, 14:51–59, 1996. [15] S. Russell and P. Norvig. Artificial Intelligence: A Modern Approach. Prentice Hall, Upper Saddle River, N.J., 1995. [16] W. Szynkiewicz. Motion planning for multi-robot systems with closed kinematic chains. In Proceedings of the 9th IEEE International Conference on Methods and Models in Automation and Robotics MMAR’2003, Miedzyzdroje, pages 779–786, August 25-28 2003. [17] O. Tahri and F. Chaumette. Point-based and region-based image moment for visual servoing of planar objects. IEEE Transactions on Robotics, 21(6):1116–1127, Dec 2005. [18] T. Tsumugiwa, R. Yokogawa, and K. Hara. Variable impedance control based on estimation of human arm stiffness for human-robot cooperative calligraphic task. pages 644–650, May 2002.

11

[19] T. Winiarski and C. Zieli´nski. 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. [20] G. Zeng and A. Hemami. An overview of robot force control. Robotica, 15:473–482, 1997. [21] J. Zhang, Y. von Collani, and A. Knoll. Interactive assembly by a two-arm-robot agent. Robotics and Autonomous Systems, 29(1):91–100, 1999. [22] C. Zieli´nski. Flexible controller for robots equipped with sensors. In 9th Symp. Theory and Practice of Robots and Manipulators, Ro.Man.Sy’92, Udine, Italy, Lect. Notes: Control and Information Sciences 187, pages 205–214. Springer-Verlag, Berlin, 1993. [23] C. Zieli´nski. Control of a multi-robot system. In 2nd Int. Symp. Methods and Models in Automation and Robotics MMAR’95, Miedzyzdroje, Poland, pages 603–608. 30 Aug.–2 Sept. 1995. [24] C. Zieli´nski. Robot Programming Methods. Publishing House of Warsaw University of Technology, Warsaw, 1995. Also: http://www.ia.pw.edu.pl/∼zielinsk/. [25] C. Zieli´nski. Object–oriented programming of multi–robot systems. In Proc. 4th Int. Symp. Methods and Models in Automation and Robotics MMAR’97, Miedzyzdroje, Poland, pages 1121–1126. August 26–29 1997. [26] C. Zieli´nski. The mrroc++ system. In 1st Workshop on Robot Motion and Control, RoMoCo’99, Kiekrz, Poland, pages 147–152. June 28–29 1999. [27] C. Zieli´nski. By how much should a general purpose programming language be extended to become a multi-robot system programming language? Advanced Robotics, 15(1):71–95, 2001. [28] R. Z-ollner, T. Asfour, and R. Dillmann. Programing by demonstration: Dual-arm manipulation tasks for humanoid tobots. In Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS2004), Sendai, Japan, pages 479–484, Sep. 28–Oct. 2 2004.