✨ Adds cumulative displacement of the robot

app/src/lib/components/Visualization.svelte

@@ -84,7 +84,13 @@
         xm: 0,
         ym: 0.5,
         zm: 0,
-        feet: kinematic.getDefaultFeetPos()
+        feet: kinematic.getDefaultFeetPos(),
+        cumulative_x: 0,
+        cumulative_y: 0,
+        cumulative_z: 0,
+        cumulative_roll: 0,
+        cumulative_pitch: 0,
+        cumulative_yaw: 0
     }
 
     let settings = {
@@ -209,7 +215,13 @@
             xm: settings.xm,
             ym: settings.ym,
             zm: settings.zm,
-            feet: body_state.feet
+            feet: body_state.feet,
+            cumulative_x: body_state.cumulative_x,
+            cumulative_y: body_state.cumulative_y,
+            cumulative_z: body_state.cumulative_z,
+            cumulative_roll: body_state.cumulative_roll,
+            cumulative_pitch: body_state.cumulative_pitch,
+            cumulative_yaw: body_state.cumulative_yaw
         }
 
         let new_angles = kinematic.calcIK(position).map((x, i) => radToDeg(x * dir[i]))
@@ -220,16 +232,32 @@
         if (settings['Robot transform controls'] || !settings['Auto orient robot']) return
         robot.position.y = robot.position.y - Math.min(...toes.map(toe => toe.y))
 
-        robot.position.z = smooth(robot.position.z, -settings.xm, 0.1)
-        robot.position.x = smooth(robot.position.x, -settings.zm, 0.1)
+        robot.position.z = smooth(
+            robot.position.z,
+            -settings.xm - body_state.cumulative_x * 1.2,
+            0.1
+        )
+        robot.position.x = smooth(
+            robot.position.x,
+            -settings.zm - body_state.cumulative_z * 1.2,
+            0.1
+        )
 
         robot.rotation.z = smooth(
             robot.rotation.z,
-            degToRad(-settings.phi + $mpu.heading + 90),
+            degToRad(-settings.phi + $mpu.heading + 90) + body_state.cumulative_yaw,
+            0.1
+        )
+        robot.rotation.y = smooth(
+            robot.rotation.y,
+            degToRad(settings.omega) + body_state.cumulative_roll,
+            0.1
+        )
+        robot.rotation.x = smooth(
+            robot.rotation.x,
+            degToRad(settings.psi - 90) + body_state.cumulative_pitch,
             0.1
         )
-        robot.rotation.y = smooth(robot.rotation.y, degToRad(settings.omega), 0.1)
-        robot.rotation.x = smooth(robot.rotation.x, degToRad(settings.psi - 90), 0.1)
     }
 
     const update_camera = (robot: URDFRobot) => {

app/src/lib/gait.ts

@@ -53,6 +53,16 @@ export abstract class GaitState {
         this.map_command(command)
         this.body_state = body_state
         this.dt = dt / 1000
+
+        if (body_state.cumulative_x === undefined) {
+            body_state.cumulative_x = 0
+            body_state.cumulative_y = 0
+            body_state.cumulative_z = 0
+            body_state.cumulative_roll = 0
+            body_state.cumulative_pitch = 0
+            body_state.cumulative_yaw = 0
+        }
+
         return body_state
     }
 
@@ -72,6 +82,11 @@ export abstract class GaitState {
 
 export class IdleState extends GaitState {
     protected name = 'Idle'
+
+    step(body_state: body_state_t, command: ControllerCommand) {
+        super.step(body_state, command)
+        return body_state
+    }
 }
 
 export class CalibrationState extends GaitState {
@@ -79,6 +94,7 @@ export class CalibrationState extends GaitState {
 
     // eslint-disable-next-line @typescript-eslint/no-unused-vars
     step(body_state: body_state_t, _command: ControllerCommand) {
+        super.step(body_state, _command)
         body_state.omega = 0
         body_state.phi = 0
         body_state.psi = 0
@@ -95,6 +111,7 @@ export class RestState extends GaitState {
 
     // eslint-disable-next-line @typescript-eslint/no-unused-vars
     step(body_state: body_state_t, _command: ControllerCommand) {
+        super.step(body_state, _command)
         body_state.omega = 0
         body_state.phi = 0
         body_state.psi = 0
@@ -110,6 +127,7 @@ export class StandState extends GaitState {
     protected name = 'Stand'
 
     step(body_state: body_state_t, command: ControllerCommand) {
+        super.step(body_state, command)
         body_state.omega = 0
         body_state.phi = command.rx * 10 * (Math.PI / 2)
         body_state.psi = command.ry * 10 * (Math.PI / 2)
@@ -135,6 +153,10 @@ export class BezierState extends GaitState {
     protected shift_start_time = 0
     protected current_shift_leg = -1
 
+    protected last_body_state: body_state_t | null = null
+    protected cumulative_position = { x: 0, y: 0, z: 0 }
+    protected cumulative_orientation = { roll: 0, pitch: 0, yaw: 0 }
+
     constructor() {
         super()
         this.set_mode(this.mode)
@@ -174,6 +196,7 @@ export class BezierState extends GaitState {
         this.update_phase()
         this.update_body_position()
         this.update_feet_positions()
+        this.update_cumulative_position()
         return this.body_state
     }
 
@@ -328,6 +351,51 @@ export class BezierState extends GaitState {
 
         return this.body_state.feet[index]
     }
+
+    update_cumulative_position() {
+        if (this.last_body_state === null) {
+            this.last_body_state = { ...this.body_state }
+            this.body_state.cumulative_x = 0
+            this.body_state.cumulative_y = 0
+            this.body_state.cumulative_z = 0
+            this.body_state.cumulative_roll = 0
+            this.body_state.cumulative_pitch = 0
+            this.body_state.cumulative_yaw = 0
+            return
+        }
+
+        const m = this.gait_state
+        const moving = m.step_x !== 0 || m.step_z !== 0 || m.step_angle !== 0
+
+        if (moving) {
+            const step_displacement_x_local =
+                m.step_x * m.step_velocity * this.dt * this.speed_factor
+            const step_displacement_z_local =
+                m.step_z * m.step_velocity * this.dt * this.speed_factor
+            const step_displacement_yaw =
+                m.step_angle * m.step_velocity * this.dt * this.speed_factor
+
+            const cos_yaw = Math.cos(this.cumulative_orientation.yaw)
+            const sin_yaw = Math.sin(this.cumulative_orientation.yaw)
+            const step_displacement_x =
+                step_displacement_x_local * cos_yaw - step_displacement_z_local * sin_yaw
+            const step_displacement_z =
+                step_displacement_x_local * sin_yaw + step_displacement_z_local * cos_yaw
+
+            this.cumulative_position.x += step_displacement_x
+            this.cumulative_position.z += step_displacement_z
+            this.cumulative_orientation.yaw += step_displacement_yaw
+        }
+
+        this.body_state.cumulative_x = this.cumulative_position.x
+        this.body_state.cumulative_y = this.cumulative_position.y
+        this.body_state.cumulative_z = this.cumulative_position.z
+        this.body_state.cumulative_roll = this.cumulative_orientation.roll
+        this.body_state.cumulative_pitch = this.cumulative_orientation.pitch
+        this.body_state.cumulative_yaw = this.cumulative_orientation.yaw
+
+        this.last_body_state = { ...this.body_state }
+    }
 }
 
 const stance_curve = (length: number, angle: number, depth: number, phase: number): number[] => {

app/src/lib/kinematic.ts

@@ -6,6 +6,12 @@ export interface body_state_t {
     ym: number
     zm: number
     feet: number[][]
+    cumulative_x: number
+    cumulative_y: number
+    cumulative_z: number
+    cumulative_roll: number
+    cumulative_pitch: number
+    cumulative_yaw: number
 }
 
 export interface position {