🚸 Update default feet positions
This commit is contained in:
Rune Harlyk
+361
-366
@@ -1,452 +1,447 @@
|
||||
import type { body_state_t } from './kinematic';
|
||||
import { fromInt8 } from './utilities';
|
||||
import type { body_state_t } from './kinematic'
|
||||
import Kinematic from './kinematic'
|
||||
import { fromInt8 } from './utilities'
|
||||
|
||||
const { sin } = Math;
|
||||
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;
|
||||
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;
|
||||
s1: number;
|
||||
stop: number
|
||||
lx: number
|
||||
ly: number
|
||||
rx: number
|
||||
ry: number
|
||||
h: number
|
||||
s: number
|
||||
s1: number
|
||||
}
|
||||
|
||||
export abstract class GaitState {
|
||||
protected abstract name: string;
|
||||
protected abstract name: string
|
||||
|
||||
protected dt = 0.02;
|
||||
protected body_state!: body_state_t;
|
||||
protected gait_state: gait_state_t = {
|
||||
step_height: 0.4,
|
||||
step_x: 0,
|
||||
step_z: 0,
|
||||
step_angle: 0,
|
||||
step_velocity: 1,
|
||||
step_depth: 0.002
|
||||
};
|
||||
protected dt = 0.02
|
||||
protected body_state!: body_state_t
|
||||
protected gait_state: gait_state_t = {
|
||||
step_height: 0.4,
|
||||
step_x: 0,
|
||||
step_z: 0,
|
||||
step_angle: 0,
|
||||
step_velocity: 1,
|
||||
step_depth: 0.002
|
||||
}
|
||||
|
||||
public get default_feet_pos() {
|
||||
return [
|
||||
[1, -1, 1, 1],
|
||||
[1, -1, -1, 1],
|
||||
[-1, -1, 1, 1],
|
||||
[-1, -1, -1, 1]
|
||||
];
|
||||
public get default_feet_pos() {
|
||||
return new Kinematic().getDefaultFeetPos()
|
||||
}
|
||||
|
||||
protected 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) {
|
||||
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() {
|
||||
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;
|
||||
}
|
||||
this.gait_state = newCommand
|
||||
}
|
||||
}
|
||||
|
||||
export class IdleState extends GaitState {
|
||||
protected name = 'Idle';
|
||||
protected name = 'Idle'
|
||||
}
|
||||
|
||||
export class CalibrationState extends GaitState {
|
||||
protected name = 'Calibration';
|
||||
protected name = 'Calibration'
|
||||
|
||||
step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
|
||||
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.zm = 0;
|
||||
body_state.feet = this.default_feet_pos;
|
||||
return body_state;
|
||||
}
|
||||
step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
|
||||
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.zm = 0
|
||||
body_state.feet = this.default_feet_pos
|
||||
return body_state
|
||||
}
|
||||
}
|
||||
|
||||
export class RestState extends GaitState {
|
||||
protected name = 'Rest';
|
||||
protected name = 'Rest'
|
||||
|
||||
step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
|
||||
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.zm = 0;
|
||||
body_state.feet = this.default_feet_pos;
|
||||
return body_state;
|
||||
}
|
||||
step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
|
||||
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.zm = 0
|
||||
body_state.feet = this.default_feet_pos
|
||||
return body_state
|
||||
}
|
||||
}
|
||||
|
||||
export class StandState extends GaitState {
|
||||
protected name = 'Stand';
|
||||
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.feet = this.default_feet_pos;
|
||||
return body_state;
|
||||
}
|
||||
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.feet = this.default_feet_pos
|
||||
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_speed_factor: number;
|
||||
protected abstract swing_stand_ratio: number;
|
||||
protected tick = 0
|
||||
protected phase = 0
|
||||
protected phase_time = 0
|
||||
protected abstract num_phases: number
|
||||
protected abstract phase_speed_factor: number
|
||||
protected abstract swing_stand_ratio: number
|
||||
|
||||
protected contact_phases!: number[][];
|
||||
protected shifts!: number[][];
|
||||
protected contact_phases!: number[][]
|
||||
protected shifts!: number[][]
|
||||
|
||||
step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
|
||||
super.step(body_state, command, dt);
|
||||
this.update_phase();
|
||||
this.update_body_position();
|
||||
this.update_feet_positions();
|
||||
return this.body_state;
|
||||
step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
|
||||
super.step(body_state, command, dt)
|
||||
this.update_phase()
|
||||
this.update_body_position()
|
||||
this.update_feet_positions()
|
||||
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() {
|
||||
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;
|
||||
}
|
||||
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
|
||||
}
|
||||
|
||||
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];
|
||||
}
|
||||
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_speed_factor = 6;
|
||||
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);
|
||||
protected name = 'Four phase walk'
|
||||
protected num_phases = 4
|
||||
protected phase_speed_factor = 6
|
||||
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();
|
||||
}
|
||||
begin() {
|
||||
super.begin()
|
||||
}
|
||||
|
||||
end() {
|
||||
super.end();
|
||||
}
|
||||
end() {
|
||||
super.end()
|
||||
}
|
||||
|
||||
step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
|
||||
return super.step(body_state, command, dt);
|
||||
}
|
||||
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_speed_factor = 4;
|
||||
protected contact_phases = [
|
||||
[1, 0, 1, 1, 1, 1, 1, 1],
|
||||
[1, 1, 1, 1, 1, 0, 1, 1],
|
||||
[1, 1, 1, 1, 1, 1, 1, 0],
|
||||
[1, 1, 1, 0, 1, 1, 1, 1]
|
||||
];
|
||||
protected shifts = [
|
||||
[-0.05, 0, -0.2],
|
||||
[0.3, 0, 0.2],
|
||||
[-0.05, 0, 0.2],
|
||||
[0.3, 0, -0.2]
|
||||
];
|
||||
protected swing_stand_ratio = 1 / (this.num_phases - 1);
|
||||
protected name = 'Eight phase walk'
|
||||
protected num_phases = 8
|
||||
protected phase_speed_factor = 4
|
||||
protected contact_phases = [
|
||||
[1, 0, 1, 1, 1, 1, 1, 1],
|
||||
[1, 1, 1, 1, 1, 0, 1, 1],
|
||||
[1, 1, 1, 1, 1, 1, 1, 0],
|
||||
[1, 1, 1, 0, 1, 1, 1, 1]
|
||||
]
|
||||
protected shifts = [
|
||||
[-0.05, 0, -0.2],
|
||||
[0.3, 0, 0.2],
|
||||
[-0.05, 0, 0.2],
|
||||
[0.3, 0, -0.2]
|
||||
]
|
||||
protected swing_stand_ratio = 1 / (this.num_phases - 1)
|
||||
|
||||
begin() {
|
||||
super.begin();
|
||||
}
|
||||
begin() {
|
||||
super.begin()
|
||||
}
|
||||
|
||||
end() {
|
||||
super.end();
|
||||
}
|
||||
end() {
|
||||
super.end()
|
||||
}
|
||||
|
||||
step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
|
||||
return super.step(body_state, command, dt);
|
||||
}
|
||||
step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
|
||||
return super.step(body_state, command, dt)
|
||||
}
|
||||
}
|
||||
|
||||
export class BezierState extends GaitState {
|
||||
protected name = 'Bezier';
|
||||
protected phase = 0;
|
||||
protected phase_num = 0;
|
||||
protected step_length: number = 0;
|
||||
offset = [0, 0.5, 0.5, 0];
|
||||
protected name = 'Bezier'
|
||||
protected phase = 0
|
||||
protected phase_num = 0
|
||||
protected step_length: number = 0
|
||||
offset = [0, 0.5, 0.5, 0]
|
||||
|
||||
begin() {
|
||||
super.begin();
|
||||
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() {
|
||||
super.end();
|
||||
update_phase() {
|
||||
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) {
|
||||
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;
|
||||
update_feet_positions() {
|
||||
for (let i = 0; i < 4; i++) {
|
||||
this.body_state.feet[i] = this.update_foot_position(i)
|
||||
}
|
||||
}
|
||||
|
||||
update_phase() {
|
||||
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;
|
||||
}
|
||||
update_foot_position(index: number): number[] {
|
||||
let phase = this.phase + this.offset[index]
|
||||
if (phase >= 1) {
|
||||
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))
|
||||
}
|
||||
|
||||
update_feet_positions() {
|
||||
for (let i = 0; i < 4; i++) {
|
||||
this.body_state.feet[i] = this.update_foot_position(i);
|
||||
}
|
||||
}
|
||||
stand_controller(index: number, phase: number) {
|
||||
let depth = this.gait_state.step_depth
|
||||
return this.controller(index, phase, stance_curve, depth)
|
||||
}
|
||||
|
||||
update_foot_position(index: number): number[] {
|
||||
let phase = this.phase + this.offset[index];
|
||||
if (phase >= 1) {
|
||||
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));
|
||||
}
|
||||
swing_controller(index: number, phase: number) {
|
||||
let height = this.gait_state.step_height
|
||||
return this.controller(index, phase, bezier_curve, height)
|
||||
}
|
||||
|
||||
stand_controller(index: number, phase: number) {
|
||||
let depth = this.gait_state.step_depth;
|
||||
return this.controller(index, phase, stance_curve, depth);
|
||||
}
|
||||
controller(
|
||||
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)
|
||||
|
||||
swing_controller(index: number, phase: number) {
|
||||
let height = this.gait_state.step_height;
|
||||
return this.controller(index, phase, bezier_curve, height);
|
||||
}
|
||||
length = this.gait_state.step_angle * 2
|
||||
angle = yawArc(this.default_feet_pos[index], this.body_state.feet[index])
|
||||
|
||||
controller(
|
||||
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);
|
||||
const delta_rot = controller(length, angle, ...args, phase)
|
||||
|
||||
length = this.gait_state.step_angle * 2;
|
||||
angle = yawArc(this.default_feet_pos[index], 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
|
||||
|
||||
const delta_rot = controller(length, angle, ...args, phase);
|
||||
|
||||
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];
|
||||
}
|
||||
return this.body_state.feet[index]
|
||||
}
|
||||
}
|
||||
|
||||
const stance_curve = (length: number, angle: number, depth: number, phase: number): number[] => {
|
||||
const X_POLAR = Math.cos(angle);
|
||||
const Y_POLAR = Math.sin(angle);
|
||||
const X_POLAR = Math.cos(angle)
|
||||
const Y_POLAR = Math.sin(angle)
|
||||
|
||||
const step = length * (1 - 2 * phase);
|
||||
const X = step * X_POLAR;
|
||||
const Z = step * Y_POLAR;
|
||||
let Y = 0;
|
||||
const step = length * (1 - 2 * phase)
|
||||
const X = step * X_POLAR
|
||||
const Z = step * Y_POLAR
|
||||
let Y = 0
|
||||
|
||||
if (length !== 0) {
|
||||
Y = -depth * Math.cos((Math.PI * (X + Y)) / (2 * length));
|
||||
}
|
||||
return [X, Y, Z];
|
||||
};
|
||||
if (length !== 0) {
|
||||
Y = -depth * Math.cos((Math.PI * (X + Y)) / (2 * length))
|
||||
}
|
||||
return [X, Y, Z]
|
||||
}
|
||||
|
||||
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_dir = Math.atan2(default_foot_pos[2], default_foot_pos[0]);
|
||||
const offsets = [
|
||||
current_foot_pos[0] - default_foot_pos[0],
|
||||
current_foot_pos[2] - default_foot_pos[2],
|
||||
current_foot_pos[1] - default_foot_pos[1]
|
||||
];
|
||||
const offset_mag = Math.sqrt(offsets[0] ** 2 + offsets[2] ** 2);
|
||||
const offset_mod = Math.atan2(offset_mag, foot_mag);
|
||||
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 offsets = [
|
||||
current_foot_pos[0] - default_foot_pos[0],
|
||||
current_foot_pos[2] - default_foot_pos[2],
|
||||
current_foot_pos[1] - default_foot_pos[1]
|
||||
]
|
||||
const offset_mag = Math.sqrt(offsets[0] ** 2 + offsets[2] ** 2)
|
||||
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 control_points = get_control_points(length, angle, height);
|
||||
const n = control_points.length - 1;
|
||||
const control_points = get_control_points(length, angle, height)
|
||||
const n = control_points.length - 1
|
||||
|
||||
const point = [0, 0, 0];
|
||||
for (let i = 0; i <= 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[1] += bernstein_poly * control_points[i][1];
|
||||
point[2] += bernstein_poly * control_points[i][2];
|
||||
}
|
||||
return point;
|
||||
};
|
||||
const point = [0, 0, 0]
|
||||
for (let i = 0; i <= 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[1] += bernstein_poly * control_points[i][1]
|
||||
point[2] += bernstein_poly * control_points[i][2]
|
||||
}
|
||||
return point
|
||||
}
|
||||
const get_control_points = (length: number, angle: number, height: number): number[][] => {
|
||||
const X_POLAR = Math.cos(angle);
|
||||
const Z_POLAR = Math.sin(angle);
|
||||
const X_POLAR = Math.cos(angle)
|
||||
const Z_POLAR = Math.sin(angle)
|
||||
|
||||
const STEP = [
|
||||
-length,
|
||||
-length * 1.4,
|
||||
-length * 1.5,
|
||||
-length * 1.5,
|
||||
-length * 1.5,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
length * 1.5,
|
||||
length * 1.5,
|
||||
length * 1.4,
|
||||
length
|
||||
];
|
||||
const STEP = [
|
||||
-length,
|
||||
-length * 1.4,
|
||||
-length * 1.5,
|
||||
-length * 1.5,
|
||||
-length * 1.5,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
length * 1.5,
|
||||
length * 1.5,
|
||||
length * 1.4,
|
||||
length
|
||||
]
|
||||
|
||||
const Y = [
|
||||
0.0,
|
||||
0.0,
|
||||
height * 0.9,
|
||||
height * 0.9,
|
||||
height * 0.9,
|
||||
height * 0.9,
|
||||
height * 0.9,
|
||||
height * 1.1,
|
||||
height * 1.1,
|
||||
height * 1.1,
|
||||
0.0,
|
||||
0.0
|
||||
];
|
||||
const Y = [
|
||||
0.0,
|
||||
0.0,
|
||||
height * 0.9,
|
||||
height * 0.9,
|
||||
height * 0.9,
|
||||
height * 0.9,
|
||||
height * 0.9,
|
||||
height * 1.1,
|
||||
height * 1.1,
|
||||
height * 1.1,
|
||||
0.0,
|
||||
0.0
|
||||
]
|
||||
|
||||
const control_points: number[][] = [];
|
||||
const control_points: number[][] = []
|
||||
|
||||
for (let i = 0; i < STEP.length; i++) {
|
||||
const X = STEP[i] * X_POLAR;
|
||||
const Z = STEP[i] * Z_POLAR;
|
||||
control_points.push([X, Y[i], Z]);
|
||||
}
|
||||
for (let i = 0; i < STEP.length; i++) {
|
||||
const X = STEP[i] * X_POLAR
|
||||
const Z = STEP[i] * Z_POLAR
|
||||
control_points.push([X, Y[i], Z])
|
||||
}
|
||||
|
||||
return control_points;
|
||||
};
|
||||
return control_points
|
||||
}
|
||||
|
||||
const comb = (n: number, k: number): number => {
|
||||
if (k < 0 || k > n) return 0;
|
||||
if (k === 0 || k === n) return 1;
|
||||
k = Math.min(k, n - k);
|
||||
let c = 1;
|
||||
for (let i = 0; i < k; i++) {
|
||||
c = (c * (n - i)) / (i + 1);
|
||||
}
|
||||
return c;
|
||||
};
|
||||
if (k < 0 || k > n) return 0
|
||||
if (k === 0 || k === n) return 1
|
||||
k = Math.min(k, n - k)
|
||||
let c = 1
|
||||
for (let i = 0; i < k; i++) {
|
||||
c = (c * (n - i)) / (i + 1)
|
||||
}
|
||||
return c
|
||||
}
|
||||
|
||||
@@ -46,11 +46,11 @@ export default class Kinematic {
|
||||
constructor() {
|
||||
this.l1 = 60.5 / 100
|
||||
this.l2 = 10 / 100
|
||||
this.l3 = 100.7 / 100
|
||||
this.l3 = 111.7 / 100
|
||||
this.l4 = 118.5 / 100
|
||||
|
||||
this.L = 230 / 100
|
||||
this.W = 75 / 100
|
||||
this.L = 207.5 / 100
|
||||
this.W = 78 / 100
|
||||
|
||||
this.mountOffsets = [
|
||||
[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[] {
|
||||
const roll = p.omega * this.DEG2RAD
|
||||
const pitch = p.phi * this.DEG2RAD
|
||||
|
||||
Reference in New Issue
Block a user