Introduction

Whole-Body Control

Whole Body Control (WBC), also referred to as constraint-based or optimization-based control, is an approach for specifying and controlling complex robotic tasks. The term was coined by Luis Sentis in his work Synthesis and Control of Whole-Body Behaviors in Humanoid Systems.

Image Credits: Dennis Mronga, DFKI

The idea is define a set of feedback controllers around an optimization problem. Each controller regulates a certain task, the control output is fed into the cost function of the optimization problem, typically a quadratic program, and thus minimized during execution. In each control cycle …

• The cost function is updated with the current robot state and controller reference
• The optimization problem is solved
• The solution is applied to the actuators of the robot

WBC is used for…

• controlling robots with redundant degrees of freedom, like humanoids or other legged robots with floating base, but also fixed-base systems like mobile manipulators, dual-arm systems or even simple manipulators. In general, the number of robot dof can be arbitraryily high like >50.

• controlling multiple tasks simultaneously while taking into account the physical constraints of the robot. E.g., on a humanoid robot do … (1) keep balance (2) Grasp an object with one arm (3) maintaining an upright body posture (4) Consider the joint torque limits, etc…

• reactive robot control, i.e., it does not involve any motion planning or trajectory optimization. However, it can be used to stabilize trajectories coming from a motion planner or trajectory optimizer and integrate them with other objectives and physical constraints of the robot. Other than in MPC, the optimization horizon has the size 1, i.e., there is no prediction model involved.

ARC-OPT: Motivation

ARC-OPT is a framework for optimization-based control of redundant robots. It contains various implementations of whole-body feedback control approaches on velocity-, acceleration- and force/torque-level. The core WBC library is written in C++, with Python bindings for most functionalilities. It aims at facilitating the specification and benchmarking of whole-body controllers for redundant robots, i.e., the target user group are software developers and control engineers. Compared to existing frameworks for optimization-based robot control, ARC-OPT provides

• A common interface to several WBC approaches on velocity, acceleration, and torque control level, using different solvers and robot models
• A learning module that allows to automatically derive, adapt, and optimize WBC tasks (will made be open-source soon)
• An approach for modeling and solving WBC problems on series-parallel hybrid robots, as described in this paper (will made be open-source soon)

Concepts

The design of WBC library separates the whole-body controller into 4 main building blocks, namely controllers(s), robot model, scene and solver. Robot models, scenes and solvers are implemented as plugins, which can be exchanged.

• Controller: A controller implements a task space function, e.g., maintain a certain contact force, follow a trajectory or avoid an obstacle. Each controller can be designed either in task or joint space of the robot. Thus, depending on the implementation, the input of a controller can be a target pose, twist or spatial acceleration (task space), as well as a joint position, velocity or acceleration (joint space). The control output, which is passed to the scene, describes the error of the task function, which is minimized during execution. All controllers are agnostic of the robot kinematics and dynamics, as well as the underlying WBC implementation. The available controllers can be found here.

• Scene: The scene sets up a quadratic program (QP). Each scene has a set of tasks, which are defined as part of the cost function, and a set of constraints, which are the physical constraints of the robot. In each control cycle, the scene is updated with the current robot kinematics/dynamics and the control outputs of all tasks. The output of the scene is the QP. The available scenes can be found here. Each scene inherits from core/Scene.
  • Task: A task is essentially a summand in the cost function of the QP. The input of a task (task reference) is the control output of a controller. Thus, the control error is minimized by minimizing the cost function. Each task inherits from core/Task and is paramerized by core/TaskConfig. There are currently three types of tasks: Joint Space tasks, Cartesian tasks or CoM Tasks. Each scene can have an arbitrary number of tasks.
  • Task weights: The importance of each task can be governed by either strict or soft priorities, where the latter are called task weights here. AThe solution of the QP will be a weighted sum of all task constributions, where tasks with higher tasks weights will be preferred. task weight of zero means that the corresponding task variable will be ignored in the solution, e.g., a certain direction in tasks space.
  • Task priority: In contrast to task weights, task priorities implement a strict task hierarchy, where tasks with lower priority will be executed in the Nullspace of the tasks with higher prioritiy. As a result, the task with highest priority will be executed first, the tasks with lower priority only if enough redundant DoF are available, and so on. Note that the only scene/solver combination that currently allows task prioritization is (velocity/hls).
  • Joint Weights: Similar as task weights, joint weights govern the importance of each joint in the solution. A joint weight of zero means that the joint will not be used at all.
  • Constraint: Constraints described the physical limitations of the robot and/or the environment, e.g., torque limits, ground friction or self-collisions. Each constraint inherits from core/Constraint. Currently the constraints a hard-coded in the Scene implementation. Thus, if you want to use different constraints, you will have to implement a new Scene.
• Robot Model: The robot model computes the kinematic and dynamic information that the scene requires to set up the optimization problem. This includes different Jacobians and their derivatives, frame transformations, gravity forces and torques, as well as mass-inertia matrices. The robot model is updated in each control cycle with the current joint status (position, velocity, acceleration) of the robot. Each robot model inherits from core/RobotModel. The available robot models can be found here.
• Solver: The solver is a generic component that solves a variant of a QP. The input is a QP, the output is a velocity, acceleration or torque command in configuration space of the robot. Each solver inherits from core/QPSolver. The available solver can be found here.

Installation

• WBC Library
• ROS2 Interface
• Rock Interface

Testing

To execute unit tests for the WBC library, run

sh scripts/run_tests.sh

from the library’s root folder. This will execute unit tests for all installed components, e.g. solvers, robot models, etc…

WBC Library Tutorials

Velocity-based WBC

1. Introductory example
2. Using a different solver
3. Adapting task and joint weights
4. Task hierarchies
5. Serial vs. Hybrid robots
6. Floating base robots

Acceleration-based WBC

1. Serial Robot
2. Hybrid Robot

ROS2 Tutorials

1. Introduction
2. Cartesian Space Example
3. Joint Space Example
4. Nullspace Example

Publications

D. Mronga, S. Kumar and F. Kirchner, “Whole-Body Control of Series-Parallel Hybrid Robots”, 2022 International Conference on Robotics and Automation (ICRA), 2022, pp. 228-234

D. Mronga and F. Kirchner, “Learning context-adaptive task constraints for robotic manipulation”, Robotics and Autonomous Systems, Volume 141, 2021

D. Mronga, T. Knobloch, J. de Gea Fernández & F. Kirchner, “A constraint-based approach for human–robot collision avoidance”, Advanced Robotics, Volume 34, Issue 5, pp. 265-281, 2020

D. Mronga, “Learning Task Constraints for Whole-Body Control of Robotic Systems”, PhD Thesis, 2022, University of Bremen