Demobot origin

src/App.tsx

@@ -158,24 +158,20 @@ class App extends React.Component<Props, State> {
 
     this.onAuthStateChangedSubscription_ = auth.onAuthStateChanged(user => {
       if (user) {
-        console.log('User detected.');
         this.props.loadUser(user.uid);
         this.props.setMe(user.uid);
         // Ensure user has obtained consent before continuing
         db.get<UserConsent>(Selector.user(user.uid))
           .then(userConsent => {
             const consentStatus = LegalAcceptance.getConsentStatus(userConsent?.legalAcceptance);
             if (consentStatus === 'valid') {
-              console.log('Consent verified');
               this.setState({ loading: false });
             } else {
-              console.log('Consent not verified, status is', consentStatus);
               this.props.login();
             }
           })
           .catch(error => {
             if (DbError.is(error) && error.code === DbError.CODE_NOT_FOUND) {
-              console.log('Consent info does not exist');
               this.props.login();
             }
 
@@ -189,19 +185,6 @@ class App extends React.Component<Props, State> {
     });
   }
 
-  componentDidUpdate(prevProps: Props, prevState: State) {
-
-    const uid = auth.currentUser?.uid;
-    if (!uid) return;
-
-    const prev = prevProps.users.users?.[uid];
-    const next = this.props.users.users?.[uid];
-
-    if (prev !== next && next?.type === 5) {
-      console.log("User state changed:", next);
-    }
-  }
-
   componentWillUnmount(): void {
     this.onAuthStateChangedSubscription_();
     this.onAuthStateChangedSubscription_ = null;
@@ -273,7 +256,6 @@ export default connect((state: ReduxState) => {
   };
 }, dispatch => ({
   login: () => {
-    console.log('Redirecting to login page', window.location.pathname);
     window.location.href = `/login${window.location.pathname === '/login' ? '' : `?from=${window.location.pathname}`}`;
   },
   setMe: (me: string) => dispatch(UsersAction.setMe({ me })),

src/pages/Root.tsx

@@ -300,7 +300,6 @@ class Root extends React.Component<Props, State> {
 
   constructor(props: Props) {
     super(props);
-    console.log("Root props:", this.props);
     this.state = {
       layout: Layout.Side,
       activeLanguage: 'c',
@@ -341,7 +340,6 @@ class Root extends React.Component<Props, State> {
     this.editorRef = React.createRef();
     this.overlayLayoutRef = React.createRef();
     Space.getInstance().scene = Async.latestValue(props.scene) || Scene.EMPTY;
-    console.log("simulatorRootTourSteps:", this.state.simulatorRootTourSteps);
   }
 
   async componentDidMount() {
@@ -496,9 +494,6 @@ class Root extends React.Component<Props, State> {
           : 'stopped';
     }
 
-    if (this.props.projects !== prevProps.projects) {
-      console.log('Projects prop changed:', this.props.projects);
-    }
     if (this.props.locale !== prevProps.locale) {
       this.setState({ simulatorRootTourSteps: getSimulatorTourSteps(this.props.locale) });
     }
@@ -555,12 +550,14 @@ class Root extends React.Component<Props, State> {
       },
       () => {
         window.localStorage.setItem(`code-${activeLanguage}`, code);
-        this.props.onSetProjectCode(
-          projectDetails ? projectDetails.project : null,
-          projectDetails ? projectDetails.fileName : '',
-          projectDetails ? projectDetails.fileType : 'src',
-          code,
-        );
+        if (projectDetails.project) {
+          this.props.onSetProjectCode(
+            projectDetails.project,
+            projectDetails.fileName,
+            projectDetails.fileType,
+            code,
+          );
+        }
       },
     );
   };
@@ -1141,10 +1138,6 @@ class Root extends React.Component<Props, State> {
         },
       },
       () => {
-        console.log(
-          'Updated code state after file selection:',
-          this.state.code,
-        );
         window.localStorage.setItem(
           `code-${selectedProject.projectLanguage}`,
           fileContent,
@@ -1212,7 +1205,6 @@ class Root extends React.Component<Props, State> {
       userDataFiles: {},
       type: 'project',
     };
-    console.log('onProjectCreate_: ', project);
     this.props.onAddProject(project);
 
     this.setState({
@@ -1221,7 +1213,6 @@ class Root extends React.Component<Props, State> {
   };
 
   private onFileCreate_ = (fileName: string) => {
-    console.log('onFileCreate_ state: ', this.state);
     let fileN = fileName;
     const { projectDetails, activeLanguage } = this.state;
     switch (projectDetails.fileType) {
@@ -1836,7 +1827,6 @@ const ConnectedRoot = connect(
     onNodeRemove: (nodeId: string) =>
       dispatch(ScenesAction.removeNode({ sceneId, nodeId })),
     onNodeChange: (nodeId: string, node: Node) => {
-      console.log("onNodeChange: ", nodeId, node);
       dispatch(ScenesAction.setNode({ sceneId, nodeId, node }));
       const origin = node.origin;
       const updateOrigin = true;

src/programming/SharedRegistersRobot.ts

@@ -167,7 +167,7 @@ export class SharedRegistersRobot implements AbstractRobot {
       this.sharedResisters_.setRegister16b(RegisterState.REG_RW_MOT_0_PWM_H + i * 2, motor.direction === Motor.Direction.Backward ? -motor.pwm : motor.pwm);
       this.sharedResisters_.setRegister32b(RegisterState.REG_W_MOT_0_GOAL_B3 + i * 4, motor.positionGoal * SharedRegistersRobot.POSITION_GOAL_SCALING);
       this.sharedResisters_.setRegister16b(RegisterState.REG_RW_MOT_0_SP_H + i * 2, motor.speedGoal);
-      this.sharedResisters_.setRegister32b(RegisterState.REG_RW_MOT_0_B3 + i * 4, motor.position);
+      this.sharedResisters_.setRegister32b(RegisterState.REG_RW_MOT_0_B3 + i * 4, motor.position * SharedRegistersRobot.POSITION_GOAL_SCALING);
 
       this.sharedResisters_.setRegister16b(RegisterState.REG_W_PID_0_P_H + i * 12, Math.trunc(motor.kP * 1000));
       this.sharedResisters_.setRegister16b(RegisterState.REG_W_PID_0_PD_H + i * 12, 1000);
@@ -193,4 +193,4 @@ export class SharedRegistersRobot implements AbstractRobot {
   }
 }
 
-export default SharedRegistersRobot;
+export default SharedRegistersRobot;

src/programming/WorkerInstance.ts

@@ -143,6 +143,12 @@ class WorkerInstance implements AbstractRobot {
   private watchedVariables_: Dict<unknown> = {};
   get watchedVariables() { return this.watchedVariables_; }
 
+  private resetMotorSpeedGoals_() {
+    for (let port = 0; port < 4; ++port) {
+      this.sharedRegisters_.setRegister16b(Registers.REG_RW_MOT_0_SP_H + port * 2, 0);
+    }
+  }
+
   /**
    * Internal method to handle the 'stopped' event.
    * Resets specific registers and triggers the onStopped event if defined.
@@ -157,6 +163,7 @@ class WorkerInstance implements AbstractRobot {
     this.sharedRegisters_.setRegister8b(Registers.REG_RW_MOT_DIRS, 0xFF);
     this.sharedRegisters_.setRegister8b(Registers.REG_RW_MOT_DONE, 0);
     this.sharedRegisters_.setRegister8b(Registers.REG_RW_MOT_SRV_ALLSTOP, 0xF0);
+    this.resetMotorSpeedGoals_();
 
     if (this.onStopped) {
       this.onStopped();
@@ -213,6 +220,7 @@ class WorkerInstance implements AbstractRobot {
     this.sharedRegisters_.setRegister8b(Registers.REG_RW_MOT_MODES, 0x00);
     this.sharedRegisters_.setRegister8b(Registers.REG_RW_MOT_DIRS, 0x00);
     this.sharedRegisters_.setRegister8b(Registers.REG_RW_MOT_SRV_ALLSTOP, 0xF0);
+    this.resetMotorSpeedGoals_();
 
     // Send start message to worker
     // Clones message and transmits it to worker's global environment. 
@@ -248,4 +256,4 @@ class WorkerInstance implements AbstractRobot {
   }
 }
 
-export default new WorkerInstance();
+export default new WorkerInstance();

src/simulator/babylonBindings/RobotBinding.ts

@@ -38,6 +38,8 @@ import SensorObject from './sensors/SensorObject';
 import SensorParameters from './sensors/SensorParameters';
 import LightSensor from './sensors/LightSensor';
 import LocalizedString from '../../util/LocalizedString';
+import { incrementalHingeRotation, parentRelativeOrientation } from './motorPosition';
+import { motorPwm } from './motorPwm';
 
 /** Max mesh vertices sampled per link when building mat footprints. */
 const MAX_PROJECTED_VERTICES_PER_LINK_ = 800;
@@ -86,7 +88,12 @@ class RobotBinding {
   get createBinding() { return this.createBinding_; }
 
   private lastTick_ = 0;
-  private lastMotorAngles_: [number, number, number, number] = [0, 0, 0, 0];
+  private lastMotorOrientations_: [RawQuaternion?, RawQuaternion?, RawQuaternion?, RawQuaternion?] = [
+    undefined,
+    undefined,
+    undefined,
+    undefined,
+  ];
 
   // Getting sensor values is async. We store the pending promises in these dictionaries.
   private outstandingDigitalGetValues_: Dict<RobotBinding.OutstandingPromise<boolean>> = {};
@@ -168,20 +175,15 @@ class RobotBinding {
     };
   };
 
-  /**
-   * Gets the angle between the parent and child links.
-   * Currently only supports angles between -90 and 90 degrees.
-   * TODO: Fix this to support angles between -180 and 180 degrees.
-   * @param id
-   * @param parentId
-   * @returns
-   */
-  private hingeAngle_ = (id: string, parentId: string): number => {
+  private motorOrientation_ = (id: string, parentId: string): RawQuaternion => {
     const { bParent, bChild } = this.bParentChild_(id, parentId);
-    const parentZ = bParent.rotationQuaternion.toEulerAngles().x;
-    const childZ = bChild.rotationQuaternion.toEulerAngles().x;
-    const diff = childZ - parentZ; // Between -90 and 90 degrees
-    return diff;
+    bParent.computeWorldMatrix(true);
+    bChild.computeWorldMatrix(true);
+
+    return parentRelativeOrientation(
+      RawQuaternion.fromBabylon(bParent.absoluteRotationQuaternion),
+      RawQuaternion.fromBabylon(bChild.absoluteRotationQuaternion),
+    );
   };
 
 
@@ -251,31 +253,22 @@ class RobotBinding {
       const bMotor = this.motors_[motorId]; // The actual motor object. (Physics6DoFConstraint)
       const ticksPerRevolution = motorNode.ticksPerRevolution ?? 2048;
 
-      // TODO: Fix wheel orientation in MotorBindings so plug is needed.
-      // const plug = (motorNode.plug === undefined || motorNode.plug === 'normal') ? 1 : -1;
-      const plug = 1;
-
-      const currentAngle = this.hingeAngle_(motorId, motorNode.parentId);
-      const lastMotorAngle = this.lastMotorAngles_[port];
+      const plug = motorNode.plug === Node.Motor.Plug.Inverted ? -1 : 1;
+      const currentOrientation = this.motorOrientation_(motorId, motorNode.parentId);
+      const lastMotorOrientation = this.lastMotorOrientations_[port];
+      const hingeAxis = RawVector3.cross(motorNode.parentAxis, motorNode.parentPerpAxis);
 
       let deltaAngle = 0;
-      if (lastMotorAngle > Math.PI / 2 && currentAngle < -Math.PI / 2) {
-        deltaAngle = currentAngle + 2 * Math.PI - lastMotorAngle;
-      } else if (lastMotorAngle < -Math.PI / 2 && currentAngle > Math.PI / 2) {
-        deltaAngle = currentAngle - 2 * Math.PI - lastMotorAngle;
-      } else {
-        deltaAngle = currentAngle - lastMotorAngle;
+      if (lastMotorOrientation) {
+        deltaAngle = incrementalHingeRotation(
+          lastMotorOrientation,
+          currentOrientation,
+          hingeAxis,
+        );
       }
-      this.lastMotorAngles_[port] = currentAngle;
+      this.lastMotorOrientations_[port] = currentOrientation;
 
-      const angularVelocity = deltaAngle / delta;
-
-      // if speedgoal is positive make deltaAngle positive
-      if (speedGoal > 0) {
-        deltaAngle = Math.abs(deltaAngle);
-      } else if (speedGoal < 0) {
-        deltaAngle = -1 * Math.abs(deltaAngle);
-      }
+      const angularVelocity = delta > 0 ? deltaAngle / delta : 0;
 
       // Convert to ticks
       const positionDeltaRaw = plug * deltaAngle / (2 * Math.PI) * ticksPerRevolution + this.positionDeltaFracs_[port];
@@ -361,11 +354,13 @@ class RobotBinding {
         this.iErrs_[port] = 0;
       }
 
-      pwm = plug * clamp(-400, pwm, 400);
+      const clampedPwm = motorPwm(pwm, plug);
 
-      if (writePwm) writeCommands.push(WriteCommand.motorPwm({ port, pwm }));
+      if (writePwm) {
+        writeCommands.push(WriteCommand.motorPwm({ port, pwm: clampedPwm.logical }));
+      }
 
-      const normalizedPwm = pwm / 400;
+      const normalizedPwm = clampedPwm.physical / 400;
       const nextAngularVelocity = normalizedPwm * velocityMax * 2 * Math.PI / ticksPerRevolution;
       // console.log("nextAngularVelocity", nextAngularVelocity);
       this.setMotorVelocity_(bMotor, nextAngularVelocity);
@@ -410,7 +405,6 @@ class RobotBinding {
         }
       }
 
-      const currentAngle = this.hingeAngle_(servoId, servo.parentId);
       const targetAangle = this.lastServoEnabledAngle_[i];
 
       let cur_angle = 0;
@@ -661,6 +655,9 @@ class RobotBinding {
     this.lastPErrs_ = [0, 0, 0, 0];
     this.iErrs_ = [0, 0, 0, 0];
     this.brakeAt_ = [undefined, undefined, undefined, undefined];
+    this.lastMotorOrientations_ = [undefined, undefined, undefined, undefined];
+    this.positionDeltaFracs_ = [0, 0, 0, 0];
+    this.lastTick_ = performance.now();
 
     const rawOrigin = ReferenceFramewUnits.toRaw(newOrigin, RENDER_SCALE);
     const rawInternalOrigin = ReferenceFramewUnits.toRaw(this.robot_.origin || ReferenceFramewUnits.IDENTITY, RENDER_SCALE);

src/simulator/babylonBindings/motorPosition.ts

@@ -0,0 +1,62 @@
+import { RawQuaternion, RawVector3 } from '../../util/math/math';
+
+const normalizeQuaternion = (q: RawQuaternion): RawQuaternion => {
+  const length = Math.sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
+  return RawQuaternion.create(q.x / length, q.y / length, q.z / length, q.w / length);
+};
+
+const multiplyQuaternions = (lhs: RawQuaternion, rhs: RawQuaternion): RawQuaternion => RawQuaternion.create(
+  lhs.w * rhs.x + lhs.x * rhs.w + lhs.y * rhs.z - lhs.z * rhs.y,
+  lhs.w * rhs.y - lhs.x * rhs.z + lhs.y * rhs.w + lhs.z * rhs.x,
+  lhs.w * rhs.z + lhs.x * rhs.y - lhs.y * rhs.x + lhs.z * rhs.w,
+  lhs.w * rhs.w - lhs.x * rhs.x - lhs.y * rhs.y - lhs.z * rhs.z,
+);
+
+const inverseQuaternion = (q: RawQuaternion): RawQuaternion => {
+  const lengthSquared = q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
+  return RawQuaternion.create(
+    -q.x / lengthSquared,
+    -q.y / lengthSquared,
+    -q.z / lengthSquared,
+    q.w / lengthSquared,
+  );
+};
+
+/**
+ * Returns the orientation of a child in its parent's coordinate system.
+ * Unlike subtracting Euler angles, this is unaffected by the robot's world pose.
+ */
+export const parentRelativeOrientation = (
+  parentOrientation: RawQuaternion,
+  childOrientation: RawQuaternion,
+): RawQuaternion => normalizeQuaternion(multiplyQuaternions(
+  inverseQuaternion(parentOrientation),
+  childOrientation,
+));
+
+/**
+ * Measures the signed hinge rotation between two parent-relative orientations.
+ * The quaternion delta is reduced to its twist around the hinge axis so small
+ * amounts of swing introduced by the physics solver do not affect the encoder.
+ */
+export const incrementalHingeRotation = (
+  previousOrientation: RawQuaternion,
+  currentOrientation: RawQuaternion,
+  hingeAxis: RawVector3,
+): number => {
+  let delta = normalizeQuaternion(multiplyQuaternions(
+    currentOrientation,
+    inverseQuaternion(previousOrientation),
+  ));
+
+  // q and -q describe the same orientation. Choose the representation whose
+  // rotation is at most PI so a quaternion sign change cannot add a revolution.
+  if (delta.w < 0) {
+    delta = RawQuaternion.create(-delta.x, -delta.y, -delta.z, -delta.w);
+  }
+
+  const axis = RawVector3.normalize(hingeAxis);
+  const projectedSinHalfAngle = RawVector3.dot(RawQuaternion.axis(delta), axis);
+
+  return 2 * Math.atan2(projectedSinHalfAngle, delta.w);
+};

src/simulator/babylonBindings/motorPwm.ts

@@ -0,0 +1,16 @@
+import { clamp } from '../../util/math/math';
+
+export interface MotorPwm {
+  /** The logical PWM retained in the shared motor registers. */
+  logical: number;
+  /** The PWM sent to the physics constraint after applying plug polarity. */
+  physical: number;
+}
+
+export const motorPwm = (requested: number, plug: 1 | -1): MotorPwm => {
+  const logical = clamp(-400, requested, 400);
+  return {
+    logical,
+    physical: plug * logical,
+  };
+};