diff --git a/src/App.tsx b/src/App.tsx index ced890c69..5cee05826 100644 --- a/src/App.tsx +++ b/src/App.tsx @@ -158,7 +158,6 @@ class App extends React.Component { 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 @@ -166,16 +165,13 @@ class App extends React.Component { .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 { }); } - 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 })), diff --git a/src/pages/Root.tsx b/src/pages/Root.tsx index 43b06dadc..40335a50c 100644 --- a/src/pages/Root.tsx +++ b/src/pages/Root.tsx @@ -300,7 +300,6 @@ class Root extends React.Component { 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 { 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 { : '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 { }, () => { 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 { }, }, () => { - 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 { userDataFiles: {}, type: 'project', }; - console.log('onProjectCreate_: ', project); this.props.onAddProject(project); this.setState({ @@ -1221,7 +1213,6 @@ class Root extends React.Component { }; 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; diff --git a/src/programming/SharedRegistersRobot.ts b/src/programming/SharedRegistersRobot.ts index 1fb912a3e..cde0d35d4 100644 --- a/src/programming/SharedRegistersRobot.ts +++ b/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; \ No newline at end of file +export default SharedRegistersRobot; diff --git a/src/programming/WorkerInstance.ts b/src/programming/WorkerInstance.ts index 40cfa7dac..f1c9c920f 100644 --- a/src/programming/WorkerInstance.ts +++ b/src/programming/WorkerInstance.ts @@ -143,6 +143,12 @@ class WorkerInstance implements AbstractRobot { private watchedVariables_: Dict = {}; 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(); \ No newline at end of file +export default new WorkerInstance(); diff --git a/src/simulator/babylonBindings/RobotBinding.ts b/src/simulator/babylonBindings/RobotBinding.ts index cd07d9c1e..23bf197cd 100644 --- a/src/simulator/babylonBindings/RobotBinding.ts +++ b/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> = {}; @@ -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); diff --git a/src/simulator/babylonBindings/motorPosition.ts b/src/simulator/babylonBindings/motorPosition.ts new file mode 100644 index 000000000..79e06f066 --- /dev/null +++ b/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); +}; diff --git a/src/simulator/babylonBindings/motorPwm.ts b/src/simulator/babylonBindings/motorPwm.ts new file mode 100644 index 000000000..65ede6cba --- /dev/null +++ b/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, + }; +}; diff --git a/src/simulator/definitions/robots/demobot.ts b/src/simulator/definitions/robots/demobot.ts index dbcca8bc4..890db1d0f 100644 --- a/src/simulator/definitions/robots/demobot.ts +++ b/src/simulator/definitions/robots/demobot.ts @@ -34,7 +34,8 @@ export const DEMOBOT: Robot = { parentId: 'chassis', mass: grams(300), origin: { - position: Vector3wUnits.meters(0, -0.019, -0.06), + // Note: The x position has been slightly adjusted from zero to improve balance. + position: Vector3wUnits.meters(0.004, -0.019, -0.06), }, }), left_wheel: Node.motor({ @@ -46,7 +47,6 @@ export const DEMOBOT: Robot = { childRotationQuaternion: Quaternion.FromEulerAngles(0, 0, Math.PI / 2), motorPort: 0, parentId: 'chassis', - plug: Node.Motor.Plug.Inverted }), left_wheel_link: Node.link({ parentId: 'left_wheel', diff --git a/src/simulator/definitions/scenes/2026BotballExplorerTable.ts b/src/simulator/definitions/scenes/2026BotballExplorerTable.ts index 1791a4061..ee326d14b 100644 --- a/src/simulator/definitions/scenes/2026BotballExplorerTable.ts +++ b/src/simulator/definitions/scenes/2026BotballExplorerTable.ts @@ -11,8 +11,8 @@ import { } from '../../../util/math/unitMath'; import tr from '@i18n'; -const TABLE_POSITION = RawVector3.create(79, 0, 10); -const TABLE_ROTATION = RawQuaternion.create(0, 1, 0, 0); +const TABLE_POSITION = RawVector3.create(-105, 8, 74); +const TABLE_ROTATION = RawQuaternion.create(0, Math.SQRT1_2, 0, Math.SQRT1_2); const IDENTITY_ROTATION = RawQuaternion.IDENTITY; // Babylon world units in this simulator are centimeters. Preserve the layout @@ -37,9 +37,9 @@ const tableOrigin = ( scale: RawVector3 = RawVector3.ONE, ): ReferenceFramewUnits => ({ position: Vector3wUnits.centimeters( - TABLE_POSITION.x - position.x, + TABLE_POSITION.x + position.z, TABLE_POSITION.y + position.y, - TABLE_POSITION.z - position.z, + TABLE_POSITION.z - position.x, ), orientation: RotationwUnits.fromRawQuaternion( normalizeQuaternion(multiplyQuaternions(TABLE_ROTATION, orientation)), diff --git a/src/simulator/definitions/scenes/26botballExplorerBase.ts b/src/simulator/definitions/scenes/26botballExplorerBase.ts index 26ade7862..0eb5f07a1 100644 --- a/src/simulator/definitions/scenes/26botballExplorerBase.ts +++ b/src/simulator/definitions/scenes/26botballExplorerBase.ts @@ -13,12 +13,13 @@ import { import tr from '@i18n'; const ROBOT_ORIGIN: ReferenceFramewUnits = { - position: Vector3wUnits.centimeters(5, -8, -95), - orientation: RotationwUnits.eulerDegrees(0, 90, 0), + position: Vector3wUnits.centimeters(0, 0, 0), + orientation: RotationwUnits.eulerDegrees(0, 0, 0), }; const LIGHT_ORIGIN: ReferenceFramewUnits = { - position: Vector3wUnits.centimeters(54, 105, 50) + position: Vector3wUnits.centimeters(-145, 113, 49), + orientation: RotationwUnits.eulerDegrees(0, -90, 0), }; /** @@ -58,14 +59,14 @@ export function createBaseSceneSurface(): Scene { camera: Camera.arcRotate({ radius: Distance.meters(5), target: { - x: Distance.meters(0), - y: Distance.meters(0.05), - z: Distance.meters(0), + x: Distance.meters(-0.95), + y: Distance.meters(0.13), + z: Distance.meters(-0.05), }, position: { - x: Distance.meters(-1.5), - y: Distance.meters(1.25), - z: Distance.meters(0), + x: Distance.meters(-0.95), + y: Distance.meters(1.33), + z: Distance.meters(-1.55), } }), gravity: { diff --git a/src/simulator/definitions/scenes/26botballExplorerSandbox.ts b/src/simulator/definitions/scenes/26botballExplorerSandbox.ts index a499c6984..b1b589903 100644 --- a/src/simulator/definitions/scenes/26botballExplorerSandbox.ts +++ b/src/simulator/definitions/scenes/26botballExplorerSandbox.ts @@ -3,29 +3,23 @@ import Node from '../../../state/State/Scene/Node'; import { ReferenceFramewUnits, RotationwUnits, Vector3wUnits } from '../../../util/math/unitMath'; import Dict from '../../../util/objectOps/Dict'; import { createBaseSceneSurface } from './26botballExplorerBase'; -import Script from '../../../state/State/Scene/Script'; import { sprintf } from 'sprintf-js'; import tr from '@i18n'; -import { setNodeVisible } from './jbcCommonComponents'; -import { RawVector3 } from '../../../util/math/math'; import Geometry from '../../../state/State/Scene/Geometry'; import { Distance } from '../../../util/math/Value'; const baseScene = createBaseSceneSurface(); -// const MAT_CENTER = RawVector3.create(34.889, -17.609, -5.180); -const MAT_CENTER = Vector3wUnits.centimeters(44.111, -15.59, 15.18); -// const CORD_DIFF = Vector3wUnits.centimeters(15.4, -13.896, -187.89); -const CORD_DIFF = Vector3wUnits.centimeters(-21.572, -10, -119.622); +const MAT_CENTER = Vector3wUnits.centimeters(-110.18, -7.59, 39.111); const LOW_2INCH_RED_CUBE_ORIGIN: ReferenceFramewUnits = { - position: Vector3wUnits.add(Vector3wUnits.centimeters(22.77, -7, -83.3), MAT_CENTER), - orientation: RotationwUnits.eulerDegrees(0, 0, 0) + position: Vector3wUnits.add(Vector3wUnits.centimeters(83.3, -7, 22.77), MAT_CENTER), + orientation: RotationwUnits.eulerDegrees(0, -90, 0) }; const HIGH_2INCH_RED_CUBE_ORIGIN: ReferenceFramewUnits = { - position: Vector3wUnits.add(Vector3wUnits.centimeters(22.77, -1, -83.3), MAT_CENTER), - orientation: RotationwUnits.eulerDegrees(0, 0, 0) + position: Vector3wUnits.add(Vector3wUnits.centimeters(83.3, -1, 22.77), MAT_CENTER), + orientation: RotationwUnits.eulerDegrees(0, -90, 0) }; export const LOW_2INCH_RED_CUBE: Node = { @@ -46,17 +40,18 @@ export const HIGH_2INCH_RED_CUBE: Node = { startingOrigin: HIGH_2INCH_RED_CUBE_ORIGIN, origin: HIGH_2INCH_RED_CUBE_ORIGIN }; -const POM_ORIENTATION: RotationwUnits = RotationwUnits.eulerDegrees(0, 90, 0); -const LO_Y = -23; +const POM_ORIENTATION: RotationwUnits = RotationwUnits.eulerDegrees(0, 0, 0); +const LO_X_1 = -43; +const LO_Y = -15; export const POM_Z_GAP = 6 * 2.61; export const POM_X_GAP = 6 * 2.61; -export const LO_Z_1 = -52; +export const LO_Z_1 = 29.4; export const LO_ORANGE_POMS: Dict = {}; for (let i = 0; i < 6; i++) { const origin: ReferenceFramewUnits = { - position: Vector3wUnits.centimeters(34.4, LO_Y, LO_Z_1 + POM_Z_GAP * i), + position: Vector3wUnits.centimeters(LO_X_1 - POM_X_GAP * i, LO_Y, LO_Z_1), orientation: POM_ORIENTATION }; LO_ORANGE_POMS[`loOrange${i}`] = { @@ -72,7 +67,7 @@ for (let i = 0; i < 6; i++) { export const LO_BLUE_POMS: Dict = {}; for (let i = 0; i < 6; i++) { const origin: ReferenceFramewUnits = { - position: Vector3wUnits.centimeters(66.8, LO_Y, LO_Z_1 + POM_Z_GAP * i), + position: Vector3wUnits.centimeters(LO_X_1 - POM_X_GAP * i, LO_Y, 61.8), orientation: POM_ORIENTATION }; @@ -89,9 +84,9 @@ for (let i = 0; i < 6; i++) { const RED_4INCH_CUBE_PALLET_ORIGIN: ReferenceFramewUnits = { - position: Vector3wUnits.add(Vector3wUnits.centimeters(-4.5, -10, -99), MAT_CENTER), + position: Vector3wUnits.add(Vector3wUnits.centimeters(99, -10, -4.5), MAT_CENTER), - orientation: RotationwUnits.eulerDegrees(0, 0, 0) + orientation: RotationwUnits.eulerDegrees(0, -90, 0) }; export const RED_4INCH_CUBE_PALLET: Node = { type: 'from-bb-template', @@ -104,8 +99,8 @@ export const RED_4INCH_CUBE_PALLET: Node = { }; const RED_4INCH_CUBE_ORIGIN: ReferenceFramewUnits = { - position: Vector3wUnits.add(Vector3wUnits.centimeters(-4.5, -1, -99), MAT_CENTER), - orientation: RotationwUnits.eulerDegrees(0, 0, 0) + position: Vector3wUnits.add(Vector3wUnits.centimeters(99, -1, -4.5), MAT_CENTER), + orientation: RotationwUnits.eulerDegrees(0, -90, 0) }; export const RED_4INCH_CUBE: Node = { type: 'from-bb-template', @@ -118,13 +113,13 @@ export const RED_4INCH_CUBE: Node = { }; const TOP_GREEN_2IN_CUBE_ORIGIN: ReferenceFramewUnits = { - position: Vector3wUnits.centimeters(18, -25, -52.4), - orientation: RotationwUnits.eulerDegrees(0, 0, 0) + position: Vector3wUnits.centimeters(-42.6, -17, 13), + orientation: RotationwUnits.eulerDegrees(0, -90, 0) }; const LOW_GREEN_2IN_CUBE_ORIGIN: ReferenceFramewUnits = { - position: Vector3wUnits.centimeters(1.6, -25, -52.4), - orientation: RotationwUnits.eulerDegrees(0, 0, 0) + position: Vector3wUnits.centimeters(-42.6, -17, -3.4), + orientation: RotationwUnits.eulerDegrees(0, -90, 0) }; export const LOW_GREEN_2IN_CUBE: Node = { @@ -147,13 +142,13 @@ export const TOP_GREEN_2IN_CUBE: Node = { }; const TOP_YELLOW_2IN_CUBE_ORIGIN: ReferenceFramewUnits = { - position: Vector3wUnits.centimeters(18, -25, -36.6), - orientation: RotationwUnits.eulerDegrees(0, 0, 0) + position: Vector3wUnits.centimeters(-58.4, -17, 13), + orientation: RotationwUnits.eulerDegrees(0, -90, 0) }; const LOW_YELLOW_2IN_CUBE_ORIGIN: ReferenceFramewUnits = { - position: Vector3wUnits.centimeters(1.6, -25, -36.6), - orientation: RotationwUnits.eulerDegrees(0, 0, 0) + position: Vector3wUnits.centimeters(-58.4, -17, -3.4), + orientation: RotationwUnits.eulerDegrees(0, -90, 0) }; export const LOW_YELLOW_2IN_CUBE: Node = { @@ -176,8 +171,8 @@ export const TOP_YELLOW_2IN_CUBE: Node = { }; const MIDDLE_PALLET_ORIGIN: ReferenceFramewUnits = { - position: Vector3wUnits.add(Vector3wUnits.centimeters(-34.2, -10, -33.5), MAT_CENTER), - orientation: RotationwUnits.eulerDegrees(0, 0, 0) + position: Vector3wUnits.add(Vector3wUnits.centimeters(33.5, -10, -34.2), MAT_CENTER), + orientation: RotationwUnits.eulerDegrees(0, -90, 0) }; export const MIDDLE_PALLET: Node = { type: 'from-bb-template', @@ -190,8 +185,8 @@ export const MIDDLE_PALLET: Node = { }; const BROWN_4IN_CUBE_ORIGIN: ReferenceFramewUnits = { - position: Vector3wUnits.add(Vector3wUnits.centimeters(-47.9, -3, 19), MAT_CENTER), - orientation: RotationwUnits.eulerDegrees(0, 0, 0) + position: Vector3wUnits.add(Vector3wUnits.centimeters(-19, -3, -47.9), MAT_CENTER), + orientation: RotationwUnits.eulerDegrees(0, -90, 0) }; export const BROWN_4IN_CUBE: Node = { type: 'from-bb-template', @@ -204,18 +199,18 @@ export const BROWN_4IN_CUBE: Node = { }; const MIDDLE_RED_2IN_CUBE_ORIGIN: ReferenceFramewUnits = { - position: Vector3wUnits.add(Vector3wUnits.centimeters(-26.1, -10, 29.5), MAT_CENTER), - orientation: RotationwUnits.eulerDegrees(0, 0, 0) + position: Vector3wUnits.add(Vector3wUnits.centimeters(-29.5, -10, -26.1), MAT_CENTER), + orientation: RotationwUnits.eulerDegrees(0, -90, 0) }; const MIDDLE_GREEN_2IN_CUBE_ORIGIN: ReferenceFramewUnits = { - position: Vector3wUnits.add(Vector3wUnits.centimeters(-31.5, -10, 27), MAT_CENTER), - orientation: RotationwUnits.eulerDegrees(0, 0, 0) + position: Vector3wUnits.add(Vector3wUnits.centimeters(-27, -10, -31.5), MAT_CENTER), + orientation: RotationwUnits.eulerDegrees(0, -90, 0) }; const MIDDLE_YELLOW_2IN_CUBE_ORIGIN: ReferenceFramewUnits = { - position: Vector3wUnits.add(Vector3wUnits.centimeters(-36.9, -10, 29.5), MAT_CENTER), - orientation: RotationwUnits.eulerDegrees(0, 0, 0) + position: Vector3wUnits.add(Vector3wUnits.centimeters(-29.5, -10, -36.9), MAT_CENTER), + orientation: RotationwUnits.eulerDegrees(0, -90, 0) }; @@ -251,8 +246,8 @@ export const MIDDLE_YELLOW_2IN_CUBE: Node = { }; const MIDDLE_GREEN_4IN_PALLET_ORIGIN: ReferenceFramewUnits = { - position: Vector3wUnits.add(Vector3wUnits.centimeters(6.3, -10, 50.5), MAT_CENTER), - orientation: RotationwUnits.eulerDegrees(0, 0, 0) + position: Vector3wUnits.add(Vector3wUnits.centimeters(-50.5, -10, 6.3), MAT_CENTER), + orientation: RotationwUnits.eulerDegrees(0, -90, 0) }; export const MIDDLE_GREEN_4IN_PALLET: Node = { type: 'from-bb-template', @@ -265,8 +260,8 @@ export const MIDDLE_GREEN_4IN_PALLET: Node = { }; const MIDDLE_GREEN_4IN_CUBE_ORIGIN: ReferenceFramewUnits = { - position: Vector3wUnits.add(Vector3wUnits.centimeters(6.3, -3, 50.5), MAT_CENTER), - orientation: RotationwUnits.eulerDegrees(0, 0, 0) + position: Vector3wUnits.add(Vector3wUnits.centimeters(-50.5, -3, 6.3), MAT_CENTER), + orientation: RotationwUnits.eulerDegrees(0, -90, 0) }; export const MIDDLE_GREEN_4IN_CUBE: Node = { type: 'from-bb-template', @@ -279,8 +274,8 @@ export const MIDDLE_GREEN_4IN_CUBE: Node = { }; const LEFT_BASKET_ORIGIN: ReferenceFramewUnits = { - position: Vector3wUnits.add(Vector3wUnits.centimeters(-39.7, -10, 82.8), MAT_CENTER), - orientation: RotationwUnits.eulerDegrees(0, 90, 0) + position: Vector3wUnits.add(Vector3wUnits.centimeters(-82.8, -10, -39.7), MAT_CENTER), + orientation: RotationwUnits.eulerDegrees(0, 0, 0) }; export const LEFT_BASKET: Node = { type: 'from-bb-template', @@ -293,8 +288,8 @@ export const LEFT_BASKET: Node = { }; const RIGHT_BASKET_ORIGIN: ReferenceFramewUnits = { - position: Vector3wUnits.add(Vector3wUnits.centimeters(-39.7, -10, 115), MAT_CENTER), - orientation: RotationwUnits.eulerDegrees(0, 90, 0) + position: Vector3wUnits.add(Vector3wUnits.centimeters(-115, -10, -39.7), MAT_CENTER), + orientation: RotationwUnits.eulerDegrees(0, 0, 0) }; export const RIGHT_BASKET: Node = { type: 'from-bb-template', @@ -307,8 +302,8 @@ export const RIGHT_BASKET: Node = { }; const LEFT_CONE_ORIGIN: ReferenceFramewUnits = { - position: Vector3wUnits.add(Vector3wUnits.centimeters(22.77, -13, 32), MAT_CENTER), - orientation: RotationwUnits.eulerDegrees(0, 0, 0) + position: Vector3wUnits.add(Vector3wUnits.centimeters(-32, -13, 22.77), MAT_CENTER), + orientation: RotationwUnits.eulerDegrees(0, -90, 0) }; export const LEFT_CONE: Node = { type: 'from-bb-template', @@ -321,8 +316,8 @@ export const LEFT_CONE: Node = { }; const RIGHT_CONE_ORIGIN: ReferenceFramewUnits = { - position: Vector3wUnits.add(Vector3wUnits.centimeters(22.77, -13, 69), MAT_CENTER), - orientation: RotationwUnits.eulerDegrees(0, 0, 0) + position: Vector3wUnits.add(Vector3wUnits.centimeters(-69, -13, 22.77), MAT_CENTER), + orientation: RotationwUnits.eulerDegrees(0, -90, 0) }; export const RIGHT_CONE: Node = { type: 'from-bb-template', @@ -335,8 +330,8 @@ export const RIGHT_CONE: Node = { }; const BOTGUY_ORIGIN: ReferenceFramewUnits = { - position: Vector3wUnits.add(Vector3wUnits.centimeters(60.45, -2, 5.4), MAT_CENTER), - orientation: RotationwUnits.eulerDegrees(0, 90, 0) + position: Vector3wUnits.add(Vector3wUnits.centimeters(-5.4, -2, 60.45), MAT_CENTER), + orientation: RotationwUnits.eulerDegrees(0, 0, 0) }; export const BOTGUY: Node = { type: 'from-bb-template', @@ -349,8 +344,8 @@ export const BOTGUY: Node = { }; const RIGHT_STACK_YELLOW_2IN_CUBE_ORIGIN: ReferenceFramewUnits = { - position: Vector3wUnits.add(Vector3wUnits.centimeters(38.9, -10, 95), MAT_CENTER), - orientation: RotationwUnits.eulerDegrees(0, 0, 0) + position: Vector3wUnits.add(Vector3wUnits.centimeters(-95, -10, 38.9), MAT_CENTER), + orientation: RotationwUnits.eulerDegrees(0, -90, 0) }; export const RIGHT_STACK_YELLOW_2IN_CUBE: Node = { type: 'from-bb-template', @@ -363,8 +358,8 @@ export const RIGHT_STACK_YELLOW_2IN_CUBE: Node = { }; const RIGHT_STACK_GREEN_2IN_CUBE_ORIGIN: ReferenceFramewUnits = { - position: Vector3wUnits.add(Vector3wUnits.centimeters(38.9, -3, 100), MAT_CENTER), - orientation: RotationwUnits.eulerDegrees(45, 0, 0) + position: Vector3wUnits.add(Vector3wUnits.centimeters(-100, -3, 38.9), MAT_CENTER), + orientation: RotationwUnits.eulerDegrees(45, -90, 0) }; export const RIGHT_STACK_GREEN_2IN_CUBE: Node = { type: 'from-bb-template', @@ -377,8 +372,8 @@ export const RIGHT_STACK_GREEN_2IN_CUBE: Node = { }; const RIGHT_STACK_RED_2IN_CUBE_ORIGIN: ReferenceFramewUnits = { - position: Vector3wUnits.add(Vector3wUnits.centimeters(38.9, -10, 105.5), MAT_CENTER), - orientation: RotationwUnits.eulerDegrees(0, 0, 0) + position: Vector3wUnits.add(Vector3wUnits.centimeters(-105.5, -10, 38.9), MAT_CENTER), + orientation: RotationwUnits.eulerDegrees(0, -90, 0) }; export const RIGHT_STACK_RED_2IN_CUBE: Node = { type: 'from-bb-template', @@ -390,7 +385,7 @@ export const RIGHT_STACK_RED_2IN_CUBE: Node = { origin: RIGHT_STACK_RED_2IN_CUBE_ORIGIN }; -export const GAME_PIECE_OFFSET = Vector3wUnits.centimeters(-5, 15, -4.5); +export const GAME_PIECE_OFFSET = Vector3wUnits.centimeters(4.5, 15, -5); export const offsetOrigin = (origin: ReferenceFramewUnits): ReferenceFramewUnits => { if (!origin) return origin; @@ -420,11 +415,11 @@ const matCenter: Node = { editable: true, startingOrigin: { position: MAT_CENTER, - orientation: RotationwUnits.eulerDegrees(0, 0, 0) + orientation: RotationwUnits.eulerDegrees(0, -90, 0) }, origin: { position: MAT_CENTER, - orientation: RotationwUnits.eulerDegrees(0, 0, 0) + orientation: RotationwUnits.eulerDegrees(0, -90, 0) } }; export const offsetGamePiece = (node: Node): Node => ({ @@ -437,11 +432,10 @@ export const BOTBALL_EXPLORER_26_SANDBOX: Scene = { ...baseScene, name: tr('2026 Botball Explorer Sandbox'), description: tr('A sandbox scene for 2026 Botball Explorer.'), - geometry: { ...baseScene.geometry, matCenter_geom }, + geometry: { ...baseScene.geometry }, scripts: {}, nodes: { ...baseScene.nodes, - matCenter, ...Dict.map({ LOW_2INCH_RED_CUBE, high2InchRedCube: HIGH_2INCH_RED_CUBE, diff --git a/src/simulator/definitions/scenes/bex26/bex1-Waypoint-Alpha.ts b/src/simulator/definitions/scenes/bex26/bex1-Waypoint-Alpha.ts index 2ef28418b..bf2cda327 100644 --- a/src/simulator/definitions/scenes/bex26/bex1-Waypoint-Alpha.ts +++ b/src/simulator/definitions/scenes/bex26/bex1-Waypoint-Alpha.ts @@ -6,6 +6,7 @@ import tr from '@i18n'; import { createBaseSceneSurface } from '../26botballExplorerBase'; import { setNodeVisible, notInStartBox } from '../jbcCommonComponents'; import { startBox_geom, startBoxA } from './bexCommonComponents'; +import { RotationwUnits } from '../../../../util/math/unitMath'; const baseScene = createBaseSceneSurface(); const reachedEnd = ` @@ -87,10 +88,11 @@ export const BEX_1: Scene = { name: tr('Not Start Box'), origin: { position: { - x: Distance.centimeters(0), - y: Distance.centimeters(-1.9), - z: Distance.meters(0.345), + x: Distance.centimeters(-129.5), + y: Distance.centimeters(6.1), + z: Distance.centimeters(-5), }, + orientation: RotationwUnits.eulerDegrees(0, -90, 0), }, material: { type: 'basic', @@ -106,10 +108,11 @@ export const BEX_1: Scene = { name: tr('End Box'), origin: { position: { - x: Distance.centimeters(45.5), - y: Distance.centimeters(-15.3), - z: Distance.centimeters(-90.9), + x: Distance.centimeters(-4.1), + y: Distance.centimeters(-7.3), + z: Distance.centimeters(40.5), }, + orientation: RotationwUnits.eulerDegrees(0, -90, 0), }, material: { type: 'pbr', @@ -125,10 +128,11 @@ export const BEX_1: Scene = { name: tr('Stop Box'), origin: { position: { - x: Distance.centimeters(64.652), - y: Distance.centimeters(-20), - z: Distance.centimeters(-90.9), + x: Distance.centimeters(-4.1), + y: Distance.centimeters(-12), + z: Distance.centimeters(59.652), }, + orientation: RotationwUnits.eulerDegrees(0, -90, 0), }, material: { type: 'pbr', @@ -139,4 +143,4 @@ export const BEX_1: Scene = { }, } } -}; \ No newline at end of file +}; diff --git a/src/simulator/definitions/scenes/bex26/bex10-Waypoint-Bravo.ts b/src/simulator/definitions/scenes/bex26/bex10-Waypoint-Bravo.ts index 36db88451..e09e7fc8b 100644 --- a/src/simulator/definitions/scenes/bex26/bex10-Waypoint-Bravo.ts +++ b/src/simulator/definitions/scenes/bex26/bex10-Waypoint-Bravo.ts @@ -5,11 +5,16 @@ import { Color } from '../../../../state/State/Scene/Color'; import tr from '@i18n'; import { createBaseSceneSurface } from '../26botballExplorerBase'; import { blackLineNodes, BLACK_LINE_GEOMETRY, startBoxB, startBox_geom } from './bexCommonComponents'; -import { RotationwUnits } from '../../../../util/math/unitMath'; +import { ReferenceFramewUnits, RotationwUnits, Vector3wUnits } from '../../../../util/math/unitMath'; const baseScene = createBaseSceneSurface(); +const ROBOT_ORIGIN: ReferenceFramewUnits = { + position: Vector3wUnits.centimeters(-155.955, 0, -0.92), + orientation: RotationwUnits.eulerDegrees(0, 0, 0), +}; + const noStop = ` scene.addOnIntersectionListener('robot', (type, otherNodeId) => { @@ -137,6 +142,14 @@ export const BEX_10: Scene = { geometry: { ...baseScene.geometry, BLACK_LINE_GEOMETRY, + BLACK_LINE_1_2_GEOMETRY: { + type: 'box', + size: { + x: Distance.centimeters(1), + y: Distance.centimeters(12), + z: Distance.meters(3), + }, + }, startBox_geom, notStartBox_geom: { type: 'box', @@ -165,32 +178,19 @@ export const BEX_10: Scene = { }, nodes: { ...baseScene.nodes, + robot: { + ...baseScene.nodes.robot, + startingOrigin: ROBOT_ORIGIN, + origin: ROBOT_ORIGIN, + }, ...blackLineNodes, - blackLine4: - { - ...blackLineNodes.blackLine4, - origin: { - orientation: blackLineNodes.blackLine4.origin.orientation, - position: { - x: Distance.centimeters(-5.46), - y: Distance.centimeters(-14.4), - z: Distance.meters(0.428), - } - - }, - + blackLine1: { + ...blackLineNodes.blackLine1, + geometryId: 'BLACK_LINE_1_2_GEOMETRY', }, - blackLine5: { - ...blackLineNodes.blackLine5, - origin: { - orientation: blackLineNodes.blackLine5.origin.orientation, - position: { - x: Distance.centimeters(-5.46), - y: Distance.centimeters(-14.4), - z: Distance.meters(0.794), - } - - }, + blackLine2: { + ...blackLineNodes.blackLine2, + geometryId: 'BLACK_LINE_1_2_GEOMETRY', }, startBoxB, @@ -201,11 +201,11 @@ export const BEX_10: Scene = { name: tr('End Box'), origin: { position: { - x: Distance.centimeters(45.276), - y: Distance.centimeters(-15), - z: Distance.centimeters(60.995), + x: Distance.centimeters(-155.995), + y: Distance.centimeters(-7), + z: Distance.centimeters(40.276), }, - orientation: RotationwUnits.eulerDegrees(0, 90, 0), + orientation: RotationwUnits.eulerDegrees(0, 0, 0), }, material: { type: 'basic', @@ -221,11 +221,11 @@ export const BEX_10: Scene = { name: tr('Stop Box'), origin: { position: { - x: Distance.centimeters(63.744), - y: Distance.centimeters(-10.6), - z: Distance.centimeters(60.974), + x: Distance.centimeters(-155.974), + y: Distance.centimeters(-2.6), + z: Distance.centimeters(58.744), }, - orientation: RotationwUnits.eulerDegrees(0, 90, 0), + orientation: RotationwUnits.eulerDegrees(0, 0, 0), }, material: { type: 'basic', @@ -236,4 +236,4 @@ export const BEX_10: Scene = { }, } } -}; \ No newline at end of file +}; diff --git a/src/simulator/definitions/scenes/bex26/bex3-Mixed-Freight.ts b/src/simulator/definitions/scenes/bex26/bex3-Mixed-Freight.ts index 068251102..60cbdebfe 100644 --- a/src/simulator/definitions/scenes/bex26/bex3-Mixed-Freight.ts +++ b/src/simulator/definitions/scenes/bex26/bex3-Mixed-Freight.ts @@ -3,8 +3,8 @@ import Script from '../../../../state/State/Scene/Script'; import { Color } from '../../../../state/State/Scene/Color'; import tr from '@i18n'; import { createBaseSceneSurface } from '../26botballExplorerBase'; -import { TOP_YELLOW_2IN_CUBE, LOW_YELLOW_2IN_CUBE, TOP_GREEN_2IN_CUBE, LOW_GREEN_2IN_CUBE, offsetGamePiece } from '../26botballExplorerSandbox'; -import { createCubeEndNode, smallCubeEnd_geom, getLowestFaceScript, getHighestFaceScript, isCubeOnTopOfScript } from './bexCommonComponents'; +import { TOP_YELLOW_2IN_CUBE, LOW_YELLOW_2IN_CUBE, TOP_GREEN_2IN_CUBE, LOW_GREEN_2IN_CUBE } from '../26botballExplorerSandbox'; +import { createCubeEndNode, smallCubeEnd_geom, getLowestFaceScript, getHighestFaceScript, isCubeOnTopOfScript, offsetChallengeNode } from './bexCommonComponents'; import Dict from '../../../../util/objectOps/Dict'; const baseScene = createBaseSceneSurface(); @@ -142,6 +142,6 @@ export const BEX_3: Scene = { lYellowFront: createCubeEndNode(tr('Low Yellow Cube Front'), 'LOW_YELLOW_2IN_CUBE', 'front', 'smallCubeEnd_geom', Color.rgb(255, 115, 0)), lYellowBack: createCubeEndNode(tr('Low Yellow Cube Back'), 'LOW_YELLOW_2IN_CUBE', 'back', 'smallCubeEnd_geom', Color.rgb(229, 97, 255)), - }, offsetGamePiece) + }, offsetChallengeNode) } -}; \ No newline at end of file +}; diff --git a/src/simulator/definitions/scenes/bex26/bex5-Top-Shelf-Delivery.ts b/src/simulator/definitions/scenes/bex26/bex5-Top-Shelf-Delivery.ts index 952b15f73..e8a519aaf 100644 --- a/src/simulator/definitions/scenes/bex26/bex5-Top-Shelf-Delivery.ts +++ b/src/simulator/definitions/scenes/bex26/bex5-Top-Shelf-Delivery.ts @@ -3,8 +3,8 @@ import Script from '../../../../state/State/Scene/Script'; import { Color } from '../../../../state/State/Scene/Color'; import tr from '@i18n'; import { createBaseSceneSurface } from '../26botballExplorerBase'; -import { LOW_2INCH_RED_CUBE, HIGH_2INCH_RED_CUBE, RED_4INCH_CUBE, RED_4INCH_CUBE_PALLET, offsetGamePiece } from '../26botballExplorerSandbox'; -import { smallCubeEnd_geom, largeCubeEnd_geom, createCubeEndNode, getLowestFaceScript, getHighestFaceScript, isCubeOnTopOfScript } from './bexCommonComponents'; +import { LOW_2INCH_RED_CUBE, HIGH_2INCH_RED_CUBE, RED_4INCH_CUBE, RED_4INCH_CUBE_PALLET } from '../26botballExplorerSandbox'; +import { smallCubeEnd_geom, largeCubeEnd_geom, createCubeEndNode, getLowestFaceScript, getHighestFaceScript, isCubeOnTopOfScript, offsetChallengeNode } from './bexCommonComponents'; import Dict from '../../../../util/objectOps/Dict'; const cubesStacked = ` @@ -122,6 +122,6 @@ export const BEX_5: Scene = { largeRedCubeFront: createCubeEndNode(tr('4-inch Red Cube Front'), 'RED_4INCH_CUBE', 'front', 'largeCubeEnd_geom', Color.rgb(255, 0, 0)), largeRedCubeRight: createCubeEndNode(tr('4-inch Red Cube Right'), 'RED_4INCH_CUBE', 'right', 'largeCubeEnd_geom', Color.rgb(255, 0, 0)), - }, offsetGamePiece) + }, offsetChallengeNode) } -}; \ No newline at end of file +}; diff --git a/src/simulator/definitions/scenes/bex26/bex6-Pallet-Builder.ts b/src/simulator/definitions/scenes/bex26/bex6-Pallet-Builder.ts index 3cd6d0272..abbf1cd44 100644 --- a/src/simulator/definitions/scenes/bex26/bex6-Pallet-Builder.ts +++ b/src/simulator/definitions/scenes/bex26/bex6-Pallet-Builder.ts @@ -4,8 +4,8 @@ import Script from '../../../../state/State/Scene/Script'; import { Color } from '../../../../state/State/Scene/Color'; import tr from '@i18n'; import { createBaseSceneSurface } from '../26botballExplorerBase'; -import { startBoxA, startBoxB, startBox_geom, getHighestFaceScript, getLowestFaceScript, isCubeOnTopOfScript, createCubeEndNode, blackLineNodes, pallet_geom, BLACK_LINE_GEOMETRY, smallCubeEnd_geom } from './bexCommonComponents'; -import { MIDDLE_PALLET, TOP_GREEN_2IN_CUBE, LOW_GREEN_2IN_CUBE, TOP_YELLOW_2IN_CUBE, LOW_YELLOW_2IN_CUBE, offsetGamePiece } from '../26botballExplorerSandbox'; +import { startBoxA, startBoxB, startBox_geom, getHighestFaceScript, getLowestFaceScript, isCubeOnTopOfScript, createCubeEndNode, blackLineNodes, pallet_geom, BLACK_LINE_GEOMETRY, smallCubeEnd_geom, offsetChallengeNode } from './bexCommonComponents'; +import { MIDDLE_PALLET, TOP_GREEN_2IN_CUBE, LOW_GREEN_2IN_CUBE, TOP_YELLOW_2IN_CUBE, LOW_YELLOW_2IN_CUBE } from '../26botballExplorerSandbox'; import { RotationwUnits } from '../../../../util/math/unitMath'; import Dict from '../../../../util/objectOps/Dict'; @@ -291,6 +291,6 @@ export const BEX_6: Scene = { lYellowRight: createCubeEndNode(tr('Low Yellow Cube Right'), 'LOW_YELLOW_2IN_CUBE', 'right', 'smallCubeEnd_geom', Color.rgb(229, 97, 255)), lYellowFront: createCubeEndNode(tr('Low Yellow Cube Front'), 'LOW_YELLOW_2IN_CUBE', 'front', 'smallCubeEnd_geom', Color.rgb(229, 97, 255)), lYellowBack: createCubeEndNode(tr('Low Yellow Cube Back'), 'LOW_YELLOW_2IN_CUBE', 'back', 'smallCubeEnd_geom', Color.rgb(229, 97, 255)), - }, offsetGamePiece) + }, offsetChallengeNode) } -}; \ No newline at end of file +}; diff --git a/src/simulator/definitions/scenes/bex26/bex7-Hazard-Containment.ts b/src/simulator/definitions/scenes/bex26/bex7-Hazard-Containment.ts index a47c2b635..219ef2e9a 100644 --- a/src/simulator/definitions/scenes/bex26/bex7-Hazard-Containment.ts +++ b/src/simulator/definitions/scenes/bex26/bex7-Hazard-Containment.ts @@ -6,6 +6,7 @@ import tr from '@i18n'; import { createBaseSceneSurface } from '../26botballExplorerBase'; import { LO_BLUE_POMS, LO_ORANGE_POMS, offsetGamePiece } from '../26botballExplorerSandbox'; import Dict from '../../../../util/objectOps/Dict'; +import { RotationwUnits } from '../../../../util/math/unitMath'; const baseScene = createBaseSceneSurface(); @@ -151,10 +152,11 @@ export const BEX_7: Scene = { name: tr('PVC Enclosure Left'), origin: { position: { - x: Distance.centimeters(72.73), - y: Distance.centimeters(-16.15), - z: Distance.centimeters(90.43), + x: Distance.centimeters(-185.43), + y: Distance.centimeters(-8.15), + z: Distance.centimeters(67.73), }, + orientation: RotationwUnits.eulerDegrees(0, -90, 0), }, material: { type: 'basic', @@ -170,10 +172,11 @@ export const BEX_7: Scene = { name: tr('PVC Enclosure Middle'), origin: { position: { - x: Distance.centimeters(-0.62), - y: Distance.centimeters(-16.15), - z: Distance.centimeters(90.43), + x: Distance.centimeters(-185.43), + y: Distance.centimeters(-8.15), + z: Distance.centimeters(-5.62), }, + orientation: RotationwUnits.eulerDegrees(0, -90, 0), }, material: { type: 'basic', @@ -190,10 +193,11 @@ export const BEX_7: Scene = { name: tr('PVC Enclosure Right'), origin: { position: { - x: Distance.centimeters(-78.95), - y: Distance.centimeters(-16.15), - z: Distance.centimeters(90.43), + x: Distance.centimeters(-185.43), + y: Distance.centimeters(-8.15), + z: Distance.centimeters(-83.95), }, + orientation: RotationwUnits.eulerDegrees(0, -90, 0), }, material: { type: 'basic', @@ -205,4 +209,4 @@ export const BEX_7: Scene = { }, } -}; \ No newline at end of file +}; diff --git a/src/simulator/definitions/scenes/bex26/bex8-Deliver-the-Red-Cube.ts b/src/simulator/definitions/scenes/bex26/bex8-Deliver-the-Red-Cube.ts index 6e9763a09..461bedec8 100644 --- a/src/simulator/definitions/scenes/bex26/bex8-Deliver-the-Red-Cube.ts +++ b/src/simulator/definitions/scenes/bex26/bex8-Deliver-the-Red-Cube.ts @@ -221,8 +221,8 @@ export const BEX_8: Scene = { geometryId: 'loadingDock_geom', name: tr('Loading Dock Top'), origin: { - position: Vector3wUnits.centimeters(78.708, -9.262, -100.797), - orientation: RotationwUnits.eulerDegrees(0, 90, 0), + position: Vector3wUnits.centimeters(5.797, -1.262, 73.708), + orientation: RotationwUnits.eulerDegrees(0, 0, 0), }, material: { type: 'basic', @@ -233,4 +233,4 @@ export const BEX_8: Scene = { }, } } -}; \ No newline at end of file +}; diff --git a/src/simulator/definitions/scenes/bex26/bex9-Recover-Botguy.ts b/src/simulator/definitions/scenes/bex26/bex9-Recover-Botguy.ts index 3cc821b1e..df96a766d 100644 --- a/src/simulator/definitions/scenes/bex26/bex9-Recover-Botguy.ts +++ b/src/simulator/definitions/scenes/bex26/bex9-Recover-Botguy.ts @@ -76,11 +76,11 @@ export const BEX_9: Scene = { name: tr('Warehouse Floor'), origin: { position: { - x: Distance.centimeters(38.147), - y: Distance.centimeters(-15.59), - z: Distance.centimeters(17.287), + x: Distance.centimeters(-112.287), + y: Distance.centimeters(-7.59), + z: Distance.centimeters(33.147), }, - orientation: RotationwUnits.eulerDegrees(0, 90, 0), + orientation: RotationwUnits.eulerDegrees(0, 0, 0), }, material: { type: 'basic', @@ -97,11 +97,11 @@ export const BEX_9: Scene = { name: tr('PVC Enclosure Left'), origin: { position: { - x: Distance.centimeters(99.43), - y: Distance.centimeters(-16.15), - z: Distance.centimeters(-56.669), + x: Distance.centimeters(-38.331), + y: Distance.centimeters(-8.15), + z: Distance.centimeters(94.43), }, - orientation: RotationwUnits.eulerDegrees(0, 90, 0), + orientation: RotationwUnits.eulerDegrees(0, 0, 0), }, material: { type: 'basic', @@ -118,11 +118,11 @@ export const BEX_9: Scene = { origin: { position: { - x: Distance.centimeters(99.43), - y: Distance.centimeters(-16.15), - z: Distance.centimeters(16.568), + x: Distance.centimeters(-111.568), + y: Distance.centimeters(-8.15), + z: Distance.centimeters(94.43), }, - orientation: RotationwUnits.eulerDegrees(0, 90, 0), + orientation: RotationwUnits.eulerDegrees(0, 0, 0), }, material: { type: 'basic', @@ -139,11 +139,11 @@ export const BEX_9: Scene = { name: tr('PVC Enclosure Right'), origin: { position: { - x: Distance.centimeters(99.43), - y: Distance.centimeters(-16.15), - z: Distance.centimeters(94.334), + x: Distance.centimeters(-189.334), + y: Distance.centimeters(-8.15), + z: Distance.centimeters(94.43), }, - orientation: RotationwUnits.eulerDegrees(0, 90, 0), + orientation: RotationwUnits.eulerDegrees(0, 0, 0), }, material: { @@ -156,4 +156,4 @@ export const BEX_9: Scene = { }, } -}; \ No newline at end of file +}; diff --git a/src/simulator/definitions/scenes/bex26/bexCommonComponents.ts b/src/simulator/definitions/scenes/bex26/bexCommonComponents.ts index 4c059222c..062adde8f 100644 --- a/src/simulator/definitions/scenes/bex26/bexCommonComponents.ts +++ b/src/simulator/definitions/scenes/bex26/bexCommonComponents.ts @@ -1,11 +1,12 @@ import LocalizedString from "../../../../util/LocalizedString"; -import { RotationwUnits, Vector3wUnits } from "../../../../util/math/unitMath"; +import { ReferenceFramewUnits, RotationwUnits, Vector3wUnits } from "../../../../util/math/unitMath"; import Node from '../../../../state/State/Scene/Node'; import { Color } from '../../../../state/State/Scene/Color'; import { Distance } from '../../../../util'; import Geometry from "../../../../state/State/Scene/Geometry"; import tr from '@i18n'; import Dict from '../../../../util/objectOps/Dict'; +import { offsetGamePiece } from '../26botballExplorerSandbox'; /** ************* @@ -169,6 +170,32 @@ export function createCubeEndNode(name: LocalizedString, parentId: string, side: }; } +const CHILD_GAME_PIECE_OFFSET = Vector3wUnits.centimeters(-5, 15, -4.5); + +const offsetChildOrigin = (origin: ReferenceFramewUnits): ReferenceFramewUnits => { + if (!origin) return origin; + return { + ...origin, + position: Vector3wUnits.add( + origin.position ?? Vector3wUnits.zero('centimeters'), + CHILD_GAME_PIECE_OFFSET, + ), + }; +}; + +/** + * Root nodes use the rebased game-piece offset. Child origins are parent-local, + * so their pre-rebase offset must remain unchanged. + */ +export const offsetChallengeNode = (node: Node): Node => { + if (!('parentId' in node) || !node.parentId) return offsetGamePiece(node); + return { + ...node, + startingOrigin: offsetChildOrigin(node.startingOrigin), + origin: offsetChildOrigin(node.origin), + } as Node; +}; + /** ************* GEOMETRIES ****************/ @@ -237,11 +264,11 @@ export const startBoxA: Node = { editable: true, origin: { position: { - x: Distance.centimeters(4.134), - y: Distance.centimeters(-15.504), - z: Distance.centimeters(-91.2), + x: Distance.centimeters(-3.8), + y: Distance.centimeters(-7.504), + z: Distance.centimeters(-0.866), }, - orientation: RotationwUnits.eulerDegrees(0, 0, 0), + orientation: RotationwUnits.eulerDegrees(0, -90, 0), }, material: { type: 'basic', @@ -260,11 +287,11 @@ export const startBoxB: Node = { editable: true, origin: { position: { - x: Distance.centimeters(4.08), - y: Distance.centimeters(-15), - z: Distance.centimeters(60.955), + x: Distance.centimeters(-155.955), + y: Distance.centimeters(-7), + z: Distance.centimeters(-0.92), }, - orientation: RotationwUnits.eulerDegrees(0, 0, 0), + orientation: RotationwUnits.eulerDegrees(0, -90, 0), }, material: { type: 'basic', @@ -281,11 +308,11 @@ export const loadingZone: Node = { name: tr('Loading Zone'), origin: { position: { - x: Distance.meters(0.053), - y: Distance.meters(-0.156), - z: Distance.centimeters(110.441) + x: Distance.centimeters(-205.441), + y: Distance.centimeters(-7.6), + z: Distance.centimeters(0.3) }, - orientation: RotationwUnits.eulerDegrees(0, 90, 0), + orientation: RotationwUnits.eulerDegrees(0, 0, 0), }, material: { @@ -305,10 +332,11 @@ export const blackLineNodes: Dict = { name: tr('Black Line 1'), origin: { position: { - x: Distance.centimeters(29.46), - y: Distance.centimeters(-14.4), - z: Distance.meters(0.133), + x: Distance.centimeters(-108.3), + y: Distance.centimeters(-6.4), + z: Distance.centimeters(24.46), }, + orientation: RotationwUnits.eulerDegrees(0, -90, 0), }, material: { type: 'basic', @@ -324,10 +352,11 @@ export const blackLineNodes: Dict = { name: tr('Black Line 2'), origin: { position: { - x: Distance.centimeters(61.619), - y: Distance.centimeters(-14.4), - z: Distance.meters(0.29), + x: Distance.centimeters(-124), + y: Distance.centimeters(-6.4), + z: Distance.centimeters(56.619), }, + orientation: RotationwUnits.eulerDegrees(0, -90, 0), }, material: { type: 'basic', @@ -343,11 +372,11 @@ export const blackLineNodes: Dict = { name: tr('Black Line 3'), origin: { position: { - x: Distance.centimeters(-12.1), - y: Distance.centimeters(-14.4), - z: Distance.meters(-0.725), + x: Distance.centimeters(-22.5), + y: Distance.centimeters(-6.4), + z: Distance.centimeters(-17.1), }, - orientation: RotationwUnits.eulerDegrees(0, 90, 0), + orientation: RotationwUnits.eulerDegrees(0, 0, 0), }, material: { type: 'basic', @@ -363,11 +392,11 @@ export const blackLineNodes: Dict = { name: tr('Black Line 4'), origin: { position: { - x: Distance.centimeters(-5.46), - y: Distance.centimeters(-14.4), - z: Distance.meters(0.428), + x: Distance.centimeters(-137.8), + y: Distance.centimeters(-6.4), + z: Distance.centimeters(-10.46), }, - orientation: RotationwUnits.eulerDegrees(0, 90, 0), + orientation: RotationwUnits.eulerDegrees(0, 0, 0), }, material: { type: 'basic', @@ -383,11 +412,11 @@ export const blackLineNodes: Dict = { name: tr('Black Line 5'), origin: { position: { - x: Distance.centimeters(-5.46), - y: Distance.centimeters(-14.4), - z: Distance.meters(0.794), + x: Distance.centimeters(-174.4), + y: Distance.centimeters(-6.4), + z: Distance.centimeters(-10.46), }, - orientation: RotationwUnits.eulerDegrees(0, 90, 0), + orientation: RotationwUnits.eulerDegrees(0, 0, 0), }, material: { type: 'basic', @@ -398,4 +427,4 @@ export const blackLineNodes: Dict = { }, } -}; \ No newline at end of file +}; diff --git a/src/state/reducer/robots.ts b/src/state/reducer/robots.ts index 4d9208945..688266ab4 100644 --- a/src/state/reducer/robots.ts +++ b/src/state/reducer/robots.ts @@ -81,7 +81,6 @@ export type RobotsAction = ( const DEFAULT_ROBOTS: Robots = { robots: Object.entries(ROBOTS).reduce((acc, [key, value]) => { acc[key.toLowerCase()] = Async.loaded({ value }); - console.log(`found default robot: ${key}`); return acc; }, {}) }; @@ -143,4 +142,4 @@ export const reduceRobots = (state: Robots = DEFAULT_ROBOTS, action: RobotsActio default: return state; } -}; \ No newline at end of file +}; diff --git a/src/state/reducer/users.ts b/src/state/reducer/users.ts index e7cc0c245..406c82922 100644 --- a/src/state/reducer/users.ts +++ b/src/state/reducer/users.ts @@ -119,7 +119,6 @@ const loadOrEmpty = async (userId: string, current: AsyncUser | undefined) => { const brief = Async.brief(current); try { const value = await db.get(Selector.user(userId)); - console.log("loadOrEmpty loaded user:", value); store.dispatch(UsersAction.setUserInternal({ user: Async.loaded({ brief, value }), userId, @@ -255,4 +254,4 @@ export const reduceUsers = (state: Users = Users.EMPTY, action: UsersAction): Us } default: return state; } -}; \ No newline at end of file +}; diff --git a/test/programming/SharedRegistersRobot.spec.ts b/test/programming/SharedRegistersRobot.spec.ts new file mode 100644 index 000000000..852beecb7 --- /dev/null +++ b/test/programming/SharedRegistersRobot.spec.ts @@ -0,0 +1,25 @@ +import AbstractRobot from '../../src/programming/AbstractRobot'; +import Motor from '../../src/programming/AbstractRobot/Motor'; +import SharedRegistersRobot from '../../src/programming/SharedRegistersRobot'; +import SharedRegisters from '../../src/programming/registers/SharedRegisters'; + +describe('SharedRegistersRobot', () => { + it('round-trips motor positions through sync without changing units', () => { + const positions = [1234, -567, 1, 0] as const; + const motors = positions.map(position => ({ + ...Motor.NIL, + position, + })) as AbstractRobot.Stateless.Motors; + const state = new AbstractRobot.Stateless( + motors, + AbstractRobot.Stateless.NIL.servos, + AbstractRobot.Stateless.NIL.analogValues, + AbstractRobot.Stateless.NIL.digitalValues, + ); + const robot = new SharedRegistersRobot(new SharedRegisters()); + + robot.sync(state); + + expect(positions.map((_, port) => robot.getMotor(port).position)).toEqual(positions); + }); +}); diff --git a/test/simulator/motorPosition.spec.ts b/test/simulator/motorPosition.spec.ts new file mode 100644 index 000000000..0994183bc --- /dev/null +++ b/test/simulator/motorPosition.spec.ts @@ -0,0 +1,60 @@ +import { Quaternion } from '@babylonjs/core'; + +import { RawQuaternion, RawVector3 } from '../../src/util/math/math'; +import { + incrementalHingeRotation, + parentRelativeOrientation, +} from '../../src/simulator/babylonBindings/motorPosition'; + +const quaternion = (q: Quaternion): RawQuaternion => RawQuaternion.fromBabylon(q); + +const worldOrientations = (world: Quaternion, angle: number) => ({ + parent: quaternion(world), + child: quaternion( + world + .multiply(Quaternion.FromEulerAngles(angle, 0, 0)) + .multiply(Quaternion.FromEulerAngles(0, 0, Math.PI / 2)), + ), +}); + +describe('motor position quaternion tracking', () => { + it.each([1, -1])('accumulates complete revolutions in direction %i', direction => { + const world = Quaternion.FromEulerAngles(0.7, -0.4, 1.1); + const samples = 32; + let previous: RawQuaternion | undefined; + let accumulated = 0; + + for (let i = 0; i <= samples; ++i) { + const orientations = worldOrientations(world, direction * 2 * Math.PI * i / samples); + const current = parentRelativeOrientation(orientations.parent, orientations.child); + if (previous) { + accumulated += incrementalHingeRotation(previous, current, RawVector3.X); + } + previous = current; + } + + expect(accumulated).toBeCloseTo(direction * 2 * Math.PI, 10); + }); + + it('does not count changes to the robot world orientation', () => { + const first = worldOrientations(Quaternion.Identity(), 0.3); + const second = worldOrientations(Quaternion.FromEulerAngles(0.8, -1.2, 0.4), 0.3); + + const previous = parentRelativeOrientation(first.parent, first.child); + const current = parentRelativeOrientation(second.parent, second.child); + + expect(incrementalHingeRotation(previous, current, RawVector3.X)).toBeCloseTo(0, 10); + }); + + it('ignores equivalent quaternion sign changes', () => { + const orientation = RawQuaternion.normalize(RawQuaternion.create(0.2, -0.3, 0.4, 0.8)); + const negated = RawQuaternion.create( + -orientation.x, + -orientation.y, + -orientation.z, + -orientation.w, + ); + + expect(incrementalHingeRotation(orientation, negated, RawVector3.X)).toBeCloseTo(0, 10); + }); +}); diff --git a/test/simulator/motorPwm.spec.ts b/test/simulator/motorPwm.spec.ts new file mode 100644 index 000000000..acaed0f9c --- /dev/null +++ b/test/simulator/motorPwm.spec.ts @@ -0,0 +1,14 @@ +import { motorPwm } from '../../src/simulator/babylonBindings/motorPwm'; + +describe('motor PWM plug handling', () => { + it.each([ + { plug: 1 as const, logical: 200, physical: 200 }, + { plug: -1 as const, logical: 200, physical: -200 }, + ])('keeps logical PWM stable for plug $plug', ({ plug, logical, physical }) => { + expect(motorPwm(logical, plug)).toEqual({ logical, physical }); + }); + + it('clamps before applying plug polarity', () => { + expect(motorPwm(-500, -1)).toEqual({ logical: -400, physical: 400 }); + }); +});