diff --git a/app/package.json b/app/package.json index 6b2a55d..ce24e3c 100644 --- a/app/package.json +++ b/app/package.json @@ -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", diff --git a/app/pnpm-lock.yaml b/app/pnpm-lock.yaml index 30837eb..dbdef5b 100644 --- a/app/pnpm-lock.yaml +++ b/app/pnpm-lock.yaml @@ -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'} diff --git a/app/src/components/Model/ModelView.svelte b/app/src/components/Model/ModelView.svelte index e4ddf9b..7621309 100644 --- a/app/src/components/Model/ModelView.svelte +++ b/app/src/components/Model/ModelView.svelte @@ -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) } +
+

Poses

+
+ + + +
+
+

Motor angles

+ {#each servoNames as name, i} +
+ {name}: + + +
+ {/each} +
+ +
+

Body rotation

+ {#each Object.entries(modelBodyAngles) as [name, angle]} +
+ {name}: + + +
+ {/each} +
+ +
+

Body position

+ {#each Object.entries(modelBodyPoint) as [name, coordinate]} +
+ {name}: + + +
+ {/each} +
+
+ \ No newline at end of file diff --git a/app/src/lib/kinematic.ts b/app/src/lib/kinematic.ts new file mode 100644 index 0000000..a471236 --- /dev/null +++ b/app/src/lib/kinematic.ts @@ -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; + } + }