RigidFusion: Robot Localisation and Mapping in Environments with Large Dynamic Rigid Objects

This work presents a novel approach to simultaneously track a robot with respect to multiple rigid entities, including the environment and additional dynamic objects in a scene. Previous approaches treat dynamic parts of a scene as outliers and are thus limited to small amount of dynamics, or rely on prior information for all objects in the scene to enable robust camera tracking. Here, we propose to formulate localisation and object tracking as the same underlying problem and simultaneously track multiple rigid transformations, therefore enabling simultaneous localisation and object tracking for mobile manipulators in dynamic scenes. We evaluate our approach on multiple challenging dynamic scenes with large occlusions. The evaluation demonstrates that our approach achieves better scene segmentation and camera pose tracking in highly dynamic scenes without requiring knowledge of the dynamic object's appearance.

Inverse Dynamics vs. Forward Dynamics in Direct Transcription Formulations for Trajectory Optimization

Benchmarks of state-of-the-art rigid-body dynamics libraries have reported better performance for solving the inverse dynamics problem than the forward alternative. Those benchmarks encouraged us to question whether this computational advantage translates to direct transcription formulations, where calculating the rigid-body dynamics and their derivatives often accounts for a significant share of computation time. In this work, we implement an optimization framework where both approaches for enforcing the system dynamics are available. We evaluate the performance of each approach for systems of varying complexity, and for domains with rigid contacts. Our tests revealed that formulations employing inverse dynamics converge faster, require less iterations, and are more robust to coarse problem discretization. These results suggest that inverse dynamics should be the preferred approach to enforce nonlinear system dynamics in simultaneous methods, such as direct transcription.

Memory Clustering using Persistent Homology for Multimodality- and Discontinuity-Sensitive Learning of Optimal Control Warm-starts

Shooting methods are an efficient approach to solving nonlinear optimal control problems. As they use local optimization, they exhibit favorable convergence when initialized with a good warm-start but may not converge at all if provided with a poor initial guess. Recent work has focused on providing an initial guess from a learned model trained on samples generated during an offline exploration of the problem space. However, in practice the solutions contain discontinuities introduced by system dynamics or the environment. Additionally, in many cases multiple equally suitable, i.e., multi-modal, solutions exist to solve a problem. Classic learning approaches smooth across the boundary of these discontinuities and thus generalize poorly. In this work, we apply tools from algebraic topology to extract information on the underlying structure of the solution space. In particular, we introduce a method based on persistent homology to automatically cluster the dataset of precomputed solutions to obtain different candidate initial guesses. We then train a Mixture-of-Experts within each cluster to predict initial guesses and provide a comparison with modality-agnostic learning. We demonstrate our method on a cart-pole toy problem and a quadrotor avoiding obstacles, and show that clustering samples based on inherent structure improves the warm-start quality.

A Direct-Indirect Hybridization Approach to Control-Limited DDP

Optimal control is a widely used tool for synthesizing motions and controls for user-defined tasks under physical constraints. A common approach is to formulate it using direct multiple-shooting and then to use off-the-shelf nonlinear programming solvers that can easily handle arbitrary constraints on the controls and states. However, these methods are not fast enough for many robotics applications such as real-time humanoid motor control. Exploiting the sparse structure of optimal control problem, such as in Differential DynamicProgramming (DDP), has proven to significantly boost the computational efficiency, and recent works have been focused on handling arbitrary constraints. Despite that, DDP has been associated with poor numerical convergence, particularly when considering long time horizons. One of the main reasons is due to system instabilities and poor warm-starting (only controls). This paper presents control-limited Feasibility-driven DDP (Box-FDDP), a solver that incorporates a direct-indirect hybridization of the control-limited DDP algorithm. Concretely, the forward and backward passes handle feasibility and control limits. We showcase the impact and importance of our method on a set of challenging optimal control problems against the Box-DDP and squashing-function approach.

Multi-modal Trajectory Optimization for Impact-aware Manipulation

The transition from free motion to contact is a challenging problem in robotics, in part due to its hybrid nature. Yet, disregarding the effects of impacts at the motion planning level might result in intractable impulsive contact forces. In this paper, we introduce an impact-aware multi-modal trajectory optimization (TO) method that comprises both hybrid dynamics and hybrid control in a coherent fashion. A key concept is the incorporation of an explicit contact force transmission model in the TO method. This allows the simultaneous optimization of the contact forces, contact timings, continuous motion trajectories and compliance, while satisfying task constraints. We compare our method against standard compliance control and an impact-agnostic TO method in physical simulations. Further, we experimentally validate the proposed method with a robot manipulator on the task of halting a large-momentum object.

Variable Autonomy of Whole-body Control for Inspection and Intervention in Industrial Environments using Legged Robots

The deployment of robots in industrial and civil scenarios is a viable solution to protect operators from danger and hazards. Shared autonomy is paramount to enable remote control of complex systems such as legged robots, allowing the operator to focus on the essential tasks instead of overly detailed execution. To realize this, we proposed a comprehensive control framework for inspection and intervention using a legged robot and validated the integration of multiple loco-manipulation algorithms optimised for improving the remote operation. The proposed control offers 3 operation modes: fully automated, semi-autonomous, and the haptic interface receiving onsite physical interaction for assisting teleoperation. Our contribution is the design of a QP-based semi-analytical whole-body control, which is the key to the various task completion subject to internal and external constraints. We demonstrated the versatility of the whole-body control in terms of decoupling tasks, singularity toleration and constraint satisfaction. We deployed our solution in field trials and evaluated in an emergency setting by an E-stop while the robot was clearing road barrier and traversing difficult terrains.

Online Dynamic Motion Planning and Control for Wheeled Biped Robots

Wheeled-legged robots combine the efficiency of wheeled robots when driving on suitably flat surfaces and versatility of legged robots when stepping over or around obstacles. This paper introduces a planning and control framework to realise dynamic locomotion for wheeled biped robots. We propose the Cart-Linear Inverted Pendulum Model (Cart-LIPM) as a template model for the rolling motion and the under-actuated LIPM for contact changes while walking. The generated motion is then tracked by an inverse dynamic whole-body controller which coordinates all joints, including the wheels. The framework has a hierarchical structure and is implemented in a model predictive control (MPC) fashion. To validate the proposed approach for hybrid motion generation, two scenarios involving different types of obstacles are designed in simulation. To the best of our knowledge, this is the first time that such online dynamic hybrid locomotion has been demonstrated on wheeled biped robots.

Bio-mimetic Adaptive Force/Position Control Using Fractal Impedance

The ability of animals to interact with complex dynamics is unmatched in robots. Especially important to the interaction performances is the online adaptation of body dynamics, which can be modeled as an impedance behaviour. However, the variable impedance controller still possesses a challenge in the current control frameworks due to the difficulties of retaining stability when adapting the controller gains. The fractal impedance controller has been recently proposed to solve this issue. However, it still has limitations such as sudden jumps in force when it starts to converge to the desired position and the lack of a force feedback loop. In this manuscript, two improvements are made to the control framework to solve these limitations. The force discontinuity has been addressed introducing a modulation of the impedance via a virtual antagonist that modulates the output force. The force tracking has been modeled after the parallel force/position controller architecture. In contrast to traditional methods, the fractal impedance controller enables the implementation of a search algorithm on the force feedback to adapt its behaviour on the external environment instead of on relying on \textit{a priori} knowledge of the external dynamics. Preliminary simulation results presented in this paper show the feasibility of the proposed approach, and it allows to evaluate the trade-off that needs to be made when relying on the proposed controller for interaction. In conclusion, the proposed method mimics the behaviour of an agonist/antagonist system adapting to unknown external dynamics, and it may find application in computational neuroscience, haptics, and interaction control.

Modeling and Control of a Hybrid Wheeled Jumping Robot

In this paper, we study a wheeled robot with a prismatic extension joint. This allows the robot to build up momentum to perform jumps over obstacles and to swing up to the upright position after the loss of balance. We propose a template model for the class of such two-wheeled jumping robots. This model can be considered as the simplest wheeled-legged system. We provide an analytical derivation of the system dynamics which we use inside a model predictive controller (MPC). We study the behavior of the model and demonstrate highly dynamic motions such as swing-up and jumping. Furthermore, these motions are discovered through optimization from first principles. We evaluate the controller on a variety of tasks and uneven terrains in a simulator.

Optimizing Dynamic Trajectories for Robustness to Disturbances Using Polytopic Projections

This paper focuses on robustness to disturbance forces and uncertain payloads. We present a novel formulation to optimize the robustness of dynamic trajectories. A straightforward transcription of this formulation into a nonlinear programming problem is not tractable for state-of-the-art solvers, but it is possible to overcome this complication by exploiting the structure induced by the kinematics of the robot. The non-trivial transcription proposed allows trajectory optimization frameworks to converge to highly robust dynamic solutions. We demonstrate the results of our approach using a quadruped robot equipped with a manipulator.