What is the structure of the generalized velocities of a robot consisting of a free body base?

Question

I have encountered a rather theoretical problem upon using the Drake library. My robot consists of a free body base (floating base), which has a traditional robot arm attached to it consisting of joints and links.

So let's say, that the generalized state vector for this robot is of size 63. (got from Drake). The first 7 entries of this state vector are the 3 Cartesian positions and the 4 quaternions for the orientation. That means, that the generalized velocity vector is 62 (6 entries for linear and angular velocity). If I construct my state vector based on the order scheme described earlier (firstly the 3 positions, secondly the 4 quaternions and then the 56 joint states of the robot arm) and then set the position of the robot accordingly, then Drake runs and does not give any errors.

However, taking the Jacobian of the end effector of the robot gives a strange mismatch. It seems to me, that when getting the Jacobian x_dot = J * q_dot with the CalcJacobianTranslationalVelocity command, the order of the generalized velocity vector is swapped. To be more precise, the order is:

1. The 3 angular velocities of the base
2. The 3 linear velocities of the base
3. The 56 other generalized velocities from the robot arm

I am quite confused by this, since I could manipulate and set the robot's state using SetPositions and SetVelocities with the state & velocity vectors where the points 1. and 2. from the above list were swapped. Am I missing something?

Thank You in advance for any help,

Best regards,

Robert

Answer 1

We follow the convention that angular quantities precede translational quantities. For a floating base that means the four quaternions come before the xyz translations, just as you saw for the velocities.

Answer 2

Probably the safest way (in terms of guaranteeing correctness) for you to get this right is to do it with a context and Drake's APIs. Eg. you can use MBP::SetFreeBodySpatialVelocity() for your floating base and Joint methods for the rest of the arm (e.g. RevoluteJoint::set_angular_rate(). Once done, you can retrieve a consistent vector of velocities with MultibodyPlant::GetVelocities().

If you want a more "index" based approach you can do the following. Each floating body's dofs start at Body::floating_velocities_start() (read docs please, this is for the full state vector. 6 numbers in the order described by @sherm in the previous answer). For joints you can retrieve Joint::velocity_start() and Joint::num_velocities().

WARNING: while Body::floating_velocities_start() indexes the full state vector x, Joint::velocity_start() indexes only the generalized velocities vector v. Please refer to docs.