Darkages/SpotMicroESP32-Leika
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

🐛 Fix the visualization world coordinate frame

Browse Source
This commit is contained in:
Rune Harlyk
2026-01-29 16:07:12 +01:00
parent 56376e6322
commit f513de0171
1 changed files with 20 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
app/src/lib/components/Visualization.svelte
+20 -16
View File
@@ -254,41 +254,45 @@
robot.position.y = robot.position.y - Math.min(...toes.map(toe => toe.y)) robot.position.y = robot.position.y - Math.min(...toes.map(toe => toe.y))
const cumulativeYaw = body_state.cumulative_yaw const cumulativeYaw = body_state.cumulative_yaw
const totalYaw = degToRad(-settings.phi) + cumulativeYaw const headingYaw = degToRad(-settings.phi + $mpu.heading)
const totalYaw = headingYaw + cumulativeYaw
const cosYaw = Math.cos(totalYaw) const cosTotal = Math.cos(totalYaw)
const sinYaw = Math.sin(totalYaw) const sinTotal = Math.sin(totalYaw)
const rotatedXm = settings.xm * cosYaw - settings.zm * sinYaw const rotatedXm = settings.xm * cosTotal - settings.zm * sinTotal
const rotatedZm = settings.xm * sinYaw + settings.zm * cosYaw const rotatedZm = settings.xm * sinTotal + settings.zm * cosTotal
const cosHead = Math.cos(headingYaw)
const sinHead = Math.sin(headingYaw)
const rotatedCumX = body_state.cumulative_x * cosHead - body_state.cumulative_z * sinHead
const rotatedCumZ = body_state.cumulative_x * sinHead + body_state.cumulative_z * cosHead
robot.position.x = smooth( robot.position.x = smooth(
robot.position.x, robot.position.x,
(-rotatedZm - body_state.cumulative_z) * THREEJS_SCALE, (-rotatedZm - rotatedCumZ) * THREEJS_SCALE,
SMOOTH_AMOUNT SMOOTH_AMOUNT
) )
robot.position.z = smooth( robot.position.z = smooth(
robot.position.z, robot.position.z,
(-rotatedXm - body_state.cumulative_x) * THREEJS_SCALE, (-rotatedXm - rotatedCumX) * THREEJS_SCALE,
SMOOTH_AMOUNT SMOOTH_AMOUNT
) )
const basePitch = degToRad(-90) const cosYaw = Math.cos(totalYaw)
const sinYaw = Math.sin(totalYaw)
const cmdPitch = degToRad(settings.psi) const cmdPitch = degToRad(settings.psi)
const cmdRoll = degToRad(settings.omega) const cmdRoll = degToRad(settings.omega)
const pitch =
const worldCmdPitch = cmdPitch * cosYaw - cmdRoll * sinYaw degToRad(-90) + cmdPitch * cosYaw - cmdRoll * sinYaw + body_state.cumulative_pitch
const worldCmdRoll = cmdPitch * sinYaw + cmdRoll * cosYaw const roll = cmdPitch * sinYaw + cmdRoll * cosYaw + body_state.cumulative_roll
const worldPitch = basePitch + worldCmdPitch + body_state.cumulative_pitch
const worldRoll = worldCmdRoll + body_state.cumulative_roll
robot.rotation.z = smooth( robot.rotation.z = smooth(
robot.rotation.z, robot.rotation.z,
degToRad(-settings.phi + $mpu.heading + 90) + cumulativeYaw, degToRad(-settings.phi + $mpu.heading + 90) + cumulativeYaw,
SMOOTH_AMOUNT SMOOTH_AMOUNT
) )
robot.rotation.y = smooth(robot.rotation.y, worldRoll, SMOOTH_AMOUNT) robot.rotation.y = smooth(robot.rotation.y, roll, SMOOTH_AMOUNT)
robot.rotation.x = smooth(robot.rotation.x, worldPitch, SMOOTH_AMOUNT) robot.rotation.x = smooth(robot.rotation.x, pitch, SMOOTH_AMOUNT)
} }
const update_camera = (robot: URDFRobot) => { const update_camera = (robot: URDFRobot) => {