Hi everyone, previously I shared a way to perform whole body IK for the stretch and I have recently extended this to perform whole body trajectory optimization. You can find a couple examples which should run out of the box and a more thorough description at stretch_trajopt. The example from the previous post is also in this repo. The main challenges with using the whole body optimization for IK were that there was no consideration of the constraints imposed by the diff drive so the resulting configurations although possible, were very suboptimal and required adding some finetuned constraints which really limited the practicality of that approach.

#### New approach

In the new approach, as before I jointly optimize the base configuration (x, y, theta) and the arm configuration (arm joint positions) but this time over an entire trajectory. Furthermore, the diff drive dynamics are added as constraints in the optimization and then errors in the end effector are calculated by transforming the arm onto the base. Here is a video demonstrating the results on a relatively difficult end effector trajectory. The desired end effector trajectory is plotted in red and the actual trajectory is in green, its not easy to see the red because they are practically overlapping.

#### Comments on solve time and practical use

The examples are relatively large optimizations with 200 timesteps over 20 seconds and are just meant to show types of motions possible with this whole body optimitaion. On my pc the figure 8 trajectory is generated in about 30 sec and the 3 point path is generated in about 200 sec. A more realistic use of this is on smaller timescales with 20 timesteps over 2 seconds where the optimization time is less than one second, so it could be implemented as an MPC. One cool application would be whole body teleop tracking with the added maneuverability of the base.

#### Potential improvements

This is not the most efficient way to solve this problem because the optimizer does not have access to the whole body jacobians due to the base and arm being modeled separately. In Modern Robotics by Kevin Lynch chapter 13 he describes how to get the whole body jacobians for non holonomic mobile manipulators which I suspect should make this a more efficient optimization. This would no longer fit into the design of optas which this example is built upon so there it would require some more effort. Nonetheless this repo should serve as a way to get started with generating whole body motions on the stretch, and more generally provides a simple template for diff drive mobile manipulators.