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

🪅 Adds 8 phase gait with body shift

Browse Source
This commit is contained in:
Rune Harlyk
2024-08-01 18:19:31 +02:00
committed by Rune Harlyk
parent 5d9343989b
commit 46a7dbd8f2
3 changed files with 254 additions and 468 deletions
Download Patch File Download Diff File Expand all files Collapse all files
app/src/lib/components/Visualization.svelte
+26 -48
View File
@@ -9,7 +9,8 @@
import SceneBuilder from '$lib/sceneBuilder';
import { lerp, degToRad } from 'three/src/math/MathUtils';
import { GUI } from 'three/addons/libs/lil-gui.module.min.js';
import Kinematic, { PhaseGaitPlanner, type body_state_t } from '$lib/kinematic';
import Kinematic, { type body_state_t } from '$lib/kinematic';
import {EightPhaseWalkState, FourPhaseWalkState, StandState} from '$lib/gait'
import { radToDeg } from 'three/src/math/MathUtils.js';
import type { URDFRobot } from 'urdf-loader';
import { get } from 'svelte/store';
@@ -35,7 +36,8 @@
let target_position = {x: 0, z: 0, yaw: 0}
let kinematic = new Kinematic()
let gaitPlanner = new PhaseGaitPlanner('walk')
let gaitPlanner = new FourPhaseWalkState()
let standState = new StandState()
const dir = [1, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1]
const Lp = [
[1, -1, 1, 1],
@@ -44,24 +46,16 @@
[-1, -1, -1, 1],
];
const body_state = {
let body_state = {
omega: 0,
phi: 0,
psi: 0,
xm: 0,
ym: 0.7,
ym: 0.5,
zm: 0,
feet: Lp
}
const gait_state = {
step_height:0.2,
step_x:1,
step_z:1,
step_velocity:1,
step_angle:0
}
let settings = {
'Internal kinematic':true,
'Robot transform controls':false,
@@ -84,31 +78,6 @@
await createScene();
if (!isEmbeddedApp && panel) createPanel();
servoAngles.subscribe(updateAnglesFromStore)
outControllerData.subscribe((buffer) => {
if (!settings['Internal kinematic']) return
const data = {
stop: buffer[0],
lx: buffer[1],
ly: buffer[2],
rx: buffer[3],
ry: buffer[4],
h: buffer[5],
s: buffer[6],
};
settings.ym = (data.h+128)*0.75 / 100
switch (get(mode)) {
case ModesEnum.Stand:
settings.omega = 0
settings.phi = data.rx / 8
settings.psi = data.ry / 8
settings.xm = data.ly / 2 / 100
settings.zm = data.lx / 2 / 100
break;
}
})
});
const updateAnglesFromStore = (angles: number[]) => {
@@ -136,9 +105,9 @@
kinematic.add(settings, 'omega', -20, 20).onChange(updateKinematicPosition).listen();
kinematic.add(settings, 'phi', -30, 30).onChange(updateKinematicPosition).listen();
kinematic.add(settings, 'psi', -20, 15).onChange(updateKinematicPosition).listen();
kinematic.add(settings, 'xm', -90, 90).onChange(updateKinematicPosition)
kinematic.add(settings, 'ym', 0, 200).onChange(updateKinematicPosition)
kinematic.add(settings, 'zm', -130, 130).onChange(updateKinematicPosition)
kinematic.add(settings, 'xm', -1, 1).onChange(updateKinematicPosition).listen()
kinematic.add(settings, 'ym', 0, 1).onChange(updateKinematicPosition).listen()
kinematic.add(settings, 'zm', -1.3, 1.3).onChange(updateKinematicPosition).listen()
const visibility = gui_panel.addFolder('Visualization');
visibility.add(settings, 'Trace feet')
@@ -246,8 +215,8 @@
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 / 100, 0.1);
robot.position.x = smooth(robot.position.x, -settings.zm / 100, 0.1);
robot.position.z = smooth(robot.position.z, -settings.xm, 0.1);
robot.position.x = smooth(robot.position.x, -settings.zm, 0.1);
robot.rotation.z = smooth(robot.rotation.z, degToRad(-settings.phi + $mpu.heading + 90), 0.1);
robot.rotation.y = smooth(robot.rotation.y, degToRad(settings.omega), 0.1);
@@ -264,13 +233,22 @@
}
const update_gait = () => {
if (get(mode) != ModesEnum.Walk) return
if (sceneManager.isDragging || !settings['Internal kinematic']) return
const controlData = get(outControllerData)
gait_state.step_x = Math.floor(fromInt8(controlData[2], -1, 1) * 10) / 10 * 2
gait_state.step_z = Math.floor(fromInt8(controlData[1], -1, 1) * 10) / 10 * 2
gait_state.step_angle = Math.floor(fromInt8(controlData[3], -1, 1) * 10) / 10 * Math.PI
gait_state.step_velocity = fromInt8(controlData[6], -1, 1)
body_state.feet = gaitPlanner._loop(body_state, gait_state);
const data = {
stop: controlData[0],
lx: controlData[1],
ly: controlData[2],
rx: controlData[3],
ry: controlData[4],
h: controlData[5],
s: controlData[6],
};
let planner = get(mode) === ModesEnum.Walk ? gaitPlanner : standState
body_state = planner.step(body_state, data);
settings.xm = body_state.xm
settings.zm = body_state.zm
}
const update_robot_position = (robot:URDFRobot) => {
app/src/lib/gait.ts
+228
View File
@@ -0,0 +1,228 @@
import type { body_state_t } from './kinematic';
import { fromInt8 } from './utilities';
const { sin } = Math;
export interface gait_state_t {
step_height: number;
step_x: number;
step_z: number;
step_angle: number;
step_velocity: number;
step_depth: number;
}
export interface ControllerCommand {
stop: number;
lx: number;
ly: number;
rx: number;
ry: number;
h: number;
s: number;
}
export abstract class GaitState {
protected abstract name: string;
protected static body_state: body_state_t;
protected get default_feet_pos() {
return [
[1, -1, 1, 1],
[1, -1, -1, 1],
[-1, -1, 1, 1],
[-1, -1, -1, 1]
];
}
protected static get default_height() {
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) {
console.log('Stepping', this.name);
}
map_command(command: ControllerCommand): gait_state_t {
return {
step_height: 0.4,
step_x: (Math.floor(fromInt8(command.ly, -1, 1) * 10) / 10) * 3,
step_z: (Math.floor(fromInt8(command.lx, -1, 1) * 10) / 10) * 3,
step_velocity: 1,
step_angle: 0,
step_depth: 0.2
};
}
}
export class IdleState extends GaitState {
protected name = 'Idle';
}
export class StandState extends GaitState {
protected name = 'Stand';
step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
body_state.omega = 0;
body_state.phi = command.rx / 8;
body_state.psi = command.ry / 8;
body_state.xm = command.ly / 2 / 100;
body_state.zm = command.lx / 2 / 100;
body_state.ym = ((command.h + 128) * 0.75) / 100;
return body_state;
}
}
abstract class PhaseGaitState extends GaitState {
protected tick = 0;
protected phase = 0;
protected phase_time = 0;
protected abstract num_phases: number;
protected abstract phase_length: number;
protected abstract swing_stand_ratio: number;
protected contact_phases!: number[][];
protected shifts!: number[][];
protected body_state!: body_state_t;
protected gait_state!: gait_state_t;
protected dt = 0.02;
step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
this.body_state = body_state;
this.gait_state = this.map_command(command);
this.dt = dt;
this.update_phase();
this.update_body_position();
this.update_feet_positions();
return this.body_state;
}
update_phase() {
this.tick += 1;
this.phase_time = this.tick / this.phase_length;
if (this.tick % this.phase_length == 0) {
this.phase += 1;
if (this.phase == this.num_phases) this.phase = 0;
this.tick = 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;
}
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 {
protected name = 'Four phase walk';
protected num_phases = 4;
protected phase_length = 15;
protected contact_phases = [
[1, 0, 1, 1],
[1, 1, 1, 0],
[1, 1, 1, 0],
[1, 0, 1, 1]
];
protected swing_stand_ratio = 1 / (this.num_phases - 1);
begin() {
super.begin();
}
end() {
super.end();
}
step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
return super.step(body_state, command, dt);
}
}
export class EightPhaseWalkState extends PhaseGaitState {
protected name = 'Eight phase walk';
protected num_phases = 8;
protected phase_length = 20;
protected contact_phases = [
[1, 0, 1, 1, 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, 1, 1, 0]
];
protected shifts = [
[-0.3, 0, -0.2],
[-0.3, 0, 0.2],
[0.3, 0, -0.2],
[0.3, 0, 0.2]
];
protected swing_stand_ratio = 1 / (this.num_phases - 1);
begin() {
super.begin();
}
end() {
super.end();
}
step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
return super.step(body_state, command, dt);
}
}
app/src/lib/kinematic.ts
-420
View File
@@ -9,14 +9,6 @@ export interface body_state_t {
feet: number[][];
}
export interface gait_state_t {
step_height: number;
step_x: number;
step_z: number;
step_angle: number;
step_velocity: number;
}
export interface position {
x: number;
y: number;
@@ -326,415 +318,3 @@ export default class Kinematic {
}
}
export class Command {
public x_vel_cmd_mps = 0;
public y_vel_cmd_mps = 0;
public yaw_rate_cmd_rps = 0;
public phi_cmd = 0;
public theta_cmd = 0;
public psi_cmd = 0;
public idle_cmd = false;
public walk_cmd = false;
public stand_cmd = false;
constructor() {}
resetCommands() {
this.x_vel_cmd_mps = 0;
this.y_vel_cmd_mps = 0;
this.yaw_rate_cmd_rps = 0;
this.phi_cmd = 0;
this.theta_cmd = 0;
this.psi_cmd = 0;
this.idle_cmd = false;
this.walk_cmd = false;
this.stand_cmd = false;
}
getXSpeedCmd() {
return this.x_vel_cmd_mps;
}
getYSpeedCmd() {
return this.y_vel_cmd_mps;
}
getYawRateCmd() {
return this.yaw_rate_cmd_rps;
}
}
const cmd = new Command();
export class GaitState {
protected name: string;
protected static body_state: body_state_t;
constructor() {
this.name = 'GaitState';
}
begin() {
console.log('Starting', this.name);
}
end() {
console.log('Ending', this.name);
}
step(dt: number) {
console.log('Stepping', this.name);
}
getNeutralStance() {
return [
[100, -100, 100, 1],
[100, -100, -100, 1],
[-100, -100, 100, 1],
[-100, -100, -100, 1]
];
}
getDefaultStandHeight = () => 70;
}
export class IdleState extends GaitState {
constructor() {
super();
this.name = 'Idle';
}
begin() {
console.log('Starting', this.name);
}
}
export class StandState extends GaitState {
constructor() {
super();
this.name = 'Stand';
}
}
export class WalkState extends GaitState {
num_phases = 4;
ticks = 0;
constructor() {
super();
this.name = 'Walk';
}
begin() {
super.begin();
WalkState.body_state.feet = this.getNeutralStance();
WalkState.body_state.omega = 0;
WalkState.body_state.phi = 0;
WalkState.body_state.psi = 0;
WalkState.body_state.xm = 0;
WalkState.body_state.ym = this.getDefaultStandHeight();
WalkState.body_state.zm = 0;
}
end() {
super.end();
}
step(dt: number) {
super.end();
this.updatePhaseData();
WalkState.body_state.feet = this.stepGait();
if (this.num_phases == 8) {
const [omega, phi, psi] = this.stepBodyShift();
WalkState.body_state.omega = omega;
WalkState.body_state.phi = phi;
WalkState.body_state.psi = psi;
}
this.ticks++;
}
updatePhaseData() {}
stepGait() {
let contact_mode;
let swing_proportion;
let foot_pos;
let new_foot_pos;
let default_stance_feet_pos = this.getNeutralStance();
}
stepBodyShift() {
let omega = 0;
let phi = 0;
let psi = 0;
return [omega, phi, psi];
}
}
export class TrotState extends GaitState {
constructor() {
super();
this.name = 'Trot';
}
}
export class PhaseGaitPlanner {
private tick = 0;
private phase = 0;
private phase_time = 0;
private total_phases_length = 60;
private num_phases = 4;
private phase_length = this.total_phases_length / this.num_phases;
private sub_phase_tick = 0;
private _frame: number[][];
private _phi: number;
private _phi_stance: number;
private _last_time: number;
private _alpha: number;
private _s: boolean;
private _offset: number[];
private step_offset: number;
private contact_phases = [
[1, 0, 1, 1],
[1, 1, 1, 0],
[1, 1, 1, 0],
[1, 0, 1, 1]
];
private default_feet_pos = [
[1, -1, 1, 1],
[1, -1, -1, 1],
[-1, -1, 1, 1],
[-1, -1, -1, 1]
];
private body_state!: body_state_t;
private gait_state!: gait_state_t;
private dt: number = 0.02;
constructor(mode: string) {
this._frame = Array.from({ length: 4 }, () => Array(3).fill(0));
this._phi = 0;
this._phi_stance = 0;
this._last_time = 0;
this._alpha = 0;
this._s = false;
if (mode === 'walk') {
this._offset = [0, 0.5, 0.5, 0];
this.step_offset = 0.5;
} else {
this._offset = [0, 0, 0.8, 0.8];
this.step_offset = 0.5;
}
}
_loop(body_state: body_state_t, gait_state: gait_state_t, dt: number = 0.02) {
this.body_state = body_state;
this.gait_state = gait_state;
this.dt = dt;
this.update_phase();
this.update_body_position();
this.update_feet_positions();
return body_state.feet;
}
update_phase() {
this.tick += 1;
this.phase_time = this.tick / this.phase_length;
if (this.tick % this.phase_length == 0) {
this.phase += 1;
if (this.phase == this.num_phases) this.phase = 0;
this.tick = 0;
}
}
update_body_position() {}
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) / 3,
0,
(-this.gait_state.step_z * this.dt) / 3
];
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];
}
private static solve_bin_factor(n: number, k: number): number {
return Number(this.factorial(n) / (this.factorial(k) * this.factorial(n - k)));
}
private bezier_curve(t: number, k: number, point: number): number {
const n = 11;
return (
point * PhaseGaitPlanner.solve_bin_factor(n, k) * Math.pow(t, k) * Math.pow(1 - t, n - k)
);
}
private static calculate_stance(phi_st: number, v: number, angle: number): number[] {
const c = Math.cos(angle * DEG2RAD);
const s = Math.sin(angle * DEG2RAD);
const A = 0.001;
const half_l = 0.05;
const p_stance = half_l * (1 - 2 * phi_st);
const stance_x = c * p_stance * Math.abs(v);
const stance_y = -s * p_stance * Math.abs(v);
const stance_z = -A * Math.cos((Math.PI / (2 * half_l)) * p_stance);
return [stance_x, stance_y, stance_z];
}
private calculate_bezier_swing(
phi_sw: number,
v: number,
angle: number,
direction: number
): number[] {
const c = Math.cos((angle * Math.PI) / 180);
const s = Math.sin((angle * Math.PI) / 180);
const X = [-0.04, -0.056, -0.06, -0.06, -0.06, 0, 0, 0, 0.06, 0.06, 0.056, 0.04].map(
(x) => Math.abs(v) * c * x * direction
);
const Y = X.map((x) => Math.abs(v) * s * -x);
const Z = [0, 0, 0.0405, 0.0405, 0.0405, 0.0405, 0.0405, 0.0495, 0.0495, 0.0495, 0, 0].map(
(x) => Math.abs(v) * x
);
let swing_x = 0,
swing_y = 0,
swing_z = 0;
for (let i = 0; i < 10; i++) {
swing_x += this.bezier_curve(phi_sw, i, X[i]);
swing_y += this.bezier_curve(phi_sw, i, Y[i]);
swing_z += this.bezier_curve(phi_sw, i, Z[i]);
}
return [swing_x, swing_y, swing_z];
}
step_trajectory(
phi: number,
v: number,
angle: number,
w_rot: number,
center_to_foot: number[],
direction: number
) {
if (phi >= 1) phi -= 1;
const r = Math.sqrt(center_to_foot[0] ** 2 + center_to_foot[1] ** 2);
const foot_angle = Math.atan2(center_to_foot[1], center_to_foot[0]);
let circle_trajectory;
if (w_rot >= 0) {
circle_trajectory = 90 - ((foot_angle - this._alpha) * 180) / Math.PI;
} else {
circle_trajectory = 270 - ((foot_angle - this._alpha) * 180) / Math.PI;
}
let stepX_long, stepY_long, stepZ_long, stepX_rot, stepY_rot, stepZ_rot;
if (phi <= this.step_offset) {
const phi_stance = phi / this.step_offset;
[stepX_long, stepY_long, stepZ_long] = PhaseGaitPlanner.calculate_stance(
phi_stance,
v,
angle
);
[stepX_rot, stepY_rot, stepZ_rot] = PhaseGaitPlanner.calculate_stance(
phi_stance,
w_rot,
circle_trajectory
);
} else {
const phiSwing = (phi - this.step_offset) / (1 - this.step_offset);
[stepX_long, stepY_long, stepZ_long] = this.calculate_bezier_swing(
phiSwing,
v,
angle,
direction
);
[stepX_rot, stepY_rot, stepZ_rot] = this.calculate_bezier_swing(
phiSwing,
w_rot,
circle_trajectory,
direction
);
}
if (center_to_foot[1] > 0) {
this._alpha =
stepX_rot < 0
? -Math.atan2(Math.sqrt(stepX_rot ** 2 + stepY_rot ** 2), r)
: Math.atan2(Math.sqrt(stepX_rot ** 2 + stepY_rot ** 2), r);
} else {
this._alpha =
stepX_rot < 0
? Math.atan2(Math.sqrt(stepX_rot ** 2 + stepY_rot ** 2), r)
: -Math.atan2(Math.sqrt(stepX_rot ** 2 + stepY_rot ** 2), r);
}
return [stepX_long + stepX_rot, stepY_long + stepY_rot, stepZ_long + stepZ_rot];
}
loop(v: number, angle: number, w_rot: number, t: number, direction: number, frames: number[][]) {
if (t <= 0.01) t = 0.01;
if (this._phi >= 0.99) this._last_time = Date.now() / 1000;
this._phi = (Date.now() / 1000 - this._last_time) / t;
for (let i = 0; i < 4; i++) {
const step_coord = this.step_trajectory(
this._phi + this._offset[i],
v,
angle,
w_rot,
frames[i],
direction
);
this._frame[i] = [
frames[i][0] + step_coord[0],
frames[i][1] + step_coord[1],
frames[i][2] + step_coord[2],
1
];
}
return this._frame;
}
private static factorial = (function () {
const cache = [1n, 1n];
let i = 2;
return function (n: number) {
if (cache[n] !== undefined) return cache[n];
for (; i <= n; i++) {
cache[i] = cache[i - 1] * BigInt(i);
}
return cache[n];
};
})();
}