VP-STO
Via-point-based Stochastic Trajectory Optimization for Reactive Robot Behavior
Julius Jankowski*, Lara Brudermüller*, Nick Hawes, Sylvain Calinon
*Authors contributed equally
Abstract: Being able to achieve reactive robot behavior in complex dynamic environments is still challenging as it relies on being able to solve trajectory optimization problems quickly enough, such that we can replan the future motion at frequencies which are sufficiently high for the task at hand. We argue that current limitations in computational efficiency arise from inefficient, high-dimensional trajectory representations and the negligence of time-optimality in the trajectory optimization process. Therefore, we propose a motion optimization framework that optimizes jointly over space and time, generating smooth and time-optimal robot trajectories in joint-space. While being task-agnostic, our formulation can incorporate additional requirements, such as collision avoidance and yet maintain real-time control rates, demonstrated in simulation and real-world robot experiments for closed-loop manipulation.
I. Experiments
We demonstrate our approach for two different closed-loop manipulation scenarios on a Franka Emika robot arm in simulation and in real-world.
A. Object Pushing
Real World
We incorporate a pushing progress term that rewards robot trajectories that result in moving the box closer to the tip of the metal stick according to an anticipating forward simulation of the fundamentally hybrid contact dynamics. Combined with trajectory constraints on maximum velocity and acceleration, joint angle limits and collisions with the table, the MPC framework is able to generate long sequences of robust closed-loop manipulation.
Simulation
In the simulated experiments, we additionally give a desired object orientation.
The white lines in the simulation videos correspond to the sampled candidate trajectories (after computing forward kinematics).
Box
Cylinder
Triangle
B. Pick & Place
This pick & place scenario requires reactive robot behavior in close-to-contact situations, since contacts between the robot gripper and the object to grasp have to be avoided until reaching a proper grasping pose to not push the object away. Grasping poses and target configurations for placing the object are optimized and heuristically selected in a synchronized, parallel optimization loop. Minimizing the movement duration subject to velocity and acceleration constraints results in long sequences of smooth and efficient robot behavior.
II. Via-Point based Stochastic Trajectory Optimization (VP-STO) Loop
In this section we show the underlying trajectory optimization process as introduced in our paper.
Minimizing trajectory duration in cluttered workspaces s.t. velocity and acceleration limits:
Presentation @ ICRA 2023
poster_icra_23_landscape.pdfAppendix
icra_23_vpsto-APPENDIX.pdf