Hi y’all!
I’m Matt Lamsey, and I want to share some recent work from my summer internship with Hello Robot. We have been experimenting with whole-body trajectory optimization for Stretch, first explored by Michal Ciebielski here. This post contains details about our implementation, as well as the results from some experiments using our code. We hope that this will serve as a useful reference for those interested in further exploring whole-body trajectory optimization.
We have reorganized the original code, added support for general position + orientation tracking for the robot’s end effector, added lots of docstrings, and wrapped the optimizer up with a small API. We also upgraded the URDF to the Stretch RE3, and we have provided a tool for generating a calibrated RE3 UDRF for use in the optimizer. This code is still limited to simulation, and does not execute trajectories on a real robot yet!
The code that we have been working on can be found in this repository with a comprehensive README. An example of its performance is shown in Figure 1.
Figure 1. Example simultaneous position and orientation tracking for a simulated Stretch RE3.
Technical Details
Like the original implementation, our code leverages OpTaS, which is based on the CasADi optimization framework, to perform the trajectory optimization. The base configuration ([x, y, θ] on the ground) and the arm + wrist configuration are jointly optimized.
The optimizer contained in the new repository supports two modes: position only tracking, and position + orientation tracking, shown in Figure 2. We also include a Trajectory class to abstract the contents of a trajectory being optimized.
Figure 2. Left: position only tracking. Right: position + orientation tracking.
Optimizer Performance
Solve Time
As noted in Michal’s original post, it would be interesting to use this optimizer for Model Predictive Control (MPC). We profiled the optimization time for various time horizons and time resolutions, as shown in Figure 3. Solving for the number of points in each trajectory and regressing suggests that the optimization time increases by ~0.07s per point on an Intel i7 7700k. While this may be too slow for high-performance MPC, these results suggest that the optimizer could be used for high-level online planning.
Figure 3. Trajectory time horizon vs. total optimization time for varying time steps (dt). The number of points in the trajectory is equal to the time horizon divided by dt.
Sparse Trajectories
We also performed some tests of the optimizer for long-horizon, sparse trajectories. We observed that the optimizer’s solutions degenerate as the horizon and spacing between points increases. An example of this is shown in Figure 4.
Figure 4. Optimizer performance for two arc paths with fixed end effector orientation. Left: 0.1m spacing. Right: 0.25m spacing. Note how the robot’s base flips around at the end of the trajectory with larger spacing. This behavior is undesirable.
If this work is interesting to you, feel free to reach out to me at lamsey@hello-robot.com! Happy hacking!