TL;DR
This paper introduces GSTOMP, a novel motion planning approach that combines Learning from Demonstration (LfD) with the STOMP optimization framework. By using demonstrated trajectories as initial guides, GSTOMP significantly improves task success rates and enables skill transfer between different robot platforms, outperforming traditional LfD methods.
What Problem Does This Paper Address?
Robots operating in human environments, like warehouses or homes, face increasingly complex manipulation tasks. While Learning from Demonstration (LfD) offers a way to teach robots tasks without explicit programming, current LfD methods often tie learned skills to specific robot platforms and struggle with variations in task parameters or environments. This limits scalability and flexibility, requiring robots to be retrained for each new task or platform. Furthermore, existing LfD approaches can be brittle, failing when environments are not static or when robot localization has errors, highlighting the need for robust motion planning that can adapt to uncertainty.
Methods
The core innovation is Guided Stochastic Optimization for Motion Planning (GSTOMP). It builds upon the Stochastic Trajectory Optimization for Motion Planning (STOMP) framework, which iteratively refines a trajectory to minimize a cost function. GSTOMP's key contributions are:
- Task-Informed Trajectory Initialization: Instead of relying on simple interpolations, GSTOMP uses Dynamical Movement Primitives (DMPs), specifically Cartesian DMPs, to encode demonstrated tasks. These DMPs generate "guide trajectories" that capture the essential characteristics of the demonstrated motion. This provides STOMP with a much better starting point, especially for complex tasks.
- Novel Cost Function: A new cost function is introduced that penalizes deviations from the guide trajectory in Cartesian space, while still incorporating obstacle and joint limit avoidance. This ensures that the optimized trajectory stays close to the learned skill while remaining safe and feasible.
- Skill Transfer: Because DMPs are used in Cartesian space, they are inherently platform-invariant. This allows a skill demonstrated on one robot to be transferred and executed by a different robot, even with varying degrees of freedom.
Datasets
The paper does not explicitly mention the use of pre-existing datasets for training. Instead, demonstrations were acquired through kinesthetic teaching on real robots for specific tasks: picking items from a shelf, opening drawers, and opening cupboards. These demonstrations were then used to train the DMPs and generate the guide trajectories for GSTOMP. The evaluation was performed using these learned skills on two different manipulator systems (Rob@work and TIAGo) in both simulation and real-world experiments.
Key Insights and Ideas
- LfD as a "Smart Initial Guess": The paper demonstrates that the real power of LfD lies not just in directly executing learned trajectories, but in providing a highly informed initial guess for more general optimization-based planners like STOMP. This bridges the gap between the specificity of LfD and the robustness of optimization.
- Cartesian DMPs Enable True Skill Transfer: By encoding skills in Cartesian space, GSTOMP overcomes the platform-specific limitations of many LfD methods. This is a crucial step towards creating more adaptable and reusable robot skills.
- Robustness to DOF Changes: The ability to handle robots with varying degrees of freedom is a significant advantage. GSTOMP shows a smooth degradation in performance as DoFs are reduced, indicating its adaptability to different robot configurations.
- Balancing Exploration and Exploitation: The novel cost function effectively balances exploring the environment for safety (collision avoidance) with exploiting the learned skill (staying close to the guide trajectory).
Key Results
- Improved Task Success: GSTOMP achieved significantly higher task success rates compared to simple LfD approaches, particularly for more complex tasks like opening drawers and cupboards. For instance, in the drawer opening task, GSTOMP achieved near 100% success, while LfD was around 80%.
- Feasible Skill Transfer: The system successfully transferred a drawer-opening skill from a TIAGo robot to a Rob@work platform and a TIAGo robot with reduced degrees of freedom, demonstrating good performance.
- Competitive Planning Performance: When compared against state-of-the-art motion planners (like RRTConnect, CHOMP, and other STOMP variants), GSTOMP performed comparably or better in terms of success rate and smoothness, though often with a higher planning time than simpler LfD methods.
- Robustness to DOF Reduction: In experiments with a TIAGo robot, GSTOMP maintained a high success rate (100%) with 8 DoFs, and showed a reasonable degradation to 40% with 7 DoFs, outperforming other approaches at lower DoFs.
Limitations and Future Work
- Planning Time: GSTOMP generally has a higher planning time than simpler LfD approaches, which is a trade-off for its increased robustness and success rate.
- Approach and Departure Paths: The current method focuses on generating the core task trajectory and does not explicitly cover approach and departure paths, which are assumed to be handled by other planners.
- Static Mobile Base: The current implementation assumes a static mobile base. Future work could integrate mobile base placement strategies to further improve performance.
- Collision Checking: The paper uses sampling-based continuous collision checking. Future work could explore the benefits of using swept-out volumes for collision checking.
- Parameter Tuning: While the authors state that GSTOMP requires minimal parameter tuning, the weights for the cost function might need adjustment for significantly different tasks or environments.
- Empirical Noise Distribution: The parameters for the joint-space noise distributions in STOMP are currently determined empirically. Future work could involve learning these parameters from data.
Summary updated