Automatic Differentiation of Rigid Body Dynamics for Optimal Control and Estimation

Many algorithms for control, optimization and estimation in robotics depend on derivatives of the underlying system dynamics, e.g. to compute linearizations, sensitivities or gradient directions. However, we show that when dealing with Rigid Body Dynamics, these derivatives are difficult to derive analytically and to implement efficiently. To overcome this issue, we extend the modelling tool `RobCoGen' to be compatible with Automatic Differentiation. Additionally, we propose how to automatically obtain the derivatives and generate highly efficient source code. We highlight the flexibility and performance of the approach in two application examples. First, we show a Trajectory Optimization example for the quadrupedal robot HyQ, which employs auto-differentiation on the dynamics including a contact model. Second, we present a hardware experiment in which a 6 DoF robotic arm avoids a randomly moving obstacle in a go-to task by fast, dynamic replanning

Batch Informed Trees (BIT*): Informed Asymptotically Optimal Anytime Search

Path planning in robotics often requires finding high-quality solutions to continuously valued and/or high-dimensional problems. These problems are challenging and most planning algorithms instead solve simplified approximations. Popular approximations include graphs and random samples, as respectively used by informed graph-based searches and anytime sampling-based planners. Informed graph-based searches, such as A*, traditionally use heuristics to search a priori graphs in order of potential solution quality. This makes their search efficient but leaves their performance dependent on the chosen approximation. If its resolution is too low then they may not find a (suitable) solution but if it is too high then they may take a prohibitively long time to do so. Anytime sampling-based planners, such as RRT*, traditionally use random sampling to approximate the problem domain incrementally. This allows them to increase resolution until a suitable solution is found but makes their search dependent on the order of approximation. Arbitrary sequences of random samples approximate the problem domain in every direction simultaneously and but may be prohibitively inefficient at containing a solution. This paper unifies and extends these two approaches to develop Batch Informed Trees (BIT*), an informed, anytime sampling-based planner. BIT* solves continuous path planning problems efficiently by using sampling and heuristics to alternately approximate and search the problem domain. Its search is ordered by potential solution quality, as in A*, and its approximation improves indefinitely with additional computational time, as in RRT*. It is shown analytically to be almost-surely asymptotically optimal and experimentally to outperform existing sampling-based planners, especially on high-dimensional planning problems.Comment: International Journal of Robotics Research (IJRR). 32 Pages. 16 Figure

Motion Planning of Uncertain Ordinary Differential Equation Systems

This work presents a novel motion planning framework, rooted in nonlinear programming theory, that treats uncertain fully and under-actuated dynamical systems described by ordinary differential equations. Uncertainty in multibody dynamical systems comes from various sources, such as: system parameters, initial conditions, sensor and actuator noise, and external forcing. Treatment of uncertainty in design is of paramount practical importance because all real-life systems are affected by it, and poor robustness and suboptimal performance result if it’s not accounted for in a given design. In this work uncertainties are modeled using Generalized Polynomial Chaos and are solved quantitatively using a least-square collocation method. The computational efficiency of this approach enables the inclusion of uncertainty statistics in the nonlinear programming optimization process. As such, the proposed framework allows the user to pose, and answer, new design questions related to uncertain dynamical systems. Specifically, the new framework is explained in the context of forward, inverse, and hybrid dynamics formulations. The forward dynamics formulation, applicable to both fully and under-actuated systems, prescribes deterministic actuator inputs which yield uncertain state trajectories. The inverse dynamics formulation is the dual to the forward dynamic, and is only applicable to fully-actuated systems; deterministic state trajectories are prescribed and yield uncertain actuator inputs. The inverse dynamics formulation is more computationally efficient as it requires only algebraic evaluations and completely avoids numerical integration. Finally, the hybrid dynamics formulation is applicable to under-actuated systems where it leverages the benefits of inverse dynamics for actuated joints and forward dynamics for unactuated joints; it prescribes actuated state and unactuated input trajectories which yield uncertain unactuated states and actuated inputs. The benefits of the ability to quantify uncertainty when planning the motion of multibody dynamic systems are illustrated through several case-studies. The resulting designs determine optimal motion plans—subject to deterministic and statistical constraints—for all possible systems within the probability space

IK-FA, a new heuristic inverse kinematics solver using firefly algorithm

In this paper, a heuristic method based on Firefly Algorithm is proposed for inverse kinematics problems in articulated robotics. The proposal is called, IK-FA. Solving inverse kinematics, IK, consists in finding a set of joint-positions allowing a specific point of the system to achieve a target position. In IK-FA, the Fireflies positions are assumed to be a possible solution for joints elementary motions. For a robotic system with a known forward kinematic model, IK-Fireflies, is used to generate iteratively a set of joint motions, then the forward kinematic model of the system is used to compute the relative Cartesian positions of a specific end-segment, and to compare it to the needed target position. This is a heuristic approach for solving inverse kinematics without computing the inverse model. IK-FA tends to minimize the distance to a target position, the fitness function could be established as the distance between the obtained forward positions and the desired one, it is subject to minimization. In this paper IK-FA is tested over a 3 links articulated planar system, the evaluation is based on statistical analysis of the convergence and the solution quality for 100 tests. The impact of key FA parameters is also investigated with a focus on the impact of the number of fireflies, the impact of the maximum iteration number and also the impact of (a, ß, ¿, d) parameters. For a given set of valuable parameters, the heuristic converges to a static fitness value within a fix maximum number of iterations. IK-FA has a fair convergence time, for the tested configuration, the average was about 2.3394 × 10-3 seconds with a position error fitness around 3.116 × 10-8 for 100 tests. The algorithm showed also evidence of robustness over the target position, since for all conducted tests with a random target position IK-FA achieved a solution with a position error lower or equal to 5.4722 × 10-9.Peer ReviewedPostprint (author's final draft