Time varying LQR control on model for DART

I am trying to apply time-varying LQR in DART. I have a wheel inverted pendulum model having two wheels and body. I am planning to move straight, hence I can approximate it as a simple wheel inverted pendulum model with one wheel and apply same force on both wheels. The wheels have ultra-resolution designed in the blender software. I tried using a conventional model from literature but it did not work. Is there a way to convert Dart mass matrix and Coriolis matrix to minimum coordinates to verify the model? Also is there a way to linearize it around the point because with out symbolic value, I am not sure how to address the problem.

Any guidance will be appreciated.

Thanks

“it did not work” is a somewhat vague assessment, so I can’t really advise on how to fix whatever problem you’re encountering. There are many many possible reasons that the results of a simulation might not match one’s expectations.

Depending on what exactly failed about your simulation, my first reflex would be that I’m suspicious of your wheels. You say they have ultra-fine resolution, but high-resolution meshes have their own significant pathologies when it comes to collision detection and handling, no matter how accurate the mesh is.

I would recommend the following: Create a reduced version of the model that uses a prismatic joint instead of wheels for the planar motion. Control the prismatic joint as if it’s attached to the wheels (so whatever command you would be sending to the wheels, multiply it by the wheel’s radius to get the command for the prismatic joint). Check if your time-varying LQR works on that reduced model. If it works okay like that, then the wheels are probably the issue.

To make the wheel work more reliably, I recommend using primitives instead of a mesh. The best luck that I’ve had with this sort of thing is the following combination:

  1. Wheels should be cylinder or sphere primitive geometries (spheres are usually better)
  2. The ground should be a plane primitive geometry
  3. The OdeCollisionDetector usually works best for primitives in my experience