ManIPA: A flexible Manipulation Framework for Collision ... - CiteSeerX

ManIPA: A flexible Manipulation Framework for Collision Avoidance and Robot Control Ulrich Reiser Fraunhofer IPA Stuttgart, Germany

Rene Volz Fraunhofer IPA Stuttgart, Germany

A BSTRACT In this paper a flexible software framework is presented, that allows for the easy configuration, modeling, simulation, and collision avoidance for the manipulators of a service robot. The configuration of the robot including the number of manipulators, their degrees of freedom, the corresponding Denavit-Hartenberg parameters, geometry, fieldbus parameters etc. can be specified easily in xml configuration files. Having configured the robot appropriately, the software provides forward kinematics and a numerical solution to the inverse kinematics of the robot arms. The manipulators can be actuated by a simple control interface which allows movements in joint and cartesian space. According to the parameters given in the configuration files, the robot is modeled by oriented bounding boxes, which are used for collision avoidance calculations. An OpenGL visualisation of the modeled robot is provided, which can be used for simulation. A simple simulation is implemented for robot manipulators with ramp based actuator control. The framework was tested and evaluated with two mobile service robots at Fraunhofer IPA, one of which consists of three manipulators with alltogether 13 degrees of freedom.

I. INTRODUCTION The variety of different robot hardware, especially manipulation hardware, has hindered development to great extent in the last years. Most research laboratories that are working in robot manipulation have developed their own manipulation hardware, because until recently no appropriate robot arms have been available on the market. Usually a lot of engineering work has to be done to get the mechanics, electrics, electronics and control of the special solutions to a state, where the actual research can be started. Commonly, the engineering effort cannot be reused for different hardware, such that the same work has to be redone for each manipulation hardware. The analogous problem can be observed on the software layer: kinematics, dynamics, simulation, modeling, collision avoidance etc. have to be adapted to changing hardware. A lot of integration frameworks for robotic applications have evolved in the past years. One of the most used is the Player Stage framework [1], [2], which bases on the UNIX device concept and offers a large variety of hardware drivers for mobile plattforms including laser scanners, cameras, etc. and a powerful simulation engine, GAZEBO. Manipulation, however, is not addressed so far. Orca [3] is an open-source framework for developing componentbased robotic systems. It provides the means for defining and

Felix Geibel Fraunhofer IPA Stuttgart, Germany

developing the building-blocks which can be pieced together to form arbitrarily complex robotic systems, from single vehicles to distributed sensor networks, exclusively manipulation. MARIE [4] constitutes a design tool for mobile and autonomous robot applications by integration existing frameworks like Player, CARMEN [5], and many others. In [6] an integration framework is presented, which provides a basis for distributed development. There are many other integration frameworks, whose enumeration would go beyond the scope of this paper. However, the effort that has been and is still being put into the development of these architectures demonstrates the need for easy and fast integration in the robotics community. The software framework presented in this paper focuses on manipulation and allows for easy adaption of the kinematics and collision avoidance modules to different manipulator hardware. In the ideal case, only the parameters in the configuration files have to be edited without the need to write one single line of code for controlling a robot arm in cartesian space with online collision checking. In the following, the modular architecture of the framework along with the main building blocks will be described. In the next section, the concept of easy reconfiguration and the structure of the configuration files is illustrated. Thereafter, the modeling of the robot and the environment and the existing collision avoidance algorithms are presented. Finally, examples for different robot configurations are given in the visualisation section. II. ARCHITECTURE ManIPA consists of the following main building blocks (see fig. 1): • Configuration • Low Level Control • Kinematics and Trajectories • Collision Avoidance • Visualisation The building blocks are connected by generic interfaces, which allows for highly modular development. All classes in the low level control module are hardware dependent and a driver or a simulation class has to be written that implements the LowLevelControl interface. All other modules are hardware-independent in the sense that no class has to be rewritten for new hardware. The hardware parameters concerning the other building blocks, e.g. Denavit-Hartenberg

parameters for the forward kinematics, can be specified in the configuration files (see section III). In the kinematics and trajectories building block class libraries are provided that implement forward kinematic calculations and a numerical solution for the inverse kinematics. If an analytical solution for the manipulator is available, it can be integrated very easily into the framework by a new class implementing the GenericArmCtrl interface. Furthermore, the interface offers smooth trajectory movements. Collision free movements are ensured by the Collision Avoidance module, which offers class libraries for offline path planning or online collision checking. The models necessary for the collision checking algorithms are built and maintained from the static configuration data, from the current joint angles of all manipulators and sensor data. The configuration module constitutes an essential part of the framework - it provides tools for the simple reconfiguration of existing hardware and fast porting of the software to different hardware. This is actually that part of the framework, which distinguishes it from other architectures. The parameters which can be specified in the configuration files will be described in section III. Finally, the framework offers a simple visualisation of the hardware setup, which is especially helpful when the hardware is (partly or completely) simulated. III. CONFIGURATION Easy hardware reconfiguration is one of the key features of the manipulation framework. Existing software based on the presented framework does not need to be rewritten if hardware partly changes or if the software is to be ported to a different robot. The adaption work is limited to editing the configuration files and possibly writing a driver for the new hardware that implements the LowLevelControl interface. Fig. 2 shows the structure of the different configuration files. There is one main file which contains the global hardware configuration of the robot including number of manipulators, robot geometry, environment models etc. The specification of the manipulators as well as the robot and environment models are stored in additional configuration files. For instance, if another manipulator is to be added to the system, the enhancement work constrains to the following actions: • Create a new manipulator configuration file • Specify the filename in the main configuration file • Increase the Number of manipulators entry in the main configuration file • If not available, write a driver implementing the LowLevelControl interface for the new manipulator IV. COLLISION AVOIDANCE The task of robust and safe grasping with a robot manipulator in dynamic environments requires not only accurate kinematics and mature robot control, but also effective collision avoidance. This includes both self collisions of the arm and the hardware it is mounted on, and external collisions with the environment.

Most collision avoidance methods are based on static collision checks for a given configuration and current environmental obstacles. These collision checks are often very time consuming, as in most cases a large number of objects is involved, depending on the level of detail of the robot and environmental models. ManIPA provides a fast collision checking algorithm based on the CULLIDE [7] paradigm. The algorithm reduces the number of objects that are to be considered for distance calculations, which provides a significant increase in processing speed. The algorithm consists of two main phases: determination of potentially colliding objects by a rough distance check based on the velocity vectors of all moving parts, and subsequent elaborate collision tests for all objects in the determined potential colliding sets. The two phased procedure allows a reduced calculation effort due to the minimization of exact distance tests. Furthermore, an enhancement of the algorithm is proposed by using a 3D time-of-flight sensor for external collision avoidance. Performance measurements were conducted and a cycle time of less than 1 ms was achieved (see table I, [8]) for one collision detection step for a mobile robot with a 7 DOF arm and a 4 DOF sensor head. This is distinctly below the control cycle time of most robot manipulators, which makes the algorithm suitable for many applications. In general, the task of collision free movement of robot manipulators can be decomposed into the following ingredients: • Appropriate modeling of the robot and its environment • Determination of potentially colliding objects • Counter measurements to avoid collisions A. Collision Detection Algorithm In the following, it is assumed that the robot (and its environment) is modeled appropriately by e.g. polyhedrons or oriented bounding boxes (OBBs) [9]. In a first step, all moving parts of the robot are included in a rough distance check based on their velocity vectors. All parts that may collide are divided into potential colliding sets (PCS). In a second step, the PCSs are further investigated with respect to actual collisions. This is done by means of distance calculations of all parts in each PCS. The distance calculations can be performed by existing proximity query libraries like e.g., PQP [10]. The twophased procedure significantly reduces the calculation effort, because only few parts contained in PCSs need to be considered for expensive distance calculations. Choosing velocity vectors as ”view ports” provides an efficient way to determine the potential colliding sets. The algorithm was enhanced to check collisions with point clouds. All points that are in a certain distance from a velocity vector are added to the PCS. Fig. 3 shows an example for collision detection in connection with a 3D point cloud. B. Experimental Results The algorithm was implemented on a mobile robot with a 7 DOF manipulator and a 4 DOF sensor head. The robot

Fig. 1.

Fig. 2.

Building blocks of the architecture.

Structure of configuration files

was modelled with OBBs (see fig. 3,6). A 3D time-of-flight sensor is used to get sensor data from the environment. The sensor basically provides a 3D point cloud, which may be used to generate a geometric model of the environment. In a first step, continuous self collision checking was implemented considering the modeled robot parts only. As one collision check lasts less than 1 ms, the robot movements could be carried out very smoothly. In a second step, the described collision detection algorithm was incorporated into a path planning algorithm based on a fast single-query probabilistic road map path planner with lazy collision checking (SBL) which is included in the Motion Planning Toolkit [11], [12]. The SBL calculated successfully a collision free path from the current position of the arm to the target position.

a fast environment model that is updated continuously based on the system sensor data. Moving robot parts Manipulator 4 DOF Manipulator 7 DOF Both Manipulators Manipulator 7 DOF and Pointcloud (25k points)

Calculation time [ms] 0.14 0.16 0.19 86.14

TABLE I C ALCULATION TIME FOR

THE COLLISION TESTS

V. S IMULATION AND V ISUALISATION A. Simulation ManIPA already provides a simulation class for the Schunk light weight arm (LWA3) (see fig. 4). It emulates the rampbased velocity control of each single power cube module, i.e. all joints are simulated individually. All modules start moving at the same time according to the preset maximum velocity and maximum acceleration.

Fig. 4.

CAD of the Schunk light weight arm (LWA3)

B. Visualisation Fig. 3. Left: One manipulator is about to collide with the robot torso. The respective joints are marked in red colour and with a circle. Middle: Successful collision detection for simultaneous movements of two manipulators. Right: Collision detection with the environment in form of a point cloud.

Table I shows results of performance tests on a 2.0 GHz PC with 1 GB RAM for different robot configurations. Obviously the calculation time for the collision checks correlates to the number of the degrees of freedom, or, more exactly, to the number of OBBs. The table also shows that using point clouds as input for the algorithm is currently not fast enough for online collision avoidance. It might be used for offline path planning at most. Future work will therefore focus on

ManIPA comprises an OpenGL visualisation, which displays the current configuration of all robot manipulators and the position of the robot in the world with respect to their environment. The manipulator joints are visualized by cylinders, it is, however, also possible to load, e.g. vrml data into the OpenGL environment. The current joint angles can be obtained both from hardware and simulation. The visualisation is very helpful to check the configuration of the robot, especially the correctness of the DenavitHartenberg parameters including the positive rotation direction. If the visualization of a manipulator for a special configuration differs from the real arm, the Denavit-Hartenberg parameters have not been set correctly in the configuration files.

1) Different Configurations: Fig. 5 shows the visualisations of different robot configurations as an example for the flexibility of the framework. Note that only the parameters in the configuration files are different, and that for both configurations movements in cartesian space (provided enough degrees of freedoms) are possible and online collision checking is active.

to help developers of new algorithms concentrate on highlevel robotic tasks like path planning or collision avoidance. ManIPA provides implementations for the kinematics, collision detection, path planning for multi-manipulator service robots, but new state-of-the-art class libraries can easily be integrated. Current work is dedicated to the appropriate modeling of 3D point clouds obtained e.g. from TOFsensors, to enable online collision avoidance with dynamic environments. R EFERENCES

Fig. 5. Examples for different robot configurations. Left: Configuration with two manipulators, one with 4 DOFs mounted on the top, one with 7 DOFs mounted in an angle of 45 degrees. Right: Configuration with two manipulators with 7 DOFs mounted on the left and right hand side.

2) Collision Detection: Fig. 6 shows the model of the mobile robot and one external object (e. g. a table) in more detail. The calculated velocity vectors of the moving links are displayed as black lines (both arm and head are moving). In the middle picture, the arm is due to be stopped because of an imminent collision of head and arm, while on the right the arm can be moved further, although the OBBs of two manipulators links are located within a critical distance.

Fig. 6. Top: Velocity vectors of moving parts. All objects located in the direction of the velocity vectors are put into PCSs. Bottom: Collision free movement although two objects are located in a critical distance from each other. The yellow boxes represent OBBs.

VI. CONCLUSION The focus of ManIPA lies in its open interfaces, its modularity and its easy reconfiguration. The framework aims

[1] R. Vaughan, B. Gerkey, and A. Howard, “On device abstractions for portable, reusable robot code,” in IEEE/RSJ International Conference on Intelligent Robots and Systems, Las Vegas, Nevada, USA., 2003. [2] B. Gerkey, R. Vaughan, K. Sty, A. Howard, G. Sukhatme, and M. Mataric, “Most valuable player: A robot device server for distributed control,” in In Proc. of the IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS), Wailea, Hawaii, 2001. [3] A. Makarenko, A. Brooks, and T. Kaupp., “Orca: Components for robotics,” in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS’06) Workshop on Robotic Standardization, 2006. [4] C. Cˆot´e, D. L´etourneau, F. Michaud, and Y. Brosseau, Robotics System Integration Frameworks : MARIE’s Approach to Software Development and Integration. Heidelberg: Springer-Verlag, March 2007, vol. 30. [5] “CARMEN robot navigation toolkit,” 2008. [Online]. Available: http://carmen.sourceforge.net/ [6] U. Reiser, C. Mies, and C. Plagemann, “Verteilte SoftwareEntwicklung in der Robotik - ein Integrations- und Testframework,” Robotik, May 2008. [7] S. Govindaraju, N.K.and Redon, M. Lin, and D. Manocha, “CULLIDE: Interactive collision detection between complex models in large environments using graphics hardware.” Proceedings of the Eurographics/SIGGRAPH Graphics Hardware Workshop, 2003. [8] R. Volz, Entwurf und Implementierung einer schnellen Kollisionsdetektion f¨ur einen 7 DOF Roboter-Manipulator. Master Thesis, Fachhochschule f¨ur Technik und Wirtschaft Berlin (FHTW), Fraunhofer IPA, 2007. [9] S. Gottschalk, M. C. Lin, and D. Manocha, “OBBTree: A hierarchical structure for rapid interference detection,” Computer Graphics, vol. 30, no. Annual Conference Series, pp. 171–180, 1996. [10] E. Larsen, S. Gottschalk, M. Lin, and D. Manocha, “Fast proximity queries with swept sphere volumes,” ICRA, pp. 3719–3726, April 2000. [11] F. Schwarzer, M. Saha, and J.-C. Latombe, “Adaptive dynamic collision checking for single and multiple articulated robots in complex environments,” IEEE Transactions on Robotics and Automation, vol. 21, no. 3, pp. 338– 353, June 2005. [12] G. Sanchez and J.-C. Latombe, “A single-query bi-directional probabilistic roadmap planner with lazy collision checking,” International Symposium on Robotics Research, November 2001.