Robust Pipeline for Mobile Brick Picking

Simon Fryc, Liyang Liu, and Teresa Vidal-CallejaCentre for Autonomous Systems at the Faculty of Engineering and IT, University of Technology Sydney

simon.fryc@uts.edu.au, liyang.liu@uts.edu.au, teresa.vidalcalleja@uts.edu.au

Abstract

In this work, we are interested in the problemof picking a single brick shaped object from anunstructured pile using a mobile manipulatorand a 3D camera system. We propose a robustmulti-stage pipeline for efficient, collision-freebrick picking given the pose of a target object.The key contribution of this work is a scoringfunction used to find the most suitable con-figuration considering the integrated kinematicchain of a mobile base and manipulator arm.Realistic simulation results show the proposedpipeline has 100% success rate as opposed to astandard off-the-shelf solution, which has high-failure rates.

1 Introduction

Although significant progress has been made towards thedevelopment of autonomous mobile manipulators in re-cent years, it remains highly desirable to develop robotsthat can navigate and manipulate objects autonomouslyin unstructured and dynamic environments such as out-doors scenarios. The work presented in this paper ismotivated by the 2020 Mohamed Bin Zayed Interna-tional Robotics Challenge (MBZIRC) [mbz, 2019] whichrequires to develop mobile manipulators capable of au-tonomously locating and picking different types of bricksfrom unorganised piles to build a predefined structure.These types of robotic systems present new opportuni-ties in a range of commerce and research applications in-cluding disaster response, logistics, manufacturing, andconstruction.

We refer to mobile manipulation as a robotic sys-tem that typically consist of a robotic manipulator armmounted on a mobile platform equipped with sensors,which removes the work space limitations imposed bystatic bases by extending the manipulators total num-ber of degrees of freedom. The robot’s mobile base givesit the freedom to manoeuvre around its environment

(a) (b)

Figure 1: Given the two inverse kinematic solutionsshown in the simulated environment, we can see (a)would be unfavourable because the robot manipulatoris overly outstretched resulting in a low manipulabilityscore, while in the (b) the robot has a much higher abil-ity to move freely in any direction. Since the solution in(a) is more favourable it will obtain a higher score.

while its manipulator enables it to achieve the preci-sion required to perform complex manipulation tasks.Mobile robots are typically divided into two distinctclasses based on their drive mechanism. Robots witha differential drive mechanism have non-holonomic con-straints which prevent them from moving perpendicu-lar to their wheels. Conversely, the driving mechanismsof omni-directional robots allow them to become holo-nomic, meaning that they are capable of driving in anydirection in a 2D plane.

A key challenge in mobile manipulation is the interac-tion between motion planning and perception that willdeliver stable and efficient solutions for the purposes ofthe application in place. In this paper, we are inter-ested in the problem of picking a single brick shapedobject from an unstructured pile using a mobile manip-ulator and a 3D camera system. To achieve this thereare a number of sub-problems that need to be consid-ered, such as where the robot should position itself toreach the target object, and the generation of efficientand collision-free motion plans.

The simplest approach to solve the brick picking prob-lem may be for the robot to drive toward a target objectuntil it reaches a predefined offset from which it assumesit can reach the object. Although this simplistic ap-proach may work in practice, there is no guarantee thatthe manipulator will be able to reach a target objecton every occasion. This means the robot would have tore-position itself by planning and executing a new tra-jectory, leading to significant losses in system efficiency.Moreover, mobile manipulators introduce multiple de-grees of freedom and a virtually infinite number of con-figurations that are achievable in a given unconstrainedenvironment.

In this paper we present a robust multi-stage pipelinefor efficient, stable and collision-free brick picking. Ourapproach combines the kinematic model of the robotbase with that of the manipulator. The proposedmethod generates motion plans for the entire robot whileconsidering 3D environment mapping, collision avoid-ance and trajectory execution. The main contribution ofthis work is a manipulability score used to find the mostsuitable configuration for the entire mobile manipulator.Figure 1 shows an example of this task with two typesof manipulability score. Off-the-shelf packages such asMoveIt [Chitta et al., 2012] can provide such functional-ity with seamless integration of perception, motion plan-ning and position control given the full kinematic chain.However, this package is commonly slow and prone tofailure when generating motions spanning large distanceswith redundant manipulators.

The proposed multi-stage pipeline aims to preventthese planning failures and improve system robustnessvia the scoring function and by breaking down the taskinto smaller planning problems, while considering thesurroundings. The key insight is to exploit collision freeplanning and decoupled motion execution at differentstages of the task. Our method uses efficient predefinedtrajectories to move the arm to a position that has beenpre-qualified as having a high planning success rate tothe most likely grasp configurations.

The proposed approach is evaluated in a realistic sim-ulated environment. The pipeline considers all aspectsto enable full autonomy to pick the objects while main-taining an awareness of its environment for collision de-tection and avoidance. A UR5 manipulator is mountedon a differential mobile base. Perception is incorporatedinto the system using a customised implementation ofthe OctoMap 3D mapping framework [Hornung et al.,2013] using a 3D camera. The results show the proposedpipeline produces no failures, while standard MoveIt!framework [Chitta et al., 2012] has a high-failure rate.

2 Related work

According to [Khatib et al., 1996] mobile manipulatorscould be used for a broad range of robotic applicationsincluding, but not limited to, space, construction andservice industries. The authors leverage their knowl-edge of traditional fixed place systems to develop newmethodologies for the analysis and control of coopera-tive mobile manipulation systems, with a focus on decen-tralised control. It is highlighted how a mobile robot canbe considered as a redundant mechanism that utilises thecoarse and adaptable base for coarse positioning, and thefast and accurate arm for fine manipulation.

The authors in [Hamner et al., 2009] support the viewthat mobile robots have immense potential for assemblytasks in both terrestrial and extra-terrestrial locationsdue to their inherent flexibility and ability to managesystem uncertainties and unknowns which conventionalrobots are unlikely to encounter in highly structured fac-tory applications. The authors recognise that in order toreach their full potential these robots must be equippedwith the appropriate sensors and control algorithms sothat they can perceive and react appropriately to dy-namic changes in their environment. Their research en-dorses a control scheme that uses coordinated motion ofthe base and arm in combination with visual feedbackand force servoing.

A common application of mobile manipulators is incivilian and military missions that involve hazardous en-vironment, as shown by [Dong Hun Shin et al., 2003].These applications often invoke a decoupled representa-tion of the mobile base and arm to maintain the ease ofhuman tele-operation. This implies that the robot basemust first be driven to some desired location before beingparked by the operator who would then switch to manip-ulator control. Such a system would not be consideredas autonomous and [Hamner et al., 2009] argues that bytreating a mobile robot as two independent robots thesystem is liable to inefficiencies, due to a discontinuousand interrupted workflow.

[Yamamoto and Xiaoping Yun, 1994] considered theimplications of robot base positioning for manipulationby observing the way in which humans position theirbodies when writing on a whiteboard. It makes sensethat a human would position their body in such a way asmaintain a high measure of manipulability in their armwhile writing to avoid an overly outstretched configura-tion. Similarly, when carrying heavy objects, humanstend to adopt a pose that minimises the load on theirbody. [Yamamoto and Xiaoping Yun, 1994] discernedthat “it is desirable to bring the manipulator into cer-tain preferred configurations by appropriately planningthe motion of the mobile platform. This is particularlychallenging if the trajectory of the manipulator is notknown a priori, in which case an autonomous system

must perform planning for the base in real time. To ad-dress this problem, the authors developed an algorithmthat accounts for the preferred operating region of thearm by observing its joint positions.

Although many existing approaches also divide mobilemanipulation into a series of sub-problems it is often thecase that the base and manipulator are decoupled andoptimised independently [Berenson et al., 2008]. Thereare few examples of robust motion planning methodssuch as ours that utilise the complete kinematic chain ofa mobile manipulator. Even fewer examples exist of ap-proaches that combine the efficiency of pre-defined tra-jectories with collision-free motion planning within dy-namic environments.

3 Problem definition

Our method aims to address the problem of efficient mo-bile manipulation for the purposes of brick picking froma random pile. The proposed pipeline takes advantageof a specific setup to quickly plan and execute a seriesof efficient, collision-free, collision-aware and fine adjust-ments motion plans. We define the mobile manipulationproblem for the purposes of brick picking from a randompile formally as follows.

Considering the manipulator and mobile base as a sin-gle kinematic chain, let us define a homogeneous trans-form from a world frame TB

W ∈ SE(3) to the robot base.The intent is to find a solution for the transform TB

W

that, when coupled with the configuration of the armTEB , transforms the robot end effector from the world

frame to the target pose TEW such that,

TEW = TB

WTEB ∈ SE(3) . (1)

Given that forward kinematics describes the relation-ship between the set of joint positions q in a robot armand the pose of the end effector ξE , the function K de-termines the pose of the end effector with respect to themanipulator’s base as a function of the manipulator’sjoint angles by applying homogeneous transformationsbetween the frame of each of the links N,

ξN = K(q)

where, q = {qj , j ∈ [1 . . .N]} .(2)

Thus, Inverse Kinematics (IK) is used to determinethe joint configurations q that satisfies the final term inEquation (1) in order to allow the end effector to reachthe target pose ξE such that:

q = K−1(ξE) . (3)

Determining the joint angles q∗ can be posed as anoptimization problem where we want to minimise the

relative pose error of the end effector between the currentpose K(q) and the desired pose ξ∗:

q∗ = argminq

|K(q) ξ∗| . (4)

We define a vector containing the pose of the base(x, y, φ) as well as the joint angle vector of the manipu-lator θ to be:

q = (x, y, φ, θ) (5)

By considering the manipulator and mobile base as asingle kinematic chain the inverse kinematic planning re-turns motion plans for the full configuration on the robotqR including joint trajectories for both the manipulatorqM and base qB such that:

qB(x, y, φ) ⊂ qRqM(θ) ⊂ qR

(6)

By defining the mobile base as a planar joint the re-sulting trajectory assumes that the robot base is omni-directional, which might not be the case for many mo-bile robots. To resolve this issue, our approach allowsthe components for the motion of the base platform andthe manipulator to be extracted from the trajectory in-dependently. That is to say that only the final waypoint,i.e. the final x and y positions within the base trajec-tory are to be considered for navigation. Note that for anomni-directional platform the trajectory can be executedin full, taking the robot from its initial configuration toa collision-free grasping configuration.

4 Multi-Stage Approach

The proposed approach considers partition of the posi-tioning task for brick picking. The full picking task issubdivided into five discrete logical stages:

• Stage 1: Generation and scoring a set of plans forentire mobile manipulator model

• Stage 2: Motion execution for mobile base only

• Stage 3: Octomap and target pose update with re-spect to new base location

• Stage 4: Motion execute a pre-planned, collision-free trajectory for manipulator only

• Stage 5: Re-planning and motion execution for ma-nipulator considering obstacles to a grasp pose q∗

In the first stage we model the mobile manipulator asa single kinematic chain where the mobile base is repre-sented as a planar joint connecting the base of the ma-nipulator to the world frame, as shown in Figure 2. AnOctoMap representation of the environment and a targetpose are required as inputs before inverse kinematics isapplied to the chain to generate a set of motion plans

Figure 2: The kinematic chain of a mobile UR5 manip-ulator using the D-H model [Hartenberg and Denavit,1955]. The D-H model is a concise way of describingthe spatial relationships between serial link mechanisms.The notation allows to define the offsets between the co-ordinate frames of 2 links using just four parameters, θ,d, a and α. The D-H parameters for the mobile UR5robot are listed in Table 1.

for the entire robot model. A scoring function is used toboth validate and evaluate each motion according to thefinal manipulability index of the manipulator and thetrajectory duration. In the case of an omni-directionalrobot the full motion plan is forwarded to the controllersfor execution. For differentially driven robots the motionplan for the base is extracted and sent to the base con-troller.

Once the base has reached its goal, its frame withrespect to the target will have changed, so perception isrun again to update the target pose. At this point themanipulator proceeds to execute a pre-defined, collision-free trajectory that moves the gripper to the left or rightside of the robot based on the relative position of thetarget. A conventional planner is used to generate thefinal motion plan that will take the gripper to the targetpose considering all obstacles.

5 Scoring Function

Our approach takes advantage of the fact that a mo-bile manipulator is a redundant mechanism which has agreater number of degrees of freedom available than arerequired of a manipulation task in 3D space. It followsthen that the inverse kinematics solution is not uniqueand there may be multiple joints configurations that en-able the robot to reach a target pose.

The proposed method uses a simple optimisation al-gorithm to validate and score a set of motion plans. Thealgorithm works by generating a set of 10 motion plansand scoring each plan based on two criteria. The first

Joint Type θ d a α1 P π/2 d1 π/2 02 P -π/2 d2 -π/2 03 R θ3 0 0 04 R θ4 0.089 -0.425 π/25 R θ5 0 -0.393 06 R θ6 0 0 07 R θ7 0.109 0 π/28 R θ8 0.095 0 -π/29 R θ9 0.082 0 0.000

Table 1: D-H parameters for a UR5 Mobile Manipula-tor that define the geometric relationships between theframes of each robot link.

criterion is the manipulability index of the arm and thesecond is the time it would take to execute the trajectory.The objective of the function is to find the plan wherethe trajectory execution time is minimised, and the ma-nipulability index is maximised. The score of each planS is calculated using a Sigmoid function [Kros et al.,2006] to map the raw metrics of arbitrary range into awell controlled range of [0...1] thereby equalising theircontribution:

Tscore =2

1 + exp ( tT10 × 5)

Mscore =4

1 + exp−( m0.1 × 5)

− 2

(7)

where, Mscore is clamped to zero if m < 0.04, then

S = Tscore ×Mscore (8)

If a base location is chosen which results in the armhaving a low manipulability index, then the capabilityof the arm in performing pick and place manoeuvres ef-ficiently would be greatly reduced. We consider thatthe manipulability index is of greater importance thanthe trajectory execution time so double the weighting isapplied to its contribution towards the score. This war-rants that a longer trajectory that results in a highermanipulability index is chosen over a shorter one.

The measure of manipulability [Yoshikawa, 1985] isused in our approach as one of the main scoring criteriawhen determining the optimal robot base position. Amanipulator that has a high measure of manipulabilitycan readily move in any direction with ease, whereas witha low measure of manipulability the configuration of therobot constrains its motion and it may be very difficultfor the robot to move in certain directions. During theplanning process, kinematic solutions that offer a highermanipulability are favoured over those that result in alower measure so that the arm retains its agility in its

workspace as depicted in Figure 1. For this process weuse the determinant of the Jacobian matrix to calculatethe robots measure of manipulability m as follows,

m =√

det JJT (θ) . (9)

6 Implementation

Our approach was implemented using the MoveIt! soft-ware platform [Chitta et al., 2012] within the RobotOperating System (ROS) development environment.MoveIt! has the ability to integrate kinematics, mo-tion planning, trajectory execution, collision avoidanceand environment monitoring in a single framework. Theframework has been featured in a range of solutions inwell-known robotics challenges requiring unstructuredautonomous pick and place and mobile manipulation,such the Amazon picking challenge and DAPRA roboticschallenge [Chitta et al., 2012].

In our implementation the functionality offered byMoveIt! was employed and built upon using its C++API user interface called the MoveGroup class. Thisis the main class responsible for handling most opera-tions such as processing pose requests, creating motionplans, executing motion and modifying the robot col-lision model and its environment. Tailored algorithmswere developed using this C++ API to demonstrate ourapproach.

The D-H parameters from Section 4 were used to de-velop the Universal Robot Descriptor Format (URDF)model of the mobile base with an integrated UR5 ma-nipulator. Note that the brick pose is considered givenand therefore outside of the scope of this paper. For thepurposes of testing our proposed method, AR markersare used to provide the pose of a target brick.

6.1 Mobile Base Planning

In our experiments inverse kinematics was implementedusing MoveIt’s TRAC IK plugin which runs both an en-hanced version of KDL’s joint-limited pseudoinverse Ja-cobian solver [Beeson and Ames, 2015] and a SequentialQuadratic Programming IK formulation.

Given a target pose, a motion planning request is gen-erated and passed to a motion planning node. In here,we constrain the final orientation of the robot colinear tothe target object. Thus, the end-effector is constrainedto always be facing perpendicular to the brick pile.

In our implementation Moveit! is set to use either therobot manipulator alone or the manipulator in conjunc-tion with the mobile base. The resulting motion plan isa trajectory that will move the mobile manipulator fromone configuration to another while following the maxi-mum joint velocities and accelerations. Each waypointin the trajectory stipulates the position, velocity and ac-celeration for all the joints in the kinematic chain at thegiven instant.

Figure 3: An OctoMap representation of the randombrick pile for collision-free planning.

6.2 Perception

Perception is incorporated into the system using a depthsensor and a custom implementation of the OctoMap 3Dmapping framework [Hornung et al., 2013] which utilisesthe octree data structure [Franklin and Akman, 1985]

to provide probabilistic 3D occupancy mapping. Theprobabilistic map gives the robot an understanding of itsenvironment which it can use for motion planning andcollision avoidance as shown in Figure 3. The algorithmoperates by subdividing 3D space into volumetric pix-els called voxels that can then be labelled as either freeor occupied based upon probabilistic estimations of 3Dsensor measurements. For our method a simulated IntelRealSense RGB-D camera was used to provide the depthimages and point cloud data required by the OctoMapnode (Figure 3).

OctoMap Customisation

OctoMap uses ray casting to determine which cells tomark as occupied and which to mark as unoccupied (Fig-ure 4a). A significant challenge when using the OctoMapframework is that it only incorporates free space into themap when it can ray trace through an occupied cell toanother occupied cell behind it (Figure 4b), or when thedistance between a point and the sensor origin is largerthan the sensors maximum range as defined in the con-figuration files. In an open, outdoor environment there isa high chance that there might not be any objects for thesensor to detect. This means that if an object is movedthe space which it previously occupied in the OctoMapwill not update to unoccupied (Figure 4c). In the casethat the any of the sensors pixels fails to receive validdepth readings the depth value of these pixels if filledwith undefined values (i.e. NaN, not a number). Toovercome this issue a virtual wall is created (Figure 4d)by in the first converting any NaN pixel values from theoriginal point cloud (Figure 5a) to the sensors maximumrange (Figure 5b).

A random sample consensus (RANSAC) [Alehdaghi etal., 2015] algorithm is run on this point cloud using thePoint Cloud Library (PCL) to segment the foregroundfrom the ground plane and the background. Ray castingis computationally expensive so in an effort to reduce the

(a) (b)

(c) (d)

(e)

Figure 4: Ray casting is a process that involves project-ing a ray between the sensor origin and each point in thepoint cloud while observing which cells in the Octree itintersects.

(a) (b)

(c) (d)

Figure 5: OctoMap Point cloud processing.

amount of ray casting required any points on the groundplane and background are projected onto a sphere whoseradius is equal to the depth sensor’s maximum range(Figure 5c). The ground plane must be projected ontothe sphere so that occupied cells close to the ground arecleared (Figure 4e).

It is not necessary to process the background at a highresolution, so to maintain a high level of fidelity in the

final OctoMap only objects in the foreground are pro-cessed at a higher resolution. An example of the re-sulting occupancy map that is capable of incorporatingdynamic changes in the environment is shown in Figure5d.

6.3 Pre-planned Trajectory Execution

Once the robot base has reached the goal coordinates inthe X-Y plane a grasp pose for the end effector can becalculated based on the updated robot position. Mobilerobots often leave the manipulation in a tucked posi-tion when driving from one location to another to keepthe robot compact and prevent collisions with the envi-ronment. Rather than generating a motion plan usingMoveIt! between such a tucked configuration and thegrasp configuration, which has a relatively high failurerate, our method uses efficient predefined trajectories tomove the arm to a position that has been pre-qualified tofind a successful plan. In this approach two pre-definedtrajectories are used to position the end effector to thefront left or front right side of the robot base in prepa-ration for picking. The use of pre-planned trajectoriesassumes that the robots immediate environment is staticand that the IK solution for the base will not positionthe robot in such a way as to introduce collisions alongthe pre-planned trajectory. The latter problem can bealleviated using a prior knowledge of the robot environ-ment and the yaw joint constraints discussed previouslyto ensure the robot is perpendicular to the target object.

6.4 Collision free motion planning

Moveit! is used within our approach in the conven-tional manner to move the arm from the pre-plannedpose to the grasp pose considering the objects withinthe environment. This final stage takes full advantageof Moveit! collision avoidance using the Open MotionPlanning Library (OMPL). OMPL is a motion plan-ning interface in MoveIt that offers a selection of ran-domized sampling-based motion planners. The plannerchosen for the manipulator was the RRT Connect plan-ner [Kuffner and Lavalle, 2000] which is an extension ofthe Rapidly Exploring Random Tree algorithm (RRT)proposed by [Lavalle, 1998]. The algorithm is designedto be an efficient single-query path planner [Kuffnerand Lavalle, 2000]. It works by constructing two RRTstructures at rooted at the start pose and goal posethat incrementally converge by randomly exploring theworkspace.

7 Simulated Results

The mobile UR5 manipulator and randomised brick pileswere simulated in the Gazebo 3D simulator which is de-signed to model dynamic outdoor environments, and in-terfaces directly with ROS. The backbone of Gazebo isthe Open Dynamics Engine (ODE) physics engine thatsimulated rigid body dynamics and collision detection.

To evaluate our approach six different pile configura-tions were generated with the target brick in differentlocations with varying grasping difficulty as shown inFigure 6. Our 5-stage planning approach was evaluatedagainst the simplistic approach where the robot drivesto a fixed offset from the pile and attempts to graspthe target brick using the conventional MoveIt planningpipeline.

The quantitative data in Table 2 confirms that our ap-proach was able to solve the mobile manipulator plan-ning problem successfully for each unique pile configura-tion. Whereas the simplistic approach often failed dueto either a lack of reach, or an inability to generate asuccessful motion plan.

Furthermore, the data shows that the 5-stage ap-proach often resulted in the manipulator obtaining ahigher manipulability index. It follows then that our ap-proach would be suitable for manipulation tasks wherethe gripper must maintain a high level of dexterity.

Our results show that generating successful motionplans using our approach often results in a longer du-ration for the picking process which can be attributed totwo factors. The first is that the simplistic approach onlycommands the robot to drive forward along a straightline trajectory, whereas the base solution generated byour planner often require the robot to navigate the en-vironment to a position further away from its startingposition. Secondly, the average time taken to generate asingle plan is in the region of 0.552 seconds which meansthat in order to generate a set of 10 plans is approxi-mately 5 seconds, during which time the robot is sta-tionary. It should be noted that the planning time isdirectly proportional to the complexity of the planningproblem and the physical distance between the robotbase and the target object.

8 Conclusions

This paper presents a robust multi-stage pipeline for mo-bile manipulation. The application is a brick pickingfrom an unstructured pile task, where the pose of targetbrick is given. Our approach considers the full kinematicmodel of a mobile base and a 6-DOF manipulator. A keyaspect of our approach is the design of a scoring func-tion that provides the best configuration of this redun-dant system. Our 5-stage pipeline includes: 1) collision-aware planning with scoring system for the full mobile

(a) (b)

(c) (d)

(e) (f)

Figure 6: Randomly oriented brick piles generated in asimulator where the target brick’s pose is returned usingan AR marker.

manipulator, 2) base-only motion execution, 3) obstaclemap and brick pose update, 4) collision-free motion ex-ecution for manipulator-only and finally, 5) re-planningand collision-aware execution for manipulator-only.

The performance of the proposed approach is evalu-ated in using realistic simulation with multiple brick pilesconfigurations. The result shows that our algorithm ismore robust than the Moveit! framework. Although insome case it takes a longer time, it is guaranteed not tofail.

In future work, we will test our approach with the realmobile manipulator that will be used in the MBZIRC2020 competition.

Success Duration (s) ManipulabilityAvg Planning

Time (s)Planning

Success RatePile 5-Stage Simple 5-Stage Simple 5-Stage Simple 5-Stage 5-StageA Yes Yes 26.595 29.418 0.04 0.03 0.334 1.00B Yes Yes 21.155 19.254 0.08 0.05 0.590 1.00C Yes Yes 26.389 12.019 0.08 0.04 0.504 1.00D Yes No 22.411 - 0.05 - 0.407 1.00E Yes No 29.147 - 0.06 - 0.784 1.00F Yes No 25.526 - 0.06 - 0.693 1.00

Table 2: A summary of our simulation results showing the comparison between the proposed pipelinewhich has a 100% success rate as opposed to a simplistic approach where the robot drives to a fixed offsetfrom the pile.

References

[Alehdaghi et al., 2015] M. Alehdaghi, M. A. Esfahani,and A. Harati. Parallel ransac: Speeding up planeextraction in rgbd image sequences using gpu. In2015 5th International Conference on Computer andKnowledge Engineering (ICCKE), pages 295–300, Oct2015.

[Beeson and Ames, 2015] Patrick Beeson and BarrettAmes. Trac-ik: An open-source library for improvedsolving of generic inverse kinematics. In Proccedingsof HUMANOIDS, 11 2015.

[Berenson et al., 2008] D. Berenson, J. Kuffner, andH. Choset. An optimization approach to planningfor mobile manipulation. In 2008 IEEE InternationalConference on Robotics and Automation, pages 1187–1192, May 2008.

[Chitta et al., 2012] S. Chitta, I. Sucan, and S. Cousins.Moveit! [ros topics]. IEEE Robotics Automation Mag-azine, 19(1):18–19, March 2012.

[Dong Hun Shin et al., 2003] Dong Hun Shin, B. S.Hamner, S. Singh, and Myung Hwangbo. Motionplanning for a mobile manipulator with imprecise lo-comotion. In Proceedings 2003 IEEE/RSJ Interna-tional Conference on Intelligent Robots and Systems(IROS 2003) (Cat. No.03CH37453), volume 1, pages847–853 vol.1, Oct 2003.

[Franklin and Akman, 1985] Wm. Randolph Franklinand Varol Akman. Octree data structures and cre-ation by stacking. In Nadia Magnenat-Thalmann andDaniel Thalmann, editors, Computer-Generated Im-ages, pages 176–185, Tokyo, 1985. Springer Japan.

[Hamner et al., 2009] Brad Hamner, Seth Koterba, JaneShi, Reid Simmons, and Sanjiv Singh. An autonomousmobile manipulator for assembly tasks. AutonomousRobots, 28(1):131, Sep 2009.

[Hartenberg and Denavit, 1955] Richard S Hartenbergand Jacques Denavit. A kinematic notation for lower

pair mechanisms based on matrices. Journal of appliedmechanics, 77(2):215–221, 1955.

[Hornung et al., 2013] Armin Hornung, Kai M. Wurm,Maren Bennewitz, Cyrill Stachniss, and Wolfram Bur-gard. OctoMap: An efficient probabilistic 3D map-ping framework based on octrees. Autonomous Robots,2013. Software available at http://octomap.github.com.

[Khatib et al., 1996] O. Khatib, K. Yokoi, K. Chang,D. Ruspini, R. Holmberg, and A. Casal. Vehicle/armcoordination and multiple mobile manipulator decen-tralized cooperation. In Proceedings of IEEE/RSJ In-ternational Conference on Intelligent Robots and Sys-tems. IROS ’96, volume 2, pages 546–553 vol.2, Nov1996.

[Kros et al., 2006] John F Kros, Mike Lin, and Marvin LBrown. Effects of the neural network s-sigmoid func-tion on kdd in the presence of imprecise data. Com-puters & operations research, 33(11):3136–3149, 2006.

[Kuffner and Lavalle, 2000] James J. Kuffner andSteven M. Lavalle. Rrt-connect: An efficient ap-proach to single-query path planning. In Proc.IEEE Intl Conf. on Robotics and Automation, pages995–1001, 2000.

[Lavalle, 1998] Steven M. Lavalle. Rapidly-exploringrandom trees: A new tool for path planning. Tech-nical report, 1998.

[mbz, 2019] MBZIRC challenge. https://www.mbzirc.com/, 2019. Accessed: 2019-09-30.

[Yamamoto and Xiaoping Yun, 1994] Y. Yamamotoand Xiaoping Yun. Coordinating locomotion andmanipulation of a mobile manipulator. IEEE Trans-actions on Automatic Control, 39(6):1326–1332, June1994.

[Yoshikawa, 1985] Tsuneo Yoshikawa. Manipulability ofrobotic mechanisms. The International Journal ofRobotics Research, 4(2):3–9, 1985.