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

🐾 Adds foot tracing and gui panel

Browse Source
This commit is contained in:
Rune Harlyk
2024-02-25 02:04:50 +01:00
committed by Rune Harlyk
parent 4c2fe9a044
commit f41d5a7949
7 changed files with 201 additions and 143 deletions
Download Patch File Download Diff File Expand all files Collapse all files
app/public/spot_micro.urdf.xacro
+118 -118
View File
@@ -1,119 +1,119 @@
<?xml version="1.0"?> <?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="spot_micro_rviz"> <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="spot_micro_rviz">
<material name="white"> <material name="shell_color">
<color rgba="1 1 1 1"/> <color rgba="1 1 1 1" />
</material> </material>
<material name="black"> <material name="body_color">
<color rgba="0.1 0.1 0.1 1"/> <color rgba="0.1 0.1 0.1 1" />
</material> </material>
<material name="blue"> <material name="foot_color">
<color rgba="0 0.75 1 1"/> <color rgba="0 0.75 1 1" />
</material> </material>
<!-- Params --> <!-- Params -->
<xacro:property name="body_length" value="0.140" /> <xacro:property name="body_length" value="0.140" />
<xacro:property name="body_width" value="0.110" /> <xacro:property name="body_width" value="0.110" />
<xacro:property name="body_height" value="0.070" /> <xacro:property name="body_height" value="0.070" />
<xacro:property name="front_length" value="0.058" /> <xacro:property name="front_length" value="0.058" />
<xacro:property name="rear_length" value="0.040" /> <xacro:property name="rear_length" value="0.040" />
<xacro:property name="shoulder_length" value="0.044" /> <xacro:property name="shoulder_length" value="0.044" />
<xacro:property name="shoulder_width" value="0.038" /> <xacro:property name="shoulder_width" value="0.038" />
<xacro:property name="leg_length" value="0.1075" /> <xacro:property name="leg_length" value="0.1075" />
<xacro:property name="foot_length" value="0.130" /> <xacro:property name="foot_length" value="0.130" />
<xacro:property name="toe_radius" value="0.020" /> <xacro:property name="toe_radius" value="0.020" />
<!-- <xacro:property name="toe_radius" value="0.014" /> --> <!-- <xacro:property name="toe_radius" value="0.014" /> -->
<xacro:property name="toe_width" value="0.020" /> <xacro:property name="toe_width" value="0.020" />
<xacro:property name="shift" value="0.055" /> <xacro:property name="shift" value="0.055" />
<xacro:property name="shiftx" value="0.093" /> <xacro:property name="shiftx" value="0.093" />
<xacro:property name="shifty" value="0.039" /> <xacro:property name="shifty" value="0.039" />
<!-- Macros --> <!-- Macros -->
<xacro:macro name="gen_shoulder" params="name left"> <xacro:macro name="gen_shoulder" params="name left">
<link name="${name}"> <link name="${name}">
<visual> <visual>
<xacro:if value="${left}"> <xacro:if value="${left}">
<geometry> <geometry>
<mesh filename="package://stl/lshoulder.stl" scale="0.001 0.001 0.001"/> <mesh filename="package://stl/lshoulder.stl" scale="0.001 0.001 0.001" />
</geometry> </geometry>
<origin rpy="0 0 3.14159" xyz="0.135 0.015 -0.01"/> <origin rpy="0 0 3.14159" xyz="0.135 0.015 -0.01" />
</xacro:if> </xacro:if>
<xacro:unless value="${left}"> <xacro:unless value="${left}">
<geometry> <geometry>
<mesh filename="package://stl/rshoulder.stl" scale="0.001 0.001 0.001"/> <mesh filename="package://stl/rshoulder.stl" scale="0.001 0.001 0.001" />
</geometry> </geometry>
<origin rpy="0 0 3.14159" xyz="0.135 0.095 -0.01"/> <origin rpy="0 0 3.14159" xyz="0.135 0.095 -0.01" />
</xacro:unless> </xacro:unless>
<material name="black"/> <material name="body_color" />
</visual> </visual>
<collision> <collision>
<geometry> <geometry>
<box size="${shoulder_length} ${shoulder_width} ${body_height}"/> <box size="${shoulder_length} ${shoulder_width} ${body_height}" />
</geometry> </geometry>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0" />
</collision> </collision>
<inertial> <inertial>
<mass value="0.10"/> <mass value="0.10" />
<inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" /> <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
</inertial> </inertial>
</link> </link>
</xacro:macro> </xacro:macro>
<xacro:macro name="gen_shoulder_joint" params="pos shiftx shifty"> <xacro:macro name="gen_shoulder_joint" params="pos shiftx shifty">
<joint name="${pos}_shoulder" type="revolute"> <joint name="${pos}_shoulder" type="revolute">
<parent link="base_link"/> <parent link="base_link" />
<child link="${pos}_shoulder_link"/> <child link="${pos}_shoulder_link" />
<axis xyz="1 0 0"/> <axis xyz="1 0 0" />
<origin rpy="0 0 0" xyz="${shiftx} ${shifty} 0"/> <origin rpy="0 0 0" xyz="${shiftx} ${shifty} 0" />
<limit effort="1000.0" lower="-0.548" upper="0.548" velocity="0.7"/> <limit effort="1000.0" lower="-0.548" upper="0.548" velocity="0.7" />
<dynamics damping="0.0" friction="0.5"/> <dynamics damping="0.0" friction="0.5" />
</joint> </joint>
</xacro:macro> </xacro:macro>
<xacro:macro name="gen_leg" params="name left"> <xacro:macro name="gen_leg" params="name left">
<link name="${name}_cover"> <link name="${name}_cover">
<visual> <visual>
<xacro:if value="${left}"> <xacro:if value="${left}">
<geometry> <geometry>
<mesh filename="package://stl/larm_cover.stl" scale="0.001 0.001 0.001"/> <mesh filename="package://stl/larm_cover.stl" scale="0.001 0.001 0.001" />
</geometry> </geometry>
<origin rpy="0 -0.139 3.14159" xyz="0.130 -0.040 -0.025"/> <origin rpy="0 -0.139 3.14159" xyz="0.130 -0.040 -0.025" />
</xacro:if> </xacro:if>
<xacro:unless value="${left}"> <xacro:unless value="${left}">
<geometry> <geometry>
<mesh filename="package://stl/rarm_cover.stl" scale="0.001 0.001 0.001"/> <mesh filename="package://stl/rarm_cover.stl" scale="0.001 0.001 0.001" />
</geometry> </geometry>
<origin rpy="0 -0.139 3.14159" xyz="0.130 0.15 -0.025"/> <origin rpy="0 -0.139 3.14159" xyz="0.130 0.15 -0.025" />
</xacro:unless> </xacro:unless>
<material name="white"/> <material name="shell_color" />
</visual> </visual>
</link> </link>
<link name="${name}"> <link name="${name}">
<visual> <visual>
<xacro:if value="${left}"> <xacro:if value="${left}">
<geometry> <geometry>
<mesh filename="package://stl/larm.stl" scale="0.001 0.001 0.001"/> <mesh filename="package://stl/larm.stl" scale="0.001 0.001 0.001" />
</geometry> </geometry>
<origin rpy="0 -0.139 3.14159" xyz="0.130 -0.040 -0.025"/> <origin rpy="0 -0.139 3.14159" xyz="0.130 -0.040 -0.025" />
</xacro:if> </xacro:if>
<xacro:unless value="${left}"> <xacro:unless value="${left}">
<geometry> <geometry>
<mesh filename="package://stl/rarm.stl" scale="0.001 0.001 0.001"/> <mesh filename="package://stl/rarm.stl" scale="0.001 0.001 0.001" />
</geometry> </geometry>
<origin rpy="0 -0.139 3.14159" xyz="0.130 0.15 -0.025"/> <origin rpy="0 -0.139 3.14159" xyz="0.130 0.15 -0.025" />
</xacro:unless> </xacro:unless>
<material name="black"/> <material name="body_color" />
<!-- <geometry> <!-- <geometry>
<box size="0.028 0.036 ${leg_length}"/> <box size="0.028 0.036 ${leg_length}"/>
</geometry> </geometry>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.050"/> <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.050"/>
<material name="white"/>--> <material name="shell_color"/>-->
</visual> </visual>
<collision> <collision>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.050"/> <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.050"/>
@@ -150,17 +150,17 @@
<visual> <visual>
<xacro:if value="${left}"> <xacro:if value="${left}">
<geometry> <geometry>
<mesh filename="package://stl/lfoot.stl" scale="0.001 0.001 0.001"/> <mesh filename="package://stl/lfoot.stl" scale="0.001 0.001 0.001" />
</geometry> </geometry>
<origin rpy="0 0 3.14159" xyz="0.120 -0.04 0.1"/> <origin rpy="0 0 3.14159" xyz="0.120 -0.04 0.1" />
</xacro:if> </xacro:if>
<xacro:unless value="${left}"> <xacro:unless value="${left}">
<geometry> <geometry>
<mesh filename="package://stl/rfoot.stl" scale="0.001 0.001 0.001"/> <mesh filename="package://stl/rfoot.stl" scale="0.001 0.001 0.001" />
</geometry> </geometry>
<origin rpy="0 0 3.14159" xyz="0.120 0.15 0.1"/> <origin rpy="0 0 3.14159" xyz="0.120 0.15 0.1" />
</xacro:unless> </xacro:unless>
<material name="black"/> <material name="body_color" />
</visual> </visual>
<collision> <collision>
<geometry> <geometry>
@@ -190,10 +190,10 @@
<link name="${name}"> <link name="${name}">
<visual> <visual>
<geometry> <geometry>
<mesh filename="package://stl/foot.stl" scale="0.001 0.001 0.001"/> <mesh filename="package://stl/foot.stl" scale="0.001 0.001 0.001" />
</geometry> </geometry>
<origin rpy="0 -0.40010 3.14159" xyz="0.00 0.01 0.015"/> <origin rpy="0 -0.40010 3.14159" xyz="0.00 0.01 0.015" />
<material name="blue"/> <material name="foot_color" />
</visual> </visual>
<collision> <collision>
<geometry> <geometry>
@@ -229,14 +229,14 @@
<xacro:gen_toe_joint pos="${pos}"/> <xacro:gen_toe_joint pos="${pos}"/>
</xacro:macro> </xacro:macro>
<!-- Robot Body --> <!-- Robot Body -->
<link name="base_link"> <link name="base_link">
<visual> <visual>
<geometry> <geometry>
<mesh filename="package://stl/mainbody.stl" scale="0.001 0.001 0.001"/> <mesh filename="package://stl/mainbody.stl" scale="0.001 0.001 0.001" />
</geometry> </geometry>
<material name="black"/> <material name="body_color" />
<origin rpy="0 0 0" xyz="-0.042 -0.055 -0.010"/> <origin rpy="0 0 0" xyz="-0.042 -0.055 -0.010"/>
</visual> </visual>
<collision> <collision>
@@ -255,10 +255,10 @@
<link name="rear_link"> <link name="rear_link">
<visual> <visual>
<geometry> <geometry>
<mesh filename="package://stl/backpart.stl" scale="0.001 0.001 0.001"/> <mesh filename="package://stl/backpart.stl" scale="0.001 0.001 0.001" />
</geometry> </geometry>
<origin rpy="0 0 3.14159" xyz="0.04 0.055 -0.010"/> <origin rpy="0 0 3.14159" xyz="0.04 0.055 -0.010" />
<material name="white"/> <material name="shell_color" />
</visual> </visual>
<collision> <collision>
<geometry> <geometry>
@@ -279,10 +279,10 @@
<link name="front_link"> <link name="front_link">
<visual> <visual>
<geometry> <geometry>
<mesh filename="package://stl/frontpart.stl" scale="0.001 0.001 0.001"/> <mesh filename="package://stl/frontpart.stl" scale="0.001 0.001 0.001" />
</geometry> </geometry>
<origin rpy="0 0 3.14159" xyz="0.040 0.055 -0.010"/> <origin rpy="0 0 3.14159" xyz="0.040 0.055 -0.010" />
<material name="white"/> <material name="shell_color" />
</visual> </visual>
<collision> <collision>
<geometry> <geometry>
@@ -300,7 +300,7 @@
<child link="front_link"/> <child link="front_link"/>
</joint> </joint>
<!-- create Legs --> <!-- create Legs -->
<xacro:gen_full_leg_joint pos="front_left" shiftx="${shiftx}" shifty="${shifty}" shift="${shift}" left="true"/> <xacro:gen_full_leg_joint pos="front_left" shiftx="${shiftx}" shifty="${shifty}" shift="${shift}" left="true"/>
<xacro:gen_full_leg_joint pos="front_right" shiftx="${shiftx}" shifty="-${shifty}" shift="-${shift}" left="false"/> <xacro:gen_full_leg_joint pos="front_right" shiftx="${shiftx}" shifty="-${shifty}" shift="-${shift}" left="false"/>
app/src/app.css
+5
View File
@@ -13,3 +13,8 @@
body { body {
margin: 0; margin: 0;
} }
#three-gui-panel {
top: 50px;
right:0px
}
app/src/components/Views/Model.svelte
+56 -15
View File
@@ -1,36 +1,55 @@
<script lang="ts"> <script lang="ts">
import { onDestroy, onMount } from 'svelte'; import { onDestroy, onMount } from 'svelte';
import { CanvasTexture, CircleGeometry, Mesh, MeshBasicMaterial } from 'three'; import { BufferGeometry, CanvasTexture, CircleGeometry, CubicBezierCurve3, Line, LineBasicMaterial, Mesh, MeshBasicMaterial, Vector3, type NormalBufferAttributes } from 'three';
import socketService from '$lib/services/socket-service'; import socketService from '$lib/services/socket-service';
import uzip from 'uzip'; import uzip from 'uzip';
import { model } from '$lib/stores'; import { model } from '$lib/stores';
import { ForwardKinematics } from '$lib/kinematic'; import { footColor, isEmbeddedApp, location, toeWorldPositions } from '$lib/utilities';
import { location } from '$lib/utilities';
import { fileService } from '$lib/services'; import { fileService } from '$lib/services';
import { servoAngles, mpu, jointNames } from '$lib/stores'; import { servoAngles, mpu, jointNames } from '$lib/stores';
import SceneBuilder from '$lib/sceneBuilder'; import SceneBuilder from '$lib/sceneBuilder';
import { lerp, degToRad } from 'three/src/math/MathUtils'; import { lerp, degToRad } from 'three/src/math/MathUtils';
import { GUI } from 'three/addons/libs/lil-gui.module.min.js';
let sceneManager: SceneBuilder; let sceneManager = new SceneBuilder();
let canvas: HTMLCanvasElement, streamCanvas: HTMLCanvasElement, stream: HTMLImageElement; let canvas: HTMLCanvasElement, streamCanvas: HTMLCanvasElement, stream: HTMLImageElement;
let context: CanvasRenderingContext2D, texture: CanvasTexture; let context: CanvasRenderingContext2D, texture: CanvasTexture;
let modelAngles: number[] | Int16Array = new Array(12).fill(0); let modelAngles: number[] | Int16Array = new Array(12).fill(0);
let modelTargetAngles: number[] | Int16Array = new Array(12).fill(0); let modelTargetAngles: number[] | Int16Array = new Array(12).fill(0);
let feet_trace = new Array(4).fill([]);
let trace_lines: BufferGeometry<NormalBufferAttributes>[] = []
const videoStream = `//${location}/api/stream`; const videoStream = `//${location}/api/stream`;
let showStream = false; let showStream = false;
let settings = {
'Trace feet':true,
'Trace points': 30
}
onMount(async () => { onMount(async () => {
await cacheModelFiles(); await cacheModelFiles()
await createScene(); await createScene();
if (!isEmbeddedApp) createPanel();
}); });
onDestroy(() => { onDestroy(() => {
canvas.remove(); canvas.remove();
}); });
const createPanel = () => {
const panel = new GUI({width: 310});
panel.close();
panel.domElement.id = 'three-gui-panel';
const visibility = panel.addFolder('Visualization');
visibility.add(settings, 'Trace feet')
visibility.add(settings, 'Trace points', 1, 1000, 1)
}
const cacheModelFiles = async () => { const cacheModelFiles = async () => {
let data = await fetch('/stl.zip').then((data) => data.arrayBuffer()); let data = await fetch('/stl.zip').then((data) => data.arrayBuffer());
@@ -54,13 +73,13 @@
}; };
const createScene = async () => { const createScene = async () => {
sceneManager = new SceneBuilder() sceneManager
.addRenderer({ antialias: true, canvas: canvas, alpha: true }) .addRenderer({ antialias: true, canvas: canvas, alpha: true })
.addPerspectiveCamera({ x: -0.5, y: 0.5, z: 1 }) .addPerspectiveCamera({ x: -0.5, y: 0.5, z: 1 })
.addOrbitControls(10, 30) .addOrbitControls(8, 30)
.addSky() .addSky()
.addGroundPlane({ x: 0, y: -2, z: 0 }) .addGroundPlane()
.addGridHelper({ size: 250, divisions: 125, y: -2 }) .addGridHelper({ size: 250, divisions: 125 })
.addAmbientLight({ color: 0xffffff, intensity: 0.7 }) .addAmbientLight({ color: 0xffffff, intensity: 0.7 })
.addDirectionalLight({ x: 10, y: 100, z: 10, color: 0xffffff, intensity: 1 }) .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 } }) .addArrowHelper({ origin: { x: 0, y: 0, z: 0 }, direction: { x: 0, y: -2, z: 0 } })
@@ -72,6 +91,14 @@
.startRenderLoop(); .startRenderLoop();
addVideoStream(); addVideoStream();
for (let i = 0; i < 4; i++) {
const geometry = new BufferGeometry();
const material = new LineBasicMaterial({ color: footColor() });
const line = new Line(geometry, material);
trace_lines.push(geometry);
sceneManager.scene.add(line);
}
}; };
const addVideoStream = () => { const addVideoStream = () => {
@@ -92,16 +119,30 @@
texture.needsUpdate = true; texture.needsUpdate = true;
}; };
const renderTraceLines = (foot_positions: Vector3[]) => {
if (!settings['Trace feet']) {
if (!feet_trace.length) return
trace_lines.forEach((line, i) => line.setFromPoints(feet_trace[i].slice(-1)))
feet_trace = new Array(4).fill([])
return
}
trace_lines.forEach((line, i) => {
feet_trace[i].push(foot_positions[i])
feet_trace[i] = feet_trace[i].slice(-settings['Trace points'])
line.setFromPoints(feet_trace[i]);
})
}
const render = () => { const render = () => {
const robot = sceneManager.model; const robot = sceneManager.model;
if (!robot) return; if (!robot) return;
const forwardKinematics = new ForwardKinematics(); const toes = toeWorldPositions(robot)
const points = forwardKinematics.calculateFootpoints( renderTraceLines(toes)
modelAngles.map((ang) => degToRad(ang)) as number[]
); robot.position.y = robot.position.y - Math.min(...toes.map(toe => toe.y));
robot.position.y = Math.max(...points.map((coord) => coord[0] / 100)) - 2.7;
robot.rotation.z = lerp(robot.rotation.z, degToRad($mpu.heading + 90), 0.1); robot.rotation.z = lerp(robot.rotation.z, degToRad($mpu.heading + 90), 0.1);
modelTargetAngles = $servoAngles; modelTargetAngles = $servoAngles;
app/src/lib/sceneBuilder.ts
+2 -2
View File
@@ -131,11 +131,11 @@ export default class SceneBuilder {
return this; return this;
}; };
public addGroundPlane = (options: position) => { public addGroundPlane = (options?: position) => {
this.ground = new Mesh(new PlaneGeometry(), new ShadowMaterial({ side: 2 })); this.ground = new Mesh(new PlaneGeometry(), new ShadowMaterial({ side: 2 }));
this.ground.rotation.x = -Math.PI / 2; this.ground.rotation.x = -Math.PI / 2;
this.ground.scale.setScalar(30); this.ground.scale.setScalar(30);
this.ground.position.set(options.x ?? 0, options.y ?? 0, options.z ?? 0); this.ground.position.set(options?.x ?? 0, options?.y ?? 0, options?.z ?? 0);
this.ground.receiveShadow = true; this.ground.receiveShadow = true;
this.scene.add(this.ground); this.scene.add(this.ground);
return this; return this;
app/src/lib/utilities/location-utilities.ts
-1
View File
@@ -1,4 +1,3 @@
export const webAppBuild = import.meta.env.MODE === 'WEB';
export const hostname = window.location.hostname; export const hostname = window.location.hostname;
export const isSecure = window.location.protocol === 'https:'; export const isSecure = window.location.protocol === 'https:';
app/src/lib/utilities/model-utilities.ts
+18 -5
View File
@@ -1,21 +1,22 @@
import { LoaderUtils, Vector3 } from 'three'; import { Color, LoaderUtils, Vector3 } from 'three';
import URDFLoader, { type URDFRobot } from 'urdf-loader'; import URDFLoader, { type URDFRobot } from 'urdf-loader';
import { XacroLoader } from 'xacro-parser'; import { XacroLoader } from 'xacro-parser';
import { Result } from '$lib/utilities'; import { Result } from '$lib/utilities';
let model_xml: XMLDocument;
export const loadModelAsync = async ( export const loadModelAsync = async (
url: string url: string
): Promise<Result<[URDFRobot, string[]], string>> => { ): Promise<Result<[URDFRobot, string[]], string>> => {
return new Promise((resolve, reject) => { return new Promise((resolve, reject) => {
const xacroLoader = new XacroLoader(); const xacroLoader = new XacroLoader();
const urdfLoader = new URDFLoader();
urdfLoader.workingPath = LoaderUtils.extractUrlBase(url);
xacroLoader.load( xacroLoader.load(
url, url,
async (xml) => { async (xml) => {
const urdfLoader = new URDFLoader(); model_xml = xml;
urdfLoader.workingPath = LoaderUtils.extractUrlBase(url);
try { try {
const model = urdfLoader.parse(xml); const model = urdfLoader.parse(xml);
model.rotation.x = -Math.PI / 2; model.rotation.x = -Math.PI / 2;
@@ -48,3 +49,15 @@ export const toeWorldPositions = (robot: URDFRobot) => {
}); });
return toe_positions; return toe_positions;
}; };
export const footColor = () => {
const colorElem = model_xml.querySelector('material[name=foot_color] > color') as Element;
const colorAttrStr = colorElem.getAttribute('rgba') as string;
const colorStr = colorAttrStr
.split(' ')
.slice(0, 3)
.map((val) => Math.floor(+val * 255))
.join(', ');
return new Color(`rgb(${colorStr})`);
};
app/src/routes/Controller.svelte
+2 -2
View File
@@ -7,9 +7,9 @@
<div class="flex justify-center items-center w-full h-full"> <div class="flex justify-center items-center w-full h-full">
{#if $emulateModel} {#if $emulateModel}
<Model /> <Model />
{:else} {:else}
<Stream /> <Stream />
{/if} {/if}
<Controls /> <Controls />
</div> </div>