Direct collocation methods are powerful tools to solve trajectory optimization problems in robotics. While their resulting trajectories tend to be dynamically accurate, they may also present large kinematic errors in the case of constrained mechanical systems, i.e., those whose state coordinates are subject to holonomic or nonholonomic constraints, such as loop-closure or rolling-contact constraints. These constraints confine the robot trajectories to an implicitly-defined manifold, which complicates the computation of accurate solutions. Discretization errors inherent to the transcription of the problem easily make the trajectories drift away from this manifold, which results in physically inconsistent motions that are difficult to track with a controller. This article reviews existing methods to deal with this problem and proposes new ones to overcome their limitations. Current approaches either disregard the kinematic constraints (which leads to drift accumulation) or modify the system dynamics to keep the trajectory close to the manifold (which adds artificial forces or energy dissipation to the system). The methods we propose, in contrast, achieve full drift elimination on the discrete trajectory, or even along the continuous one, without artificial modifications of the system dynamics. We illustrate and compare the methods using various examples of different complexity.

Pseudospectral collocation methods have proven to be powerful tools to solve optimal control problems. While these methods generally assume the dynamics is given in the first order form$x$= f(x, u, t), where$x$is the state and$u$is the control vector, robotic systems are typically governed by second order ODEs of the form$q$= g(q, q, u, t), where$q$is the configuration. To convert the second order ODE into a first order one, the usual approach is to introduce a velocity variable$v$and impose its coincidence with the time derivative of q. Lobatto methods grant this constraint by construction, as their polynomials describing the trajectory for$v$are the time derivatives of those for q, but the same cannot be said for the Gauss and Radau methods. This is problematic for such methods, as then they cannot guarantee that$q$= g(q, q, u, t) at the collocation points. On their negative side, Lobatto methods cannot be used to solve initial value problems, as given the values of$u$at the collocation points they generate an overconstrained system of equations for the states. In this paper, we propose a Legendre-Gauss collocation method that retains the advantages of the usual Lobatto, Gauss, and Radau methods, while avoiding their shortcomings. The collocation scheme we propose is applicable to solve initial value problems, preserves the consistency between the polynomials for$v$and q, and ensures that$q$= g(q, q, u, t) at the collocation points.

Kinodynamic rapidly-exploring random tree (RRT) planners are effective tools for finding feasible trajectories in many classes of robotic systems. However, they are hard to apply to systems with closed-kinematic chains, like parallel robots, collaborative arms manipulating an object, or legged robots keeping their feet in contact with the environment. The state space of such systems is an implicitly-defined manifold that complicates the design of the sampling and steering procedures, and leads to trajectories that drift from the manifold if standard integration methods are used. To address these issues, this article presents a kinodynamic RRT planner that constructs an atlas of the state space incrementally, and uses this atlas to generate random states, and to dynamically steer the system toward such states. The steering method exploits the atlas charts to compute locally optimal controls based on linear quadratic regulators. The atlas also allows the integration of the equations of motion using local coordinates, which eliminates any drift from the state space manifold and results in accurate trajectories. To the best of our knowledge, this is the first kinodynamic planner that explicitly takes closed kinematic chains into account. In this article, we illustrate the planner performance in significantly complex tasks involving planar and spatial robots that have to lift or throw a load using torque-limited actuators.

Kinodynamic RRT planners are considered to be general tools for effectively finding feasible trajectories for high-dimensional dynamical systems. However, they struggle when holonomic constraints are present in the system, such as those arising in parallel manipulators, in robots that cooperate to fulfill a given task, or in situations involving contacts with the environment. In such cases, the state space becomes an implicitly-defined manifold, which makes the diffusion heuristic inefficient and leads to inaccurate dynamical simulations. To address these issues, this paper presents an extension of the kinodynamic RRT planner that constructs an atlas of the state-space manifold incrementally, and uses this atlas both to generate random states and to dynamically steer the system towards such states. To the best of our knowledge, this is the first randomized kinodynamic planner that explicitly takes holonomic constraints into account. We validate the approach in significantly-complex systems.

Parallel robots exhibit the so-called forward singularities, which complicate substantially the planning and control of their motions. Often, such complications are circumvented by restricting the motions to singularity-free regions of the workspace. However, this comes at the expense of reducing the motion range of the robot substantially. It is for this reason that, recently, efforts are underway to control singularity-crossing trajectories. This paper proposes a reliable controller to stabilize such kind of trajectories. The controller is based on the classical theory of linear quadratic regulators, which we adapt appropriately to the case of parallel robots. As opposed to traditional computed-torque methods, the obtained controller does not rely on expensive inverse dynamics computations. Instead, it uses an optimal control law that is easy to evaluate, and does not generate instabilities at forward singularities. The performance of the controller is exemplified on a five-bar parallel robot accomplishing two tasks that require the traversal of singularities.

Motion paths of cable-driven hexapods must carefully be planned to ensure that the lengths and tensions of all cables remain within acceptable limits, for a given wrench applied to the platform. The cables cannot go slack-to keep the control of the robot-nor excessively tight-to prevent cable breakage-even in the presence of bounded perturbations of the wrench. This paper proposes a path-planning method that accommodates such constraints simultaneously. Given two configurations of the robot, the method attempts to connect them through a path that, at any point, allows the cables to counteract any wrench lying in a predefined uncertainty region. The configuration space, or C-space for short, is placed in correspondence with a smooth manifold, which facilitates the definition of a continuation strategy to search this space systematically from one configuration, until the second configuration is found, or path nonexistence is proved by exhaustion of the search. The force Jacobian is full rank everywhere on the C-space, which implies that the computed paths will naturally avoid crossing the forward singularity locus of the robot. The adjustment of tension limits, moreover, allows to maintain a meaningful clearance relative to such locus. The approach is applicable to compute paths subject to geometric constraints on the platform pose or to synthesize free-flying motions in the full 6-D C-space. Experiments illustrate the performance of the method in a real prototype.

The analysis of singularities is central to the development and control of a manipulator. However, existing methods for singularity set computation still concentrate on specific classes of manipulators. The absence of general methods able to perform such computation on a large class of manipulators is problematic because it hinders the analysis of unconventional manipulators and the development of new robot topologies. The purpose of this paper is to provide such a method for nonredundant mechanisms with algebraic lower pairs and designated input and output speeds. We formulate systems of equations that describe the whole singularity set and each one of the singularity types independently, and show how to compute the configurations in each type using a numerical technique based on linear relaxations. The method can be used to analyze manipulators with arbitrary geometry, and it isolates the singularities with the desired accuracy. We illustrate the formulation of the conditions and their numerical solution with examples, and use 3-D projections to visualize the complex partitions of the configuration space induced by the singularities.

This paper provides an algorithm for computing singularity-free paths on closed-chain manipulators. Given two nonsingular configurations of the manipulator, the method attempts to connect them through a path that maintains a minimum clearance with respect to the singularity locus at all points, which guarantees the controllability of the manipulator everywhere along the path. The method can be applied to nonredundant manipulators of general architecture, and it is resolution complete. It always returns a path whenever one exists at a given resolution or determines path nonexistence otherwise. The strategy relies on defining a smooth manifold that maintains a one-to-one correspondence with the singularity-free C-space of the manipulator, and on using a higher dimensional continuation technique to explore this manifold systematically from one configuration, until the second configuration is found. If desired, the method can also be used to compute an exhaustive atlas of the whole singularity-free component reachable from a given configuration, which is useful to rapidly resolve subsequent planning queries within such component, or to visualize the singularity-free workspace of any of the manipulator coordinates. Examples are included that demonstrate the performance of the method on illustrative situations.

This paper presents a procedure for synthesizing high-quality grasps for objects that need to be held and manipulated in a specific way, characterized by a prespecified set of contact constraints to be satisfied. Due to the multimodal nature of typical grasp quality measures, approaches that resort to local optimization methods are likely to get trapped into local extrema on such a problem. An additional difficulty is that the set of feasible grasps is a highly dimensional manifold, implicitly defined by a system of nonlinear equations. The proposed procedure finds a way around these issues by focusing the exploration on a relevant subset of grasps of lower dimension and tracing this subset exhaustively using a higher-dimensional continuation technique. A detailed atlas of the subset is obtained as a result, on which the highest quality grasp, according to any desired criterion, or a combination of criteria, can be readily identified. Examples are included that illustrate the application of the method to a three-fingered planar hand and to the Schunk anthropomorphic hand grasping different objects, using several quality indices.

This paper provides an algorithm for computing singularity-free paths on non-redundant closed-chain manipulators. Given two non-singular configurations of the manipulator, the method attempts to connect them through a configuration space path that maintains a minimum clearance with respect to the singularity locus at all points. The method is resolution-complete, in the sense that it always returns a path if one exists at a given resolution, or returns “failure” otherwise. The path is computed by defining a new manifold that maintains a one-to-one correspondence with the singularity-free configuration space of the manipulator, and then using a higher-dimensional continuation technique to explore this manifold systematically from one configuration, until the second configuration is found. Examples are included that demonstrate the performance of the method on illustrative situations.

This paper provides a method to compute all types of singularities of non-redundant manipulators with non-helical lower pairs and designated instantaneous input and output speeds. A system of equations describing each singularity type is given. Using a numerical method based on linear relaxations, the configurations in each type are computed independently. The method is general and complete: it can be applied to manipulators with arbitrary geometry; and will isolate singularities with the desired accuracy. As an example, the entire singularity set and its complete classification are computed for a two-degree-of-freedom mechanism. The complex partition of the configuration space by various singularities is illustrated by three-dimensional projections.

This paper introduces a new method for workspace boundary determination on general structure manipulators. The method uses a branch-and-prune technique to isolate a set of output singularities and then classifies the points on such a set according to whether they correspond to motion impediments in the workspace. A detailed map of the workspace is obtained as a result, where all interior and exterior regions, together with the singularity and barrier sets that separate them, get clearly identified. The method can deal with open- or closed-chain manipulators, whether planar or spatial, and is able to take joint limits into account. Advantages over previous general methods based on continuation include the ability to converge to all boundary points, even in higher dimensional cases, and the fact that manual guidance with a priori knowledge of the workspace is not required. Examples are included that show the performance of the method on benchmark problems documented in the literature, as well as on new ones unsolved so far.

This paper presents a new method to isolate all configurations that a multiloop linkage can adopt. The problem is tackled by means of formulation and resolution techniques that fit particularly well together. The adopted formulation yields a system of simple equations (only containing linear, bilinear, and quadratic monomials, and trivial trigonometric terms for the helical pair only) whose structure is later exploited by a branch-and-prune method based on linear relaxations. The method is general, as it can be applied to linkages with single or multiple loops with arbitrary topology, involving lower pairs of any kind, and complete, as all possible solutions get accurately bounded, irrespective of whether the linkage is rigid or mobile.

Many different robotic in-parallel structures have been conceived as six-component force sensors. In general, they perform well for most applications but, when accuracy is a must, two main limitations arise. First, in most designs, the legs are connected to the base and the platform through ball- and-socket joints. Although the dry friction in each of these joints can be individually neglected, the integrated effect of twelve such elements becomes noticeable. Second, dynamical measurements might not be very accurate because the natural resonance frequency of the used structures is quite low even for relatively small dimensions. This dynamical response can be obviously modified with a proper mechanical design, but this increases the complexity of the sensor. This paper discusses the design and implementation of a touch pad based on a 6-axis force sensor and shows how the above limitations degrade its behavior. Moreover, it is shown how using a tensegrity structure both problems could be alleviated because ball-and- socket joints can be substituted by point contacts and the resonance frequency of the structure can be controlled by adjusting the static tensions of the tendons.

Planning a precision grasp for a robot hand is usually decomposed into two main steps. First, a set of contact points over the object surface must be determined, ensuring they allow a stable grasp. Second, the inverse kinematics of the robot hand must be solved to verify whether the contact points can actually be reached. Whereas the first problem has been largely solved in a general posing, the second one has only been tackled with local convergence methods. These methods only provide one solution to the problem, even if many are possible, and depending on the initial estimation they use, they may fail to converge, which results in grasp re-planning in situations where it could be avoided. This paper overcomes both issues by providing a complete method to solve the kinematics of human-like hands. The method is able to find all possible configurations that reach the specified contact points, even when positive-dimensional sets of such configurations are possible.

This paper introduces box approximations as a new tool for path planning of closed-loop linkages. Box approximations are finite collections of rectangloids that tightly envelop the robot's free space at a desired resolution. They play a similar role to that of approximate cell decompositions for open-chain robots - they capture the free-space connectivity in a multi-resolutive fashion and yield rectangloid channels enclosing collision-free paths - but have the additional property of enforcing the satisfaction of loop closure constraints frequently arising in articulated linkages. We present an efficient technique to compute such approximations and show how resolution-complete path planners can be devised using them. To the authors' knowledge, this is the first space-decomposition approach to closed-loop linkage path planning proposed in the literature.

This paper presents a numerical method able to compute all possible configurations of a planar linkage. The procedure is applicable to rigid linkages (i.e., those that can only adopt a finite number of isolated configurations) and to mobile ones (i.e., those that have internal degrees of freedom). The method is based on the fact that this analysis always reduces to finding the roots of a polynomial system of linear, quadratic, and hyperbolic equations, which is here tackled with a new strategy exploiting its structure. The method is conceptually simple, geometric in nature, and easy to implement, yet it provides solutions of the desired accuracy in short computation times. Experiments are included which show its performance on the double butterfly linkage, for which an accurate an complete discretization of its configuration space is obtained

The inverse/direct kinematics of trilaterable serial/parallel manipulators can be stated as a system of distance constraints whose set of solutions can be determined using a sequence of trilaterations, possibly involving points at infinity. It is possible to decide whether a mechanism is trilaterable by relying only on its topology. Based on this fact, we here enumerate all trilaterable serial and in-parallel robots with six degrees of freedom. The relevance of the obtained family of manipulators is established when it is shown to contain the best-known commercial serial robots. As a result of this analysis, we come up with a general method to solve the inverse/direct kinematics of a wide family of manipulators.

Given some geometric elements such as points and lines in R/sup 3/, subject to a set of pairwise distance constraints, the problem tackled in this paper is that of finding all possible configurations of these elements that satisfy the constraints. Many problems in robotics (such as the position analysis of serial and parallel manipulators) and CAD/CAM (such as the interactive placement of objects) can be formulated in this way. The strategy herein proposed consists of looking for some of the a priori unknown distances, whose derivation permits solving the problem rather trivially. Finding these distances relies on a branch-and-prune technique, which iteratively eliminates from the space of distances entire regions which cannot contain any solution. This elimination is accomplished by applying redundant necessary conditions derived from the theory of distance geometry. The experimental results qualify this approach as a promising one.

This paper deals with the problem of estimating the pose of a moving rigid body by measuring the length of six wires attached to it. Since wires can be seen as extensible legs, this problem is equivalent to that of solving the forward kinematics of a six-degree-of-freedom parallel manipulator. Among all possible locations for the attachments on the moving object, the "3-2-1" configuration is shown to exhibit a large number of favorable properties. The performance analysis of this particular configuration is addressed by finding analytic expressions for the estimated pose covariance matrix and the expected value of the pose estimation error, or bias error, which has been omitted in the previous analysis of wire-based tracking devices. This analysis takes advantage of a formulation for trilateration based on Cayley-Menger determinants, which is mathematically more tractable compared to previous ones, because all terms involved are determinants with geometric meaning. This accommodates a more thorough investigation of the properties of the device.

Locating a robot from its distances, or range measurements, to three other known points or stations is a common operation, known as trilateration. This problem has been traditionally solved either by algebraic or numerical methods. An approach that avoids the direct algebrization of the problem is proposed here. Using constructive geometric arguments, a coordinate-free formula containing a small number of Cayley-Menger determinants is derived. This formulation accommodates a more thorough investigation of the effects caused by all possible sources of error, including round-off errors, for the first time in this context. New formulas for the variance and bias of the unknown robot location estimation, due to station location and range measurements errors, are derived and analyzed. They are proved to be more tractable compared with previous ones, because all their terms have geometric meaning, allowing a simple analysis of their asymptotic behavior near singularities.

Given a set of affine varieties in R/sup 3/, i.e. planes, lines, and points, the problem tackled in this paper is that of finding all possible configurations for these varieties that satisfy a set of pairwise euclidean distances between them. Many problems in robotics - such as the forward kinematics of patroller manipulators or the contact formation problem between polyhedral models - can be formulated in this way. We propose herein a strategy that consists in finding some distances, that are unknown a priori, and whose derivation permits solving the problem rather trivially. Finding these distances relies on a branch-and-prune technique that iteratively eliminates from the space of distances entire regions which cannot contain any solution. The elimination is accomplished by applying redundant necessary conditions derived from the theory of Cayley-Menger determinants. The experimental results obtained qualify this approach as a promising one.

This paper deals with the problem of estimating the pose of a rigid moving object by measuring the length of six wires attached to it. Among all possible locations for the attachments on the moving object, the "3-2-1" configuration exhibits the highest number of favorable properties. A closed-form coordinate-free solution to the forward kinematics of this particular configuration is given in terms of Cayley-Menger determinants. The proposed formulation is mathematically more tractable compared to previous ones because all terms are determinants with geometric meaning. This accommodates a more thorough investigation of the properties of the device and leads to formulas whose numerical conditioning is independent from the chosen reference frames.

Presents an algorithm for correcting incorrect line drawings-incorrect projections of a polyhedral scene. Such incorrect drawings arise, e.g., when an image of a polyhedral world is taken, the edges and vertices are extracted, and a drawing is synthesized. Along the way, the true positions of the vertices in the 2D projection are perturbed due to digitization errors and the preprocessing. As most available algorithms for interpreting line drawings are "superstrict," they judge these noisy inputs as incorrect and fail to reconstruct a three-dimensional scene from them. The presented method overcomes this problem by moving the positions of all vertices until a very close correct drawing is found. The closeness criterion is to minimize the sum of squared distances from each vertex in the input drawing to its corrected position. With this tool, any superstrict method for line drawing interpretation is now practical, as it can be applied to the corrected version of the input drawing.

Presents an ellipsoidal calculus based solely on two basic operations: propagation and fusion. Propagation refers to the problem of obtaining an ellipsoid that must satisfy an affine relation with another ellipsoid, and fusion to that of computing the ellipsoid that tightly bounds the intersection of two given ellipsoids. These two operations supersede the Minkowski sum and difference, affine transformation and intersection tight bounding of ellipsoids on which other ellipsoidal calculi are based. Actually, a Minkowski operation can be seen as a fusion followed by a propagation and an affine transformation as a particular case of propagation. Moreover, the presented formulation is numerically stable in the sense that it is immune to degeneracies of the involved ellipsoids and/or affine relations. Examples arising when manipulating uncertain geometric information in the context of the spatial interpretation of line drawings are extensively used as a testbed for the presented calculus.

Solving the direct kinematics of parallel spherical mechanisms with l legs is basically solving systems of l-1 second-order multinomials. This paper presents a recurrent expression for the control points of these multinomials when expressed in the Bernstein form. This result allows one to propose a technique for solving the direct kinematics of these mechanisms that takes advantage of the sub-division and convex hull properties of polynomials in the Bernstein form. Contrary to other numerical approaches, the one presented here is clearly less involved and, although it can be classified within the same category as interval-based techniques, it does not require any interval arithmetic computation.

This paper presents a new algorithm for correcting incorrect projections of a polyhedral scene. Such projections arise in applications where an image of a polyhedral world is taken and its edges and vertices are extracted. Along the way, the true positions of the vertices in the 2D projection are perturbed due to digitization errors and the preprocessing. As most available algorithms for reconstructing polyhedral scenes from projections are "superstrict", they judge these noisy inputs as incorrect and fail to obtain a 3D scene from them. The method presented overcomes this problem by moving the positions of all vertices until a very close correct projection is found. With this tool, any superstrict method for reconstructing scenes from projections is now practical, as it can be applied to the corrected projection.

Some thinning algorithms for 3D objects, or generalizations of existing ones for 2D, have been proposed in recent years. The paper presents a simple and very fast algorithm compared to most of them, and still it has theoretically favorable properties. It provides a connected surface skeleton that allows shapes to be reconstructed with bounded error. In addition, it is also very attractive because it allows discrete skeletons to be obtained directly from volumes in many representations without converting them to a voxel-based representation. Our algorithm is a generalization of the one presented by Cardoner et al. (1997) for 2D objects. It is based on the application of directional erosions, while retaining those voxels that introduce disconnection.

Using structural geometry, Whiteley (1991) showed that a line drawing is a correct projection of a spherical polyhedron if and only if it has a cross-section compatible with it. We extend the class of drawings to which this test applies, including those of polyhedral disks. Our proof is constructive, showing how to derive all spatial interpretations; it relies on elementary synthetic geometric arguments, and, as a by-product, it yields a simpler and shorter proof of Whiteley's result. Moreover, important properties of line drawings are visually derived as corollaries: realizability is independent of the adopted projection, it is an invariant projective property, and for trihedral drawings it can be checked with a pencil and an unmarked ruler alone.

Given two sculptured objects, described by a collection of rational Bezier patches, we propose an algorithm that provides the distance between them if the objects are not intersecting, or a measure of penetration otherwise. The algorithm extends the upper-lower bound subdivision approach to the computation of the nearest points by considering the relative orientations between the subdivided patches. All required geometric constructions can be described as rational Bezier patches so that their control points can be precomputed from those of the original patches. Additional operations have been designed to exhibit linear complexity with the total number of involved control points.

We present a simple local geometric characterization of the configuration space of two polyhedra in contact that provides a representation of all infinitesimal motions that separate them. The polyhedra considered are general in the sense that they possibly have non-convex faces and arbitrary number of holes. The approach presented has two main advantages over former ones: 1) it only relies on the classical basic contacts between polyhedra, i.e. the vertex-face and edge-edge contacts; and 2) it does not require the focal decomposition of non-convexities into convex parts.

Proves that the realizability of a line drawing without occluding segments can be verified by checking the concurrence of groups of three lines to a single point. These lines are either those supporting segments in the drawing or new ones added during the test itself. Although this result was essentially already established by Whiteley , the presented approach uses the concept of delta-star reductions to obtain all possible spatial realizations and consistent edge-labellings (convex or concave) of a given drawing. As opposed to well-known algebraic approaches, which require a Waltz filtering preprocessing step before proceeding to the global geometric test the approach presented is based on geometrically interpretable projective conditions which allows an easy localization of the source of eventual inconsistencies.