Robust, Compliant Assembly via Optimal Belief Space Planning

by   Florian Wirnshofer, et al.

In automated manufacturing, robots must reliably assemble parts of various geometries and low tolerances. Ideally, they plan the required motions autonomously. This poses a substantial challenge due to high-dimensional state spaces and non-linear contact-dynamics. Furthermore, object poses and model parameters, such as friction, are not exactly known and a source of uncertainty. The method proposed in this paper models the task of parts assembly as a belief space planning problem over an underlying impedance-controlled, compliant system. To solve this planning problem we introduce an asymptotically optimal belief space planner by extending an optimal, randomized, kinodynamic motion planner to non-deterministic domains. Under an expansiveness assumption we establish probabilistic completeness and asymptotic optimality. We validate our approach in thorough, simulated and real-world experiments of multiple assembly tasks. The experiments demonstrate our planner's ability to reliably assemble objects, solely based on CAD models as input.



There are no comments yet.


page 1


Leveraging Task Knowledge for Robot Motion Planning Under Uncertainty

Noisy observations coupled with nonlinear dynamics pose one of the bigge...

Efficient Hierarchical Robot Motion Planning Under Uncertainty and Hybrid Dynamics

Noisy observations coupled with nonlinear dynamics pose one of the bigge...

Task-assisted Motion Planning in Partially Observable Domains

We present an integrated Task-Motion Planning framework for robot naviga...

Motion Planning through Demonstration to Deal with Complex Motions in Assembly Process

Complex and skillful motions in actual assembly process are challenging ...

Planning Grasps for Assembly Tasks

This paper develops model-based grasp planning algorithms for assembly t...

Robust 2D Assembly Sequencing via Geometric Planning with Learned Scores

To compute robust 2D assembly plans, we present an approach that combine...

Assembly Planning by Subassembly Decomposition Using Blocking Reduction

The sequence in which a complex product is assembled directly impacts th...
This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

I Introduction

Industrial domains such as manufacturing or logistics require the automation of various processes that involve the joining of objects with robotic manipulators. Examples include packaging or assembly. Fig. 1 shows two exemplary applications. The state-of-the-art method of automating these processes is to rigidly and precisely fix objects in a structured environment and to hard-code all motions of the manipulator. Often problem specific hardware such as grippers, fixtures and guiding components are designed for the task at hand.

This approach is impractical for domains that demand short product life cycles and mass customization. The assembly task may even only occur once. The required degree of flexibility certainly cannot be achieved when relying on problem specific fixtures. We need an automated method that despite the increase in process uncertainties computes reliable and robust robot assembly motions.

A straightforward approach is to plan the geometric motions of the object that is to be assembled and then execute these motions on a compliant robot. This simple approach has several drawbacks. The dynamic execution of geometric paths, the uncertainty of the process and the non-linear contact-dynamics are not independent. In order to compute highly robust and optimal motions, these aspects of the assembly problem must be considered simultaneously. Practical, autonomous solutions for the assembly task must therefore address the following challenges:

Fig. 1: Two assembly tasks - An object (marked yellow) is attached to the robot and must be joined to a second object (marked blue) that is fixed to the environment.

I-1 High-dimensional and hybrid, non-linear dynamics

Assembly requires the relative motion of two parts in up to six Cartesian degrees of freedom (DoF). The process is dynamic which requires the inclusion of at least velocities, possibly further derivatives, into the configuration space. As contacts play an essential role in assembly the dynamics exhibit abrupt non-linearities or hybrid dynamics.

I-2 Uncertainties in model and process

Joining parts is subject to various sources of uncertainty. The physical properties of the process at hand are difficult to obtain. Parameters such as friction or elasticities may not be available at all. Furthermore, poses of objects are only known with limited precision.

I-3 Optimality

To achieve or exceed human-level efficiency, the robot motions should be optimal with respect to the performance requirements of the task. Key performance indicators for industrial assembly would be cycle times, reliability and low part breakage. These performance indicators require the consideration of the full dynamics and uncertainty simultaneously.

The contribution of this paper is a planning method that addresses all of the above challenges. We model the planning problem as a belief space planning problem over an underlying impedance-controlled robot. We propose an asymptotically optimal belief space planner to solve this problem class. This planner is implemented using a particle-based representation of uncertainty and a physics simulator. Extensive experiments in simulation and on a real robot show that our method reliably joins parts with high-quality trajectories.

Ii Related Work

The work presented in this paper integrates and extends three strands of the planning literature: compliant motions for parts assembly, optimal, kinodynamic motion planning and belief space planning.

Ii-a Compliant Motions for Assembly

Compliance is a key requirement for successful parts assembly in realistic, uncertain environments. Fine motion planning [1] derives compliant motions by a backward chaining of so-called preimages. However, this approach does not scale to scenarios of practically relevant dimensionality [2]. Notable extensions seek to overcome the scalability issue by hierarchization over contact states [3] or by the application of sampling-based planners [4]. Yet, these techniques require a manual specification of contact states and are non-optimal. Model-based approaches towards parts assembly show convincing results in practice, yet require manual, problem specific modeling of motion constraints [5, 6]

. Methods from reinforcement learning

[7, 8, 9] bypass the need of precise constraint modeling yet require the cumbersome design of cost functions. Additionally the proposed methods require elaborate real-world experiments, in the case of [7] more than one hundred iterations.

Ii-B Optimal, Kinodynamic Motion Planning

Planning for robotic manipulators occurs in high-dimensional configuration spaces in which sampling-based planners are proven to be highly efficient. These include the probabilistic roadmap (PRM) [10], rapidly exploring random tree (RRT) [11] and the expansive space tree (EST) [12]. These planners require the availability of a local planner or steering function, which is trivial to obtain for geometric planning but potentially unavailable for domains with differential constraints.

To allow a more general formulation of the planning domains using a control-based model, kinodynamic extensions to the RRT have been proposed in [13]. In [14] an extension to the EST planner is proposed that can handle both kinodynamic and time-varying domains. The planner proposed in this paper is direct extension of this variant of the EST to belief space planning problems.

Sampling-based planners typically return jerky and far from optimal solutions. In [15] a proof for the sub-optimality of PRM and RRT is provided along with asymptotically optimal counterparts PRM* and RRT*. These planners have been extended to kinodynamic planning [16, 17, 18].

In [19] the AO- meta-planner is introduced. This algorithm uses a probabilistically complete, kinodynamic motion planner repeatedly for optimal results in the limit. We make use of the AO- algorithm to plan optimally in the limit in a belief space instead of a configuration space.

Ii-C Belief Space Planning for Assembly

Planning under motion and sensing uncertainties is generally referred to as belief space planning. Exact solutions to the corresponding Partially-Observable Markov Decision Process (POMDP) only exist for a very limited problem class. The assumption of Gaussian process- and measurement noise

[20, 21, 22] can provide a remedy regarding computational complexity. However, these assumptions tend to be inadequate in the face of non-linear, hybrid dynamics as encountered in parts assembly. Further works [23, 24] apply RRTs to belief space planning problems. RRTs are suboptimal and require a steering function, which for our application is difficult to obtain.

Recent works focusing on parts assembly [25, 26, 27], employ sampling-based belief approximations. Robot and environment uncertainty are represented using particles. As is the case for our work, a physics simulator is used in order to integrate each particle’s system dynamics. In contrast to our work, Contact-Exploiting RRT [27] requires the distinction between free-space and contact motions. The work of [25] relies on a discretization of the action space as a fixed set of motion primitives. Neither of these methods achieve optimal solutions.

Iii Problem Statement and Notation

We consider the problem of joining two parts modeled as rigid bodies. One of the parts is rigidly attached to a robotic manipulator. A second part is rigidly attached to the robot’s environment. Object poses relative to the manipulator or environment are not exactly known. The goal of the robot is to bring both parts into a desired relative pose.

In Section III-A we introduce a corresponding belief space formulation. We state our notation for a generalized kinodynamic, belief space planning problem in Section III-B.

Iii-a Compliant Environment Interaction under Uncertainty

[width=0.93]figures/figure_trajectory IIIIIIIVVVset-pointtrajectoryobject posedistribution

Fig. 2: Generating a robust set-point trajectory (dark line) for the peg-in-hole insertion task. The yellow boxes indicate a particle representation of the distribution of peg poses. Each peg follows the set point via the spring damper dynamics of a Cartesian impedance controller. The green area visualizes the translational covariance of peg poses.

During the assembly task contact situations may occur in which the environment geometry sets constraints on the paths the robot can follow. When using stiff, position-controlled robots, even minor modeling inaccuracies or process uncertainties can build up contact forces that are far beyond the physical capabilities of both the robot and its load. Therefore, compliance plays a decisive role for a successful interaction between a robot and its environment. We choose to realize this compliance using Cartesian impedance control. Governed by the dynamics of a spring damper system, Cartesian impedance control exerts a wrench


according to the deviation of the object’s actual pose and twist from a desired, virtual set point and its twist . For the general case of six degree-of-freedom (DoF), all poses and twists are elements of the special Euclidean group and , respectively. The wrench

is a composite force/torque vector.

and are positive definite, diagonal gain matrices. We consider, without loss of generality that all of the above quantities are expressed w.r.t. a environment-fixed global frame of reference.

Equation (1) serves to illustrate the dynamical relation to a spring damper system, yet comes short of a thorough definition of stiffness in . For a geometrically consistent formulation of spring stiffness, we refer the reader to [28].

We assume a velocity-controlled set-point  . Given a control trajectory , the evolution of the object’s pose and twist can be computed using a standard physics simulator for rigid bodies with a six DoF spring-damper between set point and object as simulated impedance controller.

The pose and twist of the object are not exactly known and thus modeled as a joint probability distribution over poses and twists, denoted as the belief 

. As the dynamics of the object are deterministic, the evolution of the belief over time given a control trajectory is deterministic as well, assuming no measurements are used to update the belief. Fig. 2 depicts the evolution of the belief over the object’s pose given an initial belief, a control trajectory and the dynamics of impedance-controlled rigid body motions.

The goal for our planner operating in this belief space is now to compute a control trajectory that brings a high fraction of the object’s pose-belief close enough to the target pose. In the next section a generalized formulation of a kinodynamic belief space planning problem is introduced.

Iii-B Kinodynamic Planning in Belief Space

Let denote the state of our system. When dealing with higher order systems, states may be composed of configurations as well as their respective derivatives, e.g. for a second order process. At each time step , state evolves according to the transition probability


with being the system state at time , resulting from applying a control input . Both the state space and the control space are assumed bounded.

A belief reflects the knowledge about the system state at time , where is the set of all belief distributions and is an upper bound on the time. Accordingly the belief state at time is denoted . Note that the unusual notation of a belief state as a tuple in space aims to facilitate the later application of kinodynamic planning algorithms, wherein time is usually treated as a separate dimension. A belief state evolves according to the Bayesian laws of probability as


where is a normalization factor and is the measurement at time . We omit the measurement update with regard to the remainder of this work for the following reasons: the scope of our applications are target environments with highly non-linear, constrained and hybrid system dynamics for which the formulation of an adequate measurement update constitutes a major challenge. Furthermore, as a prerequisite of the planning algorithm presented in this paper, we require the evolution of the belief to be deterministic, a property that is lost when incorporating stochastic measurements. Note that in order to maintain determinism, one could employ approximative techniques such as maximum likelihood measurements [20]. The resulting belief space dynamics are written as


What follows is the formal definition of our target, the generation of optimal belief space trajectories.

Let be the initial configuration in space and let furthermore be the set of all valid belief states.

Definition 1.

A trajectory is said to be valid iff it lies entirely within and is generated in accordance to the system dynamics (2), by a control sequence .

Remark 1.

A region in space is a measure of probability mass and not to be confused with a region in state space

. Therefore it is possible to employ parametric probability density functions with infinitely long tails. For instance, one could declare the

valid region to be the region where more than 95 % of the probability mass is formed by states, in which the interaction forces are below a distinct upper bound.

Given an end-game region allows us to define a feasible trajectory.

Definition 2.

A trajectory with is considered feasible iff it is valid, and .

The quality of a trajectory is measured with the cost function


where and , is the immediate cost experienced in state . Optimality with respect to this cost function is what defines an optimal trajectory.

Definition 3.

A trajectory is optimal iff it is feasible and minimizes

Iv Optimal Belief Space Planning

Our approach to optimal belief space planning makes use of the work of Hauser and Zhou [19] wherein the AO- meta-algorithm is presented. This algorithm uses a probabilistically complete, kinodynamic planner repeatedly to plan optimal in the limit. To achieve optimal belief space trajectories we introduce a probabilistically complete, kinodynamic belief space planner in Section IV-A and then use it in conjunction with the AO- meta-algorithm for optimal planning in Section IV-B.

Iv-a Kinodynamic Belief Space Planning

To allow kinodynamic planning in a belief space, we adapt the EST planner [14], a kinodynamic motion planner, to probabilistic domains. We therefore call our planner Belief-EST (B-EST).

algorithm B-EST
      = =
     while withinTimeBudget do
          = sampleWeighted
          = sampleControl
          = integrate
         if isValid then
              if  then
                  return trajectory from to                             
     return failure

Our algorithm constructs a tree rooted at the initial state . Each iteration starts off with sampling a node from the already constructed tree via the procedure sampleWeighted. This procedure aims to select nodes from the tree in a way that prefers sparsely covered areas of the belief space. Next, the algorithm samples a piecewise linear control input and according to (2), integrates the system dynamics. Sampling control inputs at random allows to directly enforce actuation constraints and spares the intricate design of a steering function. The isValid function rejects an expansion towards , if the segment from to is invalid according to Definition 1. Furthermore, it discards any state that exceeds an upper cost-bound . If is contained in the end-game region , the algorithm terminates, returning the feasible trajectory.

Iv-B Optimal Belief Space Planning

The meta algorithm [19] is a generic optimality wrapper for kinodynamic planners such as the above introduced B-EST algorithm. First, B-EST is augmented by a separate cost dimension. Thereafter, achieves optimality by repeatably calling the feasible B-EST planner while lowering the upper cost dimension bound.

algorithm AO-B-EST
      = B-EST
      = cost
     while withinTimeBudget do
          = cost      

Let us briefly outline the concrete implementation AO-B-EST. In a first step B-EST is queried for an initial trajectory. Next, B-EST is repeatedly queried, with its expansion being constraint to produce results with solution costs, less than the current cost bound . Once exceeding a given time budget, the algorithm terminates returning the best solution found.

Iv-C Completeness and Optimality

In order to implement a belief space planner on a computer, the representation of probability distributions must be achieved with a finite set of parameters. Let  be the belief over a state  represented by a vector of parameters . An example for a parameter vector  would be the data, stored in a particle cloud. Given a state represented by  values and a particle cloud consisting of  particles, the parameter vector  would be of dimension .

As we assume a deterministic evolution of the belief, the planning problem in belief space can be transformed to a kinodynamic planning problem in belief-parameter space. The initial state  of this planning problem represents the parametrization of the initial belief . This initial parameter state must be transformed into a final state within  via the parameter dynamics . Costs and valid parameter spaces can be defined analogously. For a parametrized belief space, our planner B-EST is therefore equivalent to the EST [14] planner in belief parameter space.

Under the assumption of --Expansiveness [29] of the belief parameter space and a non-zero relative volume of the reachable set of goal parameters, B-EST is probabilistically complete. This follows directly from the equivalence of B-EST in belief space to EST in belief-parameter space and the proof of [29]. The question remains whether the belief space of our assembly domain fulfills this expansiveness property. Our experiments and their results in Section VI strongly indicate that this is the case.

In [19] the asymptotic optimality of the AO- algorithm is proven under the assumption of a probabilistically complete kinodynamic planner and an additional well-behavedness condition. From this follows that under the assumption of an expansive belief-parameter space and the well-behavedness condition of [19], AO-B-EST is asymptotically optimal.

V Planning Compliant Motions for Assembly

This section describes the steps to be taken in order to make the previously introduced planning methods accessible to assembly tasks.

Having outlined our realization of compliant motions in Section III-A, we can now introduce the parameters of an approximate belief state representation suitable for assembly planning. We choose to approximate the belief state by a set of particles, where each particle represents the pose and twist of the object.

The particles’ initial distribution is generated by adding Gaussian noise to the grasp transform that is, the transform from the center of the object to the robot end-effector. This aims to represent prominent sources of uncertainty such as localization errors whilst grasping the object. Further sources of uncertainty could include motion noise or a noise prone manipulator pose e.g. when considering mobile manipulators.

Each of the particles is controlled via an impedance


However, all particles share a common reference . Consequently, the belief parameter vector is chosen as

Depending on the problem setting it can be advantageous to project the particles and their reference trajectory into subspaces of , resulting in a planning space of dimensions. Fig. 2 depicts our approach of planning a set-point trajectory for a belief state represented as a particle cloud.

The immediate cost is defined as the average over the immediate cost of each individual particle

where denotes the contribution in cost of each particle. Since we seek to minimize the use of excessive forces and torques during the assembly process, we penalize any motion that further stretches the virtual spring damper system. The immediate cost is defined as the work that is attributed towards stretching the spring and is calculated as . With this choice of an immediate cost, the trajectory cost defined in (5) on average reflects the energy spent towards undesired, forceful assembly motions. Note that this choice of energy cost fortunately bypasses the need to weigh off rotational and translational quantities.

Let us briefly describe the remaining components of the above covered algorithms. The sampleWeighted method aims to select states that quickly expand the tree towards sparsely covered areas, avoiding culminations that stem from repeatedly expanding from nodes in the vicinity of the tree’s root. Literature provides several methods to facilitate a quick expansion towards a target node (e.g. [30]). We choose to group all tree nodes into cells of a coarse discretization over a sub-manifold of the planning space, namely, the mean posture of the particle cloud.

The method then returns a random node from a randomly selected, occupied cell. The B-EST algorithm’s integrate method propagates each particle given a control command. We choose the input as the reference velocity of the manipulator and furthermore constrain it to be piecewise linear. During the integration step, a node’s particle is removed whenever the norm of its end-effector wrench exceeds a hardware dependent threshold. The isValid function prohibits the tree’s expansion towards a node that either exceeds the current cost barrier, or in the case that too many of the particles have been removed during the integration step. The particle approximation of the belief state is considered to be contained within the end-game region if more than a -fraction of the particles is contained within a ball , where denotes a suitable distance function to a goal pose .

Vi Results

The following section introduces a set of benchmark problems with regard to which we evaluated our planner (Section VI-A) and gives details on our implementation (Section VI-B). We analyze key properties such as the cost-convergence or the planner’s ability to find solutions in appropriate time in Section VI-C. Section VI-D interprets the results gathered from thorough real-world experiments.

[width=0.9]figures/benchmarks_mag RailPegPuzzle


Fig. 3: Three benchmark problems: the puzzle problem requiring three consecutive joining motions, the well studied peg-in-hole problem and an assembly task that requires mounting an electrical fuse onto a rail.

Vi-a Benchmark Problems

We evaluated our approach on three notably distinct benchmark problems, depicted in Fig. 3

. Before moving on to a detailed description of the individual benchmarks, it is important to stress the following: each of the problems was solved using a common version of the algorithm. Neither did we require problem specific modifications of the cost function nor was it necessary to specify directions or surfaces of constraint or free motion. Input was given in the form of a description of the geometry (CAD files), the initial end-effector pose and a goal condition. We assumed the grasp transform to be noise prone. We chose a translational standard deviation of 2.5 mm and a rotational standard deviation of 0.015 rad. The physics simulation including collision constraint resolution and the active compliant end-effector, was carried out in

. The rotational and translational spring stiffness were chosen as 1000  and 60 . The damping coefficients were chosen such that the Cartesian impedance is aperiodically damped. We restricted the planning domain to down-projections of three and five DoF respectively.

  • Puzzle: The puzzle problem (taken form [3]) requires three consecutive mating motions. The tolerances lie within 1 mm - 2 mm.

  • Peg: The peg-in-hole is a well studied problem since it covers a wide spectrum of assembly tasks found in industrial production. The tolerance is chosen as 0.5 mm. The hole is only 5 % wider that the peg.

  • Rail: The rail problem requires mounting a fuse onto a top hat rail as typically found in industrial switch cabinets. The fit exhibits zero clearance, which in face of the above chosen uncertainties, constitutes a major challenge. This task aims to show our method’s relevance to industrial applications.

Vi-B Implementation

We simulated the compliant end-effector and its environment using the Bullet physics engine [31]. The particle dynamics were propagated in parallel using separate physics engine instances. We ran our experiments on a ten-core Intel Xeon E5-2650v3.

For the real-world experiments we used a KUKA iiwa 7 R800 redundant 7-axis manipulator with joint-torque feedback, interfaced at a high-level command rate of 200 Hz. The stiffness and damping values of the robot’s Cartesian impedance mode were chosen to match the ones used during the planning phase. The reference frame for the Cartesian impedance was set to match the object frame .

Vi-C Results - Convergence

An important property of a planner is its ability to find a solution in finite time. Fig.4 depicts the planner’s success rate with regard to planning time. For each of the benchmarks, we conducted 100 queries with a maximum planning time of 420 s and particles. The results clearly demonstrate the planner’s ability to quickly come up with solutions.


planning time

Fig. 4: Success rates of B-EST for all five benchmarks. Each line visualizes the success rate of 100 planner calls for each of the benchmark tasks.

Fig. 5 illustrates the average cost of successful assembly trajectories over time. The cost function employed is given in Section V. Again, we carried out 100 consecutive experiments with a maximum planning time of 420 s. Our results show the asymptotic decay of cost over time, which is in strong accordance to the experiments and results of [19]. It can be seen that the problems of higher dimensionality produced higher costs. This behavior is related to the fact that problems in higher dimensional spaces require more planning time and hence undergo a smaller amount of improvement iterations.

Finally, we evaluated the number of particles required in order to produce robust and successful assembly trajectories. The experiment was conducted as follows. We computed assembly trajectories for the peg-in-hole benchmark problem, increasing the amount of particles . The resulting trajectories were evaluated in 100 consecutive experiments with initial states drawn from the same distribution as used during the planning phase. We repeated this plan-evaluate procedure 50 times. The averaged failure rates are depicted in Fig. 6. The mean failure rate of the standard EST approach is as high as 31 %. By contrast, we were able to achieve an average failure rate of 1 % with twelve particles only. This result shows the gain in robustness by our particle-based approach to parts assembly.


planning time

Fig. 5: Mean costs of the five benchmark tasks. Each line visualizes the average cost of successful runs for one benchmark of 100 planner calls.



number of particles

Fig. 6: A motion plan (Peg-3D) is computed for a sample set containing particles. Each of the plans is evaluated in 100 consecutive experiments, with initial states drawn from the same distribution as used during the planning phase. The averaged results (50 repetitions) emphasize the effectiveness of our approach.

Vi-D Results - Real World Experiments

We evaluated both, our AO-B-EST with particles and an EST planner based on the Peg-5D, the Puzzle-5D and the Rail-3D benchmark problems. We computed 14 individual trajectories for each of the benchmarks. Each of the trajectories was then evaluated in 5 consecutive experiments. In total, we conducted 420 real-world assembly experiments.

The experiments proceeded as follows: we detect the object using a wrist-mounted camera in combination with industrial grade software for object pose measurement. The manipulator then grasps the object using a two-jaw gripper, transfers it to the mating object and finally executes the assembly plan. The employed setup is well calibrated and thus far more precise than assumed during the planning phase. In order to recreate the grasp uncertainty we distorted the system’s knowledge about the grasp pose using the same Gaussian noise as assumed during planning.

benchmark Peg-5D Rail-3D Puzzle-5D







success rate 67% 96% 51% 90% 54% 90%
p-value (70 samples)
Fisher’s exact test
p-value (14 means)

Welch’s t-test

TABLE I: Success rates in real world experiments

table I shows the success rates for both AO-B-EST and an EST planner. Despite the artificially added measurement uncertainty and the gap between simulation and reality, AO-B-EST yielded robust assembly trajectories throughout our experiments. During the planning phase, we allowed for a maximum end-effector force of 30 N and a maximum torque of 3 , respectively. With the purpose of reducing uncertainty about the object’s state, AO-B-EST tends to fully exploit this wrench threshold, which occasionally led to a slippage of the grasped object. This violates the assumption of rigidly grasped objects. We assume that modeling this slippage will bring the success rates closer to 100%. To evaluate robustness and compare the two planners two different scenarios must be addressed separately: on-line planning and off-line planning.

In the on-line planning scenario the trajectories for assembly are computed immediately before assembly and are used only once. To compare the two planners we use Fisher’s exact test on the full data set of 70 trials for each benchmark task and test against the null-hypothesis of equal success-rates. This analysis shows that our planner significantly (at 1% p-value) outperforms the baseline. However, the 70 samples for each pair of planner and benchmark are not independent as groups of five samples are produced by the same trajectory sample, which may bias the result.

In the off-line planning scenario an assembly trajectory is computed off-line and then used for multiple products during execution. For this case the average success-rate is a random variable dependent on the sampled trajectory. To compare both planners in the off-line case we compute the average success-rate of each trajectory which yields 14 data-points per pair of planner and benchmark, each computed with five real world experiments. We analyze the resulting data-set under the assumption of independent Gaussian distributions using Welch’s unequal variance t-test and test against the null-hypothesis of equal average success-rates. Again, this analysis shows that our planner significantly (at most 2% p-value) outperforms the baseline.

Vii Conclusion and Future Work

In this paper we introduce a novel approach to model robotic assembly tasks under uncertainty and under the full complexity of non-linear contact-dynamics. This is achieved by formulating a belief space planning problem over an impedance-controlled system. To solve this problem class we propose a randomized belief space planner that is asymptotically optimal. The planner operates over a particle-based representation of uncertainty and utilizes physics simulation to model the spring-damper dynamics of the underlying impedance-controlled robot.

As our algorithm uses a control-based system model, computed trajectories can be directly executed on the real robot without post-processing. Thorough real-world experiments demonstrate that our planner enables reliable assembly solely based on a CAD (geometry, mass, inertia) description of the respective parts. An important qualitative result is that the solutions produced by our planner include motions that actively reduce uncertainty in the assembly process.

Our approach is currently limited to a model of the assembly process that does not include sensor-feedback besides the active impedance control. Promising avenues for future research include the extension of our modeling-approach to a full POMDP, the inclusion of increasingly complex dynamics such as deformable objects and a thorough comparison to results from related state-of-the-art planners.


  • [1] T. Lozano-Perez, M. T. Mason, and R. H. Taylor, “Automatic synthesis of fine-motion strategies for robots,” The International Journal of Robotics Research, vol. 3, no. 1, pp. 3–24, 1984.
  • [2] J. Canny, “On computability of fine motion plans,” in Proceedings, 1989 International Conference on Robotics and Automation, May 1989, pp. 177–182 vol.1.
  • [3] G. Dakin and R. Popplestone, “Simplified fine-motion planning in generalized contact space,” in Proceedings of the 1992 IEEE International Symposium on Intelligent Control, Aug 1992, pp. 281–286.
  • [4] X. Ji and J. Xiao, “Planning motions compliant to complex contact states,” The International Journal of Robotics Research, vol. 20, no. 6, pp. 446–465, 2001.
  • [5] H. Bruyninckx, S. Dutre, and J. D. Schutter, “Peg-on-hole: a model based solution to peg and hole alignment,” in Proceedings of 1995 IEEE International Conference on Robotics and Automation, vol. 2, May 1995, pp. 1919–1924 vol.2.
  • [6]

    J. De Schutter, T. De Laet, J. Rutgeerts, W. Decré, R. Smits, E. Aertbeliën, K. Claes, and H. Bruyninckx, “Constraint-based task specification and estimation for sensor-based robot systems in the presence of geometric uncertainty,”

    The International Journal of Robotics Research, vol. 26, no. 5, pp. 433–455, 2007.
  • [7] V. Gullapalli, J. A. Franklin, and H. Benbrahim, “Acquiring robot skills via reinforcement learning,” IEEE Control Systems, vol. 14, no. 1, pp. 13–24, Feb 1994.
  • [8] S. Levine, N. Wagener, and P. Abbeel, “Learning contact-rich manipulation skills with guided policy search,” in 2015 IEEE International Conference on Robotics and Automation (ICRA), May 2015, pp. 156–163.
  • [9] S. Levine, C. Finn, T. Darrell, and P. Abbeel, “End-to-end training of deep visuomotor policies,”

    Journal of Machine Learning Research

    , vol. 17, no. 39, pp. 1–40, 2016.
  • [10] L. E. Kavraki, P. Svestka, J.-C. Latombe, and M. H. Overmars, “Probabilistic roadmaps for path planning in high-dimensional configuration spaces,” IEEE Transactions on Robotics and Automation, vol. 12, no. 4, pp. 566–580, 1996.
  • [11] S. M. Lavalle, “Rapidly-exploring random trees: A new tool for path planning,” Tech. Rep., 1998.
  • [12] D. Hsu, J.-C. Latombe, and R. Motwani, “Path planning in expansive configuration spaces,” in IEEE International Conference on Robotics and Automation, 1997. Proceedings., 1997, vol. 3.   IEEE, 1997, pp. 2719–2726.
  • [13] S. M. LaValle and J. J. Kuffner Jr, “Randomized kinodynamic planning,” The International Journal of Robotics Research, vol. 20, no. 5, pp. 378–400, 2001.
  • [14] R. Kindel, D. Hsu, J.-C. Latombe, and S. Rock, “Kinodynamic motion planning amidst moving obstacles,” in IEEE International Conference on Robotics and Automation 2000. Proceedings., vol. 1.   IEEE, 2000, pp. 537–543.
  • [15] S. Karaman and E. Frazzoli, “Sampling-based algorithms for optimal motion planning,” The International Journal of Robotics Research, vol. 30, no. 7, pp. 846–894, 2011.
  • [16] ——, “Optimal kinodynamic motion planning using incremental sampling-based methods,” in 49th IEEE Conference on Decision and Control (CDC), Dec 2010, pp. 7681–7687.
  • [17] ——, “Sampling-based optimal motion planning for non-holonomic dynamical systems,” in IEEE International Conference on Robotics and Automation (ICRA), 2013.   IEEE, 2013, pp. 5041–5047.
  • [18] Y. Li, Z. Littlefield, and K. E. Bekris, “Asymptotically optimal sampling-based kinodynamic planning,” The International Journal of Robotics Research, vol. 35, no. 5, pp. 528–564, 2016.
  • [19] K. Hauser and Y. Zhou, “Asymptotically optimal planning by feasible kinodynamic planning in a state–cost space,” IEEE Transactions on Robotics, vol. 32, no. 6, pp. 1431–1443, 2016.
  • [20] R. Platt, R. Tedrake, L. Kaelbling, and T. Lozano-Perez, “Belief space planning assuming maximum likelihood observations,” in Proceedings of Robotics: Science and Systems, Zaragoza, Spain, June 2010.
  • [21] A. Bry and N. Roy, “Rapidly-exploring random belief trees for motion planning under uncertainty,” in 2011 IEEE International Conference on Robotics and Automation, May 2011, pp. 723–730.
  • [22] J. Van Den Berg, P. Abbeel, and K. Goldberg, “Lqg-mp: Optimized path planning for robots with motion uncertainty and imperfect state information,” The International Journal of Robotics Research, vol. 30, no. 7, pp. 895–913, 2011.
  • [23] N. A. Melchior and R. Simmons, “Particle rrt for path planning with uncertainty,” in Proceedings 2007 IEEE International Conference on Robotics and Automation, April 2007, pp. 1617–1624.
  • [24] K. Hauser, “Randomized belief-space replanning in partially-observable continuous spaces,” in Algorithmic Foundations of Robotics IX.   Springer, 2010, pp. 193–209.
  • [25] S. K. Kim and M. Likhachev, “Parts assembly planning under uncertainty with simulation-aided physical reasoning,” in 2017 IEEE International Conference on Robotics and Automation (ICRA), May 2017, pp. 4074–4081.
  • [26] C. Phillips-Grafflin and D. Berenson, “Planning and resilient execution of policies for manipulation in contact with actuation uncertainty,” in International Workshop on the Algorithmic Foundations of Robotics (WAFR), 2016.
  • [27] A. Sieverling, C. Eppner, and O. Brock, “Interleaving motion in contact and in free space for planning under uncertainty,” in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2017.
  • [28] F. Caccavale, P. Chiacchio, A. Marino, and L. Villani, “Six-dof impedance control of dual-arm cooperative manipulators,” IEEE/ASME Transactions on Mechatronics, vol. 13, no. 5, pp. 576–586, Oct 2008.
  • [29] D. Hsu, R. Kindel, J.-C. Latombe, and S. Rock, “Randomized kinodynamic motion planning with moving obstacles,” The International Journal of Robotics Research, vol. 21, no. 3, pp. 233–255, 2002.
  • [30] J. M. Phillips, N. Bedrossian, and L. E. Kavraki, “Guided expansive spaces trees: a search strategy for motion- and cost-constrained state spaces,” in Robotics and Automation, 2004. Proceedings. ICRA ’04. 2004 IEEE International Conference on, vol. 4, April 2004, pp. 3968–3973 Vol.4.
  • [31] E. Coumans et al., “Bullet physics library,” Open source:, vol. 15, p. 49, 2013.