Usage of simulations to plan stable grasping of unknown objects with a 3-fingered Schunk hand Jimmy A. Jørgensen and Henrik G. Petersen Abstract— In this paper, we present ongoing work for using simulations to optimize grasp control where objects are located by 2D images. We use a twostep approach: First, we generate a bounding volume of the object based on the 2D image. Using this bounding volume, we use classical kinematic simulations to find an optimal collision free approach to the 2D extremal curve of the object. Our optimality criteria are here to approach perpendicular to the 2D surface and to achieve a good 2D force closure. In the second step, we use dynamic grasp simulations to optimize the online control based on tactile information. We demonstrate the current status of our system using simulations of the 3-fingered Schunk dextrous hand SDH mounted on a Schunk light weight arm LWA3. We present our preliminary results of using simulations to grasp optimization.
I. INTRODUCTION The problem of grasping unknown objects with robots is currently achieving substantial interest due to its big importance mainly within service robotics. The problem may be divided into 3 main aspects: • Object detection • Planning of a (collision free) approach toward the object (movement before first object touch) • Control of the grasping process after the first touch of the object. Within industrial settings, the objects are usually known (3D CAD model exists). In such situations, good grasp locations may be found off-line. Furthermore, even for bin picking, object detection can be achieved by comparing the 3D CAD models with either images from laser triangulations [1] or camera images [2]. Output of these systems is thus a precise 6D pose estimation of the object to be grasped. By using a good approach plan, the objects may then often be precisely grasped even with simple devices using no tactile feedback at all. It should however be pointed out, that the current off-line choice of grasper and grasp locations may be too time consuming for the rapidly consumer driven changes in future and sometimes also current manufacturing. Similar to the use of robot-scene simulators such as ABB’s RobotStudio for facilitating the setup and verification of an industrial robot platform, the use of simulators to facilitate and verify choices of graspers and grasp locations for a given known object will therefore be beneficial for industrial grasping. This work was supported by the Danish Agency for Science, Technology and Innovation through the project Movebots and by the Danish National Advanced Technology Foundation through the project Handyman Jimmy A. Jørgensen and Henrik G. Petersen are with the Maersk McKinney Møller Institute, University of Southern Denmark, Niels Bohrs Alle 1, DK-5230 Odense M, Denmark
[email protected]
[email protected]
Grasping unknown objects is a different issue as the object detection will by definition only give partial information about the size, shape, weight and pose of the object. The planning of the approach has to be based only on this information. After the first touch, tactile information may be used for optimizing the grasp. The tactile information can be of a very different type and detail ranging from simple contact detectors to detailed information about both normal and shear forces. It is however not yet established how the information from the tactile sensors should be used in an optimal way. In order to develop optimal grasp control based on different tactile information, a vast amount of experiments with grasping objects of different sizes, shapes, surfaces and weights must be carried out. As this is often practically inadequate, it is important to have a simulation system that is fast, precise and flexible, so that many of the experiments can be carried out in a virtual environment. Speed is needed to be able to carry out enough simulations to cover a subset of objects that may be encountered. The major computational bottleneck is collision detection where usage of (massively) parallel processors such as FPGA’s [3], [4] and [5] are very promising. Precision is necessary for doing reliable grasp simulations properly. It is thus important to have a precise model of contact forces (normal, shear, friction). Furthermore, the physics engine must correctly handle the impacts. There are a variety of these engines [6], each with benefits and drawbacks. Both the contact model and the physics engine should be seen as plugin components to a flexible overall grasp simulation system. With flexibility, we refer to the key for integrating various tactile sensor models that enable simulations of various online algorithms with feedback from these sensors. It is the purpose of this paper to present ongoing work for creating a new highly flexible grasp simulation system that incorporates tactile sensor models and the possibility to optimize the online grasp control. Our system may be viewed as being similar to GraspIt [7], which even has some extended functionalities for grasp analysis that our system lacks. However, GraspIt relies on one single type of contact model and an associated LCP framework to handle the associated dynamics. In our framework, both contact models and dynamics methods are open for changes. Furthermore, our simulator enables simulation of the following sensor types: Camera, 3D Scanner, Line scanner, 1D range scanner, simple on/off switches and tactile arrays. We will illustrate our system by addressing a specific setup, where well separated unknown objects are to be picked from a table. The object
detection system is a single camera (finding the objects as 2D blobs) and the grasping device is a 3-fingered Schunk dextrous hand SDH (see Figure 1 mounted on a Schunk light weight arm LWA3.
(a) Parallel grip
(b) Ball grip
Fig. 1. The 3 fingered Schunk SDH hand. Each finger has two degrees of freedom. An extra degree of freedom allows two of the fingere to rotate in opposite directions to arbitrary positions between a parallel grip (a) and a ball grip (b)
description file, which includes body dynamics, contact data, material data, dynamic devices, sensors and more. Kinematic data of the scene is described using the RobWork XML scene format. When initialised the simulator extends access to all simulated devices and sensors through RobWork control and sensor interfaces. This enables smooth integration to real hardware that comply with these interfaces. Our simulator can currently use 3 physics engines interchangeably: ODE, Bullet and RWPhysics. The reason for this is twofold: firstly applications strongly vary in requirements of the simulator (precision, speed, stacking, etc.) and to our knowledge no single physics engine support everything, supporting a set of engines is therefore reasonable. Secondly comparative studies between different aspects of simulation is made simpler with the support of multiple physics engines. A thin software wrapper of the physics engines ensure compatibility with the general functionality of our simulator.
The paper is organised in the following way: In Section 2, we give a presentation of our system. In Section 3, we discuss how grasp simulation can be used to optimize the grasp approach based on 2D images. In Section 4, we then discuss how grasp control can be optimized based on the tactile information from the Schunk hand. In Section 5, we present experimental results and Section 6 concludes the paper. II. THE SIMULATION SYSTEM AND THE EXPERIMENTAL SETUP The simulation system is based on the robotics framework RobWork (www.mmmi.sdu.dk/robwork), which has support for a variety of kinematic and mathematical tools for robotics such as forward kinematic structure, general iterative and closed form inverse kinematics, motion planning, serial chain robots, parallel robots, tree structured robots (e.g. hands), collision detection and scene graph visualisation. Furthermore a RobWork front-end based on QT enables plugin development for individual projects. Our simulator is controlled through such a plugin and the scene is visualised using the front-end RobWorkStudio. Bodies, devices and sensors are the primary components of the simulator. Bodies exist as three different types: Fixed, Kinematic and Rigid. Where fixed bodies are mere static obstacles, kinematic bodies are moving obstacles that affects the environment but does not itself get affected and lastly rigid bodies has a full dynamic description (inertia parameters) and its position and velocity are controlled entirely by the simulator. Beside the primary components, controllers for devices and tools for 2d and 3d grasp analysis and visualisation is available. Currently PD position controllers with velocity feed forward and optional synchronous joint velocity ramps are supported. In Figure 2 the overall structure of the simulator is drawn. The simulator is initialised with a XML formatted scene
Fig. 2. Overall structure of the simulator. The simulator is initialised with a scene description file and can currently use 3 different physics engines. Simulated devices are accessed through RobWork control and sensor interfaces.
The experimental setup was created to support the development of a demonstrator application for Automatica 2008 in Munich, Germany, see fig. 3. The hardware for the demonstrator included a Schunk Dextrous Hand (SDH) with tactile array sensors from Weiss robotics, a Schunk Light Weight Arm (LWA3), a Nintendo Wii wireless joystick and a AVT Guppy firewire camera. The purpose of the demonstrator was to pick up unknown objects from a table and give it to a person standing in front of the table. As all ready mentioned several difficulties are related to the development of such an application, especially regarding the robustness of the process of grasping unknown objects. To increase the robustness of the application our simulator is used to tune selected application control parameters. The complete scene with robot, environment, objects and sensors is modelled and simulated in RobWork. In Figure 4, we show a virtual image taken by RobWork of the object and robot shown in Figure 5 before the grasp is initiated. In Figure 6, we show a snapshot of the simulated grasp and the associated simulated values of the array sensor (red spot). We shall now illustrate how this framework is used for
virtual optimization of grasp control. We shall show how to use the kinematic simulation to verify that approaches will be collision free (even though the object is unknown), and subsequently we illustrate how the dynamic simulation can be used to tune the online control based on tactile information.
Fig. 3. The actual demonstrator setup at Automatica 2008 in Munich, Germany. Setup includes 7 degree of freedom (DOF) Schunk Dextrous Hand mounted on a 7 DOF Schunk Light Weight Arm with AVT Guppy camera mounted above the robot and a Nintendo Wii joystick for providing user controlled input.
Fig. 6. Illustrated is a snapshot of our grasp simulation. Notice, that the simulated tactile information is marked by the red spot
III. OPTIMIZING GRASP APPROACHES
Fig. 4.
Fig. 5.
Virtual image of object from Figure 5
The virtual scene generated in RobWork
The first stage of the grasp is the object detection which as mentioned provides a 2D blob of the object. As we only have these 2D blobs, we have chosen to set lower and upper limits zmin and zmax of the height of the object in order to ensure graspability and collision avoidance respectably. First, we use the maximum height to create a bounding volume within which the object must reside (see Figure 7a)). For simplicity in the description below, we consider the simplified case where the object distance from the optical axis is small relative to the distance between the table and the camera so that get almost as tight a bounding volume by using a rectangular box where the horizontal and vertical bounding thus can be approximated as independent(see Figure 7b)). We restrict admissible grasp approaches to those that avoid undesired collisions between the hand and the bounding volume (by definition a subset of the approaches that avoid undesired collisions with the object). As our simulator handles all collisions, it can without problems be verified in simulation whether a given approach is collision free or not. We define an approach as a hand height z > zmax , a 2D position of the hand center (x, y), the hand orientation θ and the angle φ between the to rotational fingers defined so that φ = 0 corresponds to the parallel grip and φ = π3 corresponds to the ball grip. Among the set of admissible approaches, we wish to find the best ones. Several issues could be considered. The most important have been found to be
Fig. 7. Bounding volumes extracted from 2D blobs. In a), we show the general case and in b) the simplified case used in this paper, where the object distance to the optical axis is small relative to the distance between the camera and the table.
• •
• •
Grasp the object as close to the table as possible to achieve a large vertical region of possible object grasps The 2D approach directions of all three fingers are preferably orthogonal to the 2D boundary at the planned touch The induced 2D net force and torque is as low as possible (to avoid that the object slides during the grasp) The 2D boundary is touched by the 3 fingers at approximately the same time
The considerations of 2D approach directions and 2D net forces and torques (irrespectable of the height) have been conducted by considering a range of 2D shapes. This has lead to a rather complicated parametrized objective for optimizing the pure 2D approach, which we shall only present here as having been already optimized as f2D (x, y, θ, φ). As it as mentioned also is desirable to have the hand height z as low as possible to get a good vertical region of grasp, we may define an objective as a1 (z − zmax ) + a2 f2D (x, y, θ, φ)
(1)
IV. OPTIMIZING ONLINE GRASP CONTROL STRATEGY Next, we close the hand using the found optimal approach. When the object is touched, we move to controlling the fingers based on tactile information. The available information is the distribution of normal forces over the finger surfaces. The qualitative objective here is that the grasp is stable. Similar to the approach plan, we seek to achieve this by trying to fulfil several criteria. Here, we consider only two criteria: • To turn the fingers so that several contact points may be achieved for each finger. More contact points will (at least typically) lead to an increased space of grasp wrenches that may compensate for gravity at liftoff. • To keep the 3D net forces and torques induced by the detected normal forces as low as possible to avoid undesired object movements before liftoff. We assume here, that the horizontal forces and torques are handled by the approach and we thus may establish an objective based on the vertical forces and torques. Furthermore, by ensuring that the finger movement of a finger that has touched the object is shetopped until all three fingers have achieved contact, we may from now on assume that the object is touched by all three fingers. The total vertical force is then modelled as F =
3 X
fi nzi
(3)
i=1
where fi is the force measured by the array sensor of the i’th finger and nzi is the vertical part of the outer normal of the i’th finger at the contact point. Similarly, the torque around a representative point R is modeled as T =
3 X
∆ri × (fi ni )
(4)
i=1
It may seem that we can just put z = zmin and optimize f2D , but this is not true since lowering z leads to less admissible (collision free) 2D approaches x, y, θ, φ. Rather than searching for suitable parameters a1 and a2 , we currently take the somewhat simpler approach by solving the following optimization problem min f2D (x, y, θ, φ)
(2)
subject to that the found approach is verified by the simulator to be collision free and the finger touches the boundary at a fixed height less than zmin The disadvantage of this simplified optimization is that the fixed height finger touches impose additional restrictions on the space of admissible grasp solutions so that some objects may wrongly be found ungraspable. However, in practise we have not encountered this problem yet. In Section 5, we discuss the results of using the simulator for collision checks to find good grasp approaches.
where ∆ri is the location of the i0 th contact point relative to the representative point and ni is the outer normal of the i’th finger at the contact point. In order to achieve a bigger wrench space to compensate for gravity at liftoff, we seek to vertically turn the finger in order to move the lower part inwards when the contact point is relatively high. Our variables to control are the finger angles α = (α1 , α2 , α3 ). The angles are zero when the finger is vertical and is positive when the lower part is moved inwards. We do not allow negative finger angles. Clearly, we can compute ni as the function of αi . Thus, F and T are functions of these. Our objective is b1 F (α)2 + b2 T (α)2 − b3
3 X
zi αi
(5)
i=1
We only consider the first two contributions here. In Section 5, we illustrate preliminary results of how we can use the simulator to optimize the relation between b1 and b2 . Both for the approach and the grasp control, it is by no means obvious how important the various criteria are with
respect to each other and other criteria should very likely be considered as well. Our objectives in Eq.2 and Eq.5 should therefore be viewed only as preliminary objectives that is been used as an example of how the simulator can be used to optimize any objective.
manage to grasp stably in about one third of the cases. In order to improve, it seems most promising to address the 2D approach in more detail, but also to establish better 3D criteria based on the virtual tactile measurements. We are currently working on these issues.
V. EXPERIMENTAL RESULTS The simulation framework was used to conduct a number of grasp tests on each object in the object database. Each test was setup with a place robot in home configuration; grab image and extract contour; compute offline approach grasp and motion; place robot at start of approach path; start simulation; while on approach path move robot along approach path; while joint velocity > 0 move to grasp by online grasp control; Fig. 8. Some of the objects used for optimizing control parameters for stop simulation; robustness. calculate 3d force closure of grasp; For our experiments, we considered grasping of the 10 objects illustrated in Figure 8 in 3 different support modes. The objects was chosen so as to look like typical every day objects (many taken from www.modelsbank.3dm3.com) and such that simple vision algorithms could be used for contour extraction e.g. bright color and limited shadow effects across object. We have chosen zmin and zmax simply as the extremal values of the object heights for all objects in all considered support modes. We used the described simulation based 2D optimization to find robust collision free approaches. During the approach, we move the outer finger joints at half the speed of the inner finger joints. For online control, we then considered 2 cases: In case 1, we simply continue the approach motion after the touch and thus do not use the simulated tactile input (corresponding to b1 = b2 = 0. In case 2, we seek to minimize the total net torque corresponding to b1 = 0, b2 = 1. For each of the experimental cases, the first quality measure is the number K of successfull grasps (where the object was not lost). For each successfull grasp, we measure the minimal distances fi i = 1, . . . , K to the border of the force wrench space and the minimal distances ti i = 1, . . . , K to the border of the torque wrench space. Based on these, we have overall qualities v u K u1 X t f −2 QF = K i=1 i v u K u1 X QT = t f −2 K i=1 i of our online control. Our preliminary investigations indicates that there is hardly any improvement from case 1 to case 2 in the torque wrench space. Furthermore, we only
The next step will be to include more parameters and objects with the purpose of optimising the control parameters where an iterative stochastic search heuristic is used. The heuristic starts from an initial list of parameter sets. For each iteration a local search will try and improve the parameters of each set in the list. VI. CONCLUSION We have presented ongoing work for using kinematic and dynamic simulations to optimize grasping of unknown objects. We considered avoidance of undesired hand-object collisions by extracting a bounding volume of the object. We have shown preliminary results of how to use dynamic simulations to tune online control Many issues will be addressed in the near future. These include • simulation based 2D parameter tuning that combines height and contour parameters • Incorporation of noncubic bounding volumes, bounding volumes based on multiple cameras and on laser triangulations • Support for accelerated simulation/collision check (CELL/GPU/FPGA). • Integration of a wider range of tactile sensor types In addition, we will conduct a substantial amount of new tests which includes extensive comparisions of simulations versus real experiments with our Schunk SDH hand. R EFERENCES [1] R.D. Schraft, T. Ledermann: Intelligent picking of chaotically stored objects, Assembly Automation, Vol. 23,1, 2003 , pp. 38-42 [2] www.scapetechnologies.com [3] Nuzhet Atay, John W. Lockwood, Burchan Bayazit, A Collision Detection Chip on Reconfigurable Hardware, Technical Report WUCSE2005-33, Washington University, St. Louis, 2005.
[4] Andreas Raabe, Blazej Bartyzel, Joachim K. Anlauf. Hardware Accelerated Collision Detection An Architecture and Simulation Results”, Proc. of the Conf. on Design, Automation and Test in Europe, Vol. 3, pp. 130-135, 2005 [5] Muiris Woulfe, John Dingliana and Michael Manzke, Hardware Accelerated Broad Phase Collision Detection for Realtime Simulations, Proc. of the 4th Workshop on Virtual Reality Interaction and Physical Simulations, pp.79-88, 2007. [6] See http://www.havok.com, http://www.ode.org/, http://www.bulletphysics.com, http://www.nvidia.com/object/nvidia_physx.html [7] Andrew T. Miller. GraspIt!: A Versatile Simulator for Robotic Grasping, Ph.D. Thesis, Department of Computer Science, Columbia University, 2001.
