2

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 cylinder (wheel) through a revolute joint.

Question

After constructing my MIP plant, plant.num_positions() returns 8 and plant.num_velocities() returns 7, i.e. the total continuous_state size is 15

  1. How do I make sense of this large number of states?
  2. I suppose one of these represents the angle that the pole makes with the vertical. How do I find that?
  3. Also, why is num_positions() not equal to num_velocities()?
Rufus
  • 5,111
  • 4
  • 28
  • 45

2 Answers2

3

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

Russ Tedrake
  • 4,703
  • 1
  • 7
  • 10
  • I suppose by spatial velocity, you meant angular velocities (rolldot, pitchdot, yawdot)? – Rufus Apr 17 '19 at 00:14
  • Side question, are there plans to make use of the `` tag from `.sdf` to allow drake to simulate the actual sensor values that gets fed to the controller? – Rufus Apr 17 '19 at 09:14
  • Supporting IMU parsing isn't on the main developers short list. We have some gyro + accelerometer classes that just need to be updated e.g. https://drake.mit.edu/doxygen_cxx/classdrake_1_1systems_1_1sensors_1_1_gyroscope.html , and external code contributions. – Russ Tedrake Apr 17 '19 at 09:40
  • Is it possible to specify to what link this quaternion-based floating base gets attached to? – Rufus May 22 '19 at 20:57
  • After a bit of trial and error, I managed to figure out that from the vector output of `get_state_output_port`, index 6 is the z position. I couldn't seem to find any documentation regarding this index mapping / order of the generalized positions. Is it documented anywhere? – Rufus May 23 '19 at 15:57
1

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₆).
Rufus
  • 5,111
  • 4
  • 28
  • 45