Merge pull request #9 from JuliaComputing/fbc/angle-controlled-3d

Add closed-loop 3D model with angle controller and planar-equivalence test

Author: baggepinnen

README.md

@@ -13,6 +13,10 @@ Plant models:
   position/velocity/tilt/tilt-rate out.
 - `DyadBot3D`: three-dimensional model with two individually spinning wheels
   with slip-based ground contact, allowing the body to tilt.
+- `RollingDyadBot3D`: three-dimensional model with an ideal no-slip rolling
+  wheel axle (`MultibodyComponents.RollingWheelSet`). Same interface and
+  parameters as `PlanarDyadBot`; for motion in the vertical plane it is
+  dynamically equivalent to the planar model.
 
 Closed-loop models around the planar plant:
 
@@ -27,6 +31,13 @@ Closed-loop models around the planar plant:
   integral action (state-space system loaded from `data/lqg_*.csv`).
 - `LQGTuningDyadBot`: analysis model used by the LQG design script.
 
+Closed-loop models around the 3D plant:
+
+- `AngleControlledDyadBot3D`: `RollingDyadBot3D` stabilized by the same
+  `AngleController` as `AngleControlledDyadBot`; with in-plane motion the
+  closed-loop response is identical to the planar model
+  (`test/test_stabilization.jl` verifies this).
+
 ## Controller tuning scripts (`scripts/`)
 
 Each script activates the `scripts/` environment and can be run directly:

dyad/closed_loop.dyad

@@ -230,6 +230,79 @@ metadata {
 }
 end
 
+"""
+Three-dimensional version of `AngleControlledDyadBot`: the ideal-rolling 3D
+robot `RollingDyadBot3D` stabilized by the same `AngleController` with the
+same gains. The robot starts from a tilted initial configuration in the
+vertical plane and the controller recovers the upright pose; since the motion
+stays in the plane, the closed-loop response is identical to that of the
+planar model (verified by `test/test_stabilization.jl`).
+"""
+component AngleControlledDyadBot3D
+  world = MultibodyComponents.World(g = 9.82, nominal_length = 0.1) {^world}
+  plant = RollingDyadBot3D(phi0 = phi0) {^plant}
+  controller = AngleController(k_angle = k_angle, Ti_angle = Ti_angle, Td_angle = Td_angle) {^controller}
+  "Proportional gain of the angle controller"
+  parameter k_angle::Real = 0.487401
+  "Integrator time constant of the angle controller"
+  parameter Ti_angle::Time = 0.0587352
+  "Derivative time constant of the angle controller"
+  parameter Td_angle::Time = 0.0420526
+  "Initial tilt angle of the body"
+  parameter phi0::Angle = 0.1
+relations
+  connect(controller.torque, plant.torque) {^id4}
+  connect(plant.theta, controller.measurement) {^id5}
+metadata {
+  "Dyad": {
+    "tests": {"case1": {"stop": 5}},
+    "icons": {"default": "dyad://DyadBotComponents/DyadBot3D.svg"}
+  },
+  "_links": {
+    "world": {
+      "Dyad": {
+        "placement": {
+          "diagram": {"iconName": "default", "x1": 200, "y1": 250, "x2": 100, "y2": 150, "rot": 180}
+        },
+        "tags": []
+      }
+    },
+    "plant": {
+      "Dyad": {
+        "placement": {
+          "diagram": {"iconName": "default", "x1": 560, "y1": 400, "x2": 760, "y2": 600, "rot": 0}
+        },
+        "tags": []
+      }
+    },
+    "controller": {
+      "Dyad": {"placement": {"diagram": {"x1": 270, "y1": 420, "x2": 420, "y2": 570}}}
+    },
+    "id4": {
+      "Dyad": {
+        "edges": [{"S": 1, "M": [{"x": 503.75, "y": 495}, {"x": 503.75, "y": 500}], "E": 2}]
+      }
+    },
+    "id5": {
+      "Dyad": {
+        "edges": [
+          {
+            "S": 1,
+            "M": [
+              {"x": 770, "y": 492},
+              {"x": 770, "y": 660},
+              {"x": 220, "y": 660},
+              {"x": 220, "y": 495}
+            ],
+            "E": 2
+          }
+        ]
+      }
+    }
+  }
+}
+end
+
 """
 Discrete-time version of `AngleControlledDyadBot`. Identical to the continuous
 model except that the control system is a sampled-data `DiscreteAngleController`

dyad/segway_3d.dyad

@@ -25,8 +25,8 @@ component DyadBot3D
   rod_right = MultibodyComponents.FixedTranslation(r = [0, 0, -track / 2]) {^rod_right}
   "Small mass at the center of the axle"
   axis_body = MultibodyComponents.Body(m = 0.01, r_cm = [0, 0, 0]) {^axis_body}
-  "Main body extending upward from the axle"
-  body = MultibodyComponents.BodyShape(m = body_mass, I_11 = 0.01 * 0.05 ^ 2, I_22 = 0.01 * 0.03 ^ 2, I_33 = 0.01 * 0.05 ^ 2, r = [0, body_height, 0]) {^body}
+  "Main body extending upward from the axle, with its center of mass in the middle"
+  body = MultibodyComponents.BodyShape(m = body_mass, r = [0, 2 * body_height, 0], r_cm = [0, body_height, 0], I_11 = 0.01 * 0.05 ^ 2, I_22 = 0.01 * 0.03 ^ 2, I_33 = 0.01 * 0.05 ^ 2) {^body}
   damper_left = RotationalComponents.Components.Damper(d = d_wheel) {^damper_left}
   damper_right = RotationalComponents.Components.Damper(d = d_wheel) {^damper_right}
   "Use a compliant (elastic) vertical wheel-ground contact instead of a rigid contact constraint"
@@ -188,4 +188,208 @@ metadata {
     }
   }
 }
+end
+
+"""
+Three-dimensional model of a two-wheeled balancing robot with ideal rolling
+contact.
+
+The two wheels form an ideal no-slip rolling axle
+(`MultibodyComponents.RollingWheelSet`, which resolves the kinematic loop
+through the two wheels internally), and the inverted-pendulum body is mounted
+at the center of the axle through a revolute joint about the axle axis. The
+motor torque acts between the body and the two wheels: half the commanded
+torque drives each wheel and the reaction acts on the body. Viscous motor
+friction acts between the body and each wheel in the same split.
+
+For motion in the vertical plane the model is dynamically equivalent to
+`PlanarDyadBot`: the two wheels share the planar model's wheel mass `m`, spin
+inertia `Iw` and motor friction `d`, and the body parameters are identical.
+
+Inputs and outputs (same interface as `PlanarDyadBot`):
+- `torque`: torque applied by the motor between body and wheels
+- `x`, `x_dot`: position and velocity of the wheel axis along the ground
+- `theta`, `theta_dot`: tilt angle and tilt rate of the body (zero when upright)
+
+The model must be accompanied by a `MultibodyComponents.World` component in
+the top level of the enclosing model.
+"""
+component RollingDyadBot3D
+  wheels = MultibodyComponents.RollingWheelSet(radius = R, m_wheel = m / 2, I_axis = Iw / 2, I_long = Iw / 4, track = track) {^wheels}
+  "Body tilt joint about the axle axis"
+  pitch = MultibodyComponents.Revolute(n = [0, 0, 1]) {^pitch}
+  body = MultibodyComponents.BodyShape(color = [0.2, 0.2, 0.2, 0.9], m = M, r = [0, 2 * L, 0], r_cm = [0, L, 0], I_11 = Ib, I_22 = Ib / 10, I_33 = Ib, radius = 0.03) {^body}
+  "Splits the motor torque equally between the two wheels"
+  gain_half = BlockComponents.Math.Gain(k = 0.5) {^gain_half}
+  torque_left = RotationalComponents.Sources.TorqueSource() {^torque_left}
+  torque_right = RotationalComponents.Sources.TorqueSource() {^torque_right}
+  damper_left = RotationalComponents.Components.Damper(d = d / 2) {^damper_left}
+  damper_right = RotationalComponents.Components.Damper(d = d / 2) {^damper_right}
+  "Motor torque command"
+  torque = Dyad.RealInput() {^torque}
+  "Position of the wheel axis"
+  x = Dyad.RealOutput() {^x}
+  "Body tilt angle"
+  theta = Dyad.RealOutput() {^theta}
+  "Velocity of the wheel axis"
+  x_dot = Dyad.RealOutput() {^x_dot}
+  "Body tilt rate"
+  theta_dot = Dyad.RealOutput() {^theta_dot}
+  "Body mass"
+  parameter M::Mass = 0.1
+  "Total wheel mass (split between the two wheels)"
+  parameter m::Mass = 0.05
+  "Wheel radius"
+  parameter R::Length = 0.04
+  "Distance from wheel axis to body center of mass"
+  parameter L::Length = 0.08
+  "Body moment of inertia about the axle axis"
+  parameter Ib::Inertia = 2.5e-4
+  "Total wheel + motor moment of inertia about the spin axis (split between the two wheels)"
+  parameter Iw::Inertia = 1e-4
+  "Motor viscous friction (split between the two wheels)"
+  parameter d::RotationalDampingConstant = 0.0001
+  "Initial tilt angle of the body"
+  parameter phi0::Angle = 0.0
+  "Distance between the wheels"
+  parameter track::Length = 0.13
+relations
+  initial pitch.phi = phi0
+  initial pitch.w = 0
+  connect(wheels.frame_middle, pitch.frame_a) {^id1}
+  connect(pitch.frame_b, body.frame_a) {^id2}
+  connect(torque, gain_half.u) {^id3}
+  connect(gain_half.y, torque_left.tau, torque_right.tau) {^id4}
+  connect(torque_left.spline, damper_left.spline_a, wheels.axis1) {^id5}
+  connect(torque_right.spline, damper_right.spline_a, wheels.axis2) {^id6}
+  connect(torque_left.support, torque_right.support, damper_left.spline_b, damper_right.spline_b, pitch.axis) {^id7}
+  x = wheels.x
+  theta = pitch.phi
+  x_dot = der(wheels.x)
+  theta_dot = pitch.w
+metadata {
+  "Dyad": {"icons": {"default": "dyad://DyadBotComponents/DyadBot3D.svg"}},
+  "_links": {
+    "wheels": {
+      "Dyad": {
+        "placement": {
+          "diagram": {"iconName": "default", "x1": 400, "y1": 620, "x2": 600, "y2": 820, "rot": 0}
+        },
+        "tags": []
+      }
+    },
+    "pitch": {
+      "Dyad": {"placement": {"diagram": {"x1": 450, "y1": 480, "x2": 550, "y2": 580}}}
+    },
+    "body": {
+      "Dyad": {
+        "placement": {"diagram": {"x1": 350, "y1": 130, "x2": 650, "y2": 430, "rot": 270}}
+      }
+    },
+    "gain_half": {
+      "Dyad": {"placement": {"diagram": {"x1": 180, "y1": 460, "x2": 260, "y2": 540}}}
+    },
+    "torque_left": {
+      "Dyad": {"placement": {"diagram": {"x1": 150, "y1": 620, "x2": 250, "y2": 720}}}
+    },
+    "torque_right": {
+      "Dyad": {"placement": {"diagram": {"x1": 850, "y1": 620, "x2": 750, "y2": 720}}}
+    },
+    "damper_left": {
+      "Dyad": {"placement": {"diagram": {"x1": 150, "y1": 760, "x2": 250, "y2": 860}}}
+    },
+    "damper_right": {
+      "Dyad": {"placement": {"diagram": {"x1": 850, "y1": 760, "x2": 750, "y2": 860}}}
+    },
+    "torque": {
+      "Dyad": {
+        "placement": {
+          "diagram": {"iconName": "default", "x1": 20, "y1": 460, "x2": 100, "y2": 540, "rot": 0}
+        },
+        "tags": []
+      }
+    },
+    "x": {
+      "Dyad": {
+        "placement": {
+          "diagram": {"iconName": "default", "x1": 880, "y1": 300, "x2": 960, "y2": 380, "rot": 0}
+        },
+        "tags": []
+      }
+    },
+    "theta": {
+      "Dyad": {
+        "placement": {
+          "diagram": {"iconName": "default", "x1": 880, "y1": 420, "x2": 960, "y2": 500, "rot": 0}
+        },
+        "tags": []
+      }
+    },
+    "x_dot": {
+      "Dyad": {
+        "placement": {
+          "diagram": {"iconName": "default", "x1": 880, "y1": 540, "x2": 960, "y2": 620, "rot": 0}
+        },
+        "tags": []
+      }
+    },
+    "theta_dot": {
+      "Dyad": {
+        "placement": {
+          "diagram": {"iconName": "default", "x1": 880, "y1": 660, "x2": 960, "y2": 740, "rot": 0}
+        },
+        "tags": []
+      }
+    },
+    "id1": {"Dyad": {"edges": [{"S": 1, "M": [], "E": 2}], "renderStyle": "standard"}},
+    "id2": {"Dyad": {"edges": [{"S": 1, "M": [], "E": 2}], "renderStyle": "standard"}},
+    "id3": {"Dyad": {"edges": [{"S": 1, "M": [], "E": 2}], "renderStyle": "standard"}},
+    "id4": {
+      "Dyad": {
+        "edges": [
+          {"S": 1, "M": [], "E": -1},
+          {"S": -1, "M": [{"x": 200, "y": 670}], "E": 2},
+          {"S": 3, "M": [{"x": 800, "y": 670}], "E": -1}
+        ],
+        "junctions": [{"x": 300, "y": 560}],
+        "renderStyle": "standard"
+      }
+    },
+    "id5": {
+      "Dyad": {
+        "edges": [
+          {"S": 1, "M": [], "E": -1},
+          {"S": -1, "M": [], "E": 2},
+          {"S": 3, "M": [], "E": -1}
+        ],
+        "junctions": [{"x": 350, "y": 700}],
+        "renderStyle": "standard"
+      }
+    },
+    "id6": {
+      "Dyad": {
+        "edges": [
+          {"S": 1, "M": [], "E": -1},
+          {"S": -1, "M": [], "E": 2},
+          {"S": 3, "M": [], "E": -1}
+        ],
+        "junctions": [{"x": 650, "y": 700}],
+        "renderStyle": "standard"
+      }
+    },
+    "id7": {
+      "Dyad": {
+        "edges": [
+          {"S": 1, "M": [], "E": -1},
+          {"S": -1, "M": [], "E": 2},
+          {"S": 3, "M": [], "E": -1},
+          {"S": 4, "M": [], "E": -1},
+          {"S": 5, "M": [], "E": -1}
+        ],
+        "junctions": [{"x": 500, "y": 900}],
+        "renderStyle": "standard"
+      }
+    }
+  }
+}
 end