1. Bibliographic Information

1.1. Title

Whole-body End-Effector Pose Tracking

1.2. Authors

Tifanny Portela (ETH Zurich, ANYbotics), Andrei Cramariuc (ETH Zurich), Mayank Mittal (ETH Zurich, NVIDIA), and Marco Hutter (ETH Zurich). The authors are affiliated with the Robotic Systems Lab at ETH Zurich, a world-leading institution in legged robotics and autonomous systems.

1.3. Journal/Conference

This paper was published as a preprint on arXiv (2024). Given its affiliation with the Robotic Systems Lab and its focus on whole-body control, it is characteristic of high-impact robotics research typically presented at venues like the International Conference on Robotics and Automation (ICRA) or the Conference on Robot Learning (CoRL).

1.4. Publication Year

2024 (Original submission: September 2024).

1.5. Abstract

The paper addresses the challenge of integrating robotic arms with legged robots (mobile manipulation). While legged robots offer high mobility, adding an arm introduces complexity that makes precise control difficult. Existing methods often rely on simplified models or limit the arm's workspace. The authors propose a Reinforcement Learning (RL) whole-body formulation for 6-DoF (Degrees of Freedom) end-effector pose tracking (position and orientation) across large, unstructured workspaces. Their method uses a terrain-aware sampling strategy and a game-based curriculum to train the robot on rough terrains like stairs. Experiments on the ANYmal quadruped robot with a 6-DoF arm show precise tracking with a pose error of 2.64 cm and 3.64°, outperforming traditional and learning-based baselines.

1.6. Original Source Link

• PDF Link: https://arxiv.org/pdf/2409.16048v2.pdf

• Publication Status: Preprint (v2).

  ---

2. Executive Summary

2.1. Background & Motivation

Legged robots have become adept at traversing difficult terrains (stairs, slopes, rocks), but their use is often limited to passive tasks like inspection. To be truly useful, they need to manipulate objects. However, combining a legged base with an arm (a mobile manipulator) is difficult because:

1. Redundancy: There are many ways to reach a target (moving the arm, moving the legs, or shifting the body).

2. Dynamics: The arm's movement affects the base's balance, and vice-versa.

3. Terrain: Maintaining precision while standing on uneven ground or stairs is significantly harder than on a flat laboratory floor.

   Prior work using Model Predictive Control (MPC)—which uses mathematical models to predict future movements—is often brittle when the environment doesn't match the model (e.g., slipping). Existing Reinforcement Learning (RL) approaches often focus only on the arm's position, ignoring orientation, or restrict the arm to a small area in front of the robot.

2.2. Main Contributions / Findings

• Unified RL Controller: A single neural network policy that controls all 18 joints (12 for the legs, 6 for the arm) to track a target pose.

• Keypoint-based Pose Representation: Instead of using complex angles or quaternions for the target, the authors represent the pose using the 3D coordinates of a virtual cube's corners. This makes it easier for the AI to learn.

• Terrain-Aware Sampling: A strategy to ensure the robot is trained on reachable targets even on stairs, preventing the robot from trying to reach "impossible" targets (e.g., inside the ground).

• Superior Accuracy: The system achieves higher precision (2.64 cm / 3.64°) than previous methods and maintains this precision on challenging terrains like mattresses and slopes.

  ---

3. Prerequisite Knowledge & Related Work

3.1. Foundational Concepts

To understand this paper, a beginner should be familiar with the following:

• End-Effector: The "hand" or tool at the end of a robotic arm. In this paper, it is the tip of the 6-DoF Dynaarm.
• 6-DoF (Degrees of Freedom): Refers to the ability to move in 3D space (X, Y, Z position) and rotate in 3D space (Roll, Pitch, Yaw orientation).
• Pose: The combination of position and orientation.
• Whole-Body Control (WBC): A control strategy where the movements of the legs and the arm are coordinated simultaneously, rather than treating them as separate systems.
• Reinforcement Learning (RL): A machine learning approach where an "agent" (the robot) learns to make decisions by trial and error in a simulator, receiving "rewards" for good actions and "penalties" for bad ones.
• Proprioception: The robot's internal sense of its own body, such as joint angles, speeds, and the orientation of its torso (measured by an Inertial Measurement Unit or IMU).

3.2. Previous Works

• Model-Based (MPC): Works like Sleiman et al. [5] use optimization to solve for the best joint movements. While precise, they struggle with "unmodeled" disturbances like soft ground.
• Learning-Based (RL):
  • Fu et al. [9]: Taught a robot to use its whole body for manipulation but struggled with orientation tracking and rough terrain.
  • Ha et al. [26]: Achieved good accuracy but mostly for movements directly in front of the robot, which doesn't require much leg movement.

3.3. Technological Evolution

Robotics has moved from Fixed-Base Manipulation (factory arms) to Wheel-Based Mobile Manipulation (smooth floors) and now to Legged Mobile Manipulation (unstructured environments). This paper represents the cutting edge of Legged Mobile Manipulation, moving beyond "just walking" or "just reaching" to "precise reaching while standing on complex surfaces."

3.4. Differentiation Analysis

The core difference is the representation of the goal. Most researchers use Quaternions (a 4D math representation of rotation) which are hard for neural networks to process because they are not "continuous" (small changes in math can mean huge changes in rotation). By using Keypoints, this paper provides a smooth, continuous target that allows the robot to learn both position and orientation at the same time without the "conflict" often seen in other RL methods.

---

4. Methodology

4.1. Principles

The core idea is to train an RL policy that treats the entire robot (18 joints) as a single system. The robot is given a target pose for its hand and must figure out how to stand and move its arm to reach that target while maintaining balance on various terrains.

4.2. Core Methodology In-depth

4.2.1. Command Sampling and Workspace Expansion

The robot needs to know which targets are reachable.

1. Initial Workspace: The authors first calculate 10,000 poses that the arm can reach without moving the base.

2. Expanded Workspace: To make the robot use its whole body, they apply a random transformation $T_{b}^{\Delta} \in \mathbb{R}^{6}$ to these poses. This shift covers $[-0.2, 0.2]$ m in X/Y, $[-0.3, 0.1]$ m in Z, and rotations of up to $\pm 30^{\circ}$ (or $\pi/6$ radians). This forces the robot to bend its legs or tilt its body to reach the goal.

3. Terrain Awareness: On stairs, some targets might be under the steps. The system uses a coarse height map to identify these and resample them with an 8 cm safety margin above the ground.

   The following figure (Figure 2 from the original paper) illustrates the training process and sampling strategy:

   该图像是示意图，展示了全身末端执行器姿态跟踪的训练流程与数据收集方法。图中分别展示了粗糙地形高度图（A）、采样的姿态指令（B）及机器人初始配置（C）。该流程通过地形碰撞检查等步骤，优化命令采样，以提高跟踪精度。

4.2.2. Keypoint Pose Representation

Instead of a target position (x, y, z) and a rotation (quaternion), the command is defined as 3 keypoints. Imagine a cube attached to the robot's hand. The positions of 3 of its corners in the robot's base frame uniquely define where the hand is and how it is tilted.

• Command: $p_{ee}^{cmd} = [p_1, p_2, p_3] \in \mathbb{R}^{9}$.
• This unified representation avoids the "units problem" (mixing meters and radians in a single reward).

4.2.3. Action and Observation Space

The policy runs at 50 Hz.

• Observations ($o^t$): The robot "sees" its state: $o^{t} = [g_{b}^{t}, v_{b}^{t}, q^{t}, a^{t-1}] \in \mathbb{R}^{45}$ Where $g_{b}^{t}$ is gravity in the base frame, $v_{b}^{t}$ are velocities, $q^{t}$ are joint positions, and $a^{t-1}$ are the previous actions.
• Actions ($a^t$): The policy outputs targets for a lower-level controller: $q_{target} = \sigma_{a} a^{t} + q_{def}$ Where $\sigma_{a} = 0.5$ and $q_{def}$ is the default "ready" stance.

4.2.4. Reward Function

The robot learns via a reward $R = R_T + R_P$.

• Tracking Reward ($R_t$): This is a delayed reward. It only starts giving points in the last 2 seconds of a 4-second command window. This allows the robot to take any path it wants to the target without being penalized for the path itself. $R_{t} = \begin{cases} \frac{1}{T_{r}} \sum_{k=0}^{3} e^{\frac{1}{\sigma_{t}} \lVert ^{b} p_{ee,k}^{meas} - {}^{b} p_{ee,k}^{cmd} \rVert_{2}} & \text{if } t > T - T_{r} \\ 0 & \text{otherwise} \end{cases}$ Here, $T_r = 2$s and $\sigma_t = 0.05$. It measures the distance between measured (meas) and commanded (cmd) keypoints.

• Progress Reward ($R_p$): Since the tracking reward is delayed (sparse), $R_p$ provides a "breadcrumb trail" by rewarding the robot for getting closer to the target than it has ever been before in that episode. $R_{p} = \begin{cases} \frac{1}{3} \sum_{k=0}^{3} (d_{k} - d_{k}^{t}) & \text{if } d^{t} < d \\ 0 & \text{otherwise} \end{cases}$ Where $d^t$ is current distance and $d$ is the best previous distance.

• Stability Rewards: $R_f$ rewards keeping all four feet on the ground, and $R_q$ rewards keeping legs in a natural configuration sampled from a locomotion policy.

• Penalties ($R_P$): Penalizes high torques, jerky movements, and joint limit violations.

4.2.5. Curriculum and Initialization

Training starts on flat ground. As the robot improves (error $< 20$ cm and $20^{\circ}$), the terrain gets harder (rough ground $\rightarrow$ stairs). To ensure the robot can switch from walking to reaching without falling, they initialize the training episodes using configurations from a real locomotion policy. This "warms up" the robot into a stable standing state.

---

5. Experimental Setup

5.1. Datasets & Platform

• Robot: ANYmal D (quadruped) + Dynaarm (6-DoF arm).
• Simulation: Isaac Lab (built on NVIDIA Isaac Gym), which allows simulating thousands of robots in parallel on a single GPU.
• Data: 10,000 pre-sampled poses representing the reachable workspace.

5.2. Evaluation Metrics

1. Position Error ($\bar{e}_p$): The Euclidean distance between the target and actual end-effector position.
   • Formula: $\lVert p_{actual} - p_{target} \rVert_2$ (in cm).
2. Orientation Error ($\bar{e}_o$): The angular difference between the target and actual rotation.
   • Formula: Derived from axis-angle representation of the rotation difference (in degrees).

5.3. Baselines

The authors compared their keypoint method against:

• RL with Quaternions: Standard 4D rotation representation.

• RL with Euler Angles: Roll, Pitch, Yaw.

• RL with 6D Representation: A continuous rotation representation from [27].

• Model-Based MPC: A state-of-the-art optimization controller [19].

  ---

6. Results & Analysis

6.1. Core Results Analysis

The keypoint-based RL outperformed all other representations. While 6D was the second best, Quaternions and Euler angles often failed to converge because the math for rotation is "discontinuous" (e.g., jumping from $359^{\circ}$ to $0^{\circ}$ is a small physical change but a large numerical one).

The following figure (Figure 4 from the original paper) shows how different representations affect error distribution:

该图像是一个图表，展示了在平坦地形上对10000个末端执行器姿态命令的位置信息和方向误差的分布情况。图中分别显示了四种不同姿态表示法（关键点、四元数、欧拉角和六维）的误差密度随位置误差和方向误差的变化。

6.2. Data Presentation (Tables)

The following are the results from Table I of the original paper, showing how the controller handles extra weight (payload) on its hand in simulation:

Analysis: The robot was trained with weights up to 2.0 kg. It remains very accurate up to 3.0 kg, but accuracy drops sharply after 3.5 kg, showing the limits of its "extrapolation" capabilities.

6.3. Hardware Validation

On the real ANYmal robot, the results were impressive:

• Flat Terrain: 2.03 cm / 2.86° error.

• Stairs: 2.64 cm / 3.64° error.

• Robustness: The robot successfully reached targets while standing on a soft mattress and sideways on stairs, which are notoriously difficult for model-based controllers.

  The following figure (Figure 5 from the original paper) shows the error distribution on real hardware:

  该图像是图表，展示了20个末端执行器位置和方向误差的分布，分别在平坦地形和楼梯上测量。左侧为位置误差（单位：cm），右侧为方向误差（单位：度），并显示了不同地形下的误差分布密度。

---

7. Conclusion & Reflections

7.1. Conclusion Summary

This paper successfully demonstrates that a whole-body RL policy can achieve "millimeter-level" precision (when considering the scale of the robot) for both position and orientation. By using a keypoint-based representation and terrain-aware training, the authors solved the common issues of restricted workspaces and poor orientation tracking. The system is robust enough to handle various terrains and unmodeled payloads.

7.2. Limitations & Future Work

• Self-Collision: While the training samples are collision-free, the robot doesn't have an active sensor (like a camera) to "see" and avoid new obstacles or its own body during transitions in real-time.
• Payload Limits: The accuracy degrades significantly once the payload exceeds the training range.
• Future Directions: Incorporating Long Short-Term Memory (LSTM) networks to better handle dynamic changes in payload and using 3D environment representations (point clouds) for obstacle avoidance.

7.3. Personal Insights & Critique

The most brilliant part of this work is the delayed reward ($R_t$). In many RL papers, researchers struggle with "reward shaping"—trying to find a balance between the robot reaching the goal and the robot moving "nicely." By only rewarding the final state, the authors simplify the problem significantly, letting the RL figure out the best path on its own.

However, a potential issue is the reliance on a separate locomotion policy for initialization. While this ensures a smooth transition, it means the controller is "static" (it doesn't walk while reaching). In real-world scenarios, a robot might need to take a step forward to reach just a few centimeters further; currently, this system would reach its joint limits instead. Integrating "reaching-driven locomotion" (walking because you need to reach) would be the next logical step.