Add odometry foot tracking

src/client/components/odometry/model.ts

@@ -1,6 +1,7 @@
 import { computed } from 'mobx'
 import { observable } from 'mobx'
 import { memoize } from '../../base/memoize'
+import { Matrix4 } from '../../math/matrix4'
 import { Vector3 } from '../../math/vector3'
 import { AppModel } from '../app/model'
 import { RobotModel } from '../robot/model'
@@ -29,6 +30,18 @@ export class OdometryRobotModel {
       robotModel,
       OdometryVisualizerModel.of({
         accelerometer: new Vector3(0, 0, -9.8),
+        // TODO
+        leftFoot: {
+          down: true,
+          Hwf: Matrix4.of(),
+          Htf: Matrix4.of(),
+        },
+        // TODO
+        rightFoot: {
+          down: true,
+          Hwf: Matrix4.of(),
+          Htf: Matrix4.of(),
+        },
       }),
     )
   })

src/client/components/odometry/network.ts

@@ -8,6 +8,35 @@ import { RobotModel } from '../robot/model'
 import { OdometryRobotModel } from './model'
 import { message } from '../../../shared/proto/messages'
 
+enum BodySide {
+  Left = 0,
+  Right = 1,
+}
+
+enum ServoID {
+  R_SHOULDER_PITCH = 0,
+  L_SHOULDER_PITCH = 1,
+  R_SHOULDER_ROLL = 2,
+  L_SHOULDER_ROLL = 3,
+  R_ELBOW = 4,
+  L_ELBOW = 5,
+  R_HIP_YAW = 6,
+  L_HIP_YAW = 7,
+  R_HIP_ROLL = 8,
+  L_HIP_ROLL = 9,
+  R_HIP_PITCH = 10,
+  L_HIP_PITCH = 11,
+  R_KNEE = 12,
+  L_KNEE = 13,
+  R_ANKLE_PITCH = 14,
+  L_ANKLE_PITCH = 15,
+  R_ANKLE_ROLL = 16,
+  L_ANKLE_ROLL = 17,
+  HEAD_YAW = 18,
+  HEAD_PITCH = 19,
+  NUMBER_OF_SERVOS = 20,
+}
+
 export class OdometryNetwork {
   constructor(private network: Network) {
     this.network.on(message.input.Sensors, this.onSensors)
@@ -29,5 +58,18 @@ export class OdometryNetwork {
       new THREE.Matrix4().getInverse(Matrix4.from(packet.Htw).toThree()),
     )
     robot.visualizerModel.accelerometer = Vector3.from(packet.accelerometer)
+    // TODO
+    if (packet.feet.length) {
+      robot.visualizerModel.leftFoot = {
+        down: !!packet.feet[BodySide.Left].down,
+        Hwf: Matrix4.from(packet.feet[BodySide.Left].Hwf),
+        Htf: Matrix4.from(packet.Htx[ServoID.L_ANKLE_ROLL]),
+      }
+      robot.visualizerModel.rightFoot = {
+        down: !!packet.feet[BodySide.Right].down,
+        Hwf: Matrix4.from(packet.feet[BodySide.Right].Hwf),
+        Htf: Matrix4.from(packet.Htx[ServoID.R_ANKLE_ROLL]),
+      }
+    }
   }
 }

src/client/components/odometry/odometry_visualizer/model.ts

@@ -6,26 +6,46 @@ export class OdometryVisualizerModel {
   @observable.ref Hwt: Matrix4
   @observable.ref accelerometer: Vector3
   @observable.ref camera: OdometryCamera
+  @observable.ref leftFoot: OdometryFootModel
+  @observable.ref rightFoot: OdometryFootModel
 
   constructor({
     Hwt,
     accelerometer,
     camera,
+    leftFoot,
+    rightFoot,
   }: {
     Hwt: Matrix4
     accelerometer: Vector3
     camera: OdometryCamera
+    leftFoot: OdometryFootModel
+    rightFoot: OdometryFootModel
   }) {
     this.Hwt = Hwt
     this.accelerometer = accelerometer
     this.camera = camera
+    this.leftFoot = leftFoot
+    this.rightFoot = rightFoot
   }
 
-  static of({ Hwt = Matrix4.of(), accelerometer }: { Hwt?: Matrix4; accelerometer: Vector3 }) {
+  static of({
+    Hwt = Matrix4.of(),
+    accelerometer,
+    leftFoot,
+    rightFoot,
+  }: {
+    Hwt?: Matrix4
+    accelerometer: Vector3
+    leftFoot: OdometryFootModel
+    rightFoot: OdometryFootModel
+  }) {
     return new OdometryVisualizerModel({
       Hwt,
       accelerometer,
       camera: OdometryCamera.of({ distance: 2 }),
+      leftFoot,
+      rightFoot,
     })
   }
 }
@@ -45,3 +65,9 @@ export class OdometryCamera {
     return new OdometryCamera({ distance, pitch: 0, yaw: 0 })
   }
 }
+
+interface OdometryFootModel {
+  readonly down: boolean
+  readonly Hwf: Matrix4
+  readonly Htf: Matrix4
+}

src/client/components/odometry/odometry_visualizer/stories/robot_odometry.stories.tsx

@@ -6,6 +6,7 @@ import React from 'react'
 import * as THREE from 'three'
 import { Matrix4 } from '../../../../math/matrix4'
 import { Vector3 } from '../../../../math/vector3'
+import { Vector4 } from '../../../../math/vector4'
 import { fullscreen } from '../../../storybook/fullscreen'
 import { OdometryVisualizerModel } from '../model'
 import { OdometryVisualizer } from '../view'
@@ -19,6 +20,24 @@ class OdometryVisualizerHarness extends React.Component<{ animate?: boolean }> {
   private model = OdometryVisualizerModel.of({
     Hwt: Matrix4.fromThree(new THREE.Matrix4().makeTranslation(0, 0, 1)),
     accelerometer: new Vector3(0, 0, -9.8),
+    leftFoot: {
+      down: true,
+      Hwf: new Matrix4(
+        new Vector4(1, 0, 0, 0),
+        new Vector4(0, 1, 0, 0),
+        new Vector4(0, 0, 1, 0),
+        new Vector4(0, 0.3, 0, 1),
+      ),
+    },
+    rightFoot: {
+      down: true,
+      Hwf: new Matrix4(
+        new Vector4(1, 0, 0, 0),
+        new Vector4(0, 1, 0, 0),
+        new Vector4(0, 0, 1, 0),
+        new Vector4(0, -0.3, 0, 1),
+      ),
+    },
   })
 
   componentDidMount() {

src/client/components/odometry/odometry_visualizer/view_model.ts

@@ -1,5 +1,6 @@
 import { computed } from 'mobx'
 import * as THREE from 'three'
+import { Matrix4 } from '../../../math/matrix4'
 import { Vector3 } from '../../../math/vector3'
 import { group } from '../../three/builders'
 import { scene } from '../../three/builders'
@@ -44,7 +45,16 @@ export class OdometryVisualizerViewModel {
   }
 
   private readonly scene = scene(() => ({
-    children: [this.torso(), this.floor()],
+    children: [
+      this.torso(),
+      this.floor(),
+      this.leftFoot(),
+      this.leftFoot2(),
+      this.rightFoot(),
+      this.rightFoot2(),
+      this.model.leftFoot.down && this.leftFootTorso(),
+      this.model.rightFoot.down && this.rightFootTorso(),
+    ],
   }))
 
   private readonly torso = group(() => ({
@@ -72,6 +82,105 @@ export class OdometryVisualizerViewModel {
     ],
   }))
 
+  private readonly leftFoot = group(() => {
+    const {
+      leftFoot: { Hwf },
+    } = this.model
+    return {
+      scale: new Vector3(0.2, 0.2, 0.2),
+      position: Hwf.t.vec3(),
+      rotation: Vector3.fromThree(new THREE.Euler().setFromRotationMatrix(Hwf.toThree())),
+      children: [
+        new THREE.ArrowHelper(new THREE.Vector3(1, 0, 0), undefined, 1, 0xff00ff),
+        new THREE.ArrowHelper(new THREE.Vector3(0, 1, 0), undefined, 1, 0xff00ff),
+        new THREE.ArrowHelper(new THREE.Vector3(0, 0, 1), undefined, 1, 0xff00ff),
+      ],
+    }
+  })
+
+  private readonly leftFoot2 = group(() => {
+    const {
+      Hwt,
+      leftFoot: { Htf },
+    } = this.model
+    const Hwf = Matrix4.fromThree(Hwt.toThree().multiply(Htf.toThree()))
+    return {
+      scale: new Vector3(0.2, 0.2, 0.2),
+      position: Hwf.t.vec3(),
+      rotation: Vector3.fromThree(new THREE.Euler().setFromRotationMatrix(Hwf.toThree())),
+      children: [
+        new THREE.ArrowHelper(new THREE.Vector3(1, 0, 0), undefined, 1, 0x00ffff),
+        new THREE.ArrowHelper(new THREE.Vector3(0, 1, 0), undefined, 1, 0x00ffff),
+        new THREE.ArrowHelper(new THREE.Vector3(0, 0, 1), undefined, 1, 0x00ffff),
+      ],
+    }
+  })
+
+  private readonly leftFootTorso = group(() => {
+    const Hft = new THREE.Matrix4().getInverse(this.model.leftFoot.Htf.toThree())
+    const Hwt = Matrix4.fromThree(this.model.leftFoot.Hwf.toThree().multiply(Hft))
+    return {
+      scale: new Vector3(0.2, 0.2, 0.2),
+      position: Hwt.t.vec3(),
+      rotation: Vector3.fromThree(new THREE.Euler().setFromRotationMatrix(Hwt.toThree())),
+      children: [
+        new THREE.ArrowHelper(new THREE.Vector3(1, 0, 0), undefined, 1, 0xff0000),
+        new THREE.ArrowHelper(new THREE.Vector3(0, 1, 0), undefined, 1, 0x00ff00),
+        new THREE.ArrowHelper(new THREE.Vector3(0, 0, 1), undefined, 1, 0x0000ff),
+      ],
+    }
+  })
+
+  private readonly rightFoot = group(() => {
+    const {
+      rightFoot: { Hwf: footHwf },
+    } = this.model
+    const Hwf = footHwf
+    return {
+      scale: new Vector3(0.2, 0.2, 0.2),
+      position: Hwf.t.vec3(),
+      rotation: Vector3.fromThree(new THREE.Euler().setFromRotationMatrix(Hwf.toThree())),
+      children: [
+        new THREE.ArrowHelper(new THREE.Vector3(1, 0, 0), undefined, 1, 0xff00ff),
+        new THREE.ArrowHelper(new THREE.Vector3(0, 1, 0), undefined, 1, 0xff00ff),
+        new THREE.ArrowHelper(new THREE.Vector3(0, 0, 1), undefined, 1, 0xff00ff),
+      ],
+    }
+  })
+
+  private readonly rightFoot2 = group(() => {
+    const {
+      Hwt,
+      rightFoot: { Htf },
+    } = this.model
+    const Hwf = Matrix4.fromThree(Hwt.toThree().multiply(Htf.toThree()))
+    return {
+      scale: new Vector3(0.2, 0.2, 0.2),
+      position: Hwf.t.vec3(),
+      rotation: Vector3.fromThree(new THREE.Euler().setFromRotationMatrix(Hwf.toThree())),
+      children: [
+        new THREE.ArrowHelper(new THREE.Vector3(1, 0, 0), undefined, 1, 0x00ffff),
+        new THREE.ArrowHelper(new THREE.Vector3(0, 1, 0), undefined, 1, 0x00ffff),
+        new THREE.ArrowHelper(new THREE.Vector3(0, 0, 1), undefined, 1, 0x00ffff),
+      ],
+    }
+  })
+
+  private readonly rightFootTorso = group(() => {
+    const Hft = new THREE.Matrix4().getInverse(this.model.rightFoot.Htf.toThree())
+    const Hwt = Matrix4.fromThree(this.model.rightFoot.Hwf.toThree().multiply(Hft))
+    return {
+      scale: new Vector3(0.2, 0.2, 0.2),
+      position: Hwt.t.vec3(),
+      rotation: Vector3.fromThree(new THREE.Euler().setFromRotationMatrix(Hwt.toThree())),
+      children: [
+        new THREE.ArrowHelper(new THREE.Vector3(1, 0, 0), undefined, 1, 0xff0000),
+        new THREE.ArrowHelper(new THREE.Vector3(0, 1, 0), undefined, 1, 0x00ff00),
+        new THREE.ArrowHelper(new THREE.Vector3(0, 0, 1), undefined, 1, 0x0000ff),
+      ],
+    }
+  })
+
   @computed
   private get rTWw() {
     return this.model.Hwt.t.vec3()