How to make sense of the continuous state vector?

Question

Background

I am trying to balance a mobile inverted pendulum (i.e. segway).

For the simulation, I created a simple robot that involves a pole attached to a cyl

Answer 1

By default, the urdf/sdf parser adds the system with a quaternion-based floating base. This floating base joint has 7 position variables (x,y,z + quaternion) and 6 velocities (xdot, ydot, zdot, + spatial velocity). You should use the multibody API to get the variable in the right order.

You can add a joint between the world link and your base (either in the urdf/sdf or in your code) to replace that implied floating joint with e.g. a prismatic joint at the base.

There are lots of examples of how to simulate it, in drake, but also at http://underactuated.mit.edu

Answer 2

From Spatial Velocity

Spatial velocities are 6-element quantities that are pairs of ordinary 3-vectors. Elements 0-2 are the angular velocity component while elements 3-5 are the translational velocity.

From QuaternionFloatingMobilizer

  // Note: In the context we store the quaternion's components first (q₀ to q₃),
  // followed by the position vector components (q₄ to q₆).