Literature
Display
← All papers

Elastic Motion Policy: An Adaptive Dynamical System for Robust and Efficient One-Shot Imitation Learning

Tianyu Li · Sunan Sun · Shubhodeep Shiv Aditya · Nadia Figueroa

· IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2025) ↗

Robotics6 min text · CondensedOriginal paper ↗
Contents

At a glance

EMP adapts a full-pose robot motion learned from one demonstration when relevant objects move. It deforms the policy’s Gaussian geometry, including orientation in quaternion tangent coordinates, and rapidly refits stable local dynamics. A visual pipeline identifies and tracks task keyposes. Real-robot tests show stronger generalization than rigidly transforming a fixed policy, with limitations from tracking, segmentation, and robot reachability.

I INTRODUCTION

A stable motion can still become inappropriate when the scene changes. EMP extends Elastic-DS from position trajectories to full end-effector pose and replaces slow potential fitting with a convex formulation. The reported update rate is about 30 Hz. It assumes known object meshes and usable object tracking, and integrates optional obstacle avoidance and multistep segmentation. [3]

Refer to caption
Fig. 1: EMP Framework Overview. Given a single demonstration and semantic knowledge of the scene, EMP can adjust a learned motion policy based on the changes in environment in real time. Additionally, EMP enables stable and compliant robotic control in scenarios involving obstacle avoidance and multi-step tasks.

II RELATED WORK

The paper contrasts collecting more demonstrations with constraining and adapting the policy itself. Stable DS methods help recover from unfamiliar states but typically retain a fixed task geometry. EMP focuses on changing that geometry efficiently, using few demonstrations and explicit task keyposes rather than a large end-to-end visual policy.

III BACKGROUND

EMP builds on separate but coordinated position and orientation dynamical systems, each using a mixture of local models.

III-A SE(3) Linear Parameter Varying Dynamical System

Position uses an LPV-DS in Euclidean space. Orientation uses a quaternion dynamical system represented in a tangent space. The combined construction approximates full rigid-body pose as position plus rotation.

III-A 1 LPV-DS Formulation

A Gaussian mixture supplies state-dependent weights for linear systems sharing one attractor. A positive-definite quadratic Lyapunov function and matrix inequalities constrain every component to decrease the same potential. The mixture then inherits the corresponding convergence property for the fixed policy. Fitting minimizes demonstrated velocity error subject to those constraints.

Equations 1–3
x˙=∑k=1Kγk​(x)​𝔸k​(x−x∗),\dot{x}=\sum_{k=1}^{K}\gamma_{k}(x)\mathbb{A}_{k}\left(x-x^{*}\right), (1)
V⁡(x)=(x−x∗)T​ℙ​(x−x∗)V(x)=(x-x^{*})^{T}\mathbb{P}(x-x^{*}) (2)
minΘD​S⁡J⁡(ΘD​S)=∑i=1N‖x˙iref−f⁡(xiref)‖22s.t.​{(𝔸k)T​ℙ+ℙ​𝔸k=ℚkℚk=(ℚk)T≺0​∀k=1,…,K\begin{gathered}\min_{\Theta_{DS}}J\left(\Theta_{DS}\right)=\sum_{i=1}^{N}\left\|\dot{{x}}_{i}^{\mathrm{ref}}-{f}\left({x}_{i}^{\mathrm{ref}}\right)\right\|^{2}_{2}\\ \text{s.t.}\,\,\left\{\begin{array}[]{l}\left(\mathbb{A}_{k}\right)^{T}\mathbb{P}+\mathbb{P}\mathbb{A}_{k}=\mathbb{Q}_{k}\\ \mathbb{Q}_{k}=\left(\mathbb{Q}_{k}\right)^{T}\prec 0\end{array}\right.\ \forall k=1,\dots,K\end{gathered} (3)

III-A 2 Quaternion Dynamical System

Orientations are mapped relative to the target quaternion with the Riemannian logarithm. Mixture-weighted linear maps predict the next desired tangent-space orientation, and a semidefinite program fits them under the stated stability constraints. This discrete orientation update differs from the continuous position-velocity output.

Equations 4–5
(𝔮^a​t​t)d​e​s=∑k=1Kγk​(𝕢)​𝔸k​log𝕢a​t​t​𝕢,(\hat{\mathfrak{q}}_{att})^{des}=\sum_{k=1}^{K}\gamma_{k}(\mathbb{q})\mathbb{A}_{k}\log_{\mathbb{q}_{att}}\mathbb{q}, (4)
minΘD​S\displaystyle\min_{\Theta_{DS}} J⁡(ΘD​S)=∑i=1N‖(𝔮^a​t​ti)d​e​s−(𝔮a​t​ti)d​e​s‖22\displaystyle J\left(\Theta_{DS}\right)=\sum_{i=1}^{N}\left\|(\hat{\mathfrak{q}}_{att}^{i})^{des}-(\mathfrak{q}_{att}^{i})^{des}\right\|^{2}_{2} (5)
s.t.\displaystyle\text{s.t.} 𝔸k≺0,∀k=1,…,K\displaystyle\mathbb{A}_{k}\prec 0,\quad\forall k=1,\dots,K

III-A 3 Stable SE3-LPVDS Policy

The two policies produce desired linear and angular motion. Orientation prediction requires parallel transport and an exponential map to recover a unit quaternion, followed by conversion to an angular command. The appendix retains these formulas.

Equation 6
x˙=fp​(x,Θp),ω=fo​(𝕢,Θo)\displaystyle\dot{x}=f_{p}(x;\ \Theta_{p}),\,\,\,\,\omega=f_{o}(\mathbb{q};\ \Theta_{o}) (6)

IV ELASTIC MOTION POLICY

Adaptation changes the geometric parameters describing the demonstration, then refits the motion field. Relevant object poses supply endpoint and approach constraints.

IV-A SE3 LPV-DS Policy Morphing

Neighboring Gaussians define intermediate joints along the demonstrated motion. Laplacian editing moves these joints to satisfy new endpoint transformations while approximately retaining their local relationships. The changed means and covariances define a new geometry for policy fitting.

For orientation, the method constructs a three-dimensional basis for the tangent hyperplane embedded in four-dimensional quaternion coordinates. Means and covariances are reduced to those coordinates, edited, and mapped back. The construction uses different bases for the attractor-centered means and mean-centered covariances. Figures 2 and 3 illustrate orientation and full-pose adaptation.

Equations 7–16
Σt=(Σk−1+Σk+1−1)−1\mathbb{\Sigma}_{t}=\left(\mathbb{\Sigma}_{k}^{-1}+\mathbb{\Sigma}_{k+1}^{-1}\right)^{-1} (7)
βi,k,k+1=Σt​(Σk−1​μk+Σk+1−1​μk+1).\beta_{i,k,k+1}=\mathbb{\Sigma}_{t}\left(\mathbb{\Sigma}_{k}^{-1}\mu_{k}+\mathbb{\Sigma}_{k+1}^{-1}\mu_{k+1}\right). (8)
minβi⁡J⁡(βi)=‖L​βi−Δ‖22\displaystyle\min_{\beta_{i}}J\left(\beta_{i}\right)=\left\|L\beta_{i}-\Delta\right\|_{2}^{2} (9)
s.t. ​{T0,1​(βi,0,βi,1)=Ostart Tn−1,n​(βi,n−1,βi,n)=Oend ,\displaystyle\text{ s.t. }\,\left\{\begin{array}[]{l}T_{0,1}\left(\beta_{i,0},\beta_{i,1}\right)=O_{\text{start }}\\ T_{n-1,n}\left(\beta_{i,n-1},\beta_{i,n}\right)=O_{\text{end }},\end{array}\right.
Null(𝕢a​t​t)={𝕢i∈ℝ4∣𝕢i⋅𝕢a​t​t=0,∀i=1,2,3}.\text{Null}(\mathbb{q}_{att})=\left\{\mathbb{q}_{i}\in\mathbb{R}^{4}\mid\mathbb{q}_{i}\cdot\mathbb{q}_{att}=0,\ \forall i=1,2,3\right\}. (10)
Λ𝕢a​t​t=[𝕢1​𝕢2​𝕢3]∈ℝ4×3.\mathbb{\Lambda}_{\mathbb{q}_{att}}=\begin{bmatrix}\mathbb{q}_{1}\ \mathbb{q}_{2}\ \mathbb{q}_{3}\end{bmatrix}\in\mathbb{R}^{4\times 3}. (11)
μ^k=Λ𝕢a​t​tTlog𝕢a​t​t(μ~k)∈ℝ3,∀k=1,…,K.\hat{\mu}_{k}=\mathbb{\Lambda}_{\mathbb{q}_{att}}^{T}\log_{\mathbb{q}_{att}}(\tilde{\mu}_{k})\in\mathbb{R}^{3},\ \forall k=1,\dots,K. (12)
{Λμk=[𝕢1​𝕢2​𝕢3]∣𝕢i∈Null(μ~k),∀k=1,…,K},\left\{\mathbb{\Lambda}_{\mu_{k}}=\begin{bmatrix}\mathbb{q}_{1}\ \mathbb{q}_{2}\ \mathbb{q}_{3}\end{bmatrix}\mid\mathbb{q}_{i}\in\text{Null}(\tilde{\mu}_{k}),\ \forall k=1,\dots,K\right\}, (13)
𝚺^k=ΛμkT𝚺~kΛμk∈ℝ3×3,∀k=1,…,K\hat{\boldsymbol{\Sigma}}_{k}=\mathbb{\Lambda}_{\mu_{k}}^{T}\tilde{\boldsymbol{\Sigma}}_{k}\mathbb{\Lambda}_{\mu_{k}}\in\mathbb{R}^{3\times 3},\ \forall k=1,\dots,K (14)
μ~k∗=exp𝕢a​t​t⁡(Λ𝕢a​t​t​μ^k∗)∈ℍ⊂ℝ4,\tilde{\mu}_{k}^{*}=\exp_{\mathbb{q}_{att}}(\mathbb{\Lambda}_{\mathbb{q}_{att}}\hat{\mu}_{k}^{*})\in\mathbb{H}\subset\mathbb{R}^{4}, (15)
𝚺~k∗=Λμk​𝚺^k∗​ΛμkT∈ℝ4×4.\tilde{\boldsymbol{\Sigma}}_{k}^{*}=\mathbb{\Lambda}_{\mu_{k}}\hat{\boldsymbol{\Sigma}}_{k}^{*}\mathbb{\Lambda}_{\mu_{k}}^{T}\in\mathbb{R}^{4\times 4}. (16)
Refer to caption
Fig. 2: Illustration of elastic update of orientation trajectory, where the blue trajectory and its Gaussian parameters were first projected onto the tangent plane and then reduced to the lower dimensional Euclidean space. After update, the new trajectory in red and the updated Gaussians can be recovered by reversing the operation.
Refer to caption
Fig. 3: Illustration of elastic update of full pose trajectory.

IV-B Real-Time Convex Lyapunov Function Optimization

The quadratic potential matrix is fitted with a convex sum of ReLU penalties on positive Lyapunov derivatives, subject to a positive-definiteness constraint. This is a surrogate for reducing derivative violations, not a literal count of them. Fitting subsequent local dynamics still supplies the policy’s matrix stability constraints.

Replacing all demonstrated samples with Gaussian mean positions and velocities makes optimization faster, at the cost of discarding some local detail. The measured derivative-violation rate reflects that tradeoff. The fixed-policy convergence argument should not be read as a separate guarantee for arbitrary rapid changes of targets or policy parameters.

Equations 17–18
V˙​(x)=x˙⋅∇V=2​x˙T​ℙ​(x−x∗)<0,∀x≠x∗.\displaystyle\dot{V}(x)=\dot{x}\cdot\nabla V=2\dot{x}^{T}\mathbb{P}(x-x^{*})<0,\quad\forall\ x\neq x^{*}. (17)
minℙ∑i=1N ReLU​(x˙iT​𝐏​(xi−x∗)) s.t. 𝐏⪰ϵ​I,\begin{array}[]{ll}\underset{\mathbb{P}}{\min}&\sum_{i=1}^{N}\textbf{ ReLU}({\dot{x}}_{i}^{T}\mathbf{P}({x}_{i}-x^{*}))\\ \text{ s.t. }&\mathbf{P}\succeq\epsilon I,\end{array} (18)
Refer to caption
Fig. 4: An example of fitting 𝐏\mathbf{P} (represented by the contour level set) using the original P-QLF (left), the newly proposed Convex formulation (middle), and the newly proposed GMM-informed convex formulation (right). Our methods speed up the optimization for real-time purposes while retaining performance.
Refer to caption
Fig. 5: A block diagram showing the full EMP imitation learning pipeline.

V EMP IMITATION LEARNING PIPELINE

The pipeline collects a demonstration, identifies task-relevant object keyposes, optionally segments the task, and adapts the learned policy during execution.

V-A Data Processing

Demonstration processing produces base-frame end-effector trajectories, gripper states, and object-relative task constraints.

V-A 1 Demonstration Data Collection

A modified UMI gripper carries several AprilTags for external RGB-D tracking, while a contact sensor records binary gripper state. A reference tag cube defines a consistent robot-base frame during both demonstration and execution. This supports demonstration collection without moving the robot itself.

V-A 2 Keypose/Attractor Extraction

GPT-4o identifies the relevant object from sampled video frames. Grounded SAM produces its mask, and FoundationPose estimates and tracks its pose using the available object mesh. The demonstrated final end-effector pose is stored relative to that object as a keypose; changing object pose updates the task constraint. The original prompt and response appear in the appendix.

Refer to caption
Fig. 6: The first two images show the selection of the keypose using the UMI gripper end-effector fixed in the body frame of the object. This selected keypose is then tracked using FoundationPose [35].

V-A 3 Multi-Step Trajectory Decomposition

UVD divides long demonstrations into goal-directed segments. Each segment receives its own policy and object-relative keypose. A one-hot switch activates the next policy after the current attractor is reached. Segmentation errors therefore propagate into the chosen subgoals and can cause task failure.

V-B Runtime Policy Inference

Tracked object poses continuously update geometric constraints, which drive policy deformation and refitting. A passive impedance controller converts the desired velocities to joint torques. Subtask completion triggers policy switching and gripper changes.

VI EVALUATION

Evaluation separates the potential-fitting speed/violation tradeoff from complete robot-task performance.

VI-A Convex Lyapunov Function Evaluation

On thirty LASA handwriting motions, the study compares the prior nonconvex potential fit with full-data and Gaussian-informed convex fits. Single-demonstration trials are repeated ten times per motion, with a further comparison using all seven demonstrations. The Gaussian-informed version approaches a fiftyfold fitting-time improvement in the reported larger-data setting, while its derivative-violation rate can increase. Table I preserves the exact timings and violation percentages.

LASA Dataset Methods Computation Time (s) Violation Percentage
Single Trajectory Baseline P-QLF 0.332 14.0%
Convex P-QLF 0.038 11.1%
GMM P-QLF 0.007 15.1%
All Trajectory Baseline P-QLF 2.62 14.9%
Convex P-QLF 0.24 12.3%
GMM P-QLF 0.09 15.4%
TABLE I: Comparison of different P-QLF learning formulations, with computation time measured in seconds. All experiments were conducted on an AMD Ryzen 7 5800X with 32GB of memory.
Methods Book Placing Cube Pouring Pick-and-Place
ID SE(3)-LPVDS 10/10 10/10 8/10
EMP (Ours) 10/10 10/10 7/10
OOD SE(3)-LPVDS 4/10 4/10 1/10
EMP (Ours) 8/10 9/10 7/10
TABLE II: Success rates of all experiments evaluated in ID and OOD scenarios. Franka Research 3 robot arm was used.

VI-B Real Robot Tasks Evaluation

A Franka Research 3 performs book placing, cube pouring, and multistep pick-and-place from one demonstration per task. The baseline rigidly transforms an object-centric SE(3)-LPVDS. Ten trials per task and setting compare demonstrated configurations with more varied object placements. Table II reports success rates; the small trial counts should accompany any comparison.

Refer to caption
Fig. 7: Given a single demonstration of cube pouring shown in the left, EMP can reproduce the same motion and generalize to different pot poses shown in the middle and right.
Refer to caption
Fig. 8: The multi-step task requires sequential pick and place motions. The demonstration has both objects on one side and the box on the other. During inference time, the box is placed in between the two objects.
Refer to caption
Fig. 9: In the same Book Placement task, a yellow bottle obstacle is blocking the original path. Modulation allows EMP to circumvent the obstacle, generating a new path above the bottle.

VI-B 1 Book Placing

Book placing requires both a correct approach through the rack opening and the final insertion pose. Both methods succeed in the demonstrated setup. Changing the rack pose exposes failures of a rigid whole-policy transformation, whereas EMP can preserve the approach and placement constraints through deformation.

VI-B 2 Cube Pouring

Pouring requires an arched path, wrist rotation, and an upright starting cup. Both methods succeed in the demonstrated setup, but the fixed-policy baseline fails in more than half of the varied trials. EMP better retains the required motion shape, though one case fails at a difficult pouring angle. Task-space adaptation can still generate a path outside the robot’s reach or joint limits.

VI-B 3 Multi-Step Pick-and-Place Task

Repeated pick-and-place uses UVD with γ = 0.003 to obtain sequential subgoals. Errors in segmentation or grasping reduce performance for both methods. Cutting a placing segment too early, for example, puts its attractor outside the box; convergence to that attractor then fails the actual task.

VI-C Case Study: Book Placing with Obstacle Avoidance

A book-placing case study adds a mustard bottle modeled as a sphere of diameter 0.18 m. A standard velocity-field modulation changes the motion in the obstacle-normal and tangent directions. This illustrates compatibility with obstacle avoidance; it is a separate geometric layer and a qualitative example rather than a broad safety benchmark.

VII CONCLUSIONS & DISCUSSIONS

EMP combines fast full-pose deformation with constrained dynamics fitting and shows useful one-shot adaptation. Its dependencies include accurate tracking, object meshes, reliable task segmentation, and feasible robot configurations. A single tangent-space approximation can distort quaternion geometry; richer manifold formulations and potentials are proposed for more complex motions. Multiple demonstrations are discussed as an extension, rather than the main evaluated setting.

-A Quaternion-DS Formulas

The predicted orientation displacement is transported from the attractor tangent space to the current orientation, mapped back to the quaternion manifold, and converted to an angular command using the known time interval. Equations (19)–(21) retain the exact operations.

Equations 19–21
(𝔮^b​o​d​y)d​e​s=Γ𝕢a​t​t→𝕢​(𝔮^a​t​t)d​e​s,(\hat{\mathfrak{q}}_{body})^{des}=\Gamma_{\mathbb{q}_{att}\rightarrow\mathbb{q}}(\hat{\mathfrak{q}}_{att})^{des}, (19)
(𝕢^)d​e​s=exp𝕢⁡(𝔮^b​o​d​y)d​e​s,(\hat{\mathbb{q}})^{des}=\exp_{\mathbb{q}}{(\hat{\mathfrak{q}}_{body})^{des}}, (20)
ω=(𝕢¯∘(𝕢^)d​e​s)/d​t.\omega=(\bar{\mathbb{q}}\circ(\hat{\mathbb{q}})^{des})/dt. (21)

-B GPT Prompt for Semantic Object Extraction

The preserved example gives GPT-4o demonstration frames and asks it to identify the task-relevant object. Its text response becomes the prompt for the segmentation model. This is the object-selection component of the pipeline, rather than an action-generation policy.

Refer to caption
Fig. 10: Prompt and response from GPT-4o
Concept demonstration

Open image ↗