Objectives
What are the algorithmic principles that would allow a robot to run through a rocky terrain, lift a couch while reaching for an object that rolled under it or manipulate a screwdriver while balancing on top of a ladder? By trying to answer these questions in CONT-ACT, we would like to understand the fundamental principles for robot locomotion and manipulation and endow robots with the robustness and adaptability necessary to efficiently and autonomously act in an unknown and changing environment. It is a necessary step towards a new technological age: ubiquitous robots capable of helping humans in an uncountable number of tasks.
Dynamic interactions of the robot with its environment through the creation of intermittent physical contacts is central to any locomotion or manipulation task. Indeed, in order to walk or manipulate an object, a robot needs to constantly physically interact with the environment and surrounding objects. Our approach to motion generation and control in CONT-ACT gives a central place to contact interactions. Our main hypothesis is that it will allow us to develop more adaptive and robust planning and control algorithms for locomotion and manipulation. The project is divided in three main objectives: 1) the development of a hierarchical receding horizon control architecture for multi-contact behaviors, 2) the development of algorithms to learn representations for motion generation through multi-modal sensing (e.g. force and touch sensing) and 3) the development of controllers based on multi-modal sensory information through optimal control and reinforcement learning.
Publications
2021
DeepQ Stepper: A Framework for Reactive Dynamic Walking on Uneven Terrain
Avadesh Meduri,
Majid Khadiv,
and Ludovic Righetti
In 2021 IEEE-RAS International Conference on Robotics and Automation (ICRA)
2021
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.
-
Reactive Balance Control for Legged Robots under Visco-Elastic Contacts
Thomas Flayols,
Andrea Del Prete,
Majid Khadiv,
Nicolas Mansard,
and Ludovic Righetti
Applied Sciences
2021
Contacts between robots and environment are often assumed to be rigid for control purposes. This assumption can lead to poor performance when contacts are soft and/or underdamped. However, the problem of balancing on soft contacts has not received much attention in the literature. This paper presents two novel approaches to control a legged robot balancing on visco-elastic contacts, and compares them to other two state-of-the-art methods. Our simulation results show that performance heavily depends on the contact stiffness and the noises/uncertainties introduced in the simulation. Briefly, the two novel controllers performed best for soft/medium contacts, whereas “inverse-dynamics control under rigid-contact assumptions” was the best one for stiff contacts. Admittance control was instead the most robust, but suffered in terms of performance. These results shed light on this challenging problem, while pointing out interesting directions for future investigation.
Variable Horizon MPC with Swing Foot Dynamics for Bipedal Walking Control
E Daneshmand,
M Khadiv,
F Grimminger,
and L Righetti
IEEE Robotics and Automation Letters
2021
In this paper, we present a novel two-level variable Horizon Model Predictive Control (VH-MPC) framework for bipedal locomotion. In this framework, the higher level computes the landing location and timing (horizon length) of the swing foot to stabilize the unstable part of the center of mass (CoM) dynamics, using feedback from the CoM states. The lower level takes into account the swing foot dynamics and generates dynamically consistent trajectories for landing at the desired time as close as possible to the desired location. To do that, we use a simplified model of the robot dynamics projected in swing foot space that takes into account joint torque constraints as well as the friction cone constraints of the stance foot. We show the effectiveness of our proposed control framework by implementing robust walking patterns on our torque-controlled and open-source biped robot, Bolt. We report extensive simulations and real robot experiments in the presence of various disturbances and uncertainties.
Impedance Optimization for Uncertain Contact Interactions Through Risk Sensitive Optimal Control
B. Hammoud,
M. Khadiv,
and L. Righetti
IEEE Robotics and Automation Letters
2021
This paper addresses the problem of computing optimal impedance schedules for legged locomotion tasks involving complex contact interactions. We formulate the problem of impedance regulation as a trade-off between disturbance rejection and measurement uncertainty. We extend a stochastic optimal control algorithm known as Risk Sensitive Control to take into account measurement uncertainty and propose a formal way to include such uncertainty for unknown contact locations. The approach can efficiently generate optimal state and control trajectories along with local feedback control gains, i.e. impedance schedules. Extensive simulations demonstrate the capabilities of the approach in generating meaningful stiffness and damping modulation patterns before and after contact interaction. For example, contact forces are reduced during early contacts, damping increases to anticipate a high impact event and tracking is automatically traded-off for increased stability. In particular, we show a significant improvement in performance during jumping and trotting tasks with a simulated quadruped robot.
High-Frequency Nonlinear Model Predictive Control of a Manipulator
Sebastien Kleff,
Avadesh Meduri,
Rohan Budhiraja,
Nicolas Mansard,
and Ludovic Righetti
In 2021 IEEE-RAS International Conference on Robotics and Automation (ICRA)
2021
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.
Leveraging Forward Model Prediction Error for Learning Control
Sarah Bechtle,
Bilal Hammoud,
Akshara Rai,
Franziska Meier,
and Ludovic Righetti
In 2021 IEEE-RAS International Conference on Robotics and Automation (ICRA)
2021
Learning for model based control can be sampleefficient 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.
Learning a Centroidal Motion Planner for Legged Locomotion
Julian Viereck,
and Ludovic Righetti
In 2021 IEEE-RAS International Conference on Robotics and Automation (ICRA)
2021
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.
Efficient Multi-Contact Pattern Generation with Sequential Convex Approximations of the Centroidal Dynamics
B Ponton,
M Khadiv,
A. Meduri,
and L Righetti
IEEE Transactions on Robotics
2021
This paper investigates the problem of efficient computation of physically consistent multi-contact behaviors. Recent work showed that under mild assumptions, the problem could be decomposed into simpler kinematic and centroidal dynamic optimization problems. Based on this approach, we propose a general convex relaxation of the centroidal dynamics leading to two computationally efficient algorithms based on iterative resolutions of second order cone programs. They op- timize centroidal trajectories, contact forces and, importantly, the timing of the motions. We include the approach in a kino- dynamic optimization method to generate full-body movements. Finally, the approach is embedded in a mixed-integer solver to further find dynamically consistent contact sequences. Extensive numerical experiments demonstrate the computational efficiency of the approach, suggesting that it could be used in a fast receding horizon control loop. Executions of the planned motions on simulated humanoids and quadrupeds and on a real quadruped robot further show the quality of the optimized motions.
2020
An Open Torque-Controlled Modular Robot Architecture for Legged Locomotion Research
F Grimminger,
A. Meduri,
M Khadiv,
J. Viereck,
M. Wüthrich,
M. Naveau,
V. Berenz,
S. Heim,
F. Widmaier,
T. Flayols,
J. Fiene,
A. Badri-Spröwitz,
and L. Righetti
IEEE Robotics and Automation Letters
2020
We present a new open-source torque-controlled legged robot system, with a low-cost and low-complexity actuator module at its core. It consists of a high-torque brushless DC motor and a low-gear-ratio transmission suitable for impedance and force control. We also present a novel foot contact sensor suitable for legged locomotion with hard impacts. A 2.2 kg quadruped robot with a large range of motion is assembled from eight identical actuator modules and four lower legs with foot contact sensors. Leveraging standard plastic 3D printing and off-the-shelf parts results in a lightweight and inexpensive robot, allowing for rapid distribution and duplication within the research community. We systematically characterize the achieved impedance at the foot in both static and dynamic scenarios, and measure a maximum dimensionless leg stiffness of 10.8 without active damping, which is comparable to the leg stiffness of a running human. Finally, to demonstrate the capabilities of the quadruped, we present a novel controller which combines feedforward contact forces computed from a kino-dynamic optimizer with impedance control of the center of mass and base orientation. The controller can regulate complex motions while being robust to environmental uncertainty.
Crocoddyl: An Efficient and Versatile Framework for Multi-Contact Optimal Control
C Mastalli,
R Budhiraja,
W.X. Merkt,
G Saurel,
B Hammoud,
M Naveau,
J Carpentier,
L Righetti,
S Vijayakumar,
and N. Mansard
In 2020 IEEE-RAS International Conference on Robotics and Automation (ICRA)
2020
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. Con- cretely, we propose two modifications to the classical DDP algo- rithm. First, the backward pass accepts infeasible state-control trajectories. Second, the rollout keeps the gaps open during the early “exploratory” iterations (as expected in multiple- shooting 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.
Walking Control Based on Step Timing Adaptation
M Khadiv,
A. Herzog,
S A A Moosavian,
and L Righetti
IEEE Transactions on Robotics
2020
Step adjustment can improve the gait robustness of biped robots; however, the adaptation of step timing is often ne- glected as it gives rise to nonconvex problems when optimized over several footsteps. In this article, we argue that it is not necessary to optimize walking over several steps to ensure gait viability and show that it is sufficient to merely select the next step timing and location. Using this insight, we propose a novel walking pattern generator that optimally selects step location and timing at every control cycle. Our approach is computationally simple compared to standard approaches in the literature, yet guarantees that any viable state will remain viable in the future. We propose a swing foot adaptation strategy and integrate the pattern generator with an inverse dynamics controller that does not explicitly control the center of mass nor the foot center of pressure. This is particularly useful for biped robots with limited control authority over their foot center of pressure, such as robots with point feet or passive ankles. Extensive simulations on a humanoid robot with passive ankles demonstrate the capabilities of the approach in various walking situations, including external pushes and foot slippage, and emphasize the importance of step timing adaptation to stabilize walking.
Learning Variable Impedance Control for Contact Sensitive Tasks
M. Bogdanovic,
M. Khadiv,
and L. Righetti
IEEE Robotics and Automation Letters
2020
Reinforcement learning algorithms have shown great success in solving different problems ranging from playing video games to robotics. However, they struggle to solve delicate robotic problems, especially those involving contact interactions. Though in principle a policy directly outputting joint torques should be able to learn to perform these tasks, in practice we see that it has difficulty to robustly solve the problem without any given structure in the action space. In this paper, we investigate how the choice of action space can give robust performance in presence of contact uncertainties. We propose learning a policy giving as output impedance and desired position in joint space and compare the performance of that approach to torque and position control under different contact uncertainties. Furthermore, we propose an additional reward term designed to regularize these variable impedance control policies, giving them interpretability and facilitating their transfer to real systems. We present extensive experiments in simulation of both floating and fixed-base systems in tasks involving contact uncertainties, as well as results for running the learned policies on a real system.
2019
Leveraging Contact Forces for Learning to Grasp
H. Merzic,
M Bogdanovic,
D Kappler,
L Righetti,
and J Bohg
In 2019 IEEE International Conference on Robotics and Automation (ICRA)
2019
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.
Efficient Humanoid Contact Planning using Learned Centroidal Dynamics Prediction
Y.-C. Lin,
B Ponton,
L. Righetti,
and D. Berenson
In 2019 IEEE International Conference on Robotics and Automation (ICRA)
2019
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.
A Robustness Analysis of Inverse Optimal Control of Bipedal Walking
J Rebula,
S Schaal,
J Finley,
and L Righetti
IEEE Robotics and Automation Letters
2019
Cost functions have the potential to provide compact and understandable generalizations of motion. The goal of Inverse Optimal Control (IOC) is to analyze an observed behavior which is assumed to be optimal with respect to an unknown cost function, and infer this cost function. Here we develop a method for characterizing cost functions of legged locomotion, with the goal of representing complex humanoid behavior with simple models. To test this methodology we simulate walking gaits of a simple 5 link planar walking model which optimize known cost functions, and assess the ability of our IOC method to recover them. In particular, the IOC method uses an iterative trajectory optimization process to infer cost function weightings consistent with those used to generate a single demonstrated optimal trial. We also explore sensitivity of the IOC to sensor noise in the observed trajectory, imperfect knowledge of the model or task, as well as uncertainty in the components of the cost function used. With appropriate modeling, these methods may help infer cost functions from human data, yielding a compact and generalizable representation of human-like motion for use in humanoid robot controllers, as well as providing a new tool for experimentally exploring human preferences.
Learning to Explore in Motion and Interaction Tasks
M Bogdanovic,
and L Righetti
In IEEE/RSJ International Conference on Intelligent Robots and Systems
2019
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.
Robust Humanoid Locomotion Using Trajectory Optimization and Sample-Efficient Learning
MH Yeganegi,
M Khadiv,
S A A Moosavian,
JJ Zhu,
A Del Prete,
and L Righetti
In IEEE-RAS International Conference on Humanoid Robots
2019
Trajectory optimization (TO) is one of the most powerful tools for generating feasible motions for humanoid robots. However, including uncertainties and stochasticity in the TO problem to generate robust motions can easily lead to intractable problems. Furthermore, since the models used in TO have always some level of abstraction, it can be hard to find a realistic set of uncertainties in the model space. In this paper we leverage a sample-efficient learning technique (Bayesian optimization) to robustify TO for humanoid locomotion. The main idea is to use data from full-body simulations to make the TO stage robust by tuning the cost weights. To this end, we split the TO problem into two phases. The first phase solves a convex optimization problem for generating center of mass (CoM) trajectories based on simplified linear dynamics. The second stage employs iterative Linear-Quadratic Gaussian (iLQG) as a whole-body controller to generate full body control inputs. Then we use Bayesian optimization to find the cost weights to use in the first stage that yields robust performance in the simulation/experiment, in the presence of different disturbance/uncertainties. The results show that the proposed approach is able to generate robust motions for different sets of disturbances and uncertainties.
Curious iLQR: Resolving Uncertainty in Model-based RL
S Bechtle,
Y Lin,
A Rai,
L Righetti,
and F Meier
In Proceedings of the Conference on Robot Learning
2019
Curiosity as a means to explore during reinforcement learning problems has recently become very popular. However, very little progress has been made in utilizing curiosity for learning control. In this work, we propose a model-based reinforcement learning (MBRL) framework that combines Bayesian modeling of the system dynamics with curious iLQR, an iterative LQR approach that considers model uncertainty. During trajectory optimization the curious iLQR attempts to minimize both the task-dependent cost and the uncertainty in the dynamics model. We demonstrate the approach on reaching tasks with 7-DoF manipulators in simulation and on a real robot. Our experiments show that MBRL with curious iLQR reaches desired end-effector targets more reliably and with less system rollouts when learning a new task from scratch, and that the learned model generalizes better to new reaching tasks.
2018
Learning a Structured Neural Network Policy for a Hopping Task.
Julian Viereck,
Jules Kozolinsky,
Alexander Herzog,
and Ludovic Righetti
IEEE Robotics and Automation Letters
2018
In this letter, we present a method for learning a re- active policy for a simple dynamic locomotion task involving hard impact and switching contacts where we assume the contact lo- cation and contact timing to be unknown. To learn such a policy, we use optimal control to optimize a local controller for a fixed environment and contacts. We learn the contact-rich dynamics for our underactuated systems along these trajectories in a sample ef- ficient manner. We use the optimized policies to learn the reactive policy in form of a neural network. Using a new neural network architecture, we are able to preserve more information from the local policy and make its output interpretable in the sense that its output in terms of desired trajectories, feedforward commands and gains can be interpreted. Extensive simulations demonstrate the robustness of the approach to changing environments, outper- forming a model-free gradient policy based methods on the same tasks in simulation. Finally, we show that the learned policy can be robustly transferred on a real robot.
An MPC Walking Framework With External Contact Forces
Sean Mason,
Nicholas Rotella,
Stefan Schaal,
and Ludovic Righetti
In 2018 IEEE International Conference on Robotics and Automation (ICRA)
2018
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 (\textless; 1kHz). Simulations show that the presented walking control scheme can withstand disturbances 2-3× larger with the additional force provided by a hand contact.
On Time Optimization of Centroidal Momentum Dynamics
Brahayam Ponton,
Alexander Herzog,
Andrea Del Prete,
Stefan Schaal,
and Ludovic Righetti
In 2018 IEEE International Conference on Robotics and Automation (ICRA)
2018
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.
Unsupervised Contact Learning for Humanoid Estimation and Control
Nicholas Rotella,
Stefan Schaal,
and Ludovic Righetti
In 2018 IEEE International Conference on Robotics and Automation (ICRA)
2018
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.
Learning Task-Specific Dynamics to Improve Whole-Body Control
Andrej Gams,
Sean Mason,
Ales Ude,
Stefan Schaal,
and Ludovic Righetti
In 2018 IEEE-RAS 18th International Conference on Humanoid Robots (Humanoids)
2018
In task-based inverse dynamics control, reference accelerations used to follow a desired plan can be broken down into feedforward and feedback trajectories. The feedback term accounts for tracking errors that are caused from inaccurate dynamic models or external disturbances. On underactuated, free-floating robots, such as humanoids, high feedback terms can be used to improve tracking accuracy; however, this can lead to very stiff behavior or poor tracking accuracy due to limited control bandwidth. In this paper, we show how to reduce the required contribution of the feedback controller by incorporating learned task-space reference accelerations. Thus, we i) improve the execution of the given specific task, and ii) offer the means to reduce feedback gains, providing for greater compliance of the system. With a systematic approach we also reduce heuristic tuning of the model parameters and feedback gains, often present in real-world experiments. In contrast to learning task-specific joint-torques, which might produce a similar effect but can lead to poor generalization, our approach directly learns the task-space dynamics of the center of mass of a humanoid robot. Simulated and real-world results on the lower part of the Sarcos Hermes humanoid robot demonstrate the applicability of the approach.
2017
Pattern Generation for Walking on Slippery Terrains
Majid Khadiv,
S A A Moosavian,
Alexander Herzog,
and Ludovic Righetti
In 2017 5th RSI International Conference on Robotics and Mechatronics (ICRoM)
2017
In this paper, we extend state of the art Model
Predictive Control (MPC) approaches to generate safe bipedal
walking on slippery surfaces. In this setting, we formulate
walking as a trade off between realizing a desired walking velocity
and preserving robust foot-ground contact. Exploiting this for-
mulation inside MPC, we show that safe walking on various flat
terrains can be achieved by compromising three main attributes,
i. e. walking velocity tracking, the Zero Moment Point (ZMP)
modulation, and the Required Coefficient of Friction (RCoF)
regulation. Simulation results show that increasing the walking
velocity increases the possibility of slippage, while reducing the
slippage possibility conflicts with reducing the tip-over possibility
of the contact and vice versa.
Momentum-Centered Control of Contact Interactions
Ludovic Righetti,
and Alexander Herzog
2017
The control and planning of interaction forces is fundamental for locomo- tion and manipulation tasks since it is through the interaction with the environment that a robot can walk forward or manipulate objects. In this chapter we present a control and planning strategy focused on the control of interaction forces to gen- erate multi-contact whole-body behaviors. Centered around the robot momentum dynamics, our approach consists of a hierarchical inverse dynamics controller that treats the control of the robot’s momentum as a contact force task and a trajectory optimization algorithm that can generate desired whole-body motions, momentum and desired contact forces for multiple contacts. Experimental results demonstrate the capabilities of the approach on a humanoid robot.
2016
On the Effects of Measurement Uncertainty in Optimal Control of Contact Interactions
B Ponton,
S. Schaal,
and L. Righetti
In Algorithmic Foundations of Robotics XII: Proceedings of the Twelfth Workshop on the Algorithmic Foundations of Robotics
2016
Stochastic Optimal Control (SOC) typically considers noise
only in the process model, i.e. unknown disturbances. However, in many
robotic applications involving interaction with the environment, such as
locomotion and manipulation, uncertainty also comes from lack of precise
knowledge of the world, which is not an actual disturbance. We analyze
the effects of also considering noise in the measurement model, by devel-
oping a SOC algorithm based on risk-sensitive control, that includes the
dynamics of an observer in such a way that the control law explicitly de-
pends on the current measurement uncertainty. In simulation results on a
simple 2D manipulator, we have observed that measurement uncertainty leads to low impedance behaviors, a result in contrast with the effects of
process noise that creates stiff behaviors. This suggests that taking into account measurement uncertainty could be a potentially very interesting
way to approach problems involving uncertain contact interactions.
Inertial Sensor-Based Humanoid Joint State Estimation
N Rotella,
S Mason,
S. Schaal,
and L. Righetti
In 2016 IEEE International Conference on Robotics and Automation (ICRA)
2016
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.
Structured contact force optimization for kino-dynamic motion generation
Alexander Herzog,
Stefan Schaal,
and Ludovic Righetti
In 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)
2016
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.
Stepping Stabilization Using a Combination of DCM Tracking and Step Adjustment
Majid Khadiv,
Sebastien Kleff,
Alexander Herzog,
S A A Moosavian,
Stefan Schaal,
and Ludovic Righetti
In 2016 4th International Conference on Robotics and Mechatronics (ICROM)
2016
In this paper, a method for stabilizing biped robots stepping by a combination of Divergent Component of Motion (DCM) tracking and step adjustment is proposed. In this method, the DCM trajectory is generated, consistent with the predefined footprints. Furthermore, a swing foot trajectory modification strategy is proposed to adapt the landing point, using DCM measurement. In order to apply the generated trajectories to the full robot, a Hierarchical Inverse Dynamics (HID) is employed. The HID enables us to use different combinations of the DCM tracking and step adjustment for stabilizing different biped robots. Simulation experiments on two scenarios for two different simulated robots, one with active ankles and the other with passive ankles, are carried out. Simulation results demonstrate the effectiveness of the proposed method for robots with both active and passive ankles.
Balancing and Walking Using Full Dynamics LQR Control With Contact Constraints
S Mason,
N Rotella,
S. Schaal,
and L. Righetti
In 2016 IEEE-RAS 16th International Conference on Humanoid Robots (Humanoids)
2016
Torque control algorithms which consider robot dynamics and contact constraints are important for creating dynamic behaviors for humanoids. As computational power increases, algorithms tend to also increase in complexity. However, it is not clear how much complexity is really required to create controllers which exhibit good performance. In this paper, we study the capabilities of a simple approach based on contact consistent LQR controllers designed around key poses to control various tasks on a humanoid robot. We present extensive experimental results on a hydraulic, torque controlled humanoid performing balancing and stepping tasks. This feedback control approach captures the necessary synergies between the DoFs of the robot to guarantee good control performance. We show that for the considered tasks, it is only necessary to re-linearize the dynamics of the robot at different contact configurations and that increasing the number of LQR controllers along desired trajectories does not improve performance. Our result suggest that very simple controllers can yield good performance competitive with current state of the art, but more complex, optimization-based whole-body controllers. A video of the experiments can be found at https://youtu.be/5T08CNKV1hw.
A Convex Model of Momentum Dynamics for Multi-Contact Motion Generation
Brahayam Ponton,
Alexander Herzog,
Stefan Schaal,
and Ludovic Righetti
In 2016 IEEE-RAS 16th International Conference on Humanoid Robots Humanoids
2016
Linear models for control and motion generation of humanoid robots have received significant attention in the past years, not only due to their well known theoretical guarantees, but also because of practical computational advantages. However, to tackle more challenging tasks and scenarios such as locomotion on uneven terrain, a more expressive model is required. In this paper, we are interested in contact interaction-centered motion optimization based on the momentum dynamics model. This model is non-linear and non-convex; however, we find a relaxation of the problem that allows us to formulate it as a single convex quadratically-constrained quadratic program (QCQP) that can be very efficiently optimized and is useful for multi-contact planning. This convex model is then coupled to the optimization of end-effector contact locations using a mixed integer program, which can also be efficiently solved. This becomes relevant e.g. to recover from external pushes, where a predefined stepping plan is likely to fail and an online adaptation of the contact location is needed. The performance of our algorithm is demonstrated in several multi-contact scenarios for a humanoid robot.
Step Timing Adjustement: a Step toward Generating Robust Gaits
M Khadiv,
A. Herzog,
S A A Moosavian,
and L. Righetti
In 2016 IEEE-RAS 16th International Conference on Humanoid Robots (Humanoids)
2016
Step adjustment for humanoid robots has been shown to improve robustness in gaits. However, step duration adaptation is often neglected in control strategies. In this paper, we propose an approach that combines both step location and timing adjustment for generating robust gaits. In this approach, step location and step timing are decided, based on feedback from the current state of the robot. The proposed approach is comprised of two stages. In the first stage, the nominal step location and step duration for the next step or a previewed number of steps are specified. In this stage which is done at the start of each step, the main goal is to specify the best step length and step duration for a desired walking speed. The second stage deals with finding the best landing point and landing time of the swing foot at each control cycle. In this stage, stability of the gaits is preserved by specifying a desired offset between the swing foot landing point and the Divergent Component of Motion (DCM) at the end of current step. After specifying the landing point of the swing foot at a desired time, the swing foot trajectory is regenerated at each control cycle to realize desired landing properties. Simulation on different scenarios shows the robustness of the generated gaits from our proposed approach compared to the case where no timing adjustment is employed.
2015
Trajectory generation for multi-contact momentum control
A. Herzog,
N Rotella,
S. Schaal,
and L. Righetti
In 2015 IEEE-RAS 15th International Conference on Humanoid Robots (Humanoids)
2015
Simplified models of the dynamics such as the linear inverted pendulum model (LIPM) have proven to perform well for biped walking on flat ground. However, for more complex tasks the assumptions of these models can become limiting. For example, the LIPM does not allow for the control of contact forces independently, is limited to co-planar contacts and assumes that the angular momentum is zero. In this paper, we propose to use the full momentum equations of a humanoid robot in a trajectory optimization framework to plan its center of mass, linear and angular momentum trajectories. The model also allows for planning desired contact forces for each end-effector in arbitrary contact locations. We extend our previous results on linear quadratic regulator (LQR) design for momentum control by computing the (linearized) optimal momentum feedback law in a receding horizon fashion. The resulting desired momentum and the associated feedback law are then used in a hierarchical whole body control approach. Simulation experiments show that the approach is computationally fast and is able to generate plans for locomotion on complex terrains while demonstrating good tracking performance for the full humanoid control.
Humanoid Momentum Estimation Using Sensed Contact Wrenches
N Rotella,
A. Herzog,
S. Schaal,
and L. Righetti
In 2015 IEEE-RAS 15th International Conference on Humanoid Robots (Humanoids)
2015
This work presents approaches for the estimation of quantities important for the control of the momentum of a humanoid robot. In contrast to previous approaches which use simplified models such as the Linear Inverted Pendulum Model, we present estimators based on the momentum dynamics of the robot. By using this simple yet dynamically-consistent model, we avoid the issues of using simplified models for estimation. We develop an estimator for the center of mass and full momentum which can be reformulated to estimate center of mass offsets as well as external wrenches applied to the robot. The observability of these estimators is investigated and their performance is evaluated in comparison to previous approaches.