CODEWITHSUNDEEP
drake
Shubham Vyas
Shubham Vyas

Reputation: 165

Understanding Linearization with free-floating bodies and Quaternion States

I am trying to linearize a free-floating system with a free-floating base and 3 joints (j1, j2, j3). As I understand the positions part of the system state is given by the vector (this matches MultibodyPlant::num_positions()):

q (10x1) = [base_quaternion (4x1), base_lin_position (3x1), j1_pos, j2_pos, j3_pos]

Since angular velocity requires only 3 components, the velocity part of the system state is written as (this matches MultibodyPlant::num_velocities()):

q_dot (9x1) = [base_rot_vel (3x1), base_lin_vel (3x1), j1_vel, j2_vel, j3_vel]

Using this, the full system state is given as (this works when using MultibodyPlant::SetPositionsAndVelocities) :

X (19x1) = [q (10x1),q_dot (9x1)]

With this, the system acceleration resulting from its dynamics and control forces X_dot = f(X, U) would be written as:

X_dot (18x1)= [q_dot (9x1), q_ddot (9x1)]

Due to the difference in the representation of rotations and angular velocities, the number of terms needed to define X and X_dot is different.

This brings to the following questions while linearizing the system about a point using Linearize:

  1. The A and B matrices after linearization of a continuous-time MultibodyPlant represent the equation X_dot = A*X + B*u. However, there seems to be a mismatch here in the sizes of the arrays/matrices involved as X_dot (18x1) is different from matrices given by Linearize: A (19x19) and B (19x3). I don't then understand what accelerations does the matrix X_dot from the linear system equation represents with its size 19x1?

  2. The above question is only for a continuous-time case. For a discrete-time system,the following equations hold without any issues with the matrix sizes:X[n+1] = A_d * X[n] + B_d * u[n]. However, it is not clear how the quaternion properties are maintained during this linearized forward simulation?

Upvotes: 2

Views: 140

Answers (1)

Mitiguy
Mitiguy

Reputation: 191

I think there is misunderstanding in the notation since q_dot ≠ v. Instead, q_dot is simply the ordinary time-derivative of q.

    q (10x1) =     [base_quaternion (4x1), base_lin_position (3x1), j1_pos, j2_pos, j3_pos]
q_dot (10x1) = d/dt[base_quaternion (4x1), base_lin_position (3x1), j1_pos, j2_pos, j3_pos]

Angular velocity only has 3 components, so v (the velocity part of the system state) and its time-derivative v_dot are:

    v (9x1) =     [base_rot_vel (3x1), base_lin_vel (3x1), j1_vel, j2_vel, j3_vel]
v_dot (9x1) = d/dt[base_rot_vel (3x1), base_lin_vel (3x1), j1_vel, j2_vel, j3_vel]

The full system state X and its time-derivative x_dot are shown below.

    X (19x1) = [q     (10x1),  v    ( 9x1)]
X_dot (19x1) = [q_dot (10x1),  v_dot (9x1)]

Note: X ≠ [q, q_dot], instead X = [q, v]. Similarly, X_dot ≠ [q_dot, q_ddot], instead X = [q_dot, v_dot].

Upvotes: 1

Related Questions