Merge pull request #10 from JuliaComputing/fbc/3d-yaw-steering

Steerable 3D bot: cascade balance/position control + yaw controller with control mixer

Author: baggepinnen

README.md

@@ -14,9 +14,11 @@ Plant models:
 - `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.
+  wheel axle (`MultibodyComponents.RollingWheelSet`) and individually driven
+  wheels, so the robot can be steered by differential torque. Outputs
+  odometric path position/velocity, tilt angle/rate and yaw (heading)
+  angle/rate. With equal torque on both wheels it is dynamically equivalent
+  to `PlanarDyadBot`.
 
 Closed-loop models around the planar plant:
 
@@ -37,6 +39,11 @@ Closed-loop models around the 3D plant:
   `AngleController` as `AngleControlledDyadBot`; with in-plane motion the
   closed-loop response is identical to the planar model
   (`test/test_stabilization.jl` verifies this).
+- `YawControlledDyadBot3D`: `RollingDyadBot3D` with the balance/position
+  cascade (`CascadeController`, position reference held at zero) and a
+  separate `YawController` tracking a heading reference that steps at t = 5;
+  a `ControlMixer` combines drive and yaw torque into the two wheel torque
+  commands, so the robot spins in place to the new heading.
 
 ## Controller tuning scripts (`scripts/`)
 

assets/ControlMixer.svg

@@ -275,7 +275,7 @@ metadata {
       }
     },
     "zoh": {
-      "Dyad": {"placement": {"diagram": {"x1": 840, "y1": 450, "x2": 920, "y2": 550}}}
+      "Dyad": {"placement": {"diagram": {"x1": 830, "y1": 450, "x2": 930, "y2": 550}}}
     },
     "clock": {
       "Dyad": {"placement": {"diagram": {"x1": 410, "y1": 790, "x2": 510, "y2": 890}}}

assets/YawController.svg

@@ -242,6 +242,8 @@ 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}
+  "Splits the motor torque equally between the two wheels"
+  gain_half = BlockComponents.Math.Gain(k = 0.5) {^gain_half}
   "Proportional gain of the angle controller"
   parameter k_angle::Real = 0.487401
   "Integrator time constant of the angle controller"
@@ -251,7 +253,8 @@ component AngleControlledDyadBot3D
   "Initial tilt angle of the body"
   parameter phi0::Angle = 0.1
 relations
-  connect(controller.torque, plant.torque) {^id4}
+  connect(controller.torque, gain_half.u) {^id4}
+  connect(gain_half.y, plant.torque_left, plant.torque_right) {^id6}
   connect(plant.theta, controller.measurement) {^id5}
 metadata {
   "Dyad": {
@@ -278,9 +281,18 @@ metadata {
     "controller": {
       "Dyad": {"placement": {"diagram": {"x1": 270, "y1": 420, "x2": 420, "y2": 570}}}
     },
-    "id4": {
+    "gain_half": {
+      "Dyad": {"placement": {"diagram": {"x1": 450, "y1": 455, "x2": 530, "y2": 535}}}
+    },
+    "id4": {"Dyad": {"edges": [{"S": 1, "M": [], "E": 2}]}},
+    "id6": {
       "Dyad": {
-        "edges": [{"S": 1, "M": [{"x": 503.75, "y": 495}, {"x": 503.75, "y": 500}], "E": 2}]
+        "edges": [
+          {"S": 1, "M": [], "E": -1},
+          {"S": -1, "M": [{"x": 545, "y": 460}], "E": 2},
+          {"S": 3, "M": [{"x": 545, "y": 530}], "E": -1}
+        ],
+        "junctions": [{"x": 545, "y": 495}]
       }
     },
     "id5": {
@@ -745,7 +757,7 @@ metadata {
       "Dyad": {"placement": {"diagram": {"x1": 60, "y1": 450, "x2": 160, "y2": 550}}}
     },
     "controller": {
-      "Dyad": {"placement": {"diagram": {"x1": 240, "y1": 410, "x2": 480, "y2": 600}}}
+      "Dyad": {"placement": {"diagram": {"x1": 265, "y1": 410, "x2": 455, "y2": 600}}}
     },
     "id7": {"Dyad": {"edges": [{"S": 1, "M": [], "E": 2}]}},
     "id8": {"Dyad": {"edges": [{"S": 1, "M": [], "E": 2}]}},
@@ -856,7 +868,7 @@ metadata {
       "Dyad": {"placement": {"diagram": {"x1": 120, "y1": 430, "x2": 220, "y2": 530}}}
     },
     "controller": {
-      "Dyad": {"placement": {"diagram": {"x1": 280, "y1": 390, "x2": 510, "y2": 570}}}
+      "Dyad": {"placement": {"diagram": {"x1": 305, "y1": 390, "x2": 485, "y2": 570}}}
     },
     "id7": {"Dyad": {"edges": [{"S": 1, "M": [], "E": 2}]}},
     "id8": {"Dyad": {"edges": [{"S": 1, "M": [], "E": 2}]}},

dyad/cascade_ff.dyad

@@ -288,7 +288,7 @@ metadata {
     "mux": {
       "Dyad": {
         "placement": {
-          "diagram": {"iconName": "default", "x1": 90, "y1": 450, "x2": 170, "y2": 550, "rot": 0}
+          "diagram": {"iconName": "default", "x1": 80, "y1": 450, "x2": 180, "y2": 550, "rot": 0}
         },
         "tags": []
       }

dyad/closed_loop.dyad

@@ -192,24 +192,27 @@ end
 
 """
 Three-dimensional model of a two-wheeled balancing robot with ideal rolling
-contact.
+contact and individually driven wheels.
 
 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.
+at the center of the axle through a revolute joint about the axle axis. Each
+wheel has its own motor torque input, so the robot can be steered by applying
+a differential torque; each motor torque acts between the body and its wheel.
 
-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.
+With the same torque applied to both wheels the robot moves in the vertical
+plane and 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
+Inputs and outputs:
+- `torque_left`, `torque_right`: motor torques applied between body and each wheel
+- `x`, `x_dot`: odometric path position and velocity (average wheel arc
+  length; equals the distance driven along the — possibly curved — path, and
+  coincides with the world-frame x coordinate when driving straight)
 - `theta`, `theta_dot`: tilt angle and tilt rate of the body (zero when upright)
+- `yaw`, `yaw_dot`: heading angle and rate (positive counterclockwise seen from above)
 
 The model must be accompanied by a `MultibodyComponents.World` component in
 the top level of the enclosing model.
@@ -219,22 +222,28 @@ component RollingDyadBot3D
   "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"
+  # wheels.axis1 is the wheel on the +z side of the axle, which is the right
+  # wheel when facing the forward x direction with y up
+  torquesource1 = RotationalComponents.Sources.TorqueSource() {^torquesource1}
+  torquesource2 = RotationalComponents.Sources.TorqueSource() {^torquesource2}
+  damper1 = RotationalComponents.Components.Damper(d = d / 2) {^damper1}
+  damper2 = RotationalComponents.Components.Damper(d = d / 2) {^damper2}
+  "Left-wheel motor torque command"
+  torque_left = Dyad.RealInput() {^torque_left}
+  "Right-wheel motor torque command"
+  torque_right = Dyad.RealInput() {^torque_right}
+  "Odometric path position (average wheel arc length)"
   x = Dyad.RealOutput() {^x}
   "Body tilt angle"
   theta = Dyad.RealOutput() {^theta}
-  "Velocity of the wheel axis"
+  "Odometric path velocity"
   x_dot = Dyad.RealOutput() {^x_dot}
   "Body tilt rate"
   theta_dot = Dyad.RealOutput() {^theta_dot}
+  "Yaw (heading) angle"
+  yaw = Dyad.RealOutput() {^yaw}
+  "Yaw rate"
+  yaw_dot = Dyad.RealOutput() {^yaw_dot}
   "Body mass"
   parameter M::Mass = 0.1
   "Total wheel mass (split between the two wheels)"
@@ -256,19 +265,23 @@ component RollingDyadBot3D
 relations
   initial pitch.phi = phi0
   initial pitch.w = 0
-  x = wheels.x
+  # Wheel spin and forward position have opposite signs (positive wheel torque
+  # drives the robot toward negative x, like in PlanarDyadBot)
+  x = -R * (wheels.theta1 + wheels.theta2) / 2
+  x_dot = -R * (wheels.der_theta1 + wheels.der_theta2) / 2
   theta = pitch.phi
-  x_dot = der(wheels.x)
   theta_dot = pitch.w
+  yaw = wheels.phi
+  yaw_dot = der(wheels.phi)
   connect(body.frame_a, pitch.frame_b) {^id14}
   connect(pitch.frame_a, wheels.frame_middle) {^id15}
-  connect(torque_left.spline, wheels.axis1) {^id16}
-  connect(torque_right.spline, wheels.axis2) {^id17}
-  connect(damper_right.spline_b, wheels.axis2) {^id18}
-  connect(damper_left.spline_b, wheels.axis1) {^id19}
-  connect(torque_left.tau, torque_right.tau, gain_half.y) {^id20}
-  connect(torque, gain_half.u) {^id21}
-  connect(pitch.axis, torque_left.support, damper_left.spline_a, torque_right.support, damper_right.spline_a) {^id22}
+  connect(torquesource1.spline, wheels.axis1) {^id16}
+  connect(torquesource2.spline, wheels.axis2) {^id17}
+  connect(damper2.spline_b, wheels.axis2) {^id18}
+  connect(damper1.spline_b, wheels.axis1) {^id19}
+  connect(torque_right, torquesource1.tau) {^id20}
+  connect(torque_left, torquesource2.tau) {^id21}
+  connect(pitch.axis, torquesource1.support, damper1.spline_a, torquesource2.support, damper2.spline_a) {^id22}
 metadata {
   "Dyad": {"icons": {"default": "dyad://DyadBotComponents/DyadBot3D.svg"}},
   "_links": {
@@ -290,31 +303,34 @@ metadata {
         "placement": {"diagram": {"x1": 350, "y1": -10, "x2": 650, "y2": 290, "rot": 270}}
       }
     },
-    "gain_half": {
-      "Dyad": {
-        "placement": {"diagram": {"x1": 70, "y1": 650, "x2": 150, "y2": 730, "rot": 90}}
-      }
-    },
-    "torque_left": {
+    "torquesource1": {
       "Dyad": {"placement": {"diagram": {"x1": 240, "y1": 670, "x2": 340, "y2": 570}}}
     },
-    "torque_right": {
+    "torquesource2": {
       "Dyad": {"placement": {"diagram": {"x1": 770, "y1": 670, "x2": 670, "y2": 570}}}
     },
-    "damper_left": {
+    "damper1": {
       "Dyad": {
         "placement": {"diagram": {"x1": 350, "y1": 490, "x2": 450, "y2": 590, "rot": 90}}
       }
     },
-    "damper_right": {
+    "damper2": {
       "Dyad": {
         "placement": {"diagram": {"x1": 650, "y1": 490, "x2": 550, "y2": 590, "rot": 270}}
       }
     },
-    "torque": {
+    "torque_right": {
+      "Dyad": {
+        "placement": {
+          "diagram": {"iconName": "default", "x1": 40, "y1": 580, "x2": 120, "y2": 660, "rot": 0}
+        },
+        "tags": []
+      }
+    },
+    "torque_left": {
       "Dyad": {
         "placement": {
-          "diagram": {"iconName": "default", "x1": 20, "y1": 460, "x2": 100, "y2": 540, "rot": 0}
+          "diagram": {"iconName": "default", "x1": 40, "y1": 720, "x2": 120, "y2": 800, "rot": 0}
         },
         "tags": []
       }
@@ -351,6 +367,22 @@ metadata {
         "tags": []
       }
     },
+    "yaw": {
+      "Dyad": {
+        "placement": {
+          "diagram": {"iconName": "default", "x1": 880, "y1": 780, "x2": 960, "y2": 860, "rot": 0}
+        },
+        "tags": []
+      }
+    },
+    "yaw_dot": {
+      "Dyad": {
+        "placement": {
+          "diagram": {"iconName": "default", "x1": 880, "y1": 900, "x2": 960, "y2": 980, "rot": 0}
+        },
+        "tags": []
+      }
+    },
     "id14": {"Dyad": {"edges": [{"S": 1, "M": [], "E": 2}]}},
     "id15": {
       "Dyad": {
@@ -372,21 +404,23 @@ metadata {
     "id17": {"Dyad": {"edges": [{"S": 1, "M": [], "E": 2}]}},
     "id18": {"Dyad": {"edges": [{"S": 1, "M": [], "E": 2}]}},
     "id19": {"Dyad": {"edges": [{"S": 1, "M": [], "E": 2}]}},
-    "id20": {
+    "id20": {"Dyad": {"edges": [{"S": 1, "M": [], "E": 2}]}},
+    "id21": {
       "Dyad": {
         "edges": [
-          {"S": 1, "M": [{"x": 210, "y": 620}], "E": -1},
           {
-            "S": -1,
-            "M": [{"x": 210, "y": 880}, {"x": 800, "y": 880}, {"x": 800, "y": 620}],
+            "S": 1,
+            "M": [
+              {"x": 160, "y": 760},
+              {"x": 160, "y": 900},
+              {"x": 820, "y": 900},
+              {"x": 820, "y": 620}
+            ],
             "E": 2
-          },
-          {"S": 3, "M": [{"x": 110, "y": 880}], "E": -1}
-        ],
-        "junctions": [{"x": 210, "y": 880}]
+          }
+        ]
       }
     },
-    "id21": {"Dyad": {"edges": [{"S": 1, "M": [{"x": 110, "y": 500}], "E": 2}]}},
     "id22": {
       "Dyad": {
         "edges": [