🛸 Adds tranformcontroller for body
This commit is contained in:
Rune Harlyk
+118
-42
@@ -1,10 +1,25 @@
|
||||
export interface position_t {
|
||||
import { radToDeg } from 'three/src/math/MathUtils.js';
|
||||
|
||||
export interface body_state_t {
|
||||
omega: number;
|
||||
phi: number;
|
||||
psi: number;
|
||||
xm: number;
|
||||
ym: number;
|
||||
zm: number;
|
||||
feet: number[][];
|
||||
}
|
||||
|
||||
export interface position {
|
||||
x: number;
|
||||
y: number;
|
||||
z: number;
|
||||
}
|
||||
|
||||
export interface target_position {
|
||||
x: number;
|
||||
z: number;
|
||||
yaw: number;
|
||||
}
|
||||
|
||||
const { cos, sin, atan2, sqrt, acos } = Math;
|
||||
@@ -78,22 +93,28 @@ export default class Kinematic {
|
||||
];
|
||||
}
|
||||
|
||||
public calcIK(Lp: number[][], position: position_t): number[] {
|
||||
this.bodyIK(position);
|
||||
public calcIK(body_state: body_state_t): number[] {
|
||||
this.bodyIK(body_state);
|
||||
|
||||
return [
|
||||
...this.legIK(this.multiplyVector(this.inverse(this.Tlf), Lp[0])),
|
||||
...this.legIK(this.multiplyVector(this.inverse(this.Tlf), body_state.feet[0])),
|
||||
...this.legIK(
|
||||
this.multiplyVector(this.Ix, this.multiplyVector(this.inverse(this.Trf), Lp[1]))
|
||||
this.multiplyVector(
|
||||
this.Ix,
|
||||
this.multiplyVector(this.inverse(this.Trf), body_state.feet[1])
|
||||
)
|
||||
),
|
||||
...this.legIK(this.multiplyVector(this.inverse(this.Tlb), Lp[2])),
|
||||
...this.legIK(this.multiplyVector(this.inverse(this.Tlb), body_state.feet[2])),
|
||||
...this.legIK(
|
||||
this.multiplyVector(this.Ix, this.multiplyVector(this.inverse(this.Trb), Lp[3]))
|
||||
this.multiplyVector(
|
||||
this.Ix,
|
||||
this.multiplyVector(this.inverse(this.Trb), body_state.feet[3])
|
||||
)
|
||||
)
|
||||
];
|
||||
}
|
||||
|
||||
bodyIK(p: position_t) {
|
||||
bodyIK(p: body_state_t) {
|
||||
const cos_omega = cos(p.omega * this.DEGREES2RAD);
|
||||
const sin_omega = sin(p.omega * this.DEGREES2RAD);
|
||||
const cos_phi = cos(p.phi * this.DEGREES2RAD);
|
||||
@@ -296,48 +317,103 @@ export default class Kinematic {
|
||||
}
|
||||
}
|
||||
|
||||
export class ForwardKinematics {
|
||||
private l1: number;
|
||||
private l2: number;
|
||||
private l3: number;
|
||||
private l4: number;
|
||||
const swing_time = 0.36;
|
||||
const overlap_time = 0.0;
|
||||
const dt = 0.02;
|
||||
const swing_ticks = Math.round(swing_time / dt);
|
||||
|
||||
const num_phases = 4;
|
||||
const stance_ticks = 7 * swing_ticks;
|
||||
const overlap_ticks = Math.round(overlap_time / dt);
|
||||
|
||||
const phase_ticks = new Array(4).fill(swing_ticks);
|
||||
const phase_length = num_phases * swing_ticks;
|
||||
|
||||
let rb_contact_phases = [1, 0, 1, 1];
|
||||
let rf_contact_phases = [1, 1, 1, 0];
|
||||
let lf_contact_phases = [1, 0, 1, 1];
|
||||
let lb_contact_phases = [1, 1, 1, 0];
|
||||
|
||||
export class GaitPlanner {
|
||||
gaitCycleDuration = 10;
|
||||
time = 0;
|
||||
stepHeight = 30;
|
||||
stepLength = 75;
|
||||
num_phases = 4;
|
||||
gaitCycle = 10;
|
||||
phaseOffset = Math.PI;
|
||||
|
||||
ticks_ = 0;
|
||||
phase_index_ = 0;
|
||||
subphase_ticks_ = 0;
|
||||
contact_feet_states_ = [false, false, false, false];
|
||||
default_stance_feet_pos: number[][];
|
||||
|
||||
private phase: number;
|
||||
private strideLength: number;
|
||||
private height: number;
|
||||
private cyclePeriod: number;
|
||||
|
||||
constructor() {
|
||||
this.l1 = 50;
|
||||
this.l2 = 20;
|
||||
this.l3 = 120;
|
||||
this.l4 = 155;
|
||||
let l1 = 50;
|
||||
let l2 = 20;
|
||||
let l3 = 120;
|
||||
let l4 = 155;
|
||||
|
||||
let L = 140;
|
||||
let W = 75;
|
||||
|
||||
this.default_stance_feet_pos = [
|
||||
[100, -100, 100, 1],
|
||||
[100, -100, -100, 1],
|
||||
[-W, -100, 100, 1],
|
||||
[-W, -100, -100, 1]
|
||||
];
|
||||
|
||||
this.strideLength = 2;
|
||||
this.height = 50;
|
||||
this.cyclePeriod = 10;
|
||||
this.phase = 0;
|
||||
}
|
||||
|
||||
public calculateFootpoint(theta1: number, theta2: number, theta3: number): number[] {
|
||||
const { cos, sin } = Math;
|
||||
|
||||
const x =
|
||||
this.l1 * cos(theta1) +
|
||||
this.l2 * cos(theta1) +
|
||||
this.l3 * cos(theta1 + theta2) +
|
||||
this.l4 * cos(theta1 + theta2 + theta3);
|
||||
const y =
|
||||
this.l1 * sin(theta1) +
|
||||
this.l2 * sin(theta1) +
|
||||
this.l3 * sin(theta1 + theta2) +
|
||||
this.l4 * sin(theta1 + theta2 + theta3);
|
||||
const z = 0;
|
||||
|
||||
return [x, y, z];
|
||||
public step(bodyState: body_state_t, dt: number) {
|
||||
this.updatePhase(dt);
|
||||
this.updateFootPosition(bodyState);
|
||||
this.UpdateBodyShift(bodyState);
|
||||
}
|
||||
|
||||
public calculateFootpoints(angles: number[]): number[][] {
|
||||
const footpoints: number[][] = [];
|
||||
updatePhase(dt: number) {
|
||||
this.time += dt;
|
||||
|
||||
for (let i = 0; i < angles.length; i += 3) {
|
||||
const theta1 = angles[i];
|
||||
const theta2 = angles[i + 1];
|
||||
const theta3 = angles[i + 2];
|
||||
const footpoint = this.calculateFootpoint(theta1, theta2, theta3);
|
||||
footpoints.push(footpoint);
|
||||
this.ticks_++;
|
||||
let phase_time = this.ticks_ % phase_length;
|
||||
}
|
||||
|
||||
updateFootPosition(body_state: body_state_t) {
|
||||
for (let i = 0; i < 4; i++) {
|
||||
let contact_mode = this.contact_feet_states_[i];
|
||||
|
||||
body_state.feet[i] = contact_mode
|
||||
? this.stanceController(body_state.feet[i])
|
||||
: this.swingLegController(body_state.feet[i], this.default_stance_feet_pos[i]);
|
||||
}
|
||||
}
|
||||
|
||||
return footpoints;
|
||||
UpdateBodyShift(bodyState: body_state_t) {}
|
||||
|
||||
stanceController(foot_pos: number[]) {
|
||||
foot_pos[0] = -100;
|
||||
foot_pos[1] = sin(radToDeg(this.ticks_) / 2 + Math.PI / 2) * 100;
|
||||
return foot_pos;
|
||||
}
|
||||
|
||||
swingLegController(foot_pos: number[], default_stance_foot_pos: number[]) {
|
||||
let swing_proportion = this.subphase_ticks_ / swing_ticks;
|
||||
// foot_pos[0] = default_stance_foot_pos[0];
|
||||
foot_pos[1] = default_stance_foot_pos[1] + 100;
|
||||
// foot_pos[2] = default_stance_foot_pos[2];
|
||||
// foot_pos[0] = cos(this.time / 2) * 50;
|
||||
// foot_pos[1] = default_stance_foot_pos[1] - sin(this.time / 2 + Math.PI / 2) * 50;
|
||||
return foot_pos;
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user