model test3d
  import _3D = BondGraph._3D;

  // ---------------------------------------------------------------------------
  // Parameters
  // ---------------------------------------------------------------------------
  parameter Real J[3,3] = [0.05, 0, 0;
                           0, 0.06, 0;
                           0, 0, 0.02] "Body inertia tensor in body frame";
  parameter Real m[3,3] = [1, 0, 0;
                           0, 1, 0;
                           0, 0, 1] "Translational inertia (mass matrix)";
  parameter Real r_body[3] = {0.5, 0, 0} "Vector from hinge point to COM in body frame";
  parameter Real R_rot[3,3] = [0.02, 0, 0;
                               0, 0.02, 0;
                               0, 0, 0.02] "Rotational damping matrix";
  parameter Real g = 9.81 "Gravity magnitude";

  // ---------------------------------------------------------------------------
  // Rotational dynamics (body frame)
  // ---------------------------------------------------------------------------
  _3D.J1 w_com(N = 6, s = {1, -1, -1, -1, -1, -1}) "Common angular velocity junction";
  _3D.I J_inertial(I = J) "Angular momentum storage";
  _3D.R hinge_r(R = R_rot) "Rotational damping";
  _3D.fsensor3d wsensor "Measures body angular velocity omega";

  // Gyroscopic term implementation (power-conserving structure):
  // - TF maps omega -> h = J*omega (angular momentum)
  // - mGY with modulation S(omega) generates tau_g = omega x h
  _3D.TF Hmap(n = J) "Maps between omega and angular-momentum port";
  _3D.mGY gyro "Modulated gyrator for omega x h coupling";

  // ---------------------------------------------------------------------------
  // Kinematics and frame transforms
  // ---------------------------------------------------------------------------
  _3D.TransRotUtils.mTFrot3lin mTFrot3lin(r_body = r_body) "v = omega x r_body coupling";
  _3D.J0 j0(N = 3, s = {1, 1, -1}) "Velocity composition at COM in body frame";
  _3D.J1 v_p_bff(N = 2, s = {1, 1}) "Hinge-point velocity (body frame)";
  _3D.J1 v_com_bff(N = 2, s = {-1, -1}) "COM velocity (body frame)";
  _3D.TransRotUtils.rTF3D rTF3D "Body <-> inertial frame transform";

  // ---------------------------------------------------------------------------
  // Translational dynamics (inertial frame)
  // ---------------------------------------------------------------------------
  _3D.J1 v_com_in(N = 4, s = {1, -1, -1, 1}) "COM velocity in inertial frame";
  _3D.I mass(I = m) "Translational momentum storage";
  _3D.Sf ground(f0 = {0, 0, 0}) "Ground translational velocity";
  _3D.mSe gravity "Gravity effort source";
  _3D.fsensor3d v_inertial "Measures inertial COM velocity";

  // ---------------------------------------------------------------------------
  // States and signals
  // ---------------------------------------------------------------------------
  Real omega[3] "Body angular velocity";
  Real h[3] "Body angular momentum J*omega";
  Real S_omega[3,3] "Skew matrix of omega";

  Real phi(start = 0) "Roll";
  Real theta(start = 0.1) "Pitch";
  Real psi(start = 0) "Yaw";
  Real euler_dot[3] "{phi_dot, theta_dot, psi_dot}";

  Real x(start = -0.5) "COM x (inertial)";
  Real y(start = 0) "COM y (inertial)";
  Real z(start = -0.2) "COM z (inertial)";

  Modelica.Blocks.Sources.RealExpression xSig(y = x);
  Modelica.Blocks.Sources.RealExpression ySig(y = y);
  Modelica.Blocks.Sources.RealExpression zSig(y = z);
  Modelica.Blocks.Sources.RealExpression phiSig(y = phi);
  Modelica.Blocks.Sources.RealExpression thetaSig(y = theta);
  Modelica.Blocks.Sources.RealExpression psiSig(y = psi);
  vis3d vis3d1;

equation
  // ---------------------------------------------------------------------------
  // Rotational network connections
  // ---------------------------------------------------------------------------
  connect(w_com.P[1], J_inertial.p);
  connect(w_com.P[2], mTFrot3lin.pR);
  connect(w_com.P[3], wsensor.p);
  connect(w_com.P[4], hinge_r.p);
  connect(w_com.P[5], gyro.p1);
  connect(w_com.P[6], Hmap.p1);
  connect(Hmap.p2, gyro.p2);

  // ---------------------------------------------------------------------------
  // Body-frame COM kinematics and frame transform
  // ---------------------------------------------------------------------------
  connect(mTFrot3lin.pT, j0.P[1]);
  connect(v_com_bff.P[1], j0.P[2]);
  connect(j0.P[3], v_p_bff.P[1]);
  connect(ground.p, v_p_bff.P[2]);

  connect(v_com_bff.P[2], rTF3D.p2);
  connect(rTF3D.p1, v_com_in.P[1]);

  // Feed frame-transform angles from state variables.
  connect(phiSig.y, rTF3D.phi);
  connect(thetaSig.y, rTF3D.theta);
  connect(psiSig.y, rTF3D.psi);
  connect(phiSig.y, vis3d1.phi);
  connect(thetaSig.y, vis3d1.theta);
  connect(psiSig.y, vis3d1.psi);

  // ---------------------------------------------------------------------------
  // Translational dynamics connections
  // ---------------------------------------------------------------------------
  connect(v_com_in.P[2], mass.p);
  connect(gravity.p, v_com_in.P[3]);
  connect(v_com_in.P[4], v_inertial.p);

  // Gravity as effort in inertial frame.
  gravity.e0 = 0;
  gravity.e1 = 0;
  gravity.e2 = g;

  // ---------------------------------------------------------------------------
  // Gyroscopic modulation and H-map related equations
  // ---------------------------------------------------------------------------
  omega = {wsensor.f0, wsensor.f1, wsensor.f2};
  h = J * omega;

  // Skew matrix S(omega) so that S(omega)*x = omega x x.
  S_omega = [0, -omega[3], omega[2];
             omega[3], 0, -omega[1];
             -omega[2], omega[1], 0];
  gyro.m = S_omega;

  // ---------------------------------------------------------------------------
  // 3D attitude kinematics (explicit H(q) mapping form)
  // Using ZYX convention with body rates omega -> Euler rates.
  // Note: singular at cos(theta)=0.
  // ---------------------------------------------------------------------------
  euler_dot[1] = omega[1] + sin(phi) * tan(theta) * omega[2] + cos(phi) * tan(theta) * omega[3];
  euler_dot[2] = cos(phi) * omega[2] - sin(phi) * omega[3];
  euler_dot[3] = sin(phi) / cos(theta) * omega[2] + cos(phi) / cos(theta) * omega[3];

  der(phi) = euler_dot[1];
  der(theta) = euler_dot[2];
  der(psi) = euler_dot[3];

  // ---------------------------------------------------------------------------
  // Position integration
  // ---------------------------------------------------------------------------
  der(x) = v_inertial.f0;
  der(y) = v_inertial.f1;
  der(z) = v_inertial.f2;

  connect(xSig.y, vis3d1.x);
  connect(ySig.y, vis3d1.y);
  connect(zSig.y, vis3d1.z);

  annotation(
    uses(Modelica(version = "4.1.0")),
    experiment(StartTime = 0, StopTime = 10, Tolerance = 1e-06, Interval = 0.02),
    Diagram(coordinateSystem(extent = {{-140, 160}, {200, -160}})));
end test3d;