🦿 Adds kinematic library
This commit is contained in:
Rune Harlyk
@@ -30,7 +30,6 @@
|
||||
"vite-plugin-singlefile": "^0.13.5"
|
||||
},
|
||||
"dependencies": {
|
||||
"gl-matrix": "^3.4.3",
|
||||
"mathjs": "^11.8.2",
|
||||
"nipplejs": "^0.10.1",
|
||||
"svelte-hero-icons": "^5.0.0",
|
||||
|
||||
Generated
-7
@@ -5,9 +5,6 @@ settings:
|
||||
excludeLinksFromLockfile: false
|
||||
|
||||
dependencies:
|
||||
gl-matrix:
|
||||
specifier: ^3.4.3
|
||||
version: 3.4.3
|
||||
mathjs:
|
||||
specifier: ^11.8.2
|
||||
version: 11.8.2
|
||||
@@ -1229,10 +1226,6 @@ packages:
|
||||
engines: {node: '>=10'}
|
||||
dev: true
|
||||
|
||||
/gl-matrix@3.4.3:
|
||||
resolution: {integrity: sha512-wcCp8vu8FT22BnvKVPjXa/ICBWRq/zjFfdofZy1WSpQZpphblv12/bOQLBC1rMM7SGOFS9ltVmKOHil5+Ml7gA==}
|
||||
dev: false
|
||||
|
||||
/glob-parent@5.1.2:
|
||||
resolution: {integrity: sha512-AOIgSQCepiJYwP3ARnGx+5VnTu2HBYdzbGP45eLw1vr3zB3vZLeyed1sC9hnbcOc9/SrMyM5RPQrkGz4aS9Zow==}
|
||||
engines: {node: '>= 6'}
|
||||
|
||||
@@ -12,7 +12,9 @@ import {
|
||||
sRGBEncoding,
|
||||
AmbientLight,
|
||||
MathUtils,
|
||||
LoaderUtils
|
||||
LoaderUtils,
|
||||
GridHelper,
|
||||
Camera
|
||||
} from 'three';
|
||||
import { XacroLoader } from 'xacro-parser';
|
||||
import { OrbitControls } from 'three/examples/jsm/controls/OrbitControls.js';
|
||||
@@ -21,629 +23,93 @@ import { servoBuffer } from '../../lib/socket'
|
||||
import { lerp } from '../../lib/utils';
|
||||
import uzip from 'uzip';
|
||||
import { outControllerData } from '../../lib/store';
|
||||
|
||||
import {Matrix} from 'mathjs'
|
||||
import { mat3, mat4, vec3, vec4 } from 'gl-matrix'
|
||||
import { debug } from 'svelte/internal';
|
||||
import type { Link } from 'svelte-routing';
|
||||
import Kinematic from '../../lib/kinematic';
|
||||
|
||||
let el: HTMLCanvasElement;
|
||||
let scene, camera, renderer, robot, controls;
|
||||
let angles = new Int8Array(12)
|
||||
let scene: Scene, camera: Camera, renderer: WebGLRenderer, controls: OrbitControls, robot;
|
||||
|
||||
let modelAngles:number[] | Int8Array = new Array(12).fill(0)
|
||||
let modelTargetAngles:number[] | Int8Array = new Array(12).fill(0)
|
||||
|
||||
let modelBodyAngles:EulerAngle = {omega: 0, phi: 0, psi: 0 }
|
||||
let modelTargeBodyAngles:EulerAngle = {omega: 0, phi: 0, psi: 0 }
|
||||
|
||||
let modelBodyPoint:Point = {x: 0, y: 0, z: 0 }
|
||||
let modelTargetBodyPoint:Point = {x: 0, y: 0, z: 0 }
|
||||
|
||||
const servoNames = [
|
||||
"front_left_shoulder", "front_left_leg", "front_left_foot",
|
||||
"front_right_shoulder", "front_right_leg", "front_right_foot",
|
||||
"rear_left_shoulder", "rear_left_leg", "rear_left_foot",
|
||||
"rear_right_shoulder", "rear_right_leg", "rear_right_foot"]
|
||||
"rear_right_shoulder", "rear_right_leg", "rear_right_foot"
|
||||
]
|
||||
|
||||
interface EulerAngle {
|
||||
omega: number;
|
||||
phi: number;
|
||||
psi: number;
|
||||
}
|
||||
|
||||
interface Point {
|
||||
x: number;
|
||||
y: number;
|
||||
z: number;
|
||||
}
|
||||
|
||||
interface BodyState {
|
||||
euler: EulerAngle;
|
||||
position: Point;
|
||||
legPositions:[number, number, number, number];
|
||||
}
|
||||
|
||||
const radToDeg = (val:number) => val * (180 / Math.PI)
|
||||
const degToRad = (val:number) => val * (Math.PI / 180)
|
||||
|
||||
const idle = () => {
|
||||
const angles = new Array(12).fill(0)
|
||||
servoBuffer.set(angles)
|
||||
}
|
||||
|
||||
const rest = () => {
|
||||
const angles = [0, 90, -180, 0, 90, -180, 0, 90, -180, 0, 90, -180, ]
|
||||
servoBuffer.set(angles)
|
||||
}
|
||||
|
||||
const stand = () => {
|
||||
const angles = [0, 45, -90, 0, 45, -90, 0, 45, -90, 0, 45, -90]
|
||||
servoBuffer.set(angles)
|
||||
}
|
||||
|
||||
const calculateKinematics = () => {
|
||||
const kinematic = new Kinematic();
|
||||
const angles: number[] = [degToRad(modelTargeBodyAngles.omega), degToRad(modelTargeBodyAngles.phi), degToRad(modelTargeBodyAngles.psi)];
|
||||
const center: number[] = [modelBodyPoint.x, modelBodyPoint.y, modelBodyPoint.z];
|
||||
const Lp = [[100,-100,100,1],[100,-100,-100,1],[-100,-100,100,1],[-100,-100,-100,1]]
|
||||
|
||||
const legs = kinematic.calcIK(Lp, angles, center)
|
||||
|
||||
const dir = [ -1, -1, -1, 1, -1, -1, -1, -1, -1, 1, -1, -1]
|
||||
|
||||
const legsAngles = legs
|
||||
.map(x => x.map(y => radToDeg(y)))
|
||||
.flat()
|
||||
.map((x, i) => x * dir[i])
|
||||
servoBuffer.set(legsAngles)
|
||||
}
|
||||
|
||||
onMount(async () => {
|
||||
await cacheModelFiles()
|
||||
createScene()
|
||||
// await eig.ready;
|
||||
window.solve = solve
|
||||
|
||||
sm = new SpotMicroKinematics(0, 0, 0, smnc_.smc)
|
||||
const idleBodyState:BodyState = {
|
||||
euler_angs: { phi:0, psi:0, theta:0},
|
||||
xyz_pos: { x:0, y: smnc_.lie_down_height, z:0 },
|
||||
leg_feet_pos: getNeutralStance()
|
||||
}
|
||||
sm.setBodyState(idleBodyState)
|
||||
const d2r = Math.PI / 180
|
||||
|
||||
outControllerData.subscribe(data => {
|
||||
|
||||
sm.setBodyAngles(0,data[1]/1000,0)
|
||||
//sm.setBodyState(idleBodyState);
|
||||
const joint_angs:LegsJointAngles = sm.getLegsJointAngles();
|
||||
const angles = [0,0,0,0,0,0,0,0,0,0,0,0]
|
||||
angles[0] = joint_angs.left_front.ang1
|
||||
angles[1] = joint_angs.left_front.ang2
|
||||
angles[2] = joint_angs.left_front.ang3
|
||||
|
||||
angles[3] = joint_angs.right_front.ang1
|
||||
angles[4] = joint_angs.right_front.ang2
|
||||
angles[5] = joint_angs.right_front.ang3
|
||||
|
||||
angles[6] = joint_angs.left_back.ang1
|
||||
angles[7] = joint_angs.left_back.ang2
|
||||
angles[8] = joint_angs.left_back.ang3
|
||||
|
||||
angles[9] = joint_angs.right_back.ang1
|
||||
angles[10] = joint_angs.right_back.ang2
|
||||
angles[11] = joint_angs.right_back.ang3
|
||||
servoBuffer.set(angles)
|
||||
modelTargeBodyAngles = {omega:(data[1]-128) / 3, phi:(data[2]-128) / 3, psi:(data[3]-128) / 4}
|
||||
calculateKinematics()
|
||||
//modelTargetBodyPoint = data.bodyPoint
|
||||
})
|
||||
|
||||
let lastAngles = {}
|
||||
|
||||
servoBuffer.subscribe(angles => {
|
||||
if(!robot || JSON.stringify(lastAngles) == JSON.stringify(angles)) return
|
||||
lastAngles = angles
|
||||
console.log("Got new agles", angles);
|
||||
|
||||
for (let i = 0; i < servoNames.length; i++) {
|
||||
angles[i] = angles[i]//lerp(robot.joints[servoNames[i]].angle * (180/Math.PI), angles[i], 0.2)
|
||||
robot.joints[servoNames[i]].setJointValue(MathUtils.degToRad(angles[i]));
|
||||
}
|
||||
})
|
||||
servoBuffer.subscribe(angles => modelTargetAngles = angles)
|
||||
});
|
||||
|
||||
function homogTransXyz(x:number, y:number, z:number) {
|
||||
const matrix = mat4.create();
|
||||
|
||||
mat4.translate(matrix, matrix, [x, y, z]);
|
||||
|
||||
return matrix;
|
||||
}
|
||||
|
||||
function homogRotXyz(x_ang: number, y_ang: number, z_ang: number) {
|
||||
const matrix = mat4.create();
|
||||
|
||||
mat4.rotateX(matrix, matrix, x_ang);
|
||||
mat4.rotateY(matrix, matrix, y_ang);
|
||||
mat4.rotateZ(matrix, matrix, z_ang);
|
||||
|
||||
return matrix;
|
||||
}
|
||||
|
||||
function homogInverse(ht) {
|
||||
const temp_rot = mat3.create();
|
||||
mat3.fromMat4(temp_rot, ht);
|
||||
mat3.transpose(temp_rot, temp_rot);
|
||||
|
||||
const temp_translate = vec3.create();
|
||||
vec3.set(temp_translate, -ht[12], -ht[13], -ht[14]);
|
||||
|
||||
const ht_inverted1 = mat4.create();
|
||||
mat4.fromRotationTranslation(ht_inverted1, temp_rot, vec3.create());
|
||||
|
||||
const ht_inverted2 = mat4.create();
|
||||
mat4.fromTranslation(ht_inverted2, temp_translate);
|
||||
|
||||
const result = mat4.create();
|
||||
mat4.multiply(result, ht_inverted1, ht_inverted2);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
const htLegLeftFront = (body_length:number, body_width:number):mat4 => {
|
||||
const htLegLeftFront = homogRotXyz(0, -Math.PI/2, 0)
|
||||
htLegLeftBack[13] = body_length/2
|
||||
htLegLeftBack[14] = 0
|
||||
htLegLeftBack[15] = -body_width/2.0
|
||||
return htLegLeftFront
|
||||
}
|
||||
|
||||
const htLegRightFront = (body_length:number, body_width:number):mat4 => {
|
||||
const htLegRightFront = homogRotXyz(0, -Math.PI/2, 0)
|
||||
htLegLeftBack[13] = body_length/2
|
||||
htLegLeftBack[14] = 0
|
||||
htLegLeftBack[15] = body_width/2.0
|
||||
return htLegRightFront
|
||||
}
|
||||
|
||||
const htLegLeftBack = (body_length:number, body_width:number):mat4 => {
|
||||
const htLegLeftBack = homogRotXyz(0, -Math.PI/2, 0)
|
||||
htLegLeftBack[13] = -body_length/2
|
||||
htLegLeftBack[14] = 0
|
||||
htLegLeftBack[15] = body_width/2.0
|
||||
return htLegLeftBack
|
||||
}
|
||||
|
||||
const htLegRightBack = (body_length:number, body_width:number):mat4 => {
|
||||
const htLegRightBack = homogRotXyz(0, -Math.PI/2, 0)
|
||||
htLegLeftBack[13] = -body_length/2
|
||||
htLegLeftBack[14] = 0
|
||||
htLegLeftBack[15] = -body_width/2.0
|
||||
return htLegRightBack
|
||||
}
|
||||
|
||||
function ht0To1(rot_ang, link_length) {
|
||||
// Build up the matrix as from the paper
|
||||
const ht_0_to_1 = mat4.create();
|
||||
mat4.fromXRotation(ht_0_to_1, rot_ang);
|
||||
|
||||
// Add in remaining terms
|
||||
ht_0_to_1[12] = -link_length * Math.cos(rot_ang);
|
||||
ht_0_to_1[13] = -link_length * Math.sin(rot_ang);
|
||||
|
||||
return ht_0_to_1;
|
||||
}
|
||||
|
||||
function ht1To2():mat4 {
|
||||
// Build up the matrix as from the paper
|
||||
const ht_1_to_2 = mat4.fromValues(
|
||||
0.0, 0.0, -1.0, 0.0,
|
||||
-1.0, 0.0, 0.0, 0.0,
|
||||
0.0, 1.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 1.0
|
||||
);
|
||||
|
||||
return ht_1_to_2;
|
||||
}
|
||||
|
||||
function ht2To3(rot_ang: number, link_length: number):mat4 {
|
||||
// Build up the matrix as from the paper
|
||||
const ht_2_to_3 = mat4.create();
|
||||
mat4.fromXRotation(ht_2_to_3, rot_ang);
|
||||
|
||||
// Add in remaining terms
|
||||
ht_2_to_3[12] = link_length * Math.cos(rot_ang);
|
||||
ht_2_to_3[13] = link_length * Math.sin(rot_ang);
|
||||
|
||||
return ht_2_to_3;
|
||||
}
|
||||
|
||||
function ht3To4(rot_ang: number, link_length: number):mat4 {
|
||||
// Same as the 2 to 3 transformation, so just call that function
|
||||
return ht2To3(rot_ang, link_length);
|
||||
}
|
||||
|
||||
function ht0To4(joint_angles:JointAngles, link_lengths:LinkLengths):mat4 {
|
||||
// Result is a sequential multiplication of all 4 transform matrices
|
||||
const mat0To1 = ht0To1(joint_angles.ang1, link_lengths.l1);
|
||||
const mat2To3 = ht2To3(joint_angles.ang2, link_lengths.l2);
|
||||
const mat3To4 = ht3To4(joint_angles.ang3, link_lengths.l3);
|
||||
|
||||
const result = mat4.create();
|
||||
mat4.multiply(result, mat0To1, ht1To2());
|
||||
mat4.multiply(result, result, mat2To3);
|
||||
mat4.multiply(result, result, mat3To4);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
const ikine = (point:Point, link_lengths:LinkLengths, is_leg_12:boolean):JointAngles => {
|
||||
let joint_angles:JointAngles = {ang1: 0, ang2: 0, ang3:0}
|
||||
|
||||
// Convenience variables for math
|
||||
const x4 = point.x;
|
||||
const y4 = point.y;
|
||||
const z4 = point.z;
|
||||
const l1 = link_lengths.l1;
|
||||
const l2 = link_lengths.l2;
|
||||
const l3 = link_lengths.l3;
|
||||
|
||||
// Supporting variable D
|
||||
let D = (x4*x4 + y4*y4 + z4*z4 - l1*l1 - l2*l2 - l3*l3) / (2*l2*l3);
|
||||
|
||||
// Poor man's inverse kinematics reachability protection:
|
||||
// Limit D to a maximum value of 1, otherwise the square root functions
|
||||
// below (sqrt(1 - D^2)) will attempt a square root of a negative number
|
||||
if (D > 1.0) D = 1.0;
|
||||
else if (D < -1.0) D = -1.0
|
||||
|
||||
joint_angles.ang3 = is_leg_12 ? Math.atan2(Math.sqrt(1 - D*D), D) : Math.atan2(-Math.sqrt(1 - D*D), D)
|
||||
|
||||
// if (is_leg_12) joint_angles.ang3 = Math.atan2(Math.sqrt(1 - D*D), D);
|
||||
// else joint_angles.ang3 = Math.atan2(-Math.sqrt(1 - D*D), D);
|
||||
|
||||
// Another poor mans reachability sqrt protection
|
||||
let protected_sqrt_val = x4*x4 + y4*y4 - l1*l1;
|
||||
if (protected_sqrt_val < 0) { protected_sqrt_val = 0;}
|
||||
|
||||
joint_angles.ang2 = Math.atan2(z4, Math.sqrt(protected_sqrt_val)) -
|
||||
Math.atan2(l3*Math.sin(joint_angles.ang3), l2 + l3*Math.cos(joint_angles.ang3));
|
||||
|
||||
joint_angles.ang1 = Math.atan2(y4, x4) + Math.atan2(Math.sqrt(protected_sqrt_val), -l1);
|
||||
|
||||
return joint_angles;
|
||||
}
|
||||
|
||||
// wake (0, 0, 0, -40, -170, 0);
|
||||
// sleep (0, 0, 0, 0, 0, 0)
|
||||
const smnc_ = {
|
||||
smc:{
|
||||
hip_link_length: 0.055, // Straight line distance of the hip link (horizontal leg link)
|
||||
upper_leg_link_length: 0.1075, // Straight line distance of the upper leg link, joint to joint
|
||||
lower_leg_link_length: 0.130, // Straight line distance of the lower leg link, joint to joint
|
||||
body_width: 0.078, // Horizontal width between hip joints
|
||||
body_length: 0.186, // Length between shoulder joints
|
||||
},
|
||||
|
||||
lie_down_height: 0.083, // Height of body center when sitting
|
||||
lie_down_feet_x_offset: 0.065
|
||||
}
|
||||
|
||||
const getNeutralStance = (): LegsFootPos => {
|
||||
const len = smnc_.smc.body_length; // body length
|
||||
const width = smnc_.smc.body_width; // body width
|
||||
const l1 = smnc_.smc.hip_link_length; // length of the hip link
|
||||
const x_off = smnc_.lie_down_feet_x_offset;
|
||||
|
||||
return {
|
||||
left_front: {x:-len/2 + x_off, y:0, z: width/2 - l1},
|
||||
right_front:{x: len/2 + x_off, y:0, z: width/2 + l1},
|
||||
left_back: {x: len/2 + x_off, y:0, z: -width/2 - l1},
|
||||
right_back: {x:-len/2 + x_off, y:0, z: -width/2 + l1}
|
||||
}
|
||||
}
|
||||
|
||||
const getLieDownStance = (): LegsFootPos => {
|
||||
const len = smnc_.smc.body_length; // body length
|
||||
const width = smnc_.smc.body_width; // body width
|
||||
const l1 = smnc_.smc.hip_link_length; // length of the hip link
|
||||
const x_off = smnc_.lie_down_feet_x_offset;
|
||||
|
||||
return {
|
||||
left_front: {x: len/2 + x_off, y:0, z: -width/2 - l1},
|
||||
right_front:{x: len/2 + x_off, y:0, z: width/2 + l1},
|
||||
left_back: {x:-len/2 + x_off, y:0, z: -width/2 - l1},
|
||||
right_back: {x:-len/2 + x_off, y:0, z: width/2 + l1}
|
||||
}
|
||||
}
|
||||
let sm:SpotMicroKinematics
|
||||
|
||||
|
||||
const solve = (data) => {
|
||||
//const kin = new kinematic();
|
||||
|
||||
const phi = 0//data[1]
|
||||
const theta = 0//data[1]//(data[1]- 127) / 30
|
||||
const psi = 0//data[1]//(data[2]- 127) / 30
|
||||
const z = 0//(data[3]- 127) / 30
|
||||
const y = 0//data[1]//(data[4]- 127) / 30
|
||||
const x = 0//y / z
|
||||
|
||||
// const desired:JointAngles = {ang1: 10*d2r, ang2:20*d2r, ang3:-15*d2r}
|
||||
|
||||
// const four_legs_desired_angs:LegsJointAngles = {left_front: desired, right_front: desired, left_back:desired, right_back:desired}
|
||||
|
||||
// sm.setLegJointAngles(four_legs_desired_angs);
|
||||
|
||||
// const test_legs_joint_angs:LegsJointAngles = sm.getLegsJointAngles();
|
||||
|
||||
const d2r = Math.PI/180;
|
||||
|
||||
sm.setBodyState
|
||||
|
||||
// Set an initial body height and stance cmd for idle mode
|
||||
|
||||
|
||||
//kin.calculate_foot_point()
|
||||
//kin.calculate_leg_positions()
|
||||
}
|
||||
|
||||
|
||||
|
||||
interface Point {
|
||||
x:number
|
||||
y:number
|
||||
z:number
|
||||
};
|
||||
|
||||
interface LinkLengths {
|
||||
l1:number
|
||||
l2:number
|
||||
l3:number
|
||||
};
|
||||
|
||||
interface JointAngles {
|
||||
ang1:number
|
||||
ang2:number
|
||||
ang3:number
|
||||
};
|
||||
|
||||
interface LegsJointAngles {
|
||||
right_back:JointAngles
|
||||
right_front:JointAngles
|
||||
left_front:JointAngles
|
||||
left_back:JointAngles
|
||||
};
|
||||
|
||||
interface LegsFootPos {
|
||||
right_back:Point
|
||||
right_front:Point
|
||||
left_front:Point
|
||||
left_back:Point
|
||||
};
|
||||
|
||||
// Struct to hold various configuration values of a spot micro robot frame
|
||||
interface SpotMicroConfig {
|
||||
hip_link_length:number
|
||||
upper_leg_link_length:number
|
||||
lower_leg_link_length:number
|
||||
body_width:number
|
||||
body_length:number
|
||||
};
|
||||
|
||||
interface EulerAngs {
|
||||
phi:number
|
||||
theta:number
|
||||
psi:number
|
||||
}
|
||||
|
||||
interface BodyState {
|
||||
euler_angs:EulerAngs
|
||||
xyz_pos:Point
|
||||
leg_feet_pos:LegsFootPos
|
||||
};
|
||||
|
||||
interface LegRelativeTransforms {
|
||||
t01:Matrix
|
||||
t13:Matrix
|
||||
t34:Matrix
|
||||
};
|
||||
|
||||
interface AllRobotRelativeTransforms {
|
||||
bodyCenter:Matrix
|
||||
centerToRightBack:Matrix
|
||||
centerToRightFront:Matrix
|
||||
centerToLeftFront:Matrix
|
||||
centerToLeftBack:Matrix
|
||||
rightBackLeg:LegRelativeTransforms
|
||||
rightFrontLeg:LegRelativeTransforms
|
||||
leftFrontLeg:LegRelativeTransforms
|
||||
leftBackLeg:LegRelativeTransforms
|
||||
};
|
||||
|
||||
class SpotMicroLeg {
|
||||
private is_leg_12_:boolean
|
||||
private joint_angles_:JointAngles
|
||||
private link_lengths_:LinkLengths
|
||||
|
||||
constructor(joint_angles:JointAngles, link_lengths:LinkLengths, is_leg_12:boolean) {
|
||||
this.joint_angles_ = joint_angles
|
||||
this.link_lengths_ = link_lengths
|
||||
this.is_leg_12_ = is_leg_12
|
||||
}
|
||||
|
||||
setAngles(joint_angles:JointAngles) {
|
||||
this.joint_angles_ = joint_angles
|
||||
}
|
||||
|
||||
get getAngles() {
|
||||
return this.joint_angles_
|
||||
}
|
||||
|
||||
setFootPosGlobalCoordinates(point:Point, ht_leg_start:mat4) {
|
||||
// Need to express the point in the leg's coordinate system, can do so by
|
||||
// transforming a vector of the points in global coordinate by the inverse of
|
||||
// the leg's starting homogeneous transform
|
||||
|
||||
// Make a homogeneous vector, and store the point in global coords in it
|
||||
const p4_ht_vec = vec4.create()
|
||||
p4_ht_vec[0] = point.x
|
||||
p4_ht_vec[1] = point.y
|
||||
p4_ht_vec[2] = point.z
|
||||
p4_ht_vec[3] = 1
|
||||
|
||||
// Multiply it by the inverse of the homgeneous transform of the leg start.
|
||||
// This operation yields a foot position in the foot's local coordinates
|
||||
//p4_ht_vec = vec4.multiply() homogInverse(ht_leg_start) * p4_ht_vec;
|
||||
|
||||
//Point point_local{.x = p4_ht_vec(0), .y = p4_ht_vec(1), .z = p4_ht_vec(2)};
|
||||
|
||||
vec4.transformMat4(p4_ht_vec, p4_ht_vec, homogInverse(ht_leg_start))
|
||||
|
||||
const point_local:Point = {x:p4_ht_vec[0], y:p4_ht_vec[1], z:p4_ht_vec[2]}
|
||||
|
||||
// Call this leg's method for setting foot position in local cordinates
|
||||
this.setFootPosLocalCoordinates(point_local);
|
||||
}
|
||||
|
||||
setFootPosLocalCoordinates(point:Point) {
|
||||
const joint_angles:JointAngles = ikine(point, this.link_lengths_, this.is_leg_12_);
|
||||
|
||||
// Call method to set joint angles of the leg
|
||||
this.setAngles(joint_angles);
|
||||
}
|
||||
|
||||
getFootPosGlobalCoordinates(ht_leg_start:mat4): Point {
|
||||
// Get homogeneous transform of foot
|
||||
const ht_foot = mat4.multiply(mat4.create(), ht_leg_start, ht0To4(this.joint_angles_, this.link_lengths_))
|
||||
|
||||
// Construct return point structure
|
||||
return {
|
||||
x: ht_foot[12],
|
||||
y: ht_foot[13],
|
||||
z: ht_foot[14],
|
||||
};
|
||||
}
|
||||
|
||||
get getLegJointAngles(): JointAngles {
|
||||
return this.joint_angles_;
|
||||
}
|
||||
}
|
||||
|
||||
class SpotMicroKinematics {
|
||||
private smc_:SpotMicroConfig
|
||||
private x_:number
|
||||
private y_:number
|
||||
private z_:number
|
||||
|
||||
private phi_:number
|
||||
private theta_:number
|
||||
private psi_:number
|
||||
|
||||
private right_back_leg_:SpotMicroLeg
|
||||
private right_front_leg_:SpotMicroLeg
|
||||
private left_front_leg_:SpotMicroLeg
|
||||
private left_back_leg_:SpotMicroLeg
|
||||
|
||||
constructor(x:number, y:number, z:number, smc:SpotMicroConfig) {
|
||||
this.smc_ = smc
|
||||
this.x_ = x
|
||||
this.y_ = y
|
||||
this.z_ = z
|
||||
|
||||
const joint_angles_temp:JointAngles = {ang1: 0, ang2: 0, ang3:0}
|
||||
const link_lengths_temp:LinkLengths = { l1: smc.hip_link_length, l2:smc.upper_leg_link_length, l3:smc.lower_leg_link_length}
|
||||
|
||||
this.right_back_leg_ = new SpotMicroLeg(joint_angles_temp, link_lengths_temp, true);
|
||||
this.right_front_leg_ = new SpotMicroLeg(joint_angles_temp, link_lengths_temp, true);
|
||||
this.left_front_leg_ = new SpotMicroLeg(joint_angles_temp, link_lengths_temp, false);
|
||||
this.left_back_leg_ = new SpotMicroLeg(joint_angles_temp, link_lengths_temp, false);
|
||||
}
|
||||
|
||||
getBodyHt(): mat4 { // : Matrix4f
|
||||
// Euler angle order is phi, psi, theta because the axes of the robot are x
|
||||
// pointing forward, y pointing up, z pointing right
|
||||
return mat4.multiply(mat4.create(), homogTransXyz(this.x_, this.y_, this.z_), homogRotXyz(this.phi_, this.psi_, this.theta_));
|
||||
}
|
||||
|
||||
setFeetPosGlobalCoordinates(four_legs_foot_pos:LegsFootPos) {
|
||||
// Get the body center homogeneous transform matrix
|
||||
const ht_body = this.getBodyHt();
|
||||
|
||||
// Create each leg's starting ht matrix. Made in order of right back, right
|
||||
//front, left front, left back
|
||||
const ht_rb = mat4.multiply(mat4.create(), ht_body, htLegRightBack(this.smc_.body_length, this.smc_.body_width))
|
||||
const ht_rf = mat4.multiply(mat4.create(), ht_body, htLegRightFront(this.smc_.body_length, this.smc_.body_width))
|
||||
const ht_lf = mat4.multiply(mat4.create(), ht_body, htLegLeftFront(this.smc_.body_length, this.smc_.body_width))
|
||||
const ht_lb = mat4.multiply(mat4.create(), ht_body, htLegLeftBack(this.smc_.body_length, this.smc_.body_width))
|
||||
|
||||
|
||||
// Call each leg's method to set foot position in global coordinates
|
||||
this.right_back_leg_.setFootPosGlobalCoordinates(four_legs_foot_pos.right_back, ht_rb);
|
||||
this.right_front_leg_.setFootPosGlobalCoordinates(four_legs_foot_pos.right_front, ht_rf);
|
||||
this.left_front_leg_.setFootPosGlobalCoordinates(four_legs_foot_pos.left_front, ht_lf);
|
||||
this.left_back_leg_.setFootPosGlobalCoordinates(four_legs_foot_pos.left_back, ht_lb);
|
||||
}
|
||||
|
||||
setBodyState(body_state:BodyState) {
|
||||
this.x_ = body_state.xyz_pos.x;
|
||||
this.y_ = body_state.xyz_pos.y;
|
||||
this.z_ = body_state.xyz_pos.z;
|
||||
this.phi_ = body_state.euler_angs.phi;
|
||||
this.theta_ = body_state.euler_angs.theta;
|
||||
this.psi_ = body_state.euler_angs.psi;
|
||||
|
||||
this.setFeetPosGlobalCoordinates(body_state.leg_feet_pos);
|
||||
}
|
||||
|
||||
setBodyAngles(phi:number, theta:number, psi:number) {
|
||||
const saved_foot_pos:LegsFootPos = this.getLegsFootPos();
|
||||
|
||||
this.theta_ = theta
|
||||
this.phi_ = phi
|
||||
this.psi_ = psi
|
||||
|
||||
this.setFeetPosGlobalCoordinates(saved_foot_pos);
|
||||
}
|
||||
|
||||
getLegsFootPos(): LegsFootPos {
|
||||
|
||||
// Get the body center homogeneous transform matrix
|
||||
const ht_body = this.getBodyHt()
|
||||
|
||||
// Return the leg joint angles
|
||||
let ret_val:LegsFootPos = {left_back:{x:0,y:0,z:0}, left_front:{x:0,y:0,z:0}, right_back:{x:0,y:0,z:0}, right_front:{x:0,y:0,z:0}}
|
||||
|
||||
// Create each leg's starting ht matrix. Made in order of right back, right
|
||||
// front, left front, left back
|
||||
const ht_rb = mat4.multiply(mat4.create(), ht_body, htLegRightBack(this.smc_.body_length, this.smc_.body_width))
|
||||
const ht_rf = mat4.multiply(mat4.create(), ht_body, htLegRightFront(this.smc_.body_length, this.smc_.body_width))
|
||||
const ht_lf = mat4.multiply(mat4.create(), ht_body, htLegLeftFront(this.smc_.body_length, this.smc_.body_width))
|
||||
const ht_lb = mat4.multiply(mat4.create(), ht_body, htLegLeftBack(this.smc_.body_length, this.smc_.body_width))
|
||||
|
||||
ret_val.right_back = this.right_back_leg_.getFootPosGlobalCoordinates(ht_rb);
|
||||
ret_val.right_front = this.right_front_leg_.getFootPosGlobalCoordinates(ht_rf);
|
||||
ret_val.left_front = this.left_front_leg_.getFootPosGlobalCoordinates(ht_lf);
|
||||
ret_val.left_back = this.left_back_leg_.getFootPosGlobalCoordinates(ht_lb);
|
||||
return ret_val;
|
||||
}
|
||||
|
||||
getLegsJointAngles(): LegsJointAngles {
|
||||
// Return the leg joint angles
|
||||
let ret_val:LegsJointAngles = {left_back:{ang1:0,ang2:0,ang3:0}, left_front:{ang1:0,ang2:0,ang3:0}, right_back:{ang1:0,ang2:0,ang3:0}, right_front:{ang1:0,ang2:0,ang3:0}}
|
||||
|
||||
ret_val.right_back = this.right_back_leg_.getLegJointAngles;
|
||||
ret_val.right_front = this.right_front_leg_.getLegJointAngles;
|
||||
ret_val.left_front = this.left_front_leg_.getLegJointAngles;
|
||||
ret_val.left_back = this.left_back_leg_.getLegJointAngles;
|
||||
|
||||
return ret_val;
|
||||
}
|
||||
}
|
||||
|
||||
class kinematic {
|
||||
private omega = 0;
|
||||
private phi = 0;
|
||||
private psi = 0;
|
||||
private x = 0;
|
||||
private y = 0;
|
||||
private z = 0;
|
||||
|
||||
private L = 207.5;
|
||||
private W = 78;
|
||||
private l1 = 60.5;
|
||||
private l2 = 10;
|
||||
private l3 = 100.7;
|
||||
private l4 = 118.5;
|
||||
|
||||
private height = 200
|
||||
private p = [[],[],[],[]]
|
||||
private servo_angles = []
|
||||
|
||||
calculate_foot_point = () => {
|
||||
|
||||
const tan_omega = Math.tan(this.omega);
|
||||
const tan_psi = Math.tan(this.psi);
|
||||
// Front has impact of omega
|
||||
|
||||
const h_offset = (this.W/2.0 + this.l1) * tan_omega;
|
||||
|
||||
//Front Left Leg
|
||||
this.p[0][1]= - (this.height - h_offset);
|
||||
this.p[0][0] = -this.l1 + this.p[0][1]* tan_omega ;
|
||||
this.p[0][2] = this.p[0][1]* tan_psi;
|
||||
|
||||
// Front Right leg
|
||||
this.p[1][1]= - (this.height + h_offset);
|
||||
this.p[1][0] = this.l1 - this.p[1][1]* tan_omega ;
|
||||
this.p[1][2] = this.p[1][1]* tan_psi;
|
||||
|
||||
// Rear correct based on psi
|
||||
const height_rear = this.height + this.L * tan_psi;
|
||||
//Front Left Leg
|
||||
this.p[2][1]= - (height_rear - h_offset);
|
||||
this.p[2][0] = -this.l1 + this.p[2][1]* tan_omega;
|
||||
this.p[2][2] = this.p[2][1]* tan_psi;
|
||||
|
||||
// Front Right leg
|
||||
this.p[3][1]= - (height_rear + h_offset);
|
||||
this.p[3][0] = this.l1 - this.p[3][1]* tan_omega ;
|
||||
this.p[3][2] = this.p[3][1] * Math.tan(this.psi);
|
||||
|
||||
console.log(this.p)
|
||||
}
|
||||
|
||||
calculate_leg_positions = () => {
|
||||
for (let l = 0; l < 4; l++) {
|
||||
this.leg_IK(this.p[l], l, this.servo_angles[l]);
|
||||
//console.log("IK (x,z,y) (%.1f, %.1f, %.1f) -> (%d, %d, %d) (%d)", p[l][0], p[l][2], p[l][1], servo_angles[l][0], servo_angles[l][1], servo_angles[l][2], ret);
|
||||
}
|
||||
}
|
||||
|
||||
leg_IK = (p:number[], id:number, servo_angles:number[]) => {
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
const cacheModelFiles = async () => {
|
||||
const cacheKey = "files"
|
||||
const cache = await caches.open(cacheKey)
|
||||
@@ -666,7 +132,7 @@ const loadModel = () => {
|
||||
urdfLoader.workingPath = LoaderUtils.extractUrlBase( url );
|
||||
|
||||
robot = urdfLoader.parse( xml );
|
||||
robot.rotation.x = Math.PI / 0.6666;
|
||||
robot.rotation.x = -Math.PI / 2;
|
||||
robot.rotation.z = Math.PI / 2;
|
||||
robot.traverse(c => c.castShadow = true);
|
||||
robot.updateMatrixWorld(true);
|
||||
@@ -702,6 +168,13 @@ const createScene = () => {
|
||||
ground.receiveShadow = true;
|
||||
scene.add(ground);
|
||||
|
||||
const size = 10;
|
||||
const divisions = 50;
|
||||
|
||||
const gridHelper = new GridHelper(size, divisions);
|
||||
gridHelper.position.y = -0.24
|
||||
scene.add(gridHelper);
|
||||
|
||||
controls = new OrbitControls(camera, renderer.domElement);
|
||||
controls.minDistance = 0;
|
||||
controls.maxDistance = 4;
|
||||
@@ -712,7 +185,6 @@ const createScene = () => {
|
||||
render()
|
||||
}
|
||||
|
||||
|
||||
const handleResize = () => {
|
||||
renderer.setSize(window.innerWidth, window.innerHeight);
|
||||
renderer.setPixelRatio(window.devicePixelRatio);
|
||||
@@ -724,9 +196,65 @@ const handleResize = () => {
|
||||
const render = () => {
|
||||
requestAnimationFrame(render);
|
||||
renderer.render(scene, camera);
|
||||
|
||||
if(!robot) return
|
||||
|
||||
for (let i = 0; i < servoNames.length; i++) {
|
||||
modelAngles[i] = lerp(robot.joints[servoNames[i]].angle * (180/Math.PI), modelTargetAngles[i], 0.1)
|
||||
robot.joints[servoNames[i]].setJointValue(MathUtils.degToRad(modelAngles[i]));
|
||||
}
|
||||
|
||||
modelBodyAngles.omega = lerp(robot.rotation.x * (180/Math.PI), modelTargeBodyAngles.omega - 90, 0.1)
|
||||
modelBodyAngles.phi = lerp(robot.rotation.y * (180/Math.PI), modelTargeBodyAngles.phi, 0.1)
|
||||
modelBodyAngles.psi = lerp(robot.rotation.z * (180/Math.PI), modelTargeBodyAngles.psi + 90, 0.1)
|
||||
|
||||
// robot.rotation.x = MathUtils.degToRad(modelBodyAngles.omega)
|
||||
// robot.rotation.y = MathUtils.degToRad(modelBodyAngles.phi)
|
||||
// robot.rotation.z = MathUtils.degToRad(modelBodyAngles.psi)
|
||||
}
|
||||
</script>
|
||||
|
||||
<svelte:window on:resize={handleResize}></svelte:window>
|
||||
|
||||
<div class="absolute top-0 z-10 left-0 m-10">
|
||||
<h1 class="text-on-background text-xl mb-2">Poses</h1>
|
||||
<div class="flex gap-4">
|
||||
<button class="outline outline-primary p-2 rounded-md" on:click={idle}>Idle</button>
|
||||
<button class="outline outline-primary p-2 rounded-md" on:click={rest}>Rest</button>
|
||||
<button class="outline outline-primary p-2 rounded-md" on:click={stand}>Stand</button>
|
||||
</div>
|
||||
<div class="w-full">
|
||||
<h1 class="text-on-background text-xl mt-4">Motor angles</h1>
|
||||
{#each servoNames as name, i}
|
||||
<div class="flex justify-between mb-2">
|
||||
<span class="w-40">{name}: </span>
|
||||
<input type="range" min="-180" max="180" step="0.1" class="accent-primary" bind:value={$servoBuffer[i]}>
|
||||
<input class="w-24 bg-background" min="-180" max="180" step="0.1" bind:value={$servoBuffer[i]}>
|
||||
</div>
|
||||
{/each}
|
||||
</div>
|
||||
|
||||
<div>
|
||||
<h1 class="text-on-background text-xl mb-2">Body rotation</h1>
|
||||
{#each Object.entries(modelBodyAngles) as [name, angle]}
|
||||
<div class="flex justify-between mb-2">
|
||||
<span class="w-40">{name}: </span>
|
||||
<input type="range" min="-180" max="180" step="0.1" class="accent-primary" bind:value={modelTargeBodyAngles[name]} on:input={calculateKinematics}>
|
||||
<input class="w-24 bg-background" min="-180" max="180" step="0.1" bind:value={modelTargeBodyAngles[name]} on:input={calculateKinematics}>
|
||||
</div>
|
||||
{/each}
|
||||
</div>
|
||||
|
||||
<div>
|
||||
<h1 class="text-on-background text-xl mb-2">Body position</h1>
|
||||
{#each Object.entries(modelBodyPoint) as [name, coordinate]}
|
||||
<div class="flex justify-between mb-2">
|
||||
<span class="w-40">{name}: </span>
|
||||
<input type="range" min="-180" max="180" step="0.1" class="accent-primary" bind:value={modelTargetBodyPoint[name]} on:input={calculateKinematics}>
|
||||
<input class="w-24 bg-background" min="-180" max="180" step="0.1" bind:value={modelTargetBodyPoint[name]} on:input={calculateKinematics}>
|
||||
</div>
|
||||
{/each}
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<canvas bind:this={el} class="absolute"></canvas>
|
||||
@@ -0,0 +1,342 @@
|
||||
export default class Kinematic {
|
||||
private l1: number;
|
||||
private l2: number;
|
||||
private l3: number;
|
||||
private l4: number;
|
||||
|
||||
private L: number;
|
||||
private W: number;
|
||||
|
||||
constructor() {
|
||||
this.l1 = 50;
|
||||
this.l2 = 20;
|
||||
this.l3 = 120;
|
||||
this.l4 = 155;
|
||||
|
||||
this.L = 140;
|
||||
this.W = 75;
|
||||
}
|
||||
|
||||
bodyIK(omega: number, phi: number, psi: number, xm: number, ym: number, zm: number): number[][][] {
|
||||
const { cos, sin } = Math;
|
||||
|
||||
const Rx: number[][] = [
|
||||
[1, 0, 0, 0],
|
||||
[0, cos(omega), -sin(omega), 0],
|
||||
[0, sin(omega), cos(omega), 0],
|
||||
[0, 0, 0, 1],
|
||||
];
|
||||
const Ry: number[][] = [
|
||||
[cos(phi), 0, sin(phi), 0],
|
||||
[0, 1, 0, 0],
|
||||
[-sin(phi), 0, cos(phi), 0],
|
||||
[0, 0, 0, 1],
|
||||
];
|
||||
const Rz: number[][] = [
|
||||
[cos(psi), -sin(psi), 0, 0],
|
||||
[sin(psi), cos(psi), 0, 0],
|
||||
[0, 0, 1, 0],
|
||||
[0, 0, 0, 1],
|
||||
];
|
||||
const Rxyz: number[][] = this.matrixMultiply(this.matrixMultiply(Rx, Ry), Rz);
|
||||
|
||||
const T: number[][] = [
|
||||
[0, 0, 0, xm],
|
||||
[0, 0, 0, ym],
|
||||
[0, 0, 0, zm],
|
||||
[0, 0, 0, 0],
|
||||
];
|
||||
const Tm: number[][] = this.matrixAdd(T, Rxyz);
|
||||
|
||||
const sHp = sin(Math.PI / 2);
|
||||
const cHp = cos(Math.PI / 2);
|
||||
const L = this.L;
|
||||
const W = this.W;
|
||||
|
||||
return [
|
||||
this.matrixMultiply(Tm, [
|
||||
[cHp, 0, sHp, L / 2],
|
||||
[0, 1, 0, 0],
|
||||
[-sHp, 0, cHp, W / 2],
|
||||
[0, 0, 0, 1],
|
||||
]),
|
||||
this.matrixMultiply(Tm, [
|
||||
[cHp, 0, sHp, L / 2],
|
||||
[0, 1, 0, 0],
|
||||
[-sHp, 0, cHp, -W / 2],
|
||||
[0, 0, 0, 1],
|
||||
]),
|
||||
this.matrixMultiply(Tm, [
|
||||
[cHp, 0, sHp, -L / 2],
|
||||
[0, 1, 0, 0],
|
||||
[-sHp, 0, cHp, W / 2],
|
||||
[0, 0, 0, 1],
|
||||
]),
|
||||
this.matrixMultiply(Tm, [
|
||||
[cHp, 0, sHp, -L / 2],
|
||||
[0, 1, 0, 0],
|
||||
[-sHp, 0, cHp, -W / 2],
|
||||
[0, 0, 0, 1],
|
||||
]),
|
||||
];
|
||||
}
|
||||
|
||||
private legIK(point: number[]): number[] {
|
||||
const [x, y, z] = point;
|
||||
const { atan2, cos, sin, sqrt, acos } = Math;
|
||||
const { l1, l2, l3, l4 } = this;
|
||||
|
||||
let F;
|
||||
|
||||
try {
|
||||
F = sqrt(x ** 2 + y ** 2 - l1 ** 2);
|
||||
if(isNaN(F)) throw new Error("F is NaN")
|
||||
} catch (error) {
|
||||
//console.log(error)
|
||||
F = l1
|
||||
}
|
||||
const G = F - l2;
|
||||
const H = sqrt(G ** 2 + z ** 2);
|
||||
|
||||
const theta1 = -atan2(y, x) - atan2(F, -l1);
|
||||
const D = (H ** 2 - l3 ** 2 - l4 ** 2) / (2 * l3 * l4);
|
||||
let theta3: number
|
||||
try {
|
||||
theta3 = acos(D);
|
||||
if(isNaN(theta3)) throw new Error("theta3 is NaN")
|
||||
} catch (error) {
|
||||
theta3 = 0
|
||||
}
|
||||
const theta2 = atan2(z, G) - atan2(l4 * sin(theta3), l3 + l4 * cos(theta3));
|
||||
|
||||
return [theta1, theta2, theta3];
|
||||
}
|
||||
|
||||
matrixMultiply(a: number[][], b: number[][]): number[][] {
|
||||
const result: number[][] = [];
|
||||
|
||||
for (let i = 0; i < a.length; i++) {
|
||||
const row: number[] = [];
|
||||
|
||||
for (let j = 0; j < b[0].length; j++) {
|
||||
let sum = 0;
|
||||
|
||||
for (let k = 0; k < a[i].length; k++) {
|
||||
sum += a[i][k] * b[k][j];
|
||||
}
|
||||
|
||||
row.push(sum);
|
||||
}
|
||||
|
||||
result.push(row);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
multiplyVector(matrix: number[][], vector: number[]): number[] {
|
||||
const rows = matrix.length;
|
||||
const cols = matrix[0].length;
|
||||
const vectorLength = vector.length;
|
||||
|
||||
if (cols !== vectorLength) {
|
||||
throw new Error("Matrix and vector dimensions do not match for multiplication.");
|
||||
}
|
||||
|
||||
const result = [];
|
||||
|
||||
for (let i = 0; i < rows; i++) {
|
||||
let sum = 0;
|
||||
|
||||
for (let j = 0; j < cols; j++) {
|
||||
sum += matrix[i][j] * vector[j];
|
||||
}
|
||||
|
||||
result.push(sum);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
private matrixAdd(a: number[][], b: number[][]): number[][] {
|
||||
const result: number[][] = [];
|
||||
|
||||
for (let i = 0; i < a.length; i++) {
|
||||
const row: number[] = [];
|
||||
|
||||
for (let j = 0; j < a[i].length; j++) {
|
||||
row.push(a[i][j] + b[i][j]);
|
||||
}
|
||||
|
||||
result.push(row);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
public calcLegPoints(angles: number[]): number[][] {
|
||||
const [theta1, theta2, theta3] = angles;
|
||||
const theta23 = theta2 + theta3;
|
||||
|
||||
const T0: number[] = [0, 0, 0, 1];
|
||||
const T1: number[] = this.vectorAdd(
|
||||
T0,
|
||||
[-this.l1 * Math.cos(theta1), this.l1 * Math.sin(theta1), 0, 0]
|
||||
);
|
||||
const T2: number[] = this.vectorAdd(
|
||||
T1,
|
||||
[-this.l2 * Math.sin(theta1), -this.l2 * Math.cos(theta1), 0, 0]
|
||||
);
|
||||
const T3: number[] = this.vectorAdd(
|
||||
T2,
|
||||
[
|
||||
-this.l3 * Math.sin(theta1) * Math.cos(theta2),
|
||||
-this.l3 * Math.cos(theta1) * Math.cos(theta2),
|
||||
this.l3 * Math.sin(theta2),
|
||||
0,
|
||||
]
|
||||
);
|
||||
const T4: number[] = this.vectorAdd(
|
||||
T3,
|
||||
[
|
||||
-this.l4 * Math.sin(theta1) * Math.cos(theta23),
|
||||
-this.l4 * Math.cos(theta1) * Math.cos(theta23),
|
||||
this.l4 * Math.sin(theta23),
|
||||
0,
|
||||
]
|
||||
);
|
||||
|
||||
return [T0, T1, T2, T3, T4];
|
||||
}
|
||||
|
||||
public calcIK(Lp: number[][], angles: number[], center: number[]): number[][] {
|
||||
const [omega, phi, psi] = angles;
|
||||
const [xm, ym, zm] = center;
|
||||
|
||||
const [Tlf, Trf, Tlb, Trb] = this.bodyIK(omega, phi, psi, xm, ym, zm);
|
||||
|
||||
const Ix: number[][] = [
|
||||
[-1, 0, 0, 0],
|
||||
[0, 1, 0, 0],
|
||||
[0, 0, 1, 0],
|
||||
[0, 0, 0, 1],
|
||||
];
|
||||
|
||||
return [
|
||||
this.legIK(this.multiplyVector(this.matrixInverse(Tlf), Lp[0])),
|
||||
this.legIK(this.multiplyVector(Ix, this.multiplyVector(this.matrixInverse(Trf), Lp[1]))),
|
||||
this.legIK(this.multiplyVector(this.matrixInverse(Tlb), Lp[2])),
|
||||
this.legIK(this.multiplyVector(Ix, this.multiplyVector(this.matrixInverse(Trb), Lp[3]))),
|
||||
];
|
||||
}
|
||||
|
||||
private vectorAdd(a: number[], b: number[]): number[] {
|
||||
return a.map((val, index) => val + b[index]);
|
||||
}
|
||||
|
||||
private matrixInverse(matrix: number[][]): number[][] {
|
||||
const det = this.determinant(matrix);
|
||||
const adjugate = this.adjugate(matrix);
|
||||
const scalar = 1 / det;
|
||||
const inverse: number[][] = [];
|
||||
|
||||
for (let i = 0; i < matrix.length; i++) {
|
||||
const row: number[] = [];
|
||||
|
||||
for (let j = 0; j < matrix[i].length; j++) {
|
||||
row.push(adjugate[i][j] * scalar);
|
||||
}
|
||||
|
||||
inverse.push(row);
|
||||
}
|
||||
|
||||
return inverse;
|
||||
}
|
||||
|
||||
private determinant(matrix: number[][]): number {
|
||||
if (matrix.length !== matrix[0].length) {
|
||||
throw new Error("The matrix is not square.");
|
||||
}
|
||||
|
||||
if (matrix.length === 2) {
|
||||
return matrix[0][0] * matrix[1][1] - matrix[0][1] * matrix[1][0];
|
||||
}
|
||||
|
||||
let det = 0;
|
||||
|
||||
for (let i = 0; i < matrix.length; i++) {
|
||||
const sign = i % 2 === 0 ? 1 : -1;
|
||||
const subMatrix: number[][] = [];
|
||||
|
||||
for (let j = 1; j < matrix.length; j++) {
|
||||
const row: number[] = [];
|
||||
|
||||
for (let k = 0; k < matrix.length; k++) {
|
||||
if (k !== i) {
|
||||
row.push(matrix[j][k]);
|
||||
}
|
||||
}
|
||||
|
||||
subMatrix.push(row);
|
||||
}
|
||||
|
||||
det += sign * matrix[0][i] * this.determinant(subMatrix);
|
||||
}
|
||||
|
||||
return det;
|
||||
}
|
||||
|
||||
private adjugate(matrix: number[][]): number[][] {
|
||||
if (matrix.length !== matrix[0].length) {
|
||||
throw new Error("The matrix is not square.");
|
||||
}
|
||||
|
||||
const adjugate: number[][] = [];
|
||||
|
||||
for (let i = 0; i < matrix.length; i++) {
|
||||
const row: number[] = [];
|
||||
|
||||
for (let j = 0; j < matrix[i].length; j++) {
|
||||
const sign = (i + j) % 2 === 0 ? 1 : -1;
|
||||
const subMatrix: number[][] = [];
|
||||
|
||||
for (let k = 0; k < matrix.length; k++) {
|
||||
if (k !== i) {
|
||||
const subRow: number[] = [];
|
||||
|
||||
for (let l = 0; l < matrix.length; l++) {
|
||||
if (l !== j) {
|
||||
subRow.push(matrix[k][l]);
|
||||
}
|
||||
}
|
||||
|
||||
subMatrix.push(subRow);
|
||||
}
|
||||
}
|
||||
|
||||
const cofactor = sign * this.determinant(subMatrix);
|
||||
row.push(cofactor);
|
||||
}
|
||||
|
||||
adjugate.push(row);
|
||||
}
|
||||
|
||||
return this.transpose(adjugate);
|
||||
}
|
||||
|
||||
private transpose(matrix: number[][]): number[][] {
|
||||
const transposed: number[][] = [];
|
||||
|
||||
for (let i = 0; i < matrix.length; i++) {
|
||||
const row: number[] = [];
|
||||
|
||||
for (let j = 0; j < matrix[i].length; j++) {
|
||||
row.push(matrix[j][i]);
|
||||
}
|
||||
|
||||
transposed.push(row);
|
||||
}
|
||||
|
||||
return transposed;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user