Curve-Induced Dynamical Systems on Riemannian Manifolds and Lie Groups

This paper introduces Curve-induced Dynamical Systems on Smooth Manifolds (CDSM), a real-time framework that generates stable and adaptable robotic behaviors on Riemannian manifolds and Lie groups by constructing dynamical systems with tangential and normal components relative to a nominal curve, demonstrating superior accuracy and efficiency in both benchmarks and practical robotic applications.

The Gist

Imagine you are teaching a robot to help you get dressed. You show it how to move its arm to pull a sleeve over your shoulder. But what happens if you suddenly push the robot, or if you move your arm slightly differently than before? A rigid robot might crash, stop, or get confused.

This paper introduces a new "brain" for robots called CDSM (Curve-Induced Dynamical Systems on Smooth Manifolds). Think of it as giving the robot a smart, elastic rubber band that guides its movements, rather than a rigid metal track.

Here is the breakdown of how it works, using simple analogies:

1. The Problem: Robots Live on Curved Worlds

Most computer programs assume the world is flat (like a sheet of graph paper). But in robotics, the world is often curved.

• The Analogy: Imagine trying to draw a straight line on a basketball. You can't just draw a straight line; you have to follow the curve of the ball.
• The Reality: A robot's "hand" (its pose) and its "stiffness" (how hard or soft it pushes) exist on these curved mathematical surfaces (called Manifolds and Lie Groups). If you treat them like flat paper, the robot makes geometric errors, like trying to walk in a straight line on a globe and ending up in the wrong country.

2. The Solution: The "Rubber Band" Guide

Instead of forcing the robot to follow a single, rigid path, CDSM creates a nominal curve (a reference path) based on the human's demonstration.

• The Analogy: Imagine a hiker walking along a mountain trail.
  • The Tangential Force (Moving Forward): This is like the hiker's legs. It pushes the robot along the trail, keeping it moving toward the goal.
  • The Normal Force (Staying on Track): This is like a strong, elastic rubber band attached to the hiker and the trail. If the hiker slips off the path (due to a push or a stumble), the rubber band yanks them back onto the trail.
• The Magic: The robot doesn't just memorize the path; it learns the shape of the path. If you push it, it wobbles but immediately snaps back to the correct curve and keeps going.

3. The "Smart Stiffness" (Variable Damping)

The paper also introduces a way for the robot to change how "stiff" or "soft" it is depending on where it is.

• The Analogy: Think of a person putting on a tight sweater.
  • Early Stage (Soft): When the robot is far away from your arm, it moves gently and softly. It doesn't want to bump into you.
  • Late Stage (Stiff): As it gets close to your shoulder (a tight spot), it becomes stiffer and more precise to ensure the sleeve goes on correctly.
• How it works: The system looks at the data. If the human moved their arm in many different ways at a certain point (high uncertainty), the robot becomes "softer" (more compliant). If the human was very consistent (low uncertainty), the robot becomes "stiffer" (more precise).

4. The "Speed Dial" (Phase Modulation)

Sometimes you want the robot to do the same motion, but faster or slower, without changing the shape of the movement.

• The Analogy: Imagine a song playing on a record player. You can keep the song the same (the spatial curve) but change the speed of the turntable (the phase).
• The Benefit: If a human moves their arm quickly, the robot can speed up its "phase" to keep up, or slow down if there is an obstacle, all while keeping the exact same graceful motion shape.

5. Real-World Proof

The authors tested this on two robots:

1. A stationary arm: They successfully dressed a mannequin, even when they physically pushed the robot off course. The robot wobbled, the "rubber band" pulled it back, and it finished the job.
2. A mobile robot: They put the robot on a wheeled base. Even when the wheels slipped or the robot had to reposition its base to reach the arm, the system adapted instantly.

Why is this a big deal?

• Safety: It's inherently safe because it's designed to snap back to a safe path if disturbed.
• Speed: It doesn't need hours of training like AI models. It can learn a new movement in seconds and run in real-time.
• Flexibility: It works on complex, curved mathematical spaces that other methods struggle with, making it perfect for real-world tasks like dressing, surgery, or assembly.

In short: CDSM gives robots a "muscle memory" that is flexible, safe, and mathematically perfect, allowing them to adapt to a messy, unpredictable human world without breaking a sweat.

Technical Summary

Here is a detailed technical summary of the paper "Curve-Induced Dynamical Systems on Riemannian Manifolds and Lie Groups" (CDSM).

1. Problem Statement

Deploying robots in complex, unstructured environments (e.g., household settings) requires behaviors that are safe, adaptable, and interpretable. Many robotic tasks involve data that is inherently non-Euclidean, residing on Riemannian manifolds (e.g., symmetric positive definite matrices for stiffness/damping) or Lie groups (e.g., $SE(3)$ for poses, $SO(3)$ for orientations).

Existing approaches face several limitations:

• Learning-based methods (e.g., Normalizing Flows, Diffusion policies) often require extensive training times (minutes to hours), hindering real-time adaptation to environmental changes (e.g., a human moving their arm during a dressing task). They also typically converge to a goal rather than specific demonstrated trajectories, reducing interpretability.
• Vector-field methods in Euclidean space struggle when naively projected onto manifolds, leading to geometric distortions.
• Dynamic Motion Primitives (DMPs) often focus on reproducing a single nominal trajectory and lack robustness to disturbances or the ability to handle complex geometric structures like full covariance matrices.

The core challenge is to construct a real-time, stable dynamical system (DS) directly on manifolds that can:

1. Fit a smooth curve to demonstrated data.
2. Attract the system state to this curve while propagating along it.
3. Handle variable damping/stiffness matrices ($S_{++}^n$) and poses ($SE(3)$) simultaneously.
4. Adapt timing (phase modulation) without altering the spatial path.

2. Methodology: CDSM Framework

The authors propose Curve-induced Dynamical Systems on Smooth Manifolds (CDSM). The framework constructs a DS directly on the manifold $\mathcal{M}$ (or Lie group $\mathcal{G}$) using differential geometry tools.

A. Curve Construction

• Representation: Reference data (trajectories) are fitted to a smooth, non-self-intersecting curve $\gamma: [0, 1] \to \mathcal{M}$ using composite quadratic Bézier curves.
• Optimization: The curve is optimized directly on the manifold to minimize the geodesic distance to data points, ensuring $C^1$-continuity.
• Implementation: Individual segments are encoded in local tangent spaces (using control points), while continuity constraints and distance calculations are performed on the manifold using exponential ($\exp_p$) and logarithmic ($\log_p$) maps.

B. Dynamical System Formulation

The DS $\dot{x} = f(x)$ is defined as the superposition of two components in the tangent space at the current state $x$:

1. Propagation Term (Tangential): Drives motion along the curve.
   • Computes the closest point $\pi(x)$ on the curve to the current state $x$.
   • Retrieves the curve's velocity vector $\gamma'(\tilde{s})$ at $\pi(x)$.
   • Transports this vector to the tangent space of $x$ using Parallel Transport ($P_{\pi(x) \to x}$).
2. Convergence Term (Normal): Attracts the state toward the curve.
   • Defined as the negative gradient of a potential energy function $\Psi_{NC}(x) = \frac{1}{2}d_{\mathcal{M}}^2(x, \pi(x))$.
   • This vector points along the geodesic connecting $x$ to the curve.

Key Geometric Insight: Unlike Euclidean approaches, CDSM explicitly handles the curvature of the manifold. The propagation velocity is parallel-transported from the curve to the query point, ensuring the system moves correctly along the manifold's geometry.

C. Stability Analysis

• The authors provide a practical stability analysis using a Lyapunov candidate function $V(x)$ combining the normal distance potential and a phase-dependent tangential potential.
• They prove that the system is locally asymptotically stable within a tubular neighborhood of the curve, excluding the "cut locus" (singularities where the closest point is not unique).
• The propagation and convergence terms are shown to be orthogonal (via Gauss's Lemma), ensuring the tangential motion does not degrade convergence to the curve.

D. Phase Modulation

To decouple timing from spatial geometry, a phase modulation layer is introduced.

• The spatial curve $\gamma(s)$ remains fixed, but the phase variable $s$ is evolved over time via a separate curve $\gamma_s(t)$.
• This allows for real-time optimization of execution speed (e.g., adhering to velocity constraints or reacting to dynamic obstacles) without altering the learned spatial path.

E. Coupling Poses and Damping Matrices

For tasks like robot-assisted dressing, the framework couples:

1. A DS for poses on $SE(3)$.
2. A curve of variable damping matrices on $S_{++}^6$.

• The damping matrices are derived from the covariance of demonstrated data (inverse of covariance). High variability in data leads to lower gains (softer behavior), while low variability leads to higher gains (stiffer behavior).
• These matrices are interpolated toward the identity matrix as the robot approaches the nominal curve to preserve nominal behavior.

3. Key Contributions

1. CDSM Formulation: A novel DS formulation for Riemannian manifolds and Lie groups that constructs a stable system from single or multiple demonstrations in real-time.
2. Geometric Coupling: A method to synchronize a pose-based DS ($SE(3)$) with a curve of full $S_{++}^n$ damping matrices, enabling variable impedance control.
3. Phase Modulation: A mechanism to adapt execution timing dynamically while preserving the spatial trajectory shape.
4. Real-Time Performance: Implementation using JAX for fast batch evaluation and forward simulation, enabling real-time adaptation.

4. Experimental Results

The authors validated CDSM through quantitative benchmarks and real-world deployment.

A. Quantitative Benchmarks (LASA Dataset on $S^2$)

Compared against Lieflows and PUMA:

• Accuracy: CDSM achieved the lowest trajectory distance ($0.0042$vs.$0.0131$for Lieflows) and path distance ($0.0339$vs.$0.0869$), demonstrating superior robustness to out-of-distribution initial states.
• Success Rate: 100% success rate in reaching the goal, compared to 81% for Lieflows.
• Efficiency:
  • Query Time: CDSM is $\sim$100x faster than Lieflows and $\sim$10x faster than PUMA ($0.15$ms vs.$12.6$ms).
  • Fitting Time: CDSM fits curves in $\sim$1.26 seconds, whereas training neural networks (Lieflows/PUMA) takes minutes to hours.

B. Real-World Deployment: Robotic Dressing

• Setup: A 7-axis Franka robot and a mobile manipulator (Kinova Gen3 Lite on a Clearpath Dingo) performed a dressing task, adapting to a human arm's configuration.
• Performance:
  • The system successfully tracked the human arm and adapted the end-effector pose and damping matrices online.
  • Disturbance Rejection: When the robot was physically pushed (perturbed), the DS rapidly converged back to the nominal curve.
  • Variable Damping: The system demonstrated "softer" behavior (lower gains) when far from the curve or in high-variability regions, and "stiffer" behavior (higher gains) near the shoulder, improving safety and precision.
  • Mobile Manipulation: The framework successfully handled internal perturbations (e.g., wheel slip, base reconfiguration) while maintaining task execution.

5. Significance

• Real-Time Adaptability: CDSM bridges the gap between learning-based methods (which are slow to adapt) and traditional control (which lacks learning capabilities). It allows robots to learn from a few demonstrations and adapt instantly to new constraints or disturbances.
• Geometric Correctness: By operating directly on manifolds and Lie groups, the method avoids the geometric distortions inherent in projecting high-dimensional non-Euclidean data into Euclidean space.
• Safety and Interpretability: The explicit attraction to a demonstrated curve (rather than just a goal) makes the robot's behavior more predictable and interpretable for human interaction.
• Versatility: The ability to simultaneously control poses and variable impedance (damping) matrices makes this framework highly suitable for complex human-robot interaction tasks like dressing, rehabilitation, and collaborative assembly.

In conclusion, CDSM provides a mathematically rigorous, computationally efficient, and practically robust framework for generating stable, adaptive robot behaviors on complex geometric spaces.