“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:
- Wheels should be cylinder or sphere primitive geometries (spheres are usually better)
- The ground should be a plane primitive geometry
OdeCollisionDetector usually works best for primitives in my experience