ADDED: Function that caculates the linearized rotational dynamics - #83
ADDED: Function that caculates the linearized rotational dynamics#83harrywangsw wants to merge 1 commit into
Conversation
| Linearized input matrix. | ||
| Shape: (6, 3) | ||
| """ | ||
| J_inv = np.linalg.inv(J) |
There was a problem hiding this comment.
It's not the worst thing ever with a 3x3 matrix, but it's good practice to do operations like matrix inversions once where possible (inertia matrix is such a case) because it can be computationally expensive. I would suggest adding an optional J_inv argument to the function signature and only performing this inversion here if it is not provided.
| q1, q2, q3 = q_0 | ||
| omega1, omega2, omega3 = omega_0 | ||
|
|
||
| A_omega_omega = J_inv@(skew(J @ omega_0)-skew(omega_0)@J) |
There was a problem hiding this comment.
Please add some comments clarifying the process here. Even for the purposes of review, having it bare means I have to try to figure out what you're doing/why at each line before I can even assess whether it's right. If someone with less context/experience has to read this code later it could be very difficult for them.
There was a problem hiding this comment.
I pushed a pdf latex file with more intermediate steps filled out. Should I add more here too?
|
|
||
| B = np.vstack([ | ||
| J_inv, | ||
| np.zeros((3, 3)), |
There was a problem hiding this comment.
I'm not sure if this is a mistake or a convention thing that I missed. I would expect J_inv to be the bottom term here as it applies to torques, which directly contribute to dot{omega}
There was a problem hiding this comment.
I have omega_dot in the first 3 components of the state vector and q_dot in the last 3. So the J_inv's position is correct. However, I realize that this is kinda unconventional so yes, I'll swap those two.
2 participants
I've also added a pdf file with my derivations