Ludovic Righetti

ICRA Conference 2025 Conference Paper

Collision Avoidance in Model Predictive Control Using Velocity Damper

• Arthur Haffemayer
• Armand Jordana
• Ludovic De Matteïs
• Krzysztof Wojciechowski
• Ludovic Righetti
• Florent Lamiraux
• Nicolas Mansard

We propose an advanced method for controlling the motion of a manipulator robot with strict collision avoidance in dynamic environments, leveraging a velocity damper constraint. Unlike conventional distance-based constraints, which tend to saturate near obstacles to reach optimality, the velocity damper constraint considers both distance and relative velocity, ensuring a safer separation. This constraint is incorporated into a model predictive control framework and enforced as a hard constraint through analytical derivatives supplied to the numerical solver. The approach has been fully implemented on a Franka Emika Panda robot and validated through experimental trials, demonstrating effective collision avoidance during dynamic tasks and robustness to unmodeled disturbances. An efficient open-source implementation along examples are provided here: https://gepettoweb.laas.fr/articles/haffemayer2025.html.

IROS Conference 2025 Conference Paper

Cost Function Estimation Using Inverse Reinforcement Learning with Minimal Observations

• Sarmad Mehrdad
• Avadesh Meduri
• Ludovic Righetti

We present an iterative inverse reinforcement learning algorithm to infer optimal cost functions in continuous spaces. Based on a popular maximum entropy criteria, our approach iteratively finds a weight improvement step and proposes a method to find an appropriate step size that ensures learned cost function features remain similar to the demonstrated trajectory features. In contrast to similar approaches, our algorithm can individually tune the effectiveness of each observation for the partition function based on the current estimate of the cost function parameters, guiding the algorithm towards better estimates in the following iterations. In addition, it does not need a large sample set, enabling faster learning. We generate sample trajectories by solving an optimal control problem instead of random sampling, leading to more informative trajectories. The performance of our method is compared to two state of the art algorithms to demonstrate its benefits in several simulated environments.

ICRA Conference 2024 Conference Paper

Force Feedback Model-Predictive Control via Online Estimation

• Armand Jordana
• Sébastien Kleff
• Justin Carpentier
• Nicolas Mansard
• Ludovic Righetti

Nonlinear model-predictive control has recently shown its practicability in robotics. However it remains limited in contact interaction tasks due to its inability to leverage sensed efforts. In this work, we propose a novel model-predictive control approach that incorporates direct feedback from force sensors while circumventing explicit modeling of the contact force evolution. Our approach is based on the online estimation of the discrepancy between the force predicted by the dynamics model and force measurements, combined with high-frequency nonlinear model-predictive control. We report an experimental validation on a torque-controlled manipulator in challenging tasks for which accurate force tracking is necessary. We show that a simple reformulation of the optimal control problem combined with standard estimation tools enables to achieve state-of-the-art performance in force control while preserving the benefits of model-predictive control, thereby outperforming traditional force control techniques. This work paves the way toward a more systematic integration of force sensors in model predictive control.

IROS Conference 2024 Conference Paper

iDb-RRT: Sampling-based Kinodynamic Motion Planning with Motion Primitives and Trajectory Optimization

• Joaquim Ortiz de Haro
• Wolfgang Hönig
• Valentin N. Hartmann
• Marc Toussaint
• Ludovic Righetti

Rapidly-exploring Random Trees (RRT) and its variations have emerged as a robust and efficient tool for finding collision-free paths in robotic systems. However, adding dynamic constraints makes the motion planning problem significantly harder, as it requires solving two-value boundary problems (computationally expensive) or propagating random control inputs (uninformative). Alternatively, Iterative Discontinuity Bounded A* (iDb-A*), introduced in our previous study, combines search and optimization iteratively. The search step connects short trajectories (motion primitives) while allowing a bounded discontinuity between the motion primitives, which is later repaired in the trajectory optimization step. Building upon these foundations, in this paper, we present iDb-RRT, a sampling-based kinodynamic motion planning algorithm that combines motion primitives and trajectory optimization within the RRT framework. iDb-RRT is probabilistically complete and can be implemented in forward or bidirectional mode. We have tested our algorithm across a benchmark suite comprising 30 problems, spanning 8 different systems, and shown that iDb-RRT can find solutions up to 10x faster than previous methods, especially in complex scenarios that require long trajectories or involve navigating through narrow passages.

ICRA Conference 2024 Conference Paper

Risk-Sensitive Extended Kalman Filter

• Armand Jordana
• Avadesh Meduri
• Etienne Arlaud
• Justin Carpentier
• Ludovic Righetti

Designing robust algorithms in the face of estimation uncertainty is a challenging task. Indeed, controllers seldom consider estimation uncertainty and only rely on the most likely estimated state. Consequently, sudden changes in the environment or the robot’s dynamics can lead to catastrophic behaviors. Leveraging recent results in risk-sensitive optimal control, this paper presents a risk-sensitive Extended Kalman Filter that can adapt its estimation to the control objective, hence allowing safe output-feedback Model Predictive Control (MPC). By taking a pessimistic estimate of the value function resulting from the MPC controller, the filter provides increased robustness to the controller in phases of uncertainty as compared to a standard Extended Kalman Filter (EKF). The filter has the same computational complexity as an EKF and can be used for real-time control. The paper evaluates the risk-sensitive behavior of the proposed filter when used in a nonlinear MPC loop on a planar drone and industrial manipulator in simulation, as well as on an external force estimation task on a real quadruped robot. These experiments demonstrate the ability of the approach to significantly improve performance in face of uncertainties.

IROS Conference 2023 Conference Paper

Efficient Object Manipulation Planning with Monte Carlo Tree Search

• Huaijiang Zhu
• Avadesh Meduri
• Ludovic Righetti

This paper presents an efficient approach to object manipulation planning using Monte Carlo Tree Search (MCTS) to find contact sequences and an efficient ADMM-based trajectory optimization algorithm to evaluate the dynamic feasibility of candidate contact sequences. To accelerate MCTS, we propose a methodology to learn a goal-conditioned policy-value network and a feasibility classifier to direct the search towards promising nodes. Further, manipulation-specific heuristics enable to drastically reduce the search space. Systematic object manipulation experiments in a physics simulator and on real hardware demonstrate the efficiency of our approach. In particular, our approach scales favorably for long manipulation sequences thanks to the learned policy-value network, significantly improving planning success rate. All source code including the baseline can be found at https://hzhu.io/contact-mcts.

ICLR Conference 2023 Conference Paper

Learning Simultaneous Navigation and Construction in Grid Worlds

• Wenyu Han
• Haoran Wu
• Eisuke Hirota
• Alexander Gao
• Lerrel Pinto
• Ludovic Righetti
• Chen Feng 0002

We propose to study a new learning task, mobile construction, to enable an agent to build designed structures in 1/2/3D grid worlds while navigating in the same evolving environments. Unlike existing robot learning tasks such as visual navigation and object manipulation, this task is challenging because of the interdependence between accurate localization and strategic construction planning. In pursuit of generic and adaptive solutions to this partially observable Markov decision process (POMDP) based on deep reinforcement learning (RL), we design a Deep Recurrent Q-Network (DRQN) with explicit recurrent position estimation in this dynamic grid world. Our extensive experiments show that pre-training this position estimation module before Q-learning can significantly improve the construction performance measured by the intersection-over-union score, achieving the best results in our benchmark of various baselines including model-free and model-based RL, a handcrafted SLAM-based policy, and human players. Our code is available at: https://ai4ce.github.io/SNAC/.

ICRA Conference 2023 Conference Paper

MPC with Sensor-Based Online Cost Adaptation

• Avadesh Meduri
• Huaijiang Zhu
• Armand Jordana
• Ludovic Righetti

Model predictive control is a powerful tool to generate complex motions for robots. However, it often requires solving non-convex problems online to produce rich behaviors, which is computationally expensive and not always practical in real time. Additionally, direct integration of high dimensional sensor data (e. g. RGB-D images) in the feedback loop is challenging with current state-space methods. This paper aims to address both issues. It introduces a model predictive control scheme, where a neural network constantly updates the cost function of a quadratic program based on sensory inputs, aiming to minimize a general non-convex task loss without solving a non-convex problem online. By updating the cost, the robot is able to adapt to changes in the environment directly from sensor measurement without requiring a new cost design. Furthermore, since the quadratic program can be solved efficiently with hard constraints, a safe deployment on the robot is ensured. Experiments with a wide variety of reaching tasks on an industrial robot manipulator demonstrate that our method can efficiently solve complex non-convex problems with high-dimensional visual sensory inputs, while still being robust to external disturbances.

ICRA Conference 2023 Conference Paper

On the Use of Torque Measurement in Centroidal State Estimation

• Shahram Khorshidi
• Ahmad Gazar
• Nicholas Rotella
• Maximilien Naveau
• Ludovic Righetti
• Maren Bennewitz
• Majid Khadiv

State-of-the-art legged robots are either capable of measuring torque at the output of their drive systems, or have transparent drive systems which enable the computation of joint torques from motor currents. In either case, this sensor modality is seldom used in state estimation. In this paper, we propose to use joint torque measurements to estimate the centroidal states of legged robots. To do so, we project the whole-body dynamics of a legged robot into the nullspace of the contact constraints, allowing expression of the dynamics independent of the contact forces. Using the constrained dynamics and the centroidal momentum matrix, we are able to directly relate joint torques and centroidal states dynamics. Using the resulting model as the process model of an Extended Kalman Filter (EKF), we fuse the torque measurement in the centroidal state estimation problem. Through real-world experiments on a quadruped robot executing different gaits, we demonstrate that the estimated centroidal states from our torque-based EKF drastically improve the recovery of these quantities compared to direct computation.

ICRA Conference 2023 Conference Paper

Path Planning Under Uncertainty to Localize mmWave Sources

• Kai Pfeiffer
• Yuze Jia
• Mingsheng Yin
• Akshaj Kumar Veldanda
• Yaqi Hu
• Amee Trivedi
• Jeff Zhang 0001
• Siddharth Garg

In this paper, we study a navigation problem where a mobile robot needs to locate a mmWave wireless signal. Using the directionality properties of the signal, we propose an estimation and path planning algorithm that can efficiently navigate in cluttered indoor environments. We formulate Extended Kalman filters for emitter location estimation in cases where the signal is received in line-of-sight or after reflections. We then propose to plan motion trajectories based on belief-space dynamics in order to minimize the uncertainty of the position estimates. The associated non-linear optimization problem is solved by a state-of-the-art constrained iLQR solver. In particular, we propose a method that can handle a large number of obstacles (∼ 300) with reasonable computation times. We validate the approach in an extensive set of simulations. We show that our estimators can help increase navigation success rate and that planning to reduce estimation uncertainty can improve the overall task completion speed.

ICRA Conference 2023 Conference Paper

Visual-Inertial and Leg Odometry Fusion for Dynamic Locomotion

• Victor Dhédin
• Haolong Li
• Shahram Khorshidi
• Lukas Mack
• Adithya Kumar Chinnakkonda Ravi
• Avadesh Meduri
• Paarth Shah
• Felix Grimminger

Implementing dynamic locomotion behaviors on legged robots requires a high-quality state estimation module. Especially when the motion includes flight phases, state-of-the-art approaches fail to produce reliable estimation of the robot posture, in particular base height. In this paper, we propose a novel approach for combining visual-inertial odometry (VIO) with leg odometry in an extended Kalman filter (EKF) based state estimator. The VIO module uses a stereo camera and IMU to yield low-drift 3D position and yaw orientation and drift-free pitch and roll orientation of the robot base link in the inertial frame. However, these values have a considerable amount of latency due to image processing and optimization, while the rate of update is quite low which is not suitable for low-level control. To reduce the latency, we predict the VIO state estimate at the rate of the IMU measurements of the VIO sensor. The EKF module uses the base pose and linear velocity predicted by VIO, fuses them further with a second high-rate IMU and leg odometry measurements, and produces robot state estimates with a high frequency and small latency suitable for control. We integrate this lightweight estimation framework with a nonlinear model predictive controller and show successful implementation of a set of agile locomotion behaviors, including trotting and jumping at varying horizontal speeds, on a torque-controlled quadruped robot.

IROS Conference 2022 Conference Paper

Introducing Force Feedback in Model Predictive Control

• Sébastien Kleff
• Ewen Dantec
• Guilhem Saurel
• Nicolas Mansard
• Ludovic Righetti

In the literature about model predictive control (MPC), contact forces are planned rather than controlled. In this paper, we propose a novel paradigm to incorporate effort measurements into a predictive controller, hence allowing to control them by direct measurement feedback. We first demonstrate why the classical optimal control formulation, based on position and velocity state feedback, cannot handle direct feedback on force information. Following previous approaches in force control, we then propose to augment the classical formulations with a model of the robot actuation, which naturally allows to generate online trajectories that adapt to sensed position, velocity and torques. We propose a complete implementation of this idea on the upper part of a real humanoid robot, and show through hardware experiments that this new formulation incorporating effort feedback outperforms classical MPC in challenging tasks where physical interaction with the environment is crucial.

ICRA Conference 2021 Conference Paper

DeepQ Stepper: A framework for reactive dynamic walking on uneven terrain

• Avadesh Meduri
• Majid Khadiv
• Ludovic Righetti

Reactive stepping and push recovery for biped robots is often restricted to flat terrains because of the difficulty in computing capture regions for nonlinear dynamic models. In this paper, we address this limitation by proposing a novel 3D reactive stepper, the DeepQ stepper, that can approximately learn the 3D capture regions of both simplified and full robot dynamic models using reinforcement learning, which can then be used to find optimal steps. The stepper can take into account the entire dynamics of the robot, ignored in most reactive steppers, leading to a significant improvement in performance. The DeepQ stepper can handle nonconvex terrain with obstacles, walk on restricted surfaces like stepping stones while tracking different velocities, and recover from external disturbances for a constant low computational cost.

ICRA Conference 2021 Conference Paper

High-Frequency Nonlinear Model Predictive Control of a Manipulator

• Sébastien Kleff
• Avadesh Meduri
• Rohan Budhiraja
• Nicolas Mansard
• Ludovic Righetti

Model Predictive Control (MPC) promises to endow robots with enough reactivity to perform complex tasks in dynamic environments by frequently updating their motion plan based on measurements. Despite its appeal, it has seldom been deployed on real machines because of scaling constraints. This paper presents the first hardware implementation of closed-loop nonlinear MPC on a 7-DoF torque-controlled robot. Our controller leverages a state-of-the art optimal control solver, namely Differential Dynamic Programming (DDP), in order to replan state and control trajectories at real-time rates (1kHz). In addition to this experimental proof of concept, an exhaustive performance analysis shows that our controller outperforms open-loop MPC on a rapid cyclic end-effector task. We also exhibit the importance of a sufficient preview horizon and full robot dynamics through comparisons with inverse dynamics and kinematic optimization.

ICRA Conference 2021 Conference Paper

Learning a Centroidal Motion Planner for Legged Locomotion

• Julian Viereck
• Ludovic Righetti

Whole-body optimizers have been successful at automatically computing complex dynamic locomotion behaviors. However they are often limited to offline planning as they are computationally too expensive to replan with a high frequency. Simpler models are then typically used for online replanning. In this paper we present a method to generate whole body movements in real-time for locomotion tasks. Our approach consists in learning a centroidal neural network that predicts the desired centroidal motion given the current state of the robot and a desired contact plan. The network is trained using an existing whole body motion optimizer. Our approach enables to learn with few training samples dynamic motions that can be used in a complete whole-body control framework at high frequency, which is usually not attainable with typical full-body optimizers. We demonstrate our method to generate a rich set of walking and jumping motions on a real quadruped robot.

ICRA Conference 2021 Conference Paper

Leveraging Forward Model Prediction Error for Learning Control

• Sarah Bechtle
• Bilal Hammoud
• Akshara Rai
• Franziska Meier
• Ludovic Righetti

Learning for model based control can be sample-efficient and generalize well, however successfully learning models and controllers that represent the problem at hand can be challenging for complex tasks. Using inaccurate models for learning can lead to sub-optimal solutions that are unlikely to perform well in practice. In this work, we present a learning approach which iterates between model learning and data collection and leverages forward model prediction error for learning control. We show how using the controller’s prediction as input to a forward model can create a differentiable connection between the controller and the model, allowing us to formulate a loss in the state space. This lets us include forward model prediction error during controller learning and we show that this creates a loss objective that significantly improves learning on different motor control tasks. We provide empirical and theoretical results that show the benefits of our method and present evaluations in simulation for learning control on a 7 DoF manipulator and an underactuated 12 DoF quadruped. We show that our approach successfully learns controllers for challenging motor control tasks involving contact switching.

IROS Conference 2021 Conference Paper

Rapid Convex Optimization of Centroidal Dynamics using Block Coordinate Descent

• Paarth Shah
• Avadesh Meduri
• Wolfgang Merkt
• Majid Khadiv
• Ioannis Havoutis
• Ludovic Righetti

In this paper we explore the use of block coordinate descent (BCD) to optimize the centroidal momentum dynamics for dynamically consistent multi-contact behaviors. The centroidal dynamics have recently received a large amount of attention in order to create physically realizable motions for robots with hands and feet while being computationally more tractable than full rigid body dynamics models. Our contribution lies in exploiting the structure of the dynamics in order to simplify the original non-convex problem into two convex subproblems. We iterate between these two subproblems for a set number of iterations or until a consensus is reached. We explore the properties of the proposed optimization method for the centroidal dynamics and verify in simulation that motions generated by our approach can be tracked by the quadruped Solo12. In addition, we compare our method to a recently proposed convexification using a sequence of convex relaxations as well as a more standard interior point method used in the off-the-shelf solver IPOPT to show that our approach finds similar, if not better, trajectories (in terms of cost), and is more than four times faster than both approaches. Finally, compared to previous approaches, we note its practicality due to the convex nature of each subproblem which allows our method to be used with any off-the-shelf quadratic programming solver.

ICRA Conference 2020 Conference Paper

A Real-Robot Dataset for Assessing Transferability of Learned Dynamics Models

• Diego Agudelo-España
• Andrii Zadaianchuk
• Philippe Wenk
• Aditya Garg
• Joel Akpo
• Felix Grimminger
• Julian Viereck
• Maximilien Naveau

In the context of model-based reinforcement learning and control, a large number of methods for learning system dynamics have been proposed in recent years. The purpose of these learned models is to synthesize new control policies. An important open question is how robust current dynamics-learning methods are to shifts in the data distribution due to changes in the control policy. We present a real-robot dataset which allows to systematically investigate this question. This dataset contains trajectories of a 3 degrees-of-freedom (DOF) robot being controlled by a diverse set of policies. For comparison, we also provide a simulated version of the dataset. Finally, we benchmark a few widely-used dynamics-learning methods using the proposed dataset. Our results show that the iid test error of a learned model is not necessarily a good indicator of its accuracy under control policies different from the one which generated the training data. This suggests that it may be important to evaluate dynamics-learning methods in terms of their transfer performance, rather than only their iid error.

ICRA Conference 2020 Conference Paper

Crocoddyl: An Efficient and Versatile Framework for Multi-Contact Optimal Control

• Carlos Mastalli
• Rohan Budhiraja
• Wolfgang Merkt
• Guilhem Saurel
• Bilal Hammoud
• Maximilien Naveau
• Justin Carpentier
• Ludovic Righetti

We introduce Crocoddyl (Contact RObot COntrol by Differential DYnamic Library), an open-source framework tailored for efficient multi-contact optimal control. Crocoddyl efficiently computes the state trajectory and the control policy for a given predefined sequence of contacts. Its efficiency is due to the use of sparse analytical derivatives, exploitation of the problem structure, and data sharing. It employs differential geometry to properly describe the state of any geometrical system, e. g. floating-base systems. Additionally, we propose a novel optimal control algorithm called Feasibility-driven Differential Dynamic Programming (FDDP). Our method does not add extra decision variables which often increases the computation time per iteration due to factorization. FDDP shows a greater globalization strategy compared to classical Differential Dynamic Programming (DDP) algorithms. Concretely, we propose two modifications to the classical DDP algorithm. First, the backward pass accepts infeasible state-control trajectories. Second, the rollout keeps the gaps open during the early "exploratory" iterations (as expected in multipleshooting methods with only equality constraints). We showcase the performance of our framework using different tasks. With our method, we can compute highly-dynamic maneuvers (e. g. jumping, front-flip) within few milliseconds.

IROS Conference 2020 Conference Paper

Enabling Remote Whole-Body Control with 5G Edge Computing

• Huaijiang Zhu
• Manali Sharma
• Kai Pfeiffer
• Marco Mezzavilla
• Jia Shen
• Sundeep Rangan
• Ludovic Righetti

Real-world applications require light-weight, energy-efficient, fully autonomous robots. Yet, increasing autonomy is oftentimes synonymous with escalating computational requirements. It might thus be desirable to offload intensive computation—not only sensing and planning, but also low-level whole-body control—to remote servers in order to reduce on-board computational needs. Fifth Generation (5G) wireless cellular technology, with its low latency and high bandwidth capabilities, has the potential to unlock cloud-based high performance control of complex robots. However, state-of-the-art control algorithms for legged robots can only tolerate very low control delays, which even ultra-low latency 5G edge computing can sometimes fail to achieve. In this work, we investigate the problem of cloud-based whole-body control of legged robots over a 5G link. We propose a novel approach that consists of a standard optimization-based controller on the network edge and a local linear, approximately optimal controller that significantly reduces on-board computational needs while increasing robustness to delay and possible loss of communication. Simulation experiments on humanoid balancing and walking tasks that includes a realistic 5G communication model demonstrate significant improvement of the reliability of robot locomotion under jitter and delays likely to be experienced in 5G wireless links.

ICRA Conference 2019 Conference Paper

Efficient Humanoid Contact Planning using Learned Centroidal Dynamics Prediction

• Yu-Chi Lin
• Brahayam Ponton
• Ludovic Righetti
• Dmitry Berenson

Humanoid robots dynamically navigate an environment by interacting with it via contact wrenches exerted at intermittent contact poses. Therefore, it is important to consider dynamics when planning a contact sequence. Traditional contact planning approaches assume a quasi-static balance criterion to reduce the computational challenges of selecting a contact sequence over a rough terrain. This however limits the applicability of the approach when dynamic motions are required, such as when walking down a steep slope or crossing a wide gap. Recent methods overcome this limitation with the help of efficient mixed integer convex programming solvers capable of synthesizing dynamic contact sequences. Nevertheless, its exponential-time complexity limits its applicability to short time horizon contact sequences within small environments. In this paper, we go beyond current approaches by learning a prediction of the dynamic evolution of the robot centroidal momenta, which can then be used for quickly generating dynamically robust contact sequences for robots with arms and legs using a search-based contact planner. We demonstrate the efficiency and quality of the results of the proposed approach in a set of dynamically challenging scenarios.

IROS Conference 2019 Conference Paper

Learning to Explore in Motion and Interaction Tasks

• Miroslav Bogdanovic
• Ludovic Righetti

Model free reinforcement learning suffers from the high sampling complexity inherent to robotic manipulation or locomotion tasks. Most successful approaches typically use random sampling strategies which leads to slow policy convergence. In this paper we present a novel approach for efficient exploration that leverages previously learned tasks. We exploit the fact that the same system is used across many tasks and build a generative model for exploration based on data from previously solved tasks to improve learning new tasks. The approach also enables continuous learning of improved exploration strategies as novel tasks are learned. Extensive simulations on a robot manipulator performing a variety of motion and contact interaction tasks demonstrate the capabilities of the approach. In particular, our experiments suggest that the exploration strategy can more than double learning speed, especially when rewards are sparse. Moreover, the algorithm is robust to task variations and parameter tuning, making it beneficial for complex robotic problems.

ICRA Conference 2019 Conference Paper

Leveraging Contact Forces for Learning to Grasp

• Hamza Merzic
• Miroslav Bogdanovic
• Daniel Kappler
• Ludovic Righetti
• Jeannette Bohg

Grasping objects under uncertainty remains an open problem in robotics research. This uncertainty is often due to noisy or partial observations of the object pose or shape. To enable a robot to react appropriately to unforeseen effects, it is crucial that it continuously takes sensor feedback into account. While visual feedback is important for inferring a grasp pose and reaching for an object, contact feedback offers valuable information during manipulation and grasp acquisition. In this paper, we use model-free deep reinforcement learning to synthesize control policies that exploit contact sensing to generate robust grasping under uncertainty. We demonstrate our approach on a multi-fingered hand that exhibits more complex finger coordination than the commonly used two-fingered grippers. We conduct extensive experiments in order to assess the performance of the learned policies, with and without contact sensing. While it is possible to learn grasping policies without contact sensing, our results suggest that contact feedback allows for a significant improvement of grasping robustness under object pose uncertainty and for objects with a complex shape.

ICRA Conference 2018 Conference Paper

An MPC Walking Framework with External Contact Forces

• Sean Mason
• Nicholas Rotella
• Stefan Schaal
• Ludovic Righetti

In this work, we present an extension to a linear Model Predictive Control (MPC) scheme that plans external contact forces for the robot when given multiple contact locations and their corresponding friction cone. To this end, we set up a two-step optimization problem. In the first optimization, we compute the Center of Mass (CoM) trajectory, foot step locations, and introduce slack variables to account for violating the imposed constraints on the Zero Moment Point (ZMP). We then use the slack variables to trigger the second optimization, in which we calculate the optimal external force that compensates for the ZMP tracking error. This optimization considers multiple contacts positions within the environment by formulating the problem as a Mixed Integer Quadratic Program (MIQP) that can be solved at a speed between 100-300 Hz. Once contact is created, the MIQP reduces to a single Quadratic Program (QP) that can be solved in real-time (<; 1kHz). Simulations show that the presented walking control scheme can withstand disturbances 2-3× larger with the additional force provided by a hand contact.

ICRA Conference 2018 Conference Paper

On Time Optimization of Centroidal Momentum Dynamics

• Brahayam Ponton
• Alexander Herzog
• Andrea Del Prete
• Stefan Schaal
• Ludovic Righetti

Recently, the centroidal momentum dynamics has received substantial attention to plan dynamically consistent motions for robots with arms and legs in multi-contact scenarios. However, it is also non convex which renders any optimization approach difficult and timing is usually kept fixed in most trajectory optimization techniques to not introduce additional non convexities to the problem. But this can limit the versatility of the algorithms. In our previous work, we proposed a convex relaxation of the problem that allowed to efficiently compute momentum trajectories and contact forces. However, our approach could not minimize a desired angular momentum objective which seriously limited its applicability. Noticing that the non-convexity introduced by the time variables is of similar nature as the centroidal dynamics one, we propose two convex relaxations to the problem based on trust regions and soft constraints. The resulting approaches can compute time-optimized dynamically consistent trajectories sufficiently fast to make the approach realtime capable. The performance of the algorithm is demonstrated in several multi-contact scenarios for a humanoid robot. In particular, we show that the proposed convex relaxation of the original problem finds solutions that are consistent with the original non-convex problem and illustrate how timing optimization allows to find motion plans that would be difficult to plan with fixed timing † †Implementation details and demos can be found in the source code available at https://git-amd.tuebingen.mpg.de/bponton/timeoptimization.

ICRA Conference 2018 Conference Paper

Unsupervised Contact Learning for Humanoid Estimation and Control

• Nicholas Rotella
• Stefan Schaal
• Ludovic Righetti

This work presents a method for contact state estimation using fuzzy clustering to learn contact probability for full, six-dimensional humanoid contacts. The data required for training is solely from proprioceptive sensors - endeffector contact wrench sensors and inertial measurement units (IMUs) - and the method is completely unsupervised. The resulting cluster means are used to efficiently compute the probability of contact in each of the six endeffector degrees of freedom (DoFs) independently. This clustering-based contact probability estimator is validated in a kinematics-based base state estimator in a simulation environment with realistic added sensor noise for locomotion over rough, low-friction terrain on which the robot is subject to foot slip and rotation. The proposed base state estimator which utilizes these six DoF contact probability estimates is shown to perform considerably better than that which determines kinematic contact constraints purely based on measured normal force.

ICRA Conference 2016 Conference Paper

Inertial sensor-based humanoid joint state estimation

• Nicholas Rotella
• Sean Mason
• Stefan Schaal
• Ludovic Righetti

This work presents methods for the determination of a humanoid robot's joint velocities and accelerations directly from link-mounted Inertial Measurement Units (IMUs) each containing a three-axis gyroscope and a three-axis accelerometer. No information about the global pose of the floating base or its links is required and precise knowledge of the link IMU poses is not necessary due to presented calibration routines. Additionally, a filter is introduced to fuse gyroscope angular velocities with joint position measurements and compensate the computed joint velocities for time-varying gyroscope biases. The resulting joint velocities are subject to less noise and delay than filtered velocities computed from numerical differentiation of joint potentiometer signals, leading to superior performance in joint feedback control as demonstrated in experiments performed on a SARCOS hydraulic humanoid.

IROS Conference 2016 Conference Paper

Structured contact force optimization for kino-dynamic motion generation

• Alexander Herzog
• Stefan Schaal
• Ludovic Righetti

Optimal control approaches in combination with trajectory optimization have recently proven to be a promising control strategy for legged robots. Computationally efficient and robust algorithms were derived using simplified models of the contact interaction between robot and environment such as the linear inverted pendulum model (LIPM). However, as humanoid robots enter more complex environments, less restrictive models become increasingly important. As we leave the regime of linear models, we need to build dedicated solvers that can compute interaction forces together with consistent kinematic plans for the whole-body. In this paper, we address the problem of planning robot motion and interaction forces for legged robots given predefined contact surfaces. The motion generation process is decomposed into two alternating parts computing force and motion plans in coherence. We focus on the properties of the momentum computation leading to sparse optimal control formulations to be exploited by a dedicated solver. In our experiments, we demonstrate that our motion generation algorithm computes consistent contact forces and joint trajectories for our humanoid robot. We also demonstrate the favorable time complexity due to our formulation and composition of the momentum equations.

IROS Conference 2014 Conference Paper

Balancing experiments on a torque-controlled humanoid with hierarchical inverse dynamics

• Alexander Herzog
• Ludovic Righetti
• Felix Grimminger
• Peter Pastor
• Stefan Schaal

Recently several hierarchical inverse dynamics controllers based on cascades of quadratic programs have been proposed for application on torque controlled robots. They have important theoretical benefits but have never been implemented on a torque controlled robot where model inaccuracies and real-time computation requirements can be problematic. In this contribution we present an experimental evaluation of these algorithms in the context of balance control for a humanoid robot. The presented experiments demonstrate the applicability of the approach under real robot conditions (i. e. model uncertainty, estimation errors, etc). We propose a simplification of the optimization problem that allows us to decrease computation time enough to implement it in a fast torque control loop. We implement a momentum-based balance controller which shows robust performance in face of unknown disturbances, even when the robot is standing on only one foot. In a second experiment, a tracking task is evaluated to demonstrate the performance of the controller with more complicated hierarchies. Our results show that hierarchical inverse dynamics controllers can be used for feedback control of humanoid robots and that momentum-based balance control can be efficiently implemented on a real robot.

IROS Conference 2014 Conference Paper

Dual execution of optimized contact interaction trajectories

• Marc Toussaint
• Nathan D. Ratliff
• Jeannette Bohg
• Ludovic Righetti
• Peter Englert
• Stefan Schaal

Efficient manipulation requires contact to reduce uncertainty. The manipulation literature refers to this as funneling: a methodology for increasing reliability and robustness by leveraging haptic feedback and control of environmental interaction. However, there is a fundamental gap between traditional approaches to trajectory optimization and this concept of robustness by funneling: traditional trajectory optimizers do not discover force feedback strategies. From a POMDP perspective, these behaviors could be regarded as explicit observation actions planned to sufficiently reduce uncertainty thereby enabling a task. While we are sympathetic to the full POMDP view, solving full continuous-space POMDPs in high-dimensions is hard. In this paper, we propose an alternative approach in which trajectory optimization objectives are augmented with new terms that reward uncertainty reduction through contacts, explicitly promoting funneling. This augmentation shifts the responsibility of robustness toward the actual execution of the optimized trajectories. Directly tracing trajectories through configuration space would lose all robustness—dual execution achieves robustness by devising force controllers to reproduce the temporal interaction profile encoded in the dual solution of the optimization problem. This work introduces dual execution in depth and analyze its performance through robustness experiments in both simulation and on a real-world robotic platform.

IROS Conference 2014 Conference Paper

State estimation for a humanoid robot

• Nicholas Rotella
• Michael Bloesch
• Ludovic Righetti
• Stefan Schaal

This paper introduces a framework for state estimation on a humanoid robot platform using only common proprioceptive sensors and knowledge of leg kinematics. The presented approach extends that detailed in prior work on a point-foot quadruped platform by adding the rotational constraints imposed by the humanoid's flat feet. As in previous work, the proposed Extended Kalman Filter accommodates contact switching and makes no assumptions about gait or terrain, making it applicable on any humanoid platform for use in any task. A nonlinear observability analysis is performed on both the point-foot and flat-foot filters and it is concluded that the addition of rotational constraints significantly simplifies singular cases and improves the observability characteristics of the system. Results on a simulated walking dataset demonstrate the performance gain of the flat-foot filter as well as confirm the results of the presented observability analysis.

ICRA Conference 2013 Conference Paper

Learning objective functions for manipulation

• Mrinal Kalakrishnan
• Peter Pastor
• Ludovic Righetti
• Stefan Schaal

We present an approach to learning objective functions for robotic manipulation based on inverse reinforcement learning. Our path integral inverse reinforcement learning algorithm can deal with high-dimensional continuous state-action spaces, and only requires local optimality of demonstrated trajectories. We use L 1 regularization in order to achieve feature selection, and propose an efficient algorithm to minimize the resulting convex objective function. We demonstrate our approach by applying it to two core problems in robotic manipulation. First, we learn a cost function for redundancy resolution in inverse kinematics. Second, we use our method to learn a cost function over trajectories, which is then used in optimization-based motion planning for grasping and manipulation tasks. Experimental results show that our method outperforms previous algorithms in high-dimensional settings.

ICRA Conference 2013 Conference Paper

Learning task error models for manipulation

• Peter Pastor
• Mrinal Kalakrishnan
• Jonathan Binney
• Jonathan Kelly
• Ludovic Righetti
• Gaurav S. Sukhatme
• Stefan Schaal

Precise kinematic forward models are important for robots to successfully perform dexterous grasping and manipulation tasks, especially when visual servoing is rendered infeasible due to occlusions. A lot of research has been conducted to estimate geometric and non-geometric parameters of kinematic chains to minimize reconstruction errors. However, kinematic chains can include non-linearities, e. g. due to cable stretch and motor-side encoders, that result in significantly different errors for different parts of the state space. Previous work either does not consider such non-linearities or proposes to estimate non-geometric parameters of carefully engineered models that are robot specific. We propose a data-driven approach that learns task error models that account for such unmodeled non-linearities. We argue that in the context of grasping and manipulation, it is sufficient to achieve high accuracy in the task relevant state space. We identify this relevant state space using previously executed joint configurations and learn error corrections for those. Therefore, our system is developed to generate subsequent executions that are similar to previous ones. The experiments show that our method successfully captures the non-linearities in the head kinematic chain (due to a counterbalancing spring) and the arm kinematic chains (due to cable stretch) of the considered experimental platform, see Fig. 1. The feasibility of the presented error learning approach has also been evaluated in independent DARPA ARM-S testing contributing to successfully complete 67 out of 72 grasping and manipulation tasks.

ICML Conference 2012 Conference Paper

Learning Force Control Policies for Compliant Robotic Manipulation

• Mrinal Kalakrishnan
• Ludovic Righetti
• Peter Pastor
• Stefan Schaal

ICRA Conference 2012 Conference Paper

Probabilistic depth image registration incorporating nonvisual information

• Manuel Wüthrich
• Peter Pastor
• Ludovic Righetti
• Aude Billard
• Stefan Schaal

In this paper, we derive a probabilistic registration algorithm for object modeling and tracking. In many robotics applications, such as manipulation tasks, nonvisual information about the movement of the object is available, which we will combine with the visual information. Furthermore we do not only consider observations of the object, but we also take space into account which has been observed to not be part of the object. Furthermore we are computing a posterior distribution over the relative alignment and not a point estimate as typically done in for example Iterative Closest Point (ICP). To our knowledge no existing algorithm meets these three conditions and we thus derive a novel registration algorithm in a Bayesian framework. Experimental results suggest that the proposed methods perform favorably in comparison to PCL [1] implementations of feature mapping and ICP, especially if nonvisual information is available.

ICRA Conference 2012 Conference Paper

Template-based learning of grasp selection

• Alexander Herzog
• Peter Pastor
• Mrinal Kalakrishnan
• Ludovic Righetti
• Tamim Asfour
• Stefan Schaal

The ability to grasp unknown objects is an important skill for personal robots, which has been addressed by many present and past research projects, but still remains an open problem. A crucial aspect of grasping is choosing an appropriate grasp configuration, i. e. the 6d pose of the hand relative to the object and its finger configuration. Finding feasible grasp configurations for novel objects, however, is challenging because of the huge variety in shape and size of these objects. Moreover, possible configurations also depend on the specific kinematics of the robotic arm and hand in use. In this paper, we introduce a new grasp selection algorithm able to find object grasp poses based on previously demonstrated grasps. Assuming that objects with similar shapes can be grasped in a similar way, we associate to each demonstrated grasp a grasp template. The template is a local shape descriptor for a possible grasp pose and is constructed using 3d information from depth sensors. For each new object to grasp, the algorithm then finds the best grasp candidate in the library of templates. The grasp selection is also able to improve over time using the information of previous grasp attempts to adapt the ranking of the templates. We tested the algorithm on two different platforms, the Willow Garage PR2 and the Barrett WAM arm which have very different hands. Our results show that the algorithm is able to find good grasp configurations for a large set of objects from a relatively small set of demonstrations, and does indeed improve its performance over time.

ICRA Conference 2011 Conference Paper

Inverse dynamics control of floating-base robots with external constraints: A unified view

• Ludovic Righetti
• Jonas Buchli
• Michael N. Mistry
• Stefan Schaal

Inverse dynamics controllers and operational space controllers have proved to be very efficient for compliant control of fully actuated robots such as fixed base manipulators. However legged robots such as humanoids are inherently different as they are underactuated and subject to switching external contact constraints. Recently several methods have been proposed to create inverse dynamics controllers and operational space controllers for these robots. In an attempt to compare these different approaches, we develop a general framework for inverse dynamics control and show that these methods lead to very similar controllers. We are then able to greatly simplify recent whole-body controllers based on operational space approaches using kinematic projections, bringing them closer to efficient practical implementations. We also generalize these controllers such that they can be optimal under an arbitrary quadratic cost in the commands.

IROS Conference 2011 Conference Paper

Learning force control policies for compliant manipulation

• Mrinal Kalakrishnan
• Ludovic Righetti
• Peter Pastor
• Stefan Schaal

Developing robots capable of fine manipulation skills is of major importance in order to build truly assistive robots. These robots need to be compliant in their actuation and control in order to operate safely in human environments. Manipulation tasks imply complex contact interactions with the external world, and involve reasoning about the forces and torques to be applied. Planning under contact conditions is usually impractical due to computational complexity, and a lack of precise dynamics models of the environment. We present an approach to acquiring manipulation skills on compliant robots through reinforcement learning. The initial position control policy for manipulation is initialized through kinesthetic demonstration. We augment this policy with a force/torque profile to be controlled in combination with the position trajectories. We use the Policy Improvement with Path Integrals (PI 2 ) algorithm to learn these force/torque profiles by optimizing a cost function that measures task success. We demonstrate our approach on the Barrett WAM robot arm equipped with a 6-DOF force/torque sensor on two different manipulation tasks: opening a door with a lever door handle, and picking up a pen off the table. We show that the learnt force control policies allow successful, robust execution of the tasks.

IROS Conference 2011 Conference Paper

Learning motion primitive goals for robust manipulation

• Freek Stulp
• Evangelos A. Theodorou
• Mrinal Kalakrishnan
• Peter Pastor
• Ludovic Righetti
• Stefan Schaal

Applying model-free reinforcement learning to manipulation remains challenging for several reasons. First, manipulation involves physical contact, which causes discontinuous cost functions. Second, in manipulation, the end-point of the movement must be chosen carefully, as it represents a grasp which must be adapted to the pose and shape of the object. Finally, there is uncertainty in the object pose, and even the most carefully planned movement may fail if the object is not at the expected position. To address these challenges we 1) present a simplified, computationally more efficient version of our model-free reinforcement learning algorithm PI 2; 2) extend PI 2 so that it simultaneously learns shape parameters and goal parameters of motion primitives; 3) use shape and goal learning to acquire motion primitives that are robust to object pose uncertainty. We evaluate these contributions on a manipulation platform consisting of a 7-DOF arm with a 4-DOF hand.

IROS Conference 2011 Conference Paper

Online movement adaptation based on previous sensor experiences

• Peter Pastor
• Ludovic Righetti
• Mrinal Kalakrishnan
• Stefan Schaal

Personal robots can only become widespread if they are capable of safely operating among humans. In uncertain and highly dynamic environments such as human households, robots need to be able to instantly adapt their behavior to unforseen events. In this paper, we propose a general framework to achieve very contact-reactive motions for robotic grasping and manipulation. Associating stereotypical movements to particular tasks enables our system to use previous sensor experiences as a predictive model for subsequent task executions. We use dynamical systems, named Dynamic Movement Primitives (DMPs), to learn goal-directed behaviors from demonstration. We exploit their dynamic properties by coupling them with the measured and predicted sensor traces. This feedback loop allows for online adaptation of the movement plan. Our system can create a rich set of possible motions that account for external perturbations and perception uncertainty to generate truly robust behaviors. As an example, we present an application to grasping with the WAM robot arm.

IROS Conference 2008 Conference Paper

Experimental study of limit cycle and chaotic controllers for the locomotion of centipede robots

• Loic Matthey
• Ludovic Righetti
• Auke Jan Ijspeert

In this contribution we present a CPG (central pattern generator) controller based on coupled Rössler systems. It is able to generate both limit cycle and chaotic behaviors through bifurcation. We develop an experimental test bench to measure quantitatively the performance of different controllers on unknown terrains of increasing difficulty. First, we show that for flat terrains, open loop limit cycle systems are the most efficient (in terms of speed of locomotion) but that they are quite sensitive to environmental changes. Second, we show that sensory feedback is a crucial addition for unknown terrains. Third, we show that the chaotic controller with sensory feedback outperforms the other controllers in very difficult terrains and actually promotes the emergence of short synchronized movement patterns. All that is done using an unified framework for the generation of limit cycle and chaotic behaviors, where a simple parameter change can switch from one behavior to the other through bifurcation. Such flexibility would allow the automatic adaptation of the robot locomotion strategy to the terrain uncertainty.

ICRA Conference 2008 Conference Paper

Pattern generators with sensory feedback for the control of quadruped locomotion

• Ludovic Righetti
• Auke Jan Ijspeert

Central Pattern Generators (CPGs) are becoming a popular model for the control of locomotion of legged robots. Biological CPGs are neural networks responsible for the generation of rhythmic movements, especially locomotion. In robotics, a systematic way of designing such CPGs as artificial neural networks or systems of coupled oscillators with sensory feedback inclusion is still missing.

IROS Conference 2007 Conference Paper

Hand placement during quadruped locomotion in a humanoid robot: A dynamical system approach

• Sarah Dégallier
• Ludovic Righetti
• Auke Jan Ijspeert

Locomotion on an irregular surface is a challenging task in robotics. Among different problems to solve to obtain robust locomotion, visually guided locomotion and accurate foot placement are of crucial importance. Robust controllers able to adapt to sensory-motor feedbacks, in particular to properly place feet on specific locations, are thus needed. Dynamical systems are well suited for this task as any online modification of the parameters leads to a smooth adaptation of the trajectories, allowing a safe integration of sensory-motor feedback. In this contribution, as a first step in the direction of locomotion on irregular surfaces, we present a controller that allows hand placement during crawling in a simulated humanoid robot. The goal of the controller is to superimpose rhythmic movements for crawling with discrete (i. e. short-term) modulations of the hand placements to reach specific marks on the ground.

IROS Conference 2007 Conference Paper

Lower body realization of the baby humanoid - 'iCub'

• Nikos G. Tsagarakis
• Francesco Becchi
• Ludovic Righetti
• Auke Jan Ijspeert
• Darwin G. Caldwell

Nowadays, the understanding of the human cognition and it application to robotic systems forms a great challenge of research. The iCub is a robotic platform that was developed within the RobotCub European project to provide the cognition research community with an open baby- humanoid platform for understanding and development of cognitive systems. In this paper we present the design requirements and mechanical realization of the lower body developed for the "iCub". In particular the leg and the waist mechanisms adopted for lower body to match the size and physical abilities of a 2 frac12 year old human baby are introduced.

ICRA Conference 2006 Conference Paper

Programmable Central Pattern Generators: an Application to Biped Locomotion Control

• Ludovic Righetti
• Auke Jan Ijspeert

We present a system of coupled nonlinear oscillators to be used as programmable central pattern generators, and apply it to control the locomotion of a humanoid robot. Central pattern generators are biological neural networks that can produce coordinated multidimensional rhythmic signals, under the control of simple input signals. They are found both in vertebrate and invertebrate animals for the control of locomotion. In this article, we present a novel system composed of coupled adaptive nonlinear oscillators that can learn arbitrary rhythmic signals in a supervised learning framework. Using adaptive rules implemented as differential equations, parameters such as intrinsic frequencies, amplitudes, and coupling weights are automatically adjusted to replicate a teaching signal. Once the teaching signal is removed, the trajectories remain embedded as the limit cycle of the dynamical system. An interesting aspect of this approach is that the learning is completely embedded into the dynamical system, and does not require external optimization algorithms. We use our system to encapsulate rhythmic trajectories for biped locomotion with a simulated humanoid robot, and demonstrate how it can be used to do online trajectory generation. The system can modulate the speed of locomotion, and even allow the reversal of direction (i. e. walking backwards). The integration of sensory feedback allows the online modulation of trajectories such as to increase the basin of stability of the gaits, and therefore the range of speeds that can be produced