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.

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.

The problem of restoring the orthonormality of a noisy rotation matrix by finding its nearest correct rotation matrix arises in many areas of robotics, computer graphics, and computer vision. When the Frobenius norm is taken as the measure of closeness, the solution is usually computed using the singular value decomposition (SVD). A closed-form formula exists but, as it involves the roots of a polynomial of third degree, it is assumed to be too complicated and numerically ill-conditioned. In this article, we show how, by carefully using some algebraic recipes scattered in the literature, it is possible to derive a simple and yet numerically stable formula for most practical applications. Moreover, by relying on a result that permits obtaining the quaternion corresponding to the sought optimal rotation matrix, we present another closed-form formula that provides a good approximation to the optimal one using only the elementary algebraic operations of addition, subtraction, multiplication, and division. These two closed-form formulas are compared with respect to the SVD in terms of accuracy and computational cost.

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.

The forward kinematics of the Gough-Stewart platform, and their simplified versions in which some leg endpoints coalesce, has been typically solved using variable elimination methods. In this paper, we cast doubts on whether this is the easiest way to solve the problem. We will see how the indirect approach in which the length of some extra virtual legs is first computed leads to important simplifications. In particular, we provide a procedure to solve 30 out of 34 possible topologies for a Gough-Stewart platform without variable elimination.

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.

Distance Bound Smoothing (DBS) is a basic operation originally developed in Computational Chemistry to determine point configurations that are within certain pairwise ranges of distances. This operation consist in the iterative application of filtering processes that reduce the given ranges using triangular and tetrangular inequalities. Standard DBS has a limited range of applications because it does not take into account constraints on the orientations of simplices (triangles or tetrahedra, depending on the dimension of the problem). This paper discusses an extension of DBS that permits incorporating these constraints. This paves the way for the application of DBS techniques to a broad range of problems in Robotics.

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.

The situation arising in path planning under kinematic constraints, where the valid configurations define a manifold embedded in the joint ambient space, can be seen as a limit case of the well-known narrow corridor problem. With kinematic constraints, the probability of obtaining a valid configuration by sampling in the joint ambient space is not low but null, which complicates the direct application of sampling-based path planners. This paper presents the AtlasRRT algorithm, which is a planner especially tailored for such constrained systems that builds on recently developed tools for higher-dimensional continuation. These tools provide procedures to define charts that locally parametrize a manifold and to coordinate the charts, forming an atlas that fully covers it. AtlasRRT simultaneously builds an atlas and a bidirectional rapidly exploring random tree (RRT), using the atlas to sample configurations and to grow the branches of the RRTs, and the RRTs to devise directions of expansion for the atlas. The efficiency of AtlasRRT is evaluated in several benchmarks involving high-dimensional manifolds embedded in large ambient spaces. The results show that the combined use of the atlas and the RRTs produces a more rapid exploration of the configuration space manifolds than existing approaches.

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.

The maps that are built by standard feature-based simultaneous localization and mapping (SLAM) methods cannot be directly used to compute paths for navigation, unless enriched with obstacle or traversability information, with the consequent increase in complexity. Here, we propose a method that directly uses the Pose SLAM graph of constraints to determine the path between two robot configurations with lowest accumulated pose uncertainty, i.e., the most reliable path to the goal. The method shows improved navigation results when compared with standard path-planning strategies over both datasets and real-world experiments.

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.

We present an algorithm to simultaneously recover non-rigid shape and camera poses from point correspondences between a reference shape and a sequence of input images. The key novel contribution of our approach is in bringing the tools of the probabilistic SLAM methodology from a rigid to a deformable domain. Under the assumption that the shape may be represented as a weighted sum of deformation modes, we show that the problem of estimating the modal weights along with the camera poses, may be probabilistically formulated as a maximum a posterior estimate and solved using an iterative least squares optimization. An extensive evaluation on synthetic and real data, shows that our approach has several significant advantages over current approaches, such as performing robustly under large amounts of noise and outliers, and neither requiring to track points over the whole sequence nor initializations close from the ground truth solution.

The probabilistic belief networks that result from standard feature-based simultaneous localization and map building cannot be directly used to plan trajectories. The reason is that they produce a sparse graph of landmark estimates and their probabilistic relations, which is of little value to find collision free paths for navigation. In contrast, we argue in this paper that Pose SLAM graphs can be directly used as belief roadmaps. We present a method that devises optimal navigation strategies by searching for the path in the pose graph with lowest accumulated robot pose uncertainty, independently of the map reference frame. The method shows improved navigation results when compared to shortest paths both over synthetic data and real datasets.

Existing sampling-based robot motion planning methods are often inefficient at finding trajectories for kinodynamic systems, especially in the presence of narrow passages between obstacles and uncertainty in control and sensing. To address this, we propose EG-RRT, an Environment-Guided variant of RRT designed for kinodynamic robot systems that combines elements from several prior approaches and may incorporate a cost model based on the LQG-MP framework to estimate the probability of collision under uncertainty in control and sensing. We compare the performance of EG-RRT with several prior approaches on challenging sample problems. Results suggest that EG-RRT offers significant improvements in performance.

Pose SLAM is the variant of simultaneous localization and map building (SLAM) is the variant of SLAM, in which only the robot trajectory is estimated and where landmarks are only used to produce relative constraints between robot poses. To reduce the computational cost of the information filter form of Pose SLAM and, at the same time, to delay inconsistency as much as possible, we introduce an approach that takes into account only highly informative loop-closure links and nonredundant poses. This approach includes constant time procedures to compute the distance between poses, the expected information gain for each potential link, and the exact marginal covariances while moving in open loop, as well as a procedure to recover the state after a loop closure that, in practical situations, scales linearly in terms of both time and memory. Using these procedures, the robot operates most of the time in open loop, and the cost of the loop closure is amortized over long trajectories. This way, the computational bottleneck shifts to data association, which is the search over the set of previously visited poses to determine good candidates for sensor registration. To speed up data association, we introduce a method to search for neighboring poses whose complexity ranges from logarithmic in the usual case to linear in degenerate situations. The method is based on organizing the pose information in a balanced tree whose internal levels are defined using interval arithmetic. The proposed Pose-SLAM approach is validated through simulations, real mapping sessions, and experiments using standard SLAM data sets.

This paper introduces an approach that reduces the size of the state and maximizes the sparsity of the information matrix in exactly sparse delayed-state SLAM. We propose constant time procedures to measure the distance between a given pair of poses, the mutual information gain for a given candidate link, and the joint marginals required for both measures. Using these measures, we can readily identify non redundant poses and highly informative links and use only those to augment and to update the state, respectively. The result is a delayed-state SLAM system that reduces both the use of memory and the execution time and that delays filter inconsistency by reducing the number of linearization introduced when adding new loop closure links. We evaluate the advantage of the proposed approach using simulations and data sets collected with real robots.

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.

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

We propose a novel approach to optimize Partially Observable Markov Decisions Processes (POMDPs) defined on continuous spaces. To date, most algorithms for model-based POMDPs are restricted to discrete states, actions, and observations, but many real-world problems such as, for instance, robot navigation, are naturally defined on continuous spaces. In this work, we demonstrate that the value function for continuous POMDPs is convex in the beliefs over continuous state spaces, and piecewise-linear convex for the particular case of discrete observations and actions but still continuous states. We also demonstrate that continuous Bellman backups are contracting and isotonic ensuring the monotonic convergence of value-iteration algorithms. Relying on those properties, we extend the algorithm, originally developed for discrete POMDPs, to work in continuous state spaces by representing the observation, transition, and reward models using Gaussian mixtures, and the beliefs using Gaussian mixtures or particle sets. With these representations, the integrals that appear in the Bellman backup can be computed in closed form and, therefore, the algorithm is computationally feasible. Finally, we further extend to deal with continuous action and observation sets by designing effective sampling approaches.

In this paper, we depart from the fact that Simultaneous Localization and Mapping (SLAM) is a sub-case of the general kinematic problem, and, thus, all techniques used in kinematics are potentially applicable to SLAM. We describe how to formalize a SLAM problem as a typical kinematic problem and we propose a simple SLAM algorithm based on an interval-based kinematic method called Cuik previously developed in our group. This new algorithm solves the SLAM problem taking advantage of the structure imposed in the SLAM problem by the motion and sensing capabilities of the autonomous robots. However, since we use a kinematic approach instead of a probabilistic one (the usual approach for SLAM) we can perfectly model the constraints between robot poses and between robot poses and landmarks, including the nonlinearities, and we can ensure those constraints to be fulfilled at any time during the map construction and refinement. The viability of the new algorithm is shown with a small test.

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.

In this paper, we confront the problem of applying reinforcement learning to agents that perceive the environment through many sensors and that can perform parallel actions using many actuators as is the case in complex autonomous robots. We argue that reinforcement learning can only be successfully applied to this case if strong assumptions are made on the characteristics of the environment in which the learning is performed, so that the relevant sensor readings and motor commands can be readily identified. The introduction of such assumptions leads to strongly-biased learning systems that can eventually lose the generality of traditional reinforcement-learning algorithms. In this line, we observe that, in realistic situations, the reward received by the robot depends only on a reduced subset of all the executed actions and that only a reduced subset of the sensor inputs (possibly different in each situation and for each action) are relevant to predict the reward. We formalize this property in the so called 'categorizability assumption' and we present an algorithm that takes advantage of the categorizability of the environment, allowing a decrease in the learning time with respect to existing reinforcement-learning algorithms. Results of the application of the algorithm to a couple of simulated realistic-robotic problems (landmark-based navigation and the six-legged robot gait generation) are reported to validate our approach and to compare it to existing flat and generalization-based reinforcement-learning approaches.

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.

The main drawback of appearance-based robot localization with respect to landmark-based one is that it requires a map including images taken at known positions in the area where the robot is expected to move. In this paper, we describe a concurrent map-building and localization (CML) system developed within the appearance-base robot localization paradigm. This allows us to combine the good features of appearance-base localization, such as simple sensor processing or robustness, without having to deal with its inconveniences. In our CML system, both the robot's position and the map are represented using Gaussian mixtures. Using this kind of representation, we can deal with the global localization problem while being efficient both in memory and in execution time.

In this paper, we extend our appearance-based localization system moving from a passive approach to an active one where the robot can execute actions with the only purpose of gaining information about its location in the environment. We present a general framework for entropy-based action selection and we describe how this framework can be implemented in our localization system. The result is an action evaluation process more efficient in memory and in execution time than previously existing ones. The experiments we present show that the action selection mechanism effectively decreases the error on localization in environments with a high degree of aliasing. This can be of great help to improve the performance of our localization system in dynamic environments.

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.

In this paper, we enhance appearance-based robot localization by using disparity maps. Disparity maps provide the same type of information as range based sensors (distance to objects) and thus, they are likely to be less sensitive to changes of illumination than plain images, that are the source of information generally used in appearance-based localization. The main drawback of disparity maps is that they can include very noisy depth values: points for which the algorithms can not determine reliable depth information. These noisy values have to be discarded resulting in missing values. The presence of missing values makes principal component analysis (the standard method used to compress images in the appearance-based framework) unfeasible. We describe a novel expectation-maximization algorithm to determine the principal components of a data set including missing values and we apply it to disparity maps. The results we present show that disparity maps are a valid alternative to increase the robustness of appearance-based localization.

Legged robots are well suited to walk on difficult terrains at the expense of requiring complex control systems to walk even on flat surfaces. But simply walking on a flat surface is not worth using a legged robot. It should be assumed that walking on abrupt terrain is the typical situation for a legged robot. With this premise in mind, we have developed a robust controller for a six-legged robot that allows it to walk over difficult terrains in an autonomous way, with a limited use of sensory information (no vision is involved). This walk controller can be driven by an upper level which need not be concerned about the details of foot placement or leg movements, taking care only of high level aspects such as global speed and direction.