|
19 | 19 | #include <mujoco/mjsan.h> // IWYU pragma: keep |
20 | 20 | #include "engine/engine_core_util.h" |
21 | 21 | #include "engine/engine_crossplatform.h" |
| 22 | +#include "engine/engine_inline.h" |
22 | 23 | #include "engine/engine_memory.h" |
23 | 24 | #include "engine/engine_passive.h" |
24 | 25 | #include "engine/engine_sleep.h" |
@@ -705,6 +706,186 @@ static void mjd_rne_vel(const mjModel* m, mjData* d) { |
705 | 706 | } |
706 | 707 |
|
707 | 708 |
|
| 709 | +// 3x3 sub-blocks of (d qfrc_bias / d qvel) for a standalone free body |
| 710 | +// outputs the two 3x3 blocks lin and rot such that the rotational columns |
| 711 | +// of the full 6x6 bias Jacobian B are [-mass*lin; rot] (linear columns are zero) |
| 712 | +// |
| 713 | +// derivation: let R = xmat, s = xipos - xpos, w = R*qvel[rot] (world angular velocity), |
| 714 | +// Iw = ximat * diag(body_inertia) * ximat' (world inertia about the CoM). with qacc = 0, |
| 715 | +// the CoM acceleration is w x (w x s) and the world bias force/torque at the CoM are |
| 716 | +// f = mass * w x (w x s), tau = w x Iw*w |
| 717 | +// projected onto the joint coordinates: bias = [f; R'*(s x f + tau)]. differentiating |
| 718 | +// w.r.t. the rotational dofs (through w = R*qvel[rot]), with K = [w x s]_x + [w]_x [s]_x: |
| 719 | +// d f / d w = -mass * K => lin = K * R |
| 720 | +// d tau / d w = [w]_x Iw - [Iw*w]_x => rot = R' * (-mass*[s]_x K + d tau/d w) * R |
| 721 | +static void freeBias_vel_blocks(mjtNum mass, const mjtNum R[9], const mjtNum Xi[9], |
| 722 | + const mjtNum inertia[3], const mjtNum s[3], |
| 723 | + const mjtNum qvel_rot[3], mjtNum lin[9], mjtNum rot[9]) { |
| 724 | + // world-frame angular velocity |
| 725 | + mjtNum w[3]; |
| 726 | + mji_mulMatVec3(w, R, qvel_rot); |
| 727 | + |
| 728 | + // world-frame inertia about CoM: Iw = Xi * diag(inertia) * Xi^T |
| 729 | + mjtNum Xi_I[9]; |
| 730 | + for (int i=0; i < 3; i++) { |
| 731 | + Xi_I[3*i+0] = Xi[3*i+0] * inertia[0]; |
| 732 | + Xi_I[3*i+1] = Xi[3*i+1] * inertia[1]; |
| 733 | + Xi_I[3*i+2] = Xi[3*i+2] * inertia[2]; |
| 734 | + } |
| 735 | + mjtNum Iw[9]; |
| 736 | + Iw[0] = Xi_I[0]*Xi[0] + Xi_I[1]*Xi[1] + Xi_I[2]*Xi[2]; |
| 737 | + Iw[4] = Xi_I[3]*Xi[3] + Xi_I[4]*Xi[4] + Xi_I[5]*Xi[5]; |
| 738 | + Iw[8] = Xi_I[6]*Xi[6] + Xi_I[7]*Xi[7] + Xi_I[8]*Xi[8]; |
| 739 | + Iw[1] = Iw[3] = Xi_I[0]*Xi[3] + Xi_I[1]*Xi[4] + Xi_I[2]*Xi[5]; |
| 740 | + Iw[2] = Iw[6] = Xi_I[0]*Xi[6] + Xi_I[1]*Xi[7] + Xi_I[2]*Xi[8]; |
| 741 | + Iw[5] = Iw[7] = Xi_I[3]*Xi[6] + Xi_I[4]*Xi[7] + Xi_I[5]*Xi[8]; |
| 742 | + |
| 743 | + // intermediate vectors: ws = w x s (CoM offset velocity), Iww = Iw * w (angular momentum) |
| 744 | + mjtNum ws[3], Iww[3]; |
| 745 | + mji_cross(ws, w, s); |
| 746 | + mji_mulMatVec3(Iww, Iw, w); |
| 747 | + |
| 748 | + // K = [w x s]_x + [w]_x [s]_x = s w^T - (w . s) I + [ws]_x |
| 749 | + mjtNum w_dot_s = w[0]*s[0] + w[1]*s[1] + w[2]*s[2]; |
| 750 | + mjtNum K[9]; |
| 751 | + K[0] = s[0]*w[0] - w_dot_s; |
| 752 | + K[1] = s[0]*w[1] - ws[2]; |
| 753 | + K[2] = s[0]*w[2] + ws[1]; |
| 754 | + |
| 755 | + K[3] = s[1]*w[0] + ws[2]; |
| 756 | + K[4] = s[1]*w[1] - w_dot_s; |
| 757 | + K[5] = s[1]*w[2] - ws[0]; |
| 758 | + |
| 759 | + K[6] = s[2]*w[0] - ws[1]; |
| 760 | + K[7] = s[2]*w[1] + ws[0]; |
| 761 | + K[8] = s[2]*w[2] - w_dot_s; |
| 762 | + |
| 763 | + // lin = K * R |
| 764 | + mji_mulMatMat3(lin, K, R); |
| 765 | + |
| 766 | + // C = -mass * [s]_x K + [w]_x Iw - [Iww]_x, column by column |
| 767 | + // the last term (-[Iww]_x) is the negated cross-product matrix, added via ternaries |
| 768 | + mjtNum C[9]; |
| 769 | + for (int c=0; c < 3; c++) { |
| 770 | + mjtNum s_x_K_row0 = s[1]*K[6+c] - s[2]*K[3+c]; |
| 771 | + mjtNum s_x_K_row1 = s[2]*K[c] - s[0]*K[6+c]; |
| 772 | + mjtNum s_x_K_row2 = s[0]*K[3+c] - s[1]*K[c]; |
| 773 | + |
| 774 | + mjtNum w_x_Iw_row0 = w[1]*Iw[6+c] - w[2]*Iw[3+c]; |
| 775 | + mjtNum w_x_Iw_row1 = w[2]*Iw[c] - w[0]*Iw[6+c]; |
| 776 | + mjtNum w_x_Iw_row2 = w[0]*Iw[3+c] - w[1]*Iw[c]; |
| 777 | + |
| 778 | + C[c] = -mass * s_x_K_row0 + w_x_Iw_row0 + (c == 1 ? Iww[2] : (c == 2 ? -Iww[1] : 0)); |
| 779 | + C[3 + c] = -mass * s_x_K_row1 + w_x_Iw_row1 + (c == 0 ? -Iww[2] : (c == 2 ? Iww[0] : 0)); |
| 780 | + C[6 + c] = -mass * s_x_K_row2 + w_x_Iw_row2 + (c == 0 ? Iww[1] : (c == 1 ? -Iww[0] : 0)); |
| 781 | + } |
| 782 | + |
| 783 | + // rot = R^T * C * R |
| 784 | + mjtNum tmp[9]; |
| 785 | + mji_mulMatTMat3(tmp, R, C); |
| 786 | + mji_mulMatMat3(rot, tmp, R); |
| 787 | +} |
| 788 | + |
| 789 | + |
| 790 | +// 6x6 block B = d qfrc_bias / d qvel for a standalone free body |
| 791 | +// assembles the full 6x6 from the 3x3 sub-blocks computed by freeBias_vel_blocks |
| 792 | +// rows/cols ordered like the free joint dofs: [linear(3); rotational(3)] |
| 793 | +// linear columns are zero: the bias force does not depend on linear velocity |
| 794 | +void mjd_freeBias_vel(const mjModel* m, const mjData* d, int jnt, mjtNum B[36]) { |
| 795 | + int body = m->jnt_bodyid[jnt]; |
| 796 | + int adr = m->jnt_dofadr[jnt]; |
| 797 | + mjtNum mass = m->body_mass[body]; |
| 798 | + const mjtNum* R = d->xmat + 9*body; // body -> world |
| 799 | + const mjtNum* Xi = d->ximat + 9*body; // inertia -> world |
| 800 | + const mjtNum* inertia = m->body_inertia + 3*body; |
| 801 | + |
| 802 | + // CoM offset from joint origin, world frame |
| 803 | + mjtNum s[3]; |
| 804 | + mji_sub3(s, d->xipos + 3*body, d->xpos + 3*body); |
| 805 | + |
| 806 | + mjtNum lin[9], rot[9]; |
| 807 | + freeBias_vel_blocks(mass, R, Xi, inertia, s, d->qvel + adr + 3, lin, rot); |
| 808 | + |
| 809 | + mju_zero(B, 36); |
| 810 | + for (int r=0; r < 3; r++) { |
| 811 | + for (int c=0; c < 3; c++) { |
| 812 | + B[6*r + 3+c] = -mass * lin[3*r+c]; |
| 813 | + B[6*(3+r) + 3+c] = rot[3*r+c]; |
| 814 | + } |
| 815 | + } |
| 816 | +} |
| 817 | + |
| 818 | + |
| 819 | +// 6x6 block A = M - h * (d qfrc_smooth / d qvel) for the free joint of a standalone body |
| 820 | +// returns 1 and writes A if jnt is the free joint of a standalone awake body, 0 otherwise |
| 821 | +// requires valid d->qDeriv rows for the block, computed with flg_bias = 0; the bias |
| 822 | +// derivative excluded from qDeriv is added here via freeBias_vel_blocks |
| 823 | +int mjd_freeMhat(const mjModel* m, const mjData* d, int jnt, mjtNum h, mjtNum A[36]) { |
| 824 | + // must be a free joint |
| 825 | + if (m->jnt_type[jnt] != mjJNT_FREE) { |
| 826 | + return 0; |
| 827 | + } |
| 828 | + |
| 829 | + int body = m->jnt_bodyid[jnt]; |
| 830 | + int adr = m->jnt_dofadr[jnt]; |
| 831 | + int tree = m->dof_treeid[adr]; |
| 832 | + mjtNum mass = m->body_mass[body]; |
| 833 | + |
| 834 | + // must be a standalone 6-DOF tree with no children, awake |
| 835 | + if (m->tree_dofnum[tree] != 6 || |
| 836 | + m->body_subtreemass[body] != mass || |
| 837 | + !d->tree_awake[tree]) { |
| 838 | + return 0; |
| 839 | + } |
| 840 | + |
| 841 | + // D rows of a standalone free body are exactly the 6x6 block (D sparsity is tree-local); |
| 842 | + // guard the gathers below against any violation of this invariant |
| 843 | + if (m->D_rownnz[adr] != 6) { |
| 844 | + return 0; |
| 845 | + } |
| 846 | + |
| 847 | + // A = M block (gather from sparse lower triangle) |
| 848 | + mju_zero(A, 36); |
| 849 | + for (int r=0; r < 6; r++) { |
| 850 | + int rowadr = m->M_rowadr[adr+r]; |
| 851 | + int rownnz = m->M_rownnz[adr+r]; |
| 852 | + for (int k=0; k < rownnz; k++) { |
| 853 | + int c = m->M_colind[rowadr+k] - adr; |
| 854 | + A[6*r+c] = A[6*c+r] = d->M[rowadr+k]; |
| 855 | + } |
| 856 | + } |
| 857 | + |
| 858 | + // A -= h * qDeriv block (actuator and passive derivatives) |
| 859 | + for (int r=0; r < 6; r++) { |
| 860 | + int rowadr = m->D_rowadr[adr+r]; |
| 861 | + int rownnz = m->D_rownnz[adr+r]; |
| 862 | + for (int k=0; k < rownnz; k++) { |
| 863 | + int c = m->D_colind[rowadr+k] - adr; |
| 864 | + A[6*r+c] -= h * d->qDeriv[rowadr+k]; |
| 865 | + } |
| 866 | + } |
| 867 | + |
| 868 | + // A -= h * d(qfrc_smooth)/d(qvel) for the bias term missing from qDeriv; |
| 869 | + // qfrc_smooth includes -qfrc_bias, so subtracting its derivative adds +h*B |
| 870 | + mjtNum s[3]; |
| 871 | + mji_sub3(s, d->xipos + 3*body, d->xpos + 3*body); |
| 872 | + |
| 873 | + mjtNum lin[9], rot[9]; |
| 874 | + freeBias_vel_blocks(mass, d->xmat + 9*body, d->ximat + 9*body, |
| 875 | + m->body_inertia + 3*body, s, d->qvel + adr + 3, lin, rot); |
| 876 | + |
| 877 | + mjtNum h_mass = -h * mass; |
| 878 | + for (int r=0; r < 3; r++) { |
| 879 | + for (int c=0; c < 3; c++) { |
| 880 | + A[6*r + 3+c] += h_mass * lin[3*r+c]; |
| 881 | + A[6*(3+r) + 3+c] += h * rot[3*r+c]; |
| 882 | + } |
| 883 | + } |
| 884 | + |
| 885 | + return 1; |
| 886 | +} |
| 887 | + |
| 888 | + |
708 | 889 | //--------------------- utility functions for (d force / d vel) Jacobians -------------------------- |
709 | 890 |
|
710 | 891 | // add J'*B*J to qDeriv |
|
0 commit comments