adding reference traj view

Author: jonas-eschmann

index.js

@@ -28,6 +28,9 @@ class ProxyController {
     reset() {
         this.policy.reset()
     }
+    get_reference(states){
+        return this.policy.get_reference(states)
+    }
 }
 class MultiController {
     constructor(Controller) {
@@ -46,11 +49,15 @@ class MultiController {
         }
         this.controllers.forEach(controller => controller.reset())
     }
+    get_reference(states){
+        console.assert(this.controllers.length == states.length)
+        return this.controllers.map((c) => c.get_reference())
+    }
 }
 
 let model = null
 let trajectory = null
-class Policy {
+class Policy{
     constructor() {
         this.step = 0
         this.policy_states = null
@@ -106,7 +113,7 @@ class Policy {
             state.observe()
             const observation_description = document.getElementById("observations").observation
             let input = math.matrix([observation_description.split(".").map(x => this.get_observation(state, x)).flat()])
-            const input_offset = trajectory.evaluate(this.step / 100)
+            const input_offset = this._get_reference()
             input_offset.forEach((x, i) => {
                 input._data[0][i] = input._data[0][i] - x
             })
@@ -119,6 +126,13 @@ class Policy {
         this.step = 0
         this.policy_states = null
     }
+    _get_reference(){
+        return trajectory.evaluate(this.step / 100)
+    }
+    get_reference(states){
+        const ref = this._get_reference()
+        return states.map(() => ref)
+    }
 }
 
 

l2f.js

@@ -1,5 +1,6 @@
 // import * as ui from "./ui.js"
 import createModule from "l2f-interface";
+import * as THREE from "three"
 // const DEBUG = true
 const DEBUG = false
 
@@ -82,6 +83,7 @@ export class L2F{
             this.control()
         });
         this.dt = null
+        this.references = null
     }
     async change_num_quadrotors(num){
         const diff = num - this.states.length
@@ -113,6 +115,21 @@ export class L2F{
     }
     simulate_step(){
         const actions = this.policy.evaluate_step(this.states)
+        const references = this.policy.get_reference(this.states)
+        if(this.references === null){
+            // create three.js reference ball
+            this.references_ui = references.map((reference, i) => {
+                const geometry = new THREE.SphereGeometry(Math.cbrt(this.parameters[i].dynamics.mass) / 20, 32, 32);
+                const material = new THREE.MeshStandardMaterial({ color: 0xff4444 });
+                const ball = new THREE.Mesh(geometry, material);
+                const reference_ui_objects = this.ui_state.simulator.add(ball)
+                return ball
+            })
+        }
+        this.references = references
+        this.references.forEach((reference, i) => {
+            this.references_ui[i].position.set(reference[0], reference[1], reference[2])
+        })
         console.assert(actions.length === this.states.length, "Action dimension mismatch")
         this.states.forEach((state, i) => {
             const action = actions[i]
@@ -176,6 +193,7 @@ export class L2F{
         if(this.render_states && this.render_actions){
             this.ui.render_multi(this.ui_state, this.parameters, this.render_states, this.render_actions)
         }
+        this.ui_state
         requestAnimationFrame(() => this.render());
         if(this.DEBUG){
             this.stats.end()