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

♻️ Change kinematics units to SI

Browse Source
This commit is contained in:
Rune Harlyk
2025-12-24 13:40:33 +01:00
parent 3be08a31ed
commit 5e2e29d2a4
5 changed files with 114 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files
app/src/lib/components/Visualization.svelte
+15 -7
View File
@@ -74,13 +74,14 @@
let lastTick = performance.now()
const dir = [1, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1]
const THREEJS_SCALE = 10
let body_state = {
omega: 0,
phi: 0,
psi: 0,
xm: 0,
ym: 0.5,
ym: 0.15,
zm: 0,
feet: kinematic.getDefaultFeetPos(),
cumulative_x: 0,
@@ -104,7 +105,7 @@
phi: 0,
psi: 0,
xm: 0,
ym: 0.7,
ym: 0.15,
zm: 0,
Background: defaultColor
}
@@ -239,8 +240,16 @@
const rotatedXm = settings.xm * cosYaw - settings.zm * sinYaw
const rotatedZm = settings.xm * sinYaw + settings.zm * cosYaw
robot.position.x = smooth(robot.position.x, -rotatedZm - body_state.cumulative_z * 1.2, 0.1)
robot.position.z = smooth(robot.position.z, -rotatedXm - body_state.cumulative_x * 1.2, 0.1)
robot.position.x = smooth(
robot.position.x,
(-rotatedZm - body_state.cumulative_z) * THREEJS_SCALE,
0.1
)
robot.position.z = smooth(
robot.position.z,
(-rotatedXm - body_state.cumulative_x) * THREEJS_SCALE,
0.1
)
const pitch = degToRad(settings.psi - 90) + body_state.cumulative_pitch
const roll = degToRad(settings.omega) + body_state.cumulative_roll
@@ -275,7 +284,6 @@
s: controlData[5],
s1: controlData[6]
}
body_state.ym = data.h
let planner = planners[get(mode)]
const delta = performance.now() - lastTick
@@ -296,8 +304,8 @@
settings.omega = radToDeg(robot.rotation.y)
settings.phi = radToDeg(robot.rotation.z) + $mpu.heading - 90
settings.psi = radToDeg(robot.rotation.x) + 90
settings.xm = robot.position.z * 100
settings.zm = -robot.position.x * 100
settings.xm = robot.position.z / THREEJS_SCALE
settings.zm = -robot.position.x / THREEJS_SCALE
}
const updateTargetPosition = () => {
app/src/lib/gait.ts
+35 -20
View File
@@ -26,21 +26,34 @@ export abstract class GaitState {
protected dt = 0.02
protected body_state!: body_state_t
protected get kinematic() {
return get(currentKinematic)
}
protected gait_state: gait_state_t = {
step_height: 0.4,
step_height: 0,
step_x: 0,
step_z: 0,
step_angle: 0,
step_velocity: 1,
step_depth: 0.002
step_depth: 0
}
public get default_feet_pos() {
return get(currentKinematic).getDefaultFeetPos()
return this.kinematic.getDefaultFeetPos()
}
protected get default_height() {
return 0.5
return this.kinematic.default_body_height
}
protected get default_step_depth() {
return this.kinematic.default_step_depth
}
protected get default_step_height() {
return this.kinematic.default_step_height
}
begin() {
@@ -67,16 +80,15 @@ export abstract class GaitState {
}
map_command(command: ControllerCommand) {
const newCommand = {
step_height: command.s1,
step_x: command.ly,
step_z: -command.lx,
const kin = this.kinematic
this.gait_state = {
step_height: command.s1 * kin.max_step_height,
step_x: command.ly * kin.max_step_length,
step_z: -command.lx * kin.max_step_length,
step_velocity: command.s,
step_angle: command.rx,
step_depth: 0.002
step_depth: kin.default_step_depth
}
this.gait_state = newCommand
}
}
@@ -92,14 +104,13 @@ export class IdleState extends GaitState {
export class CalibrationState extends GaitState {
protected name = 'Calibration'
// 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
body_state.xm = 0
body_state.ym = this.default_height * 10
body_state.ym = this.kinematic.max_body_height
body_state.zm = 0
body_state.feet = this.default_feet_pos
return body_state
@@ -109,14 +120,13 @@ export class CalibrationState extends GaitState {
export class RestState extends GaitState {
protected name = 'Rest'
// 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
body_state.xm = 0
body_state.ym = this.default_height / 2
body_state.ym = this.kinematic.min_body_height
body_state.zm = 0
body_state.feet = this.default_feet_pos
return body_state
@@ -128,11 +138,13 @@ export class StandState extends GaitState {
step(body_state: body_state_t, command: ControllerCommand) {
super.step(body_state, command)
const kin = this.kinematic
body_state.omega = 0
body_state.phi = command.rx * 10 * (Math.PI / 2)
body_state.psi = command.ry * 10 * (Math.PI / 2)
body_state.xm = command.ly / 4
body_state.zm = command.lx / 4
body_state.ym = kin.min_body_height + command.h * kin.body_height_range
body_state.psi = command.ry * kin.max_pitch
body_state.phi = command.rx * kin.max_roll
body_state.xm = command.ly * kin.max_body_shift_x
body_state.zm = command.lx * kin.max_body_shift_z
body_state.feet = this.default_feet_pos
return body_state
}
@@ -191,6 +203,8 @@ export class BezierState extends GaitState {
step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
super.step(body_state, command, dt)
const kin = this.kinematic
this.body_state.ym = kin.min_body_height + command.h * kin.body_height_range
this.step_length = Math.sqrt(this.gait_state.step_x ** 2 + this.gait_state.step_z ** 2)
if (this.gait_state.step_x < 0) this.step_length = -this.step_length
this.update_phase()
@@ -339,7 +353,8 @@ export class BezierState extends GaitState {
let angle = Math.atan2(this.gait_state.step_z, this.step_length) * 2
const delta_pos = controller(length, angle, ...args, phase)
length = this.gait_state.step_angle * 2
const kin = this.kinematic
length = this.gait_state.step_angle * kin.max_step_length
angle = yawArc(this.default_feet_pos[index], this.body_state.feet[index])
const delta_rot = controller(length, angle, ...args, phase)
app/src/lib/kinematic.ts
+34 -3
View File
@@ -50,7 +50,22 @@ export default class Kinematic {
DEG2RAD = DEG2RAD
max_roll: number
max_pitch: number
max_body_shift_x: number
max_body_shift_z: number
max_leg_reach: number
min_body_height: number
max_body_height: number
body_height_range: number
max_step_length: number
max_step_height: number
default_step_depth: number
default_body_height: number
default_step_height: number
mountOffsets: number[][]
default_feet_positions: number[][]
invMountRot = [
[0, 0, -1],
@@ -66,18 +81,34 @@ export default class Kinematic {
this.L = params.L
this.W = params.W
this.max_roll = 15 * (Math.PI / 2)
this.max_pitch = 15 * (Math.PI / 2)
this.max_body_shift_x = this.W / 3
this.max_body_shift_z = this.W / 3
this.max_leg_reach = this.femur + this.tibia - this.coxa_offset
this.min_body_height = this.max_leg_reach * 0.45
this.max_body_height = this.max_leg_reach * 1
this.body_height_range = this.max_body_height - this.min_body_height
this.max_step_length = this.max_leg_reach * 0.8
this.max_step_height = this.max_leg_reach / 2
this.default_step_depth = 0.002
this.default_body_height = this.min_body_height + this.body_height_range / 2
this.default_step_height = this.default_body_height / 2
this.mountOffsets = [
[this.L / 2, 0, this.W / 2],
[this.L / 2, 0, -this.W / 2],
[-this.L / 2, 0, this.W / 2],
[-this.L / 2, 0, -this.W / 2]
]
this.default_feet_positions = this.mountOffsets.map((offset, i) => {
return [offset[0], 0, offset[2] + (i % 2 === 1 ? -this.coxa : this.coxa)]
})
}
getDefaultFeetPos(): number[][] {
return this.mountOffsets.map((offset, i) => {
return [offset[0], -1, offset[2] + (i % 2 === 1 ? -this.coxa : this.coxa)]
})
return this.default_feet_positions.map(pos => [...pos])
}
calcIK(p: body_state_t): number[] {
app/src/lib/stores/featureFlags.ts
+12 -12
View File
@@ -29,24 +29,24 @@ export const variants = {
model: `${base}spot_micro.urdf.xacro`,
stl: `${base}stl.zip`,
kinematics: {
coxa: 60.5 / 100,
coxa_offset: 10 / 100,
femur: 111.7 / 100,
tibia: 118.5 / 100,
L: 207.5 / 100,
W: 78 / 100
coxa: 0.0605,
coxa_offset: 0.01,
femur: 0.1112,
tibia: 0.1185,
L: 0.2075,
W: 0.078
}
},
SPOTMICRO_YERTLE: {
model: `${base}yertle.URDF`,
stl: `${base}URDF.zip`,
kinematics: {
coxa: 35 / 100,
coxa_offset: 0 / 100,
femur: 130 / 100,
tibia: 130 / 100,
L: 240 / 100,
W: 78 / 100
coxa: 0.035,
coxa_offset: 0.0,
femur: 0.13,
tibia: 0.13,
L: 0.24,
W: 0.078
}
}
}