pre-big change
This commit is contained in:
158
test3d.mo
Normal file
158
test3d.mo
Normal file
@@ -0,0 +1,158 @@
|
||||
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;
|
||||
Reference in New Issue
Block a user