🪅 Adds 8 phase gait with body shift
This commit is contained in:
Rune Harlyk
@@ -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) => {
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -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];
|
||||
};
|
||||
})();
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user