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.
Wow! Nice work @michalciebielski, I’m looking forward to trying this out! I’ve removed the 1 link limit for your account, so you should now be able to embed the videos.
Very cool! In the source, it looks like the paths specify the position at each waypoint, but not the orientation. Do you think that it would be possible to add one or more orientation constraints to a waypoint? With the 1DOF Stretch wrist, I guess that this would only include the yaw angle of the end effector, but with the dex wrist, maybe it could be full 6DOF specifications? I’m not familiar with optas, so I’m not sure how the separate diff drive / arm models would interact in this case.
Curious to hear your thoughts on whether it’s feasible - this could be useful for a project I’m working on.
It is definitely possible to do that! Optas has spatial math included so you could get the orientation error and include that as a cost just like the end effector tracking cost. You would just have to be careful to calculate this properly as there are special rules for quaternion operations and there are a few different ways to get the angle error. This would give you full 6dof tracking. As far as the diff-drive and arm model separation it shouldnt make a difference
Fantastic work! Really love how self-contained the code is, compared to other libraries I’ve tried to do this with.
I would also like to add end-effector orientation constraints to the existing code – would it be possible for you to provide a minimal example by any chance? I tried doing this myself but have been struggling to do so.
Thanks once again for putting this work out there for all of us to use!
@arjung@lamsey glad you guys are finding this useful
I added a rough example where i track the initial end effector orientation, this would be analogous to tracking an end effector orientation trajectory. You can find the lines here on the orientation_tracking_example branch. Its easiest to see the differences if you run the three_point example with those lines and then comment them out. That is essentially tracking an end effector orientation in the global frame
I had a quick follow-up question: Is there an easy way to change qc, the initial arm configuration? I am trying to set the 6 numbers manually such that the target end-effector pose quat_des is set to my desired value, but I’m unsure how the casadi.casadi.SX object qc can be changed… Alternatively, if there is a way to directly set quat_des directly to some desired target quaternion, that would work too!
qc is entered as a parameter to the setup_solver() method. this method sets up the optimization problem symbolically so any parameters that you change will have to end up as symbolic casadi data types. In the reset(params…) method you can pass any additional parameters you want and then read them into the setup_solver() method. this is already done with a few parameters so you can just follow that example. That way you can add the quat_des from the main function.
