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 stateoftheart method of automating these processes is to rigidly and precisely fix objects in a structured environment and to hardcode 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 nonlinear contactdynamics 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:
I1 Highdimensional and hybrid, nonlinear 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 nonlinearities or hybrid dynamics.
I2 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.
I3 Optimality
To achieve or exceed humanlevel 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 impedancecontrolled robot. We propose an asymptotically optimal belief space planner to solve this problem class. This planner is implemented using a particlebased representation of uncertainty and a physics simulator. Extensive experiments in simulation and on a real robot show that our method reliably joins parts with highquality 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.
Iia 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 socalled 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 samplingbased planners [4]. Yet, these techniques require a manual specification of contact states and are nonoptimal. Modelbased 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 realworld experiments, in the case of [7] more than one hundred iterations.IiB Optimal, Kinodynamic Motion Planning
Planning for robotic manipulators occurs in highdimensional configuration spaces in which samplingbased 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 controlbased 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 timevarying domains. The planner proposed in this paper is direct extension of this variant of the EST to belief space planning problems.
Samplingbased planners typically return jerky and far from optimal solutions. In [15] a proof for the suboptimality 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 metaplanner 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.
IiC Belief Space Planning for Assembly
Planning under motion and sensing uncertainties is generally referred to as belief space planning. Exact solutions to the corresponding PartiallyObservable 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 nonlinear, 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 samplingbased 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, ContactExploiting RRT [27] requires the distinction between freespace 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 IIIA we introduce a corresponding belief space formulation. We state our notation for a generalized kinodynamic, belief space planning problem in Section IIIB.
Iiia Compliant Environment Interaction under Uncertainty
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, positioncontrolled 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
(1) 
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 degreeoffreedom (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 environmentfixed 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 velocitycontrolled setpoint . 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 springdamper 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 impedancecontrolled 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 posebelief close enough to the target pose. In the next section a generalized formulation of a kinodynamic belief space planning problem is introduced.
IiiB 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
(2) 
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
(3) 
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 nonlinear, 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
(4) 
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 endgame 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
(5) 
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 metaalgorithm 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 IVA and then use it in conjunction with the AO metaalgorithm for optimal planning in Section IVB.
Iva 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 BeliefEST (BEST).
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 costbound . If is contained in the endgame region , the algorithm terminates, returning the feasible trajectory.
IvB Optimal Belief Space Planning
The meta algorithm [19] is a generic optimality wrapper for kinodynamic planners such as the above introduced BEST algorithm. First, BEST is augmented by a separate cost dimension. Thereafter, achieves optimality by repeatably calling the feasible BEST planner while lowering the upper cost dimension bound.
Let us briefly outline the concrete implementation AOBEST. In a first step BEST is queried for an initial trajectory. Next, BEST 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.
IvC 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 beliefparameter 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 BEST 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 nonzero relative volume of the reachable set of goal parameters, BEST is probabilistically complete. This follows directly from the equivalence of BEST in belief space to EST in beliefparameter 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 wellbehavedness condition. From this follows that under the assumption of an expansive beliefparameter space and the wellbehavedness condition of [19], AOBEST 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 IIIA, 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 endeffector. 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
(6) 
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 setpoint 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 submanifold 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 BEST 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 endeffector 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 endgame 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 VIA) and gives details on our implementation (Section VIB). We analyze key properties such as the costconvergence or the planner’s ability to find solutions in appropriate time in Section VIC. Section VID interprets the results gathered from thorough realworld experiments.
Via 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 endeffector 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 endeffector, 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 downprojections 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 peginhole 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.
ViB Implementation
We simulated the compliant endeffector 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 tencore Intel Xeon E52650v3.
For the realworld experiments we used a KUKA iiwa 7 R800 redundant 7axis manipulator with jointtorque feedback, interfaced at a highlevel 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 .
ViC 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.
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 peginhole 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 planevaluate 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 particlebased approach to parts assembly.
ViD Results  Real World Experiments
We evaluated both, our AOBEST with particles and an EST planner based on the Peg5D, the Puzzle5D and the Rail3D 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 realworld assembly experiments.
The experiments proceeded as follows: we detect the object using a wristmounted camera in combination with industrial grade software for object pose measurement. The manipulator then grasps the object using a twojaw 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  Peg5D  Rail3D  Puzzle5D  

planner 
EST 
AOBEST 
EST 
AOBEST 
EST 
AOBEST 
success rate  67%  96%  51%  90%  54%  90% 
pvalue (70 samples)  
Fisher’s exact test  
pvalue (14 means)  
Welch’s ttest 
table I shows the success rates for both AOBEST and an EST planner. Despite the artificially added measurement uncertainty and the gap between simulation and reality, AOBEST yielded robust assembly trajectories throughout our experiments. During the planning phase, we allowed for a maximum endeffector force of 30 N and a maximum torque of 3 , respectively. With the purpose of reducing uncertainty about the object’s state, AOBEST 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: online planning and offline planning.
In the online 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 nullhypothesis of equal successrates. This analysis shows that our planner significantly (at 1% pvalue) 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 offline planning scenario an assembly trajectory is computed offline and then used for multiple products during execution. For this case the average successrate is a random variable dependent on the sampled trajectory. To compare both planners in the offline case we compute the average successrate of each trajectory which yields 14 datapoints per pair of planner and benchmark, each computed with five real world experiments. We analyze the resulting dataset under the assumption of independent Gaussian distributions using Welch’s unequal variance ttest and test against the nullhypothesis of equal average successrates. Again, this analysis shows that our planner significantly (at most 2% pvalue) 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 nonlinear contactdynamics. This is achieved by formulating a belief space planning problem over an impedancecontrolled system. To solve this problem class we propose a randomized belief space planner that is asymptotically optimal. The planner operates over a particlebased representation of uncertainty and utilizes physics simulation to model the springdamper dynamics of the underlying impedancecontrolled robot.
As our algorithm uses a controlbased system model, computed trajectories can be directly executed on the real robot without postprocessing. Thorough realworld 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 sensorfeedback besides the active impedance control. Promising avenues for future research include the extension of our modelingapproach to a full POMDP, the inclusion of increasingly complex dynamics such as deformable objects and a thorough comparison to results from related stateoftheart planners.
References
 [1] T. LozanoPerez, M. T. Mason, and R. H. Taylor, “Automatic synthesis of finemotion 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 finemotion 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, “Pegonhole: 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, “Constraintbased task specification and estimation for sensorbased 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 contactrich 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, “Endtoend 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 highdimensional configuration spaces,” IEEE Transactions on Robotics and Automation, vol. 12, no. 4, pp. 566–580, 1996.
 [11] S. M. Lavalle, “Rapidlyexploring 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, “Samplingbased 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 samplingbased methods,” in 49th IEEE Conference on Decision and Control (CDC), Dec 2010, pp. 7681–7687.
 [17] ——, “Samplingbased optimal motion planning for nonholonomic 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 samplingbased 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. LozanoPerez, “Belief space planning assuming maximum likelihood observations,” in Proceedings of Robotics: Science and Systems, Zaragoza, Spain, June 2010.
 [21] A. Bry and N. Roy, “Rapidlyexploring 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, “Lqgmp: 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 beliefspace replanning in partiallyobservable 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 simulationaided physical reasoning,” in 2017 IEEE International Conference on Robotics and Automation (ICRA), May 2017, pp. 4074–4081.
 [26] C. PhillipsGrafflin 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, “Sixdof impedance control of dualarm 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 costconstrained 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: bulletphysics.org, vol. 15, p. 49, 2013.
Comments
There are no comments yet.