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

🚸 Update default feet positions

Browse Source
This commit is contained in:
Rune Harlyk
2025-06-30 22:00:26 +02:00
parent 753e692fe2
commit 3ee096bfab
2 changed files with 370 additions and 369 deletions
Download Patch File Download Diff File Expand all files Collapse all files
app/src/lib/gait.ts
+361 -366
View File
@@ -1,452 +1,447 @@
import type { body_state_t } from './kinematic'; import type { body_state_t } from './kinematic'
import { fromInt8 } from './utilities'; import Kinematic from './kinematic'
import { fromInt8 } from './utilities'
const { sin } = Math; const { sin } = Math
export interface gait_state_t { export interface gait_state_t {
step_height: number; step_height: number
step_x: number; step_x: number
step_z: number; step_z: number
step_angle: number; step_angle: number
step_velocity: number; step_velocity: number
step_depth: number; step_depth: number
} }
export interface ControllerCommand { export interface ControllerCommand {
stop: number; stop: number
lx: number; lx: number
ly: number; ly: number
rx: number; rx: number
ry: number; ry: number
h: number; h: number
s: number; s: number
s1: number; s1: number
} }
export abstract class GaitState { export abstract class GaitState {
protected abstract name: string; protected abstract name: string
protected dt = 0.02; protected dt = 0.02
protected body_state!: body_state_t; protected body_state!: body_state_t
protected gait_state: gait_state_t = { protected gait_state: gait_state_t = {
step_height: 0.4, step_height: 0.4,
step_x: 0, step_x: 0,
step_z: 0, step_z: 0,
step_angle: 0, step_angle: 0,
step_velocity: 1, step_velocity: 1,
step_depth: 0.002 step_depth: 0.002
}; }
public get default_feet_pos() { public get default_feet_pos() {
return [ return new Kinematic().getDefaultFeetPos()
[1, -1, 1, 1], }
[1, -1, -1, 1],
[-1, -1, 1, 1], protected get default_height() {
[-1, -1, -1, 1] return 0.5
]; }
begin() {
console.log('Starting', this.name)
}
end() {
console.log('Ending', this.name)
}
step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
this.map_command(command)
this.body_state = body_state
this.dt = dt / 1000
return body_state
}
map_command(command: ControllerCommand) {
const newCommand = {
step_height: 0.4 + (command.s1 / 128 + 1) / 2,
step_x: Math.floor(fromInt8(command.ly, -1, 1) * 10) / 10,
step_z: -(Math.floor(fromInt8(command.lx, -1, 1) * 10) / 10),
step_velocity: command.s / 128 + 1,
step_angle: command.rx / 128,
step_depth: 0.002
} }
protected get default_height() { this.gait_state = newCommand
return 0.5; }
}
begin() {
console.log('Starting', this.name);
}
end() {
console.log('Ending', this.name);
}
step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
this.map_command(command);
this.body_state = body_state;
this.dt = dt / 1000;
return body_state;
}
map_command(command: ControllerCommand) {
const newCommand = {
step_height: 0.4 + (command.s1 / 128 + 1) / 2,
step_x: Math.floor(fromInt8(command.ly, -1, 1) * 10) / 10,
step_z: -(Math.floor(fromInt8(command.lx, -1, 1) * 10) / 10),
step_velocity: command.s / 128 + 1,
step_angle: command.rx / 128,
step_depth: 0.002
};
this.gait_state = newCommand;
}
} }
export class IdleState extends GaitState { export class IdleState extends GaitState {
protected name = 'Idle'; protected name = 'Idle'
} }
export class CalibrationState extends GaitState { export class CalibrationState extends GaitState {
protected name = 'Calibration'; protected name = 'Calibration'
step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) { step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
body_state.omega = 0; body_state.omega = 0
body_state.phi = 0; body_state.phi = 0
body_state.psi = 0; body_state.psi = 0
body_state.xm = 0; body_state.xm = 0
body_state.ym = this.default_height * 10; body_state.ym = this.default_height * 10
body_state.zm = 0; body_state.zm = 0
body_state.feet = this.default_feet_pos; body_state.feet = this.default_feet_pos
return body_state; return body_state
} }
} }
export class RestState extends GaitState { export class RestState extends GaitState {
protected name = 'Rest'; protected name = 'Rest'
step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) { step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
body_state.omega = 0; body_state.omega = 0
body_state.phi = 0; body_state.phi = 0
body_state.psi = 0; body_state.psi = 0
body_state.xm = 0; body_state.xm = 0
body_state.ym = this.default_height / 2; body_state.ym = this.default_height / 2
body_state.zm = 0; body_state.zm = 0
body_state.feet = this.default_feet_pos; body_state.feet = this.default_feet_pos
return body_state; return body_state
} }
} }
export class StandState extends GaitState { export class StandState extends GaitState {
protected name = 'Stand'; protected name = 'Stand'
step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) { step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
body_state.omega = 0; body_state.omega = 0
body_state.phi = command.rx / 8; body_state.phi = command.rx / 8
body_state.psi = command.ry / 8; body_state.psi = command.ry / 8
body_state.xm = command.ly / 2 / 100; body_state.xm = command.ly / 2 / 100
body_state.zm = command.lx / 2 / 100; body_state.zm = command.lx / 2 / 100
body_state.feet = this.default_feet_pos; body_state.feet = this.default_feet_pos
return body_state; return body_state
} }
} }
abstract class PhaseGaitState extends GaitState { abstract class PhaseGaitState extends GaitState {
protected tick = 0; protected tick = 0
protected phase = 0; protected phase = 0
protected phase_time = 0; protected phase_time = 0
protected abstract num_phases: number; protected abstract num_phases: number
protected abstract phase_speed_factor: number; protected abstract phase_speed_factor: number
protected abstract swing_stand_ratio: number; protected abstract swing_stand_ratio: number
protected contact_phases!: number[][]; protected contact_phases!: number[][]
protected shifts!: number[][]; protected shifts!: number[][]
step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) { step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
super.step(body_state, command, dt); super.step(body_state, command, dt)
this.update_phase(); this.update_phase()
this.update_body_position(); this.update_body_position()
this.update_feet_positions(); this.update_feet_positions()
return this.body_state; return this.body_state
}
update_phase() {
this.phase_time += this.dt * this.phase_speed_factor * this.gait_state.step_velocity
if (this.phase_time >= 1) {
this.phase += 1
if (this.phase == this.num_phases) this.phase = 0
this.phase_time = 0
}
}
update_body_position() {
if (this.num_phases === 4) return
const shift = this.shifts[Math.floor(this.phase / 2)]
this.body_state.xm += (shift[0] - this.body_state.xm) * this.dt * 4
this.body_state.zm += (shift[2] - this.body_state.zm) * this.dt * 4
}
update_feet_positions() {
for (let i = 0; i < 4; i++) {
this.body_state.feet[i] = this.update_foot_position(i)
}
}
update_foot_position(index: number): number[] {
const contact = this.contact_phases[index][this.phase]
return contact ? this.stand(index) : this.swing(index)
}
stand(index: number): number[] {
const delta_pos = [
-this.gait_state.step_x * this.dt * this.swing_stand_ratio,
0,
-this.gait_state.step_z * this.dt * this.swing_stand_ratio
]
this.body_state.feet[index][0] = this.body_state.feet[index][0] + delta_pos[0]
this.body_state.feet[index][1] = this.default_feet_pos[index][1]
this.body_state.feet[index][2] = this.body_state.feet[index][2] + delta_pos[2]
return this.body_state.feet[index]
}
swing(index: number): number[] {
const delta_pos = [this.gait_state.step_x * this.dt, 0, this.gait_state.step_z * this.dt]
if (this.gait_state.step_x == 0) {
delta_pos[0] =
(this.default_feet_pos[index][0] - this.body_state.feet[index][0]) * this.dt * 8
} }
update_phase() { if (this.gait_state.step_z == 0) {
this.phase_time += this.dt * this.phase_speed_factor * this.gait_state.step_velocity; delta_pos[2] =
(this.default_feet_pos[index][2] - this.body_state.feet[index][2]) * this.dt * 8
if (this.phase_time >= 1) {
this.phase += 1;
if (this.phase == this.num_phases) this.phase = 0;
this.phase_time = 0;
}
} }
update_body_position() { this.body_state.feet[index][0] = this.body_state.feet[index][0] + delta_pos[0]
if (this.num_phases === 4) return; this.body_state.feet[index][1] =
this.default_feet_pos[index][1] + sin(this.phase_time * Math.PI) * this.gait_state.step_height
const shift = this.shifts[Math.floor(this.phase / 2)]; this.body_state.feet[index][2] = this.body_state.feet[index][2] + delta_pos[2]
return this.body_state.feet[index]
this.body_state.xm += (shift[0] - this.body_state.xm) * this.dt * 4; }
this.body_state.zm += (shift[2] - this.body_state.zm) * this.dt * 4;
}
update_feet_positions() {
for (let i = 0; i < 4; i++) {
this.body_state.feet[i] = this.update_foot_position(i);
}
}
update_foot_position(index: number): number[] {
const contact = this.contact_phases[index][this.phase];
return contact ? this.stand(index) : this.swing(index);
}
stand(index: number): number[] {
const delta_pos = [
-this.gait_state.step_x * this.dt * this.swing_stand_ratio,
0,
-this.gait_state.step_z * this.dt * this.swing_stand_ratio
];
this.body_state.feet[index][0] = this.body_state.feet[index][0] + delta_pos[0];
this.body_state.feet[index][1] = this.default_feet_pos[index][1];
this.body_state.feet[index][2] = this.body_state.feet[index][2] + delta_pos[2];
return this.body_state.feet[index];
}
swing(index: number): number[] {
const delta_pos = [this.gait_state.step_x * this.dt, 0, this.gait_state.step_z * this.dt];
if (this.gait_state.step_x == 0) {
delta_pos[0] =
(this.default_feet_pos[index][0] - this.body_state.feet[index][0]) * this.dt * 8;
}
if (this.gait_state.step_z == 0) {
delta_pos[2] =
(this.default_feet_pos[index][2] - this.body_state.feet[index][2]) * this.dt * 8;
}
this.body_state.feet[index][0] = this.body_state.feet[index][0] + delta_pos[0];
this.body_state.feet[index][1] =
this.default_feet_pos[index][1] +
sin(this.phase_time * Math.PI) * this.gait_state.step_height;
this.body_state.feet[index][2] = this.body_state.feet[index][2] + delta_pos[2];
return this.body_state.feet[index];
}
} }
export class FourPhaseWalkState extends PhaseGaitState { export class FourPhaseWalkState extends PhaseGaitState {
protected name = 'Four phase walk'; protected name = 'Four phase walk'
protected num_phases = 4; protected num_phases = 4
protected phase_speed_factor = 6; protected phase_speed_factor = 6
protected contact_phases = [ protected contact_phases = [
[1, 0, 1, 1], [1, 0, 1, 1],
[1, 1, 1, 0], [1, 1, 1, 0],
[1, 1, 1, 0], [1, 1, 1, 0],
[1, 0, 1, 1] [1, 0, 1, 1]
]; ]
protected swing_stand_ratio = 1 / (this.num_phases - 1); protected swing_stand_ratio = 1 / (this.num_phases - 1)
begin() { begin() {
super.begin(); super.begin()
} }
end() { end() {
super.end(); super.end()
} }
step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) { step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
return super.step(body_state, command, dt); return super.step(body_state, command, dt)
} }
} }
export class EightPhaseWalkState extends PhaseGaitState { export class EightPhaseWalkState extends PhaseGaitState {
protected name = 'Eight phase walk'; protected name = 'Eight phase walk'
protected num_phases = 8; protected num_phases = 8
protected phase_speed_factor = 4; protected phase_speed_factor = 4
protected contact_phases = [ protected contact_phases = [
[1, 0, 1, 1, 1, 1, 1, 1], [1, 0, 1, 1, 1, 1, 1, 1],
[1, 1, 1, 1, 1, 0, 1, 1], [1, 1, 1, 1, 1, 0, 1, 1],
[1, 1, 1, 1, 1, 1, 1, 0], [1, 1, 1, 1, 1, 1, 1, 0],
[1, 1, 1, 0, 1, 1, 1, 1] [1, 1, 1, 0, 1, 1, 1, 1]
]; ]
protected shifts = [ protected shifts = [
[-0.05, 0, -0.2], [-0.05, 0, -0.2],
[0.3, 0, 0.2], [0.3, 0, 0.2],
[-0.05, 0, 0.2], [-0.05, 0, 0.2],
[0.3, 0, -0.2] [0.3, 0, -0.2]
]; ]
protected swing_stand_ratio = 1 / (this.num_phases - 1); protected swing_stand_ratio = 1 / (this.num_phases - 1)
begin() { begin() {
super.begin(); super.begin()
} }
end() { end() {
super.end(); super.end()
} }
step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) { step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
return super.step(body_state, command, dt); return super.step(body_state, command, dt)
} }
} }
export class BezierState extends GaitState { export class BezierState extends GaitState {
protected name = 'Bezier'; protected name = 'Bezier'
protected phase = 0; protected phase = 0
protected phase_num = 0; protected phase_num = 0
protected step_length: number = 0; protected step_length: number = 0
offset = [0, 0.5, 0.5, 0]; offset = [0, 0.5, 0.5, 0]
begin() { begin() {
super.begin(); super.begin()
}
end() {
super.end()
}
step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
super.step(body_state, command, dt)
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()
this.update_feet_positions()
return this.body_state
}
end() { update_phase() {
super.end(); this.phase += this.dt * this.gait_state.step_velocity * 2
if (this.phase >= 1) {
this.phase_num += 1
this.phase_num %= 2
this.phase = 0
} }
}
step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) { update_feet_positions() {
super.step(body_state, command, dt); for (let i = 0; i < 4; i++) {
this.step_length = Math.sqrt(this.gait_state.step_x ** 2 + this.gait_state.step_z ** 2); this.body_state.feet[i] = this.update_foot_position(i)
if (this.gait_state.step_x < 0) {
this.step_length = -this.step_length;
}
this.update_phase();
this.update_feet_positions();
return this.body_state;
} }
}
update_phase() { update_foot_position(index: number): number[] {
this.phase += this.dt * this.gait_state.step_velocity * 2; let phase = this.phase + this.offset[index]
if (this.phase >= 1) { if (phase >= 1) {
this.phase_num += 1; phase -= 1
this.phase_num %= 2;
this.phase = 0;
}
} }
this.body_state.feet[index][0] = this.default_feet_pos[index][0]
this.body_state.feet[index][1] = this.default_feet_pos[index][1]
this.body_state.feet[index][2] = this.default_feet_pos[index][2]
return phase <= 0.75 ?
this.stand_controller(index, phase / 0.75)
: this.swing_controller(index, (phase - 0.75) / (1 - 0.75))
}
update_feet_positions() { stand_controller(index: number, phase: number) {
for (let i = 0; i < 4; i++) { let depth = this.gait_state.step_depth
this.body_state.feet[i] = this.update_foot_position(i); return this.controller(index, phase, stance_curve, depth)
} }
}
update_foot_position(index: number): number[] { swing_controller(index: number, phase: number) {
let phase = this.phase + this.offset[index]; let height = this.gait_state.step_height
if (phase >= 1) { return this.controller(index, phase, bezier_curve, height)
phase -= 1; }
}
this.body_state.feet[index][0] = this.default_feet_pos[index][0];
this.body_state.feet[index][1] = this.default_feet_pos[index][1];
this.body_state.feet[index][2] = this.default_feet_pos[index][2];
return phase <= 0.75 ?
this.stand_controller(index, phase / 0.75)
: this.swing_controller(index, (phase - 0.75) / (1 - 0.75));
}
stand_controller(index: number, phase: number) { controller(
let depth = this.gait_state.step_depth; index: number,
return this.controller(index, phase, stance_curve, depth); phase: number,
} controller: (length: number, angle: number, ...args: number[]) => number[],
...args: number[]
) {
let length = this.step_length / 2
let angle = Math.atan2(this.gait_state.step_z, this.step_length) * 2
const delta_pos = controller(length, angle, ...args, phase)
swing_controller(index: number, phase: number) { length = this.gait_state.step_angle * 2
let height = this.gait_state.step_height; angle = yawArc(this.default_feet_pos[index], this.body_state.feet[index])
return this.controller(index, phase, bezier_curve, height);
}
controller( const delta_rot = controller(length, angle, ...args, phase)
index: number,
phase: number,
controller: (length: number, angle: number, ...args: number[]) => number[],
...args: number[]
) {
let length = this.step_length / 2;
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; this.body_state.feet[index][0] += delta_pos[0] + delta_rot[0] * 0.2
angle = yawArc(this.default_feet_pos[index], this.body_state.feet[index]); this.body_state.feet[index][2] += delta_pos[2] + delta_rot[2] * 0.2
if (this.gait_state.step_x || this.gait_state.step_z || this.gait_state.step_angle)
this.body_state.feet[index][1] += delta_pos[1] + delta_rot[1] * 0.2
const delta_rot = controller(length, angle, ...args, phase); return this.body_state.feet[index]
}
this.body_state.feet[index][0] += delta_pos[0] + delta_rot[0] * 0.2;
this.body_state.feet[index][2] += delta_pos[2] + delta_rot[2] * 0.2;
if (this.gait_state.step_x || this.gait_state.step_z || this.gait_state.step_angle)
this.body_state.feet[index][1] += delta_pos[1] + delta_rot[1] * 0.2;
return this.body_state.feet[index];
}
} }
const stance_curve = (length: number, angle: number, depth: number, phase: number): number[] => { const stance_curve = (length: number, angle: number, depth: number, phase: number): number[] => {
const X_POLAR = Math.cos(angle); const X_POLAR = Math.cos(angle)
const Y_POLAR = Math.sin(angle); const Y_POLAR = Math.sin(angle)
const step = length * (1 - 2 * phase); const step = length * (1 - 2 * phase)
const X = step * X_POLAR; const X = step * X_POLAR
const Z = step * Y_POLAR; const Z = step * Y_POLAR
let Y = 0; let Y = 0
if (length !== 0) { if (length !== 0) {
Y = -depth * Math.cos((Math.PI * (X + Y)) / (2 * length)); Y = -depth * Math.cos((Math.PI * (X + Y)) / (2 * length))
} }
return [X, Y, Z]; return [X, Y, Z]
}; }
const yawArc = (default_foot_pos: number[], current_foot_pos: number[]): number => { const yawArc = (default_foot_pos: number[], current_foot_pos: number[]): number => {
const foot_mag = Math.sqrt(default_foot_pos[0] ** 2 + default_foot_pos[2] ** 2); const foot_mag = Math.sqrt(default_foot_pos[0] ** 2 + default_foot_pos[2] ** 2)
const foot_dir = Math.atan2(default_foot_pos[2], default_foot_pos[0]); const foot_dir = Math.atan2(default_foot_pos[2], default_foot_pos[0])
const offsets = [ const offsets = [
current_foot_pos[0] - default_foot_pos[0], current_foot_pos[0] - default_foot_pos[0],
current_foot_pos[2] - default_foot_pos[2], current_foot_pos[2] - default_foot_pos[2],
current_foot_pos[1] - default_foot_pos[1] current_foot_pos[1] - default_foot_pos[1]
]; ]
const offset_mag = Math.sqrt(offsets[0] ** 2 + offsets[2] ** 2); const offset_mag = Math.sqrt(offsets[0] ** 2 + offsets[2] ** 2)
const offset_mod = Math.atan2(offset_mag, foot_mag); const offset_mod = Math.atan2(offset_mag, foot_mag)
return Math.PI / 2.0 + foot_dir + offset_mod; return Math.PI / 2.0 + foot_dir + offset_mod
}; }
const bezier_curve = (length: number, angle: number, height: number, phase: number): number[] => { const bezier_curve = (length: number, angle: number, height: number, phase: number): number[] => {
const control_points = get_control_points(length, angle, height); const control_points = get_control_points(length, angle, height)
const n = control_points.length - 1; const n = control_points.length - 1
const point = [0, 0, 0]; const point = [0, 0, 0]
for (let i = 0; i <= n; i++) { for (let i = 0; i <= n; i++) {
const bernstein_poly = comb(n, i) * Math.pow(phase, i) * Math.pow(1 - phase, n - i); const bernstein_poly = comb(n, i) * Math.pow(phase, i) * Math.pow(1 - phase, n - i)
point[0] += bernstein_poly * control_points[i][0]; point[0] += bernstein_poly * control_points[i][0]
point[1] += bernstein_poly * control_points[i][1]; point[1] += bernstein_poly * control_points[i][1]
point[2] += bernstein_poly * control_points[i][2]; point[2] += bernstein_poly * control_points[i][2]
} }
return point; return point
}; }
const get_control_points = (length: number, angle: number, height: number): number[][] => { const get_control_points = (length: number, angle: number, height: number): number[][] => {
const X_POLAR = Math.cos(angle); const X_POLAR = Math.cos(angle)
const Z_POLAR = Math.sin(angle); const Z_POLAR = Math.sin(angle)
const STEP = [ const STEP = [
-length, -length,
-length * 1.4, -length * 1.4,
-length * 1.5, -length * 1.5,
-length * 1.5, -length * 1.5,
-length * 1.5, -length * 1.5,
0.0, 0.0,
0.0, 0.0,
0.0, 0.0,
length * 1.5, length * 1.5,
length * 1.5, length * 1.5,
length * 1.4, length * 1.4,
length length
]; ]
const Y = [ const Y = [
0.0, 0.0,
0.0, 0.0,
height * 0.9, height * 0.9,
height * 0.9, height * 0.9,
height * 0.9, height * 0.9,
height * 0.9, height * 0.9,
height * 0.9, height * 0.9,
height * 1.1, height * 1.1,
height * 1.1, height * 1.1,
height * 1.1, height * 1.1,
0.0, 0.0,
0.0 0.0
]; ]
const control_points: number[][] = []; const control_points: number[][] = []
for (let i = 0; i < STEP.length; i++) { for (let i = 0; i < STEP.length; i++) {
const X = STEP[i] * X_POLAR; const X = STEP[i] * X_POLAR
const Z = STEP[i] * Z_POLAR; const Z = STEP[i] * Z_POLAR
control_points.push([X, Y[i], Z]); control_points.push([X, Y[i], Z])
} }
return control_points; return control_points
}; }
const comb = (n: number, k: number): number => { const comb = (n: number, k: number): number => {
if (k < 0 || k > n) return 0; if (k < 0 || k > n) return 0
if (k === 0 || k === n) return 1; if (k === 0 || k === n) return 1
k = Math.min(k, n - k); k = Math.min(k, n - k)
let c = 1; let c = 1
for (let i = 0; i < k; i++) { for (let i = 0; i < k; i++) {
c = (c * (n - i)) / (i + 1); c = (c * (n - i)) / (i + 1)
} }
return c; return c
}; }
app/src/lib/kinematic.ts
+9 -3
View File
@@ -46,11 +46,11 @@ export default class Kinematic {
constructor() { constructor() {
this.l1 = 60.5 / 100 this.l1 = 60.5 / 100
this.l2 = 10 / 100 this.l2 = 10 / 100
this.l3 = 100.7 / 100 this.l3 = 111.7 / 100
this.l4 = 118.5 / 100 this.l4 = 118.5 / 100
this.L = 230 / 100 this.L = 207.5 / 100
this.W = 75 / 100 this.W = 78 / 100
this.mountOffsets = [ this.mountOffsets = [
[this.L / 2, 0, this.W / 2], [this.L / 2, 0, this.W / 2],
@@ -60,6 +60,12 @@ export default class Kinematic {
] ]
} }
getDefaultFeetPos(): number[][] {
return this.mountOffsets.map((offset, i) => {
return [offset[0], -1, offset[2] + (i % 2 === 1 ? -this.l1 : this.l1)]
})
}
calcIK(p: body_state_t): number[] { calcIK(p: body_state_t): number[] {
const roll = p.omega * this.DEG2RAD const roll = p.omega * this.DEG2RAD
const pitch = p.phi * this.DEG2RAD const pitch = p.phi * this.DEG2RAD