Trajectory Manifold Optimization for Fast and Adaptive Kinodynamic Motion Planning

Under review

Yonghyeon Lee
Korea Institute For Advanced Study
Paper Code (Coming soon)

TL;DR: We propose a two-step method for fast kinodynamic motion planning: (i) learning a manifold of task-relevant trajectories that satisfy kinodynamic constraints offline, and (ii) searching for a trajectory within this manifold online, enabling the system to adapt to dynamic environments.

Example: Throwing with a 7-DoF Robot Arm

Throwing to a nearby target
Throwing to a distant target
DMMP: Fast Generation of Throwing Trajectories
(Gray → Deep Red → Transparent Red)

Online Adaptation to Dynamic Target Changes

Abstract

Fast kinodynamic motion planning is crucial for systems to effectively adapt to dynamically changing environments. Despite some efforts, existing approaches still struggle with rapid planning in high-dimensional, complex problems. Not surprisingly, the primary challenge arises from the high-dimensionality of the search space, specifically the trajectory space. We address this issue with a two-step method: initially, we identify a lower-dimensional trajectory manifold offline, comprising diverse trajectories specifically relevant to the task at hand while meeting kinodynamic constraints. Subsequently, we search for solutions within this manifold online, significantly enhancing the planning speed. To encode and generate a manifold of continuous-time, differentiable trajectories, we propose a novel neural network model, Differentiable Motion Manifold Primitives (DMMP), along with a practical training strategy. Experiments with a 7-DoF robot arm tasked with dynamic throwing to arbitrary target positions demonstrate that our method surpasses existing approaches in planning speed, task success, and constraint satisfaction.

Summary

1 Challenging Kinodynamic Motion Planning

• Planning problems that require the robot to fully utilize its kinodynamic limits to complete a task are particularly challenging, taking a long time to solve.

The left GIF is adopted from the Timur Garipov’s project video on Robotic Arm Weightlifting via Trajectory Optimization in YouTube.

2 Planning in Dynamic Environments

• As a concrete example, we will work through the dynamic throwing task.

Trajectorie Optimization: Given an analytic inverse dynamics equation, we have a nonlinear and nonconvex objective function \( J(q(t);\tau) \) — where the task variable \( \tau \) is the target box position and the joint trajectory \( q(t) \) is an optimization variable — and constraints \( C(q, \dot{q}, \ddot{q}, \dddot{q}) \leq 0 \) for all \( t \).

Adam: Adaptive Moment Estimation (Kingma et al., International Conference on Learning Representations 2015)
COBYLA: Constrained Optimization BY Linear Approximation (Powell., Advances in Optimization and Numerical Analysis 1994)
SLSQP: Sequential Least SQuares Programming (Kraft., Tech. Rep. DFVLR-FB 88-28 1988)

!Conventional trajectory optimization algorithms are too slow for online replanning and sometimes fail depending on the initialization in challenging kinodynamic planning problems.

3 How can humans perform fast planning?

• Through experience, we have learned motor patterns relevant to throwing motions, and these are encoded in our muscle memory.

Images are generated by ChatGPT 4o.

4 Key Idea: Learning a trajectory manifold offline, Planning within it online

Four-Step Training Method:
1. Data collection via trajectory optimizations
2. Differentiable motion manifold learning
3. Latent flow model learning via flow matching
4. Trajectory manifold optimization

Citation


      @article{lee2024trajectory,
        title={Trajectory Manifold Optimization for Fast and Adaptive Kinodynamic Motion Planning},
        author={Lee, Yonghyeon},
        journal={arXiv preprint arXiv:2410.12193},
        year={2024}
      }