Constraint-based sample propagation for improved state estimation in robotic assembly

In fast changing assembly scenarios, it is required to adapt the task execution to the current state of the setup without extensive calibration routines. Therefore, it is important to estimate the geometric uncertainties and contact states during the assembly execution. We use a sequential Monte Carlo (SMC) method to track the relative poses between workpieces during a robotic assembly based on joint torque and position measurements only. In contrast to existing approaches, we focus on assembly tasks where the workpiece is not fixed in the workcell, but can, for example, slide on a table surface. We propose a new constraint-based propagation model for the SMC approach: a compensation motion for the samples dependent on the violation of contact constraints is derived. This allows us to track the motion of the workpieces in cases where a common random diffusion model fails. The method is evaluated with experiments using an assembly scenario with two KUKA LBR iiwa robot arms and shows accurate tracking performance.