Darkages/SpotMicroESP32-Leika
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge pull request #3 from runeharlyk/More-robot-control

Browse Source 
More robot control
This commit is contained in:
Rune Harlyk
2024-02-02 21:29:22 +01:00
committed by GitHub
parent 4c8c74aaa3 b985ee4727
commit 30a7aeebdf
5 changed files with 894 additions and 645 deletions
Download Patch File Download Diff File Expand all files Collapse all files
app/package.json
+20 -20
View File
@@ -11,31 +11,31 @@
"format": "prettier --plugin-search-dir . --write ."
},
"devDependencies": {
"@sveltejs/vite-plugin-svelte": "^2.0.3",
"@tsconfig/svelte": "^4.0.1",
"@types/three": "^0.154.0",
"@typescript-eslint/eslint-plugin": "^5.45.0",
"@typescript-eslint/parser": "^5.45.0",
"autoprefixer": "^10.4.14",
"husky": "^8.0.3",
"lint-staged": "^13.2.2",
"postcss": "^8.4.23",
"prettier": "2.8.8",
"svelte": "^3.57.0",
"svelte-check": "^2.10.3",
"@sveltejs/vite-plugin-svelte": "^3.0.2",
"@tsconfig/svelte": "^5.0.2",
"@types/three": "^0.160.0",
"@typescript-eslint/eslint-plugin": "^6.20.0",
"@typescript-eslint/parser": "^6.20.0",
"autoprefixer": "^10.4.17",
"husky": "^9.0.7",
"lint-staged": "^15.2.0",
"postcss": "^8.4.33",
"prettier": "3.2.4",
"svelte": "^4.2.9",
"svelte-check": "^3.6.3",
"svelte-hero-icons": "^5.0.0",
"tailwindcss": "^3.3.2",
"tslib": "^2.5.0",
"typescript": "^5.0.2",
"vite": "^4.3.2",
"tailwindcss": "^3.4.1",
"tslib": "^2.6.2",
"typescript": "^5.3.3",
"vite": "^5.0.12",
"vite-plugin-compression": "^0.5.1",
"vite-plugin-singlefile": "^0.13.5"
"vite-plugin-singlefile": "^1.0.0"
},
"dependencies": {
"nipplejs": "^0.10.1",
"svelte-routing": "^1.11.0",
"three": "^0.154.0",
"urdf-loader": "^0.12.0",
"svelte-routing": "^2.11.0",
"three": "^0.160.1",
"urdf-loader": "^0.12.1",
"uzip": "^0.20201231.0",
"xacro-parser": "^0.3.9"
},
app/pnpm-lock.yaml
Generated
+757 -617
View File
File diff suppressed because it is too large Load Diff
app/src/components/Model/ModelView.svelte
+9 -4
View File
@@ -5,7 +5,7 @@ import { dataBuffer, servoBuffer } from '../../lib/socket'
import { lerp } from '../../lib/utils';
import uzip from 'uzip';
import { outControllerData } from '../../lib/store';
import Kinematic from '../../lib/kinematic';
import Kinematic, { ForwardKinematics } from '../../lib/kinematic';
import location from '../../lib/location';
import FileCache from '../../lib/cache';
import SceneBuilder from './sceneBuilder';
@@ -122,11 +122,12 @@ const createScene = () => {
.addOrbitControls(10, 30)
.addGroundPlane({x:0, y:-2, z:0})
.addGridHelper({size:250, divisions:125, y:-2})
.addAmbientLight({color:0xffffff, intensity:0.3})
.addDirectionalLight({x:50, y:100, z:100, color:0xffffff, intensity:0.9})
.addAmbientLight({color:0xffffff, intensity:0.7})
.addDirectionalLight({x:10, y:100, z:10, color:0xffffff, intensity:1})
.addArrowHelper({origin:{x:0, y:0, z:0}, direction:{x:0, y:-2, z:0}})
.addFogExp2(0xcccccc, 0.015)
.loadModel('/spot_micro.urdf.xacro')
.addDragControl((name:string, angle:number) => modelTargetAngles[servoNames.indexOf(name)] = angle * (180/Math.PI))
.handleResize()
.addRenderCb(render)
.startRenderLoop()
@@ -153,7 +154,11 @@ const render = () => {
const robot = sceneManager.model
if(!robot) return
sceneManager.model.rotation.z = lerp(robot.rotation.z, degToRad($dataBuffer[1] + 90), 0.1)
const forwardKinematics = new ForwardKinematics()
const points = forwardKinematics.calculateFootpoints(modelTargetAngles.map(ang => degToRad(ang)) as number[])
robot.position.y = Math.max(...points.map(coord => coord[0] / 100)) - 2.7
robot.rotation.z = lerp(robot.rotation.z, degToRad($dataBuffer[1] + 90), 0.1)
handleVideoStream()
app/src/components/Model/sceneBuilder.ts
+69 -4
View File
@@ -16,9 +16,11 @@ import { Mesh,
CanvasTexture,
type ColorRepresentation,
type WebGLRendererParameters,
MeshPhongMaterial,
} from "three";
import { OrbitControls } from "three/examples/jsm/controls/OrbitControls";
import URDFLoader from "urdf-loader";
import URDFLoader, { type URDFMimicJoint } from "urdf-loader";
import { PointerURDFDragControls } from 'urdf-loader/src/URDFDragControls'
import { XacroLoader } from "xacro-parser";
export const addScene = () => new Scene()
@@ -62,6 +64,7 @@ export default class SceneBuilder {
public liveStreamTexture: CanvasTexture
private fog:FogExp2
private isLoaded:boolean = false
highlightMaterial: any;
constructor() {
this.scene = new Scene()
@@ -156,13 +159,45 @@ export default class SceneBuilder {
}
public addArrowHelper = (options?:arrowOptions) => {
const dir = new Vector3(options.direction.x ?? 0, options.direction.y ?? 0, options.direction.z ?? 0);
const origin = new Vector3(options.origin.x ?? 0, options.origin.y ?? 0, options.origin.z ?? 0);
const arrowHelper = new ArrowHelper( dir, origin, options.length ?? 1.5, options.color ?? 0xff0000 );
const dir = new Vector3(options?.direction.x ?? 0, options?.direction.y ?? 0, options?.direction.z ?? 0);
const origin = new Vector3(options?.origin.x ?? 0, options?.origin.y ?? 0, options?.origin.z ?? 0);
const arrowHelper = new ArrowHelper( dir, origin, options?.length ?? 1.5, options?.color ?? 0xff0000 );
this.scene.add( arrowHelper );
return this
}
private setJointValue(jointName:string, angle:number) {
if (!this.model) return;
if (!this.model.joints[jointName]) return;
this.model.joints[jointName].setJointValue(angle)
}
isJoint = j => j.isURDFJoint && j.jointType !== 'fixed';
highlightLinkGeometry = (m, revert:boolean, material) => {
const traverse = c => {
if (c.type === 'Mesh') {
if (revert) {
c.material = c.__origMaterial;
delete c.__origMaterial;
} else {
c.__origMaterial = c.material;
c.material = material;
}
}
if (c === m || !this.isJoint(c)) {
for (let i = 0; i < c.children.length; i++) {
const child = c.children[i];
if (!child.isURDFCollider) {
traverse(c.children[i]);
}
}
}
};
traverse(m);
};
public loadModel = (urlXacro:string) => {
const xacroLoader = new XacroLoader();
xacroLoader.load(urlXacro, xml => {
@@ -182,6 +217,36 @@ export default class SceneBuilder {
return this
}
public addDragControl = (updateAngle) => {
const highlightColor = '#FFFFFF'
const highlightMaterial =
new MeshPhongMaterial({
shininess: 10,
color: highlightColor,
emissive: highlightColor,
emissiveIntensity: 0.25,
});
const dragControls = new PointerURDFDragControls(this.scene, this.camera, this.renderer.domElement);
dragControls.updateJoint = (joint:URDFMimicJoint, angle:number) => {
this.setJointValue(joint.name, angle);
updateAngle(joint.name, angle)
};
dragControls.onDragStart = () => {
this.controls.enabled = false;
};
dragControls.onDragEnd = () => {
this.controls.enabled = true;
};
dragControls.onHover = (joint:URDFMimicJoint) => {
this.highlightLinkGeometry(joint, false, highlightMaterial);
}
dragControls.onUnhover = (joint:URDFMimicJoint) => {
this.highlightLinkGeometry(joint, true, highlightMaterial);
}
return this
}
public toggleFog = () => {
this.scene.fog = this.scene.fog ? null : this.fog;
}
app/src/lib/kinematic.ts
+39
View File
@@ -340,3 +340,42 @@ export default class Kinematic {
return transposed;
}
}
export class ForwardKinematics {
private l1: number;
private l2: number;
private l3: number;
private l4: number;
constructor() {
this.l1 = 50;
this.l2 = 20;
this.l3 = 120;
this.l4 = 155;
}
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 calculateFootpoints(angles: number[]): number[][] {
const footpoints: number[][] = [];
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);
}
return footpoints;
}
}