Tom Bertalan
Home > Robotics Simulators > Things to Do NextCode as of 18 May

Things to Do Next

Next things I need to do--I'll report here as I work on them:

Get data out of the game engine
  • Decide on which metrics we want to publish, and find them as properties of the undocumented Vehicle object. See my existing file TelemRecSpec.lua, which is able to print much of this for all loaded vehicles at once.
  • Write a singleton data-aggregator that all vehicles reference. It will periodically dump data (probably on some background thread) to either file or socket.
  • Find an efficient way to get that data into a running Python process. Probably dumping to/reading from a file isn't going to cut it.

(That which is really important for my initial plans is bolded below.)

For ego-data,

  1. pose, pose rate, and pose acceleration (both translation and rotation),
  2. Drive torque (before or after PTO?)
  3. Steering torque (or some other indicator of steering control input)
  4. Bounding box (or other collision information).

For world-data,

  1. Maybe some of the above for nearby agents (vehicles and maybe also pedestrians).
  2. Local ground plane ...
  3. ... or, on script startup, the whole map so that we can use our pose to infer local ground plane.
  4. Nearby static obstacles' bounding boxes.
Get control input into the game engine
  • Make a non-Lua interface to accept throttle/brake (no gears) and steering (angle or, better, torque).
  • Clarify the mapping between these inputs and the ego-data that we read.

To start with, this can be a socket or just a simple file that I'll read repeatedly. Eventually, it will be a ROS twist, or Ackerman steering command. Probably this will integrate with the data-aggregator object mentioned above.

For the clarification part, one issue I see is that there are multiple possible ways to provide input. A human playing with keyboard can go from nothing W (resp., ++s+) to instantaneously turn on forward (reverse) drive torque at some set level (or set the rate of torque increase/decrease to something fixed?). Alternately, if they use a pedal input or controller ... something else happens (not sure; I don't have those).

For steering, using keys A and D probably starts the steering column moving at a set rate, while using a wheel or controller probably directly updates a steering set point.

On the other hand, if a computer is doing the driving, I see in the code two different ways for steering to be actuated:

  1. Steering "times" (which I think are effectively steering angle set point, under the assumption that steering changes at a constant rate until that time is reached). I think this is used by more "realistic" controllers like Courseplay or Autodrive.
  2. Specified-curvature paths, where the steering angle is (I think) immediately realized. I think this is used for on-rails controllers like the built-in road traffic.
Train a dynamics model

Once I have ego-motion data coming out (and forcing information like ground plane and control inputs), I can start to

  • Train a neural dynamics model.

Given

  1. pose $\mathbf{x}$ (translational and rotational),
  2. pose rate $\dot{\mathbf{x}}$
  3. fixed pose-observer function $g$, and
  4. forcing inputs $\mathbf{u}$ (both controls and external forces--ultimately, all contacts, but, for now, just the relative orientation of and distance from local ground plane)

this will be a network $F$ that will generate dynamics like this:

$\frac{\mathrm{d} \mathbf{x}} {\mathrm{d} t} = \dot{\mathbf{x}}$ $\frac{\mathrm{d}^2 \mathbf{x}}{\mathrm{d} t^2} = F(\dot{\mathbf{x}}, g(\mathbf{x}), \mathbf{u})$

That is, the network outputs the accelerations only, not whole whole second-order ODE.

Another important restriction is $g$, which I'll be constructing to ensure that we do not have free access to the absolute system state. Our dynamics must be invariant to pose. I'll let some selected information through, like the angle from the gravity vector (that is, the pitch and roll, really).

This will be trained as a Neural ODE, though probably with simple discretize-then-optimize differentiation rather than full adjoint backprop. - [ ] Skim [[Constrained Optimization and Optimal Control for Partial Differential Equations - Discretization of Optimal Control Problems.pdf]]

Try doing some MPC with our dynamics model
  • Try low-level MPC with the neural dynamics predictor. On an empty plain (no obstacles), generate some random but reasonably-feasible paths $\hat{\mathbf{x}}(t)$ (like some gentle arcs or forward-only Dubins curves), and then (at each timestep) find the optimal control trajectory $\mathbf{u}(t)$ such that, if we integrate the joint first-order ODE $(\dot{\mathbf{x}}, F)$, the resulting predicted trajectory is as close as possible to $\hat{\mathbf{x}}(t)$ pointwise.
Improve point clouds
  • Use COLMAP to estimate camera parameters from a few frames, and for a few vehicles like this guy says he did.
  • Follow notes from [[FS22 2023-05-16]] to select a better depth network.
Redo some of the above prototyping with ROS
  • Set up ROS2 on this machine; maybe in a container.
  • Publish as ROS topics these segmented point clouds in some usable format, with suitably backdated timestamps.
  • Assign the segmented classes to different bins based on how bad (or good) it would be to drive on/through them.
  • Publish topics from the simulator
  • Publish acceleration and orientation data as an IMU topic
  • Maybe publish some sparse ray tracings as ultrasound topics.
  • Accept control commands as listened-to topics.
  • Use e.g. RTAB-Map to integrate these.
  • Use a standard global planner to find coarse paths around RTAB-Map's obstacles.
  • Either use an off-the-shelf MPC controller or write my own. Using off-the-shelf C++ ones mostly hinges on how difficult it ends up being to compile their code against Torch and so call my trained model from C++.