This paper presents a general method for exact distance computation between convex objects represented as intersections of implicit surfaces. Exact distance computation algorithms are particularly important for applications involving objects that make intermittent contact, such as in dynamic simulations and in haptic interactions. They can also be used in the narrow phase of hierarchical collision detection. In contrast to geometric approaches developed for polyhedral objects, we formulate the distance computation problem as a convex optimization problem. We use an interior point method to solve the optimization problem and demonstrate that, for general convex objects represented as implicit surfaces, interior point approaches are globally convergent, and fast in practice. Further, they provide polynomial-time guarantees for implicit surface objects when the implicit surfaces have self-concordant barrier functions. We use a primal-dual interior point algorithm that solves the Karush-Kuhn-Tucker (KKT) conditions obtained from the convex programming formulation. For the case of polyhedra and quadrics, we establish a theoretical time complexity of O(n1.5), where n is the number of constraints. We present implementation results for example implicit surface objects, including polyhedra, quadrics, and generalizations of quadrics such as superquadrics and hyperquadrics, as well as intersections of these surfaces. We demonstrate that in practice, the algorithm takes time linear in the number of constraints, and that distance computation rates of about 1 kHz can be achieved. We also extend the approach to proximity queries between deforming convex objects. Finally, we show that continuous collision detection for linearly translating objects can be performed by solving two related convex optimization problems. For polyhedra and quadrics, we establish that the computational complexity of this problem is also O(n1.5).

In this paper, we present an interior point approach to exact distance computation between convex objects represented as intersections of implicit surfaces. The implicit surfaces considered include planes (polyhedra), quadrics, and generalizations of quadrics including superquadrics and hyperquadrics, as well as intersections of these surfaces. Exact distance computation algorithms are particularly important for applications involving objects that make contact, such as in dynamic simulations and in contact point prediction for dextrous manipulation. They can also be used in the narrow phase of hierarchical collision detection. In contrast to geometric approaches developed for polyhedral objects, we formulate the distance computation problem as a convex optimization problem; this optimization formulation has been previously described for polyhedral objects. We demonstrate that for general convex objects represented as implicit surfaces, interior point approaches are reasonably fast and in some cases, owing to their global convergence properties, are the only probably good choice for solving proximity query problems. We use an interior point algorithm that solves the KKT conditions obtained from the convex programming formulation. We present implementation results for example implicit surface objects and demonstrate that distance computation rates of about 1 kHz can be achieved

This paper focuses on finding the global minimum time control for the collision-free coordination of multiple robots with double integrator dynamics and with additional robot state constraints and control constraints. We initially assume each robot's path is specified and decompose it into collision segments and collision-free segments. The collision avoidance constraints for pairs of robots and the dynamics constraints can then be combined to formulate the coordination problem as a mixed integer nonlinear program (MINLP). In this paper, we first show convexity of the constraints for an individual robot path segment under certain assumptions. We next establish that we are guaranteed to find the global optimum of the MINLP because each subproblem of the MINLP is a convex program, based on the convexity result on individual robot segments. To the best of our knowledge, this is the first result on directly obtaining the global optimum coordination of multiple (more than two) robots with dynamics constraints. Finally, we extend these results to the task of coordinating robots on a given roadmap, where the roadmap has multiple candidate paths for each robot. We present an approach to simultaneously select each robot's traversal path and generate its continuous velocity profile. These robot velocity profiles satisfy the dynamics constraints, avoid collisions, and globally minimize the completion time. We use the MINLP Solver [25], which combines a branch-and-bound algorithm with a filterSQP algorithm, to solve the MINLP coordination problems. We illustrate the approach with multiple robot coordination examples with up to 156 collision zones.

Coordinating multiple manipulators in a shared workspace while considering their dynamics is an important problem. This problem of collision-free coordination arises in assembly, materials transfer, and welding workcells. Previous approaches that considered robot dynamics have typically been restricted to coordinating just two or three manipulators, even when their paths are specified. We address the task of coordinating the motions of multiple manipulators when either their trajectories or their paths are given. By exploiting a fundamental time scaling law for manipulators based on their dynamics, we identify sufficient conditions for collision-free coordination of the robots when the velocity profiles can be uniformly time-scaled and the robot start times can be varied. We describe an approach that develops mixed integer programming formulations of these problems, where the time scaling factors are linear variables, to automatically minimize completion time. This method can potentially coordinate the motions of many manipulators.

This paper studies optimal control of multiple robot systems with frictional contact. The task is to move the robots into goal regions with specified velocities using external control forces. The robots have nonlinear dynamics, which may arise from the robot body dynamics, friction between robot and environment, and friction between robot and robot. Nonpenetration constraints between robots are imposed, and the robots are assumed rigid. The problem is modeled as a mathematical program with complementarity constraints (MPCC). The MPCC model is solved by using its elastic mode, which is a well-behaved nonlinear programming problem. Preliminary results with this approach are illustrated on example problems. The main contributions of this paper are a novel optimal control model that can deal with friction in the multiple robot system, and application of a new mathematical programming algorithm to solve the MPCC formulation. This optimal control model has potential applications in robot systems with friction, such as multifinger manipulation, manipulation with ropes, and multirobot pushing coordination.

This paper focuses on the coordination of multiple robots with kinodynamic constraints along specified paths. The presented approach generates continuous velocity profiles that avoid collisions and minimize the completion time for the robots. The approach identifies collision segments along each robot's path and then optimizes the motions of the robots along their collision and collision-free segments. For each path segment for each robot, the minimum and maximum possible traversal times that satisfy the dynamics constraints are computed by solving the corresponding two-point boundary value problems. Then the collision avoidance constraints for pairs of robots can be combined to formulate a mixed integer nonlinear programming (MINLP) problem. Since this nonconvex MINLP model is difficult to solve, we describe two related mixed integer linear programming (MILP) formulations that provide schedules that are lower and upper bounds on the optimum; the upper bound schedule is a continuous velocity schedule. The approach is illustrated with robots modeled as double integrators subject to velocity and acceleration constraints. An implementation that coordinates 12 nonholonomic car-like robots is described.