├── examples ├── move-forward.ts ├── move-square.ts ├── dance.ts ├── move-joints.ts ├── get-state.ts └── avoid-obstacle.ts ├── src ├── mqtt │ ├── topics.ts │ ├── message-handler.ts │ ├── receivers │ │ ├── bms-receiver.ts │ │ └── robot-receiver.ts │ ├── go1-state.ts │ └── go1-mqtt.ts └── go1.ts ├── tsconfig.json ├── README.md ├── package.json ├── LICENSE └── .gitignore /examples/move-forward.ts: -------------------------------------------------------------------------------- 1 | import { Go1, Go1Mode } from "../src/go1"; 2 | 3 | let dog = new Go1(); 4 | dog.init(); 5 | dog.setMode(Go1Mode.walk); 6 | dog.goForward(0.25, 2000); 7 | //dog.go(0, -0.25, -0.25, 1000); 8 | -------------------------------------------------------------------------------- /src/mqtt/topics.ts: -------------------------------------------------------------------------------- 1 | export type BmsSubTopic = "bms/state"; 2 | export type FirmwareSubTopic = "firmware/version"; 3 | export type SubTopic = BmsSubTopic | FirmwareSubTopic; 4 | export type PubTopic = 5 | | "controller/action" 6 | | "controller/stick" 7 | | "programming/code"; 8 | -------------------------------------------------------------------------------- /tsconfig.json: -------------------------------------------------------------------------------- 1 | { 2 | "compilerOptions": { 3 | "target": "ES2016", 4 | "module": "commonjs", 5 | "strict": true, 6 | "declaration": true, 7 | "rootDir": "./src", 8 | "outDir": "./dist", 9 | "types": ["node"] 10 | }, 11 | "exclude": ["tests", "dist", "node_modules", "examples"] 12 | } 13 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # go1-js 2 | 3 | ## Node JS Library for Unitree Go1 4 | 5 | ### Note: users upgrading to version 0.1.4 or newer you must initialize Go1 like the following: 6 | 7 | ``` 8 | let dog = new Go1(); 9 | dog.init(); // version 0.1.4+ requirement 10 | dog.setMode(Go1Mode.walk); 11 | dog.goForward(0.25, 2000); 12 | ``` 13 | 14 | Take a look at the examples directory for more details. 15 | -------------------------------------------------------------------------------- /examples/move-square.ts: -------------------------------------------------------------------------------- 1 | import { Go1, Go1Mode } from "../src/go1"; 2 | 3 | async function run() { 4 | let dog = new Go1(); 5 | dog.init(); 6 | dog.setMode(Go1Mode.walk); 7 | await dog.goForward(0.1, 2000); 8 | await dog.goRight(0.1, 2000); 9 | await dog.goBackward(0.1, 2000); 10 | await dog.goLeft(0.1, 2000); 11 | dog.disconnect(); 12 | console.log("done!"); 13 | } 14 | 15 | run(); 16 | -------------------------------------------------------------------------------- /package.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "@droneblocks/go1-js", 3 | "version": "0.1.5", 4 | "description": "", 5 | "main": "./dist/go1.js", 6 | "scripts": { 7 | "test": "echo \"Error: no test specified\" && exit 1", 8 | "build": "tsc", 9 | "start": "ts-node ./examples/move-forward.ts" 10 | }, 11 | "files": [ 12 | "dist" 13 | ], 14 | "keywords": [ 15 | "unitree", 16 | "go1" 17 | ], 18 | "author": "Dennis Baldwin, DroneBlocks", 19 | "license": "MIT", 20 | "dependencies": { 21 | "mqtt": "^4.3.7" 22 | }, 23 | "devDependencies": { 24 | "@types/node": "^18.11.9", 25 | "@types/ws": "^8.5.3", 26 | "ts-node": "^10.9.1", 27 | "typescript": "^4.9.3" 28 | } 29 | } 30 | -------------------------------------------------------------------------------- /examples/dance.ts: -------------------------------------------------------------------------------- 1 | import { Go1, Go1Mode } from "../src/go1"; 2 | 3 | let dog: Go1; 4 | 5 | async function main() { 6 | dog = new Go1(); 7 | dog.init(); 8 | 9 | await dog.wait(3000); 10 | 11 | dog.setMode(Go1Mode.stand); 12 | 13 | await dog.wait(1000); 14 | 15 | await dog.lookUp(0.5, 1000); 16 | await dog.lookDown(0.5, 1000); 17 | await dog.leanLeft(0.5, 1000); 18 | await dog.leanRight(0.5, 1000); 19 | await dog.twistLeft(0.5, 1000); 20 | await dog.twistRight(0.5, 1000); 21 | 22 | await dog.resetBody(); 23 | 24 | await dog.lookUp(1, 1000); 25 | await dog.lookDown(1, 1000); 26 | await dog.leanLeft(1, 1000); 27 | await dog.leanRight(1, 1000); 28 | await dog.twistLeft(1, 1000); 29 | await dog.twistRight(1, 1000); 30 | 31 | await dog.resetBody(); 32 | 33 | dog.setMode(Go1Mode.walk); 34 | 35 | console.log("done"); 36 | } 37 | 38 | main(); 39 | -------------------------------------------------------------------------------- /src/mqtt/message-handler.ts: -------------------------------------------------------------------------------- 1 | import { Go1State } from "./go1-state"; 2 | import { SubTopic } from "./topics"; 3 | import bmsReceivers from "./receivers/bms-receiver"; 4 | import robotReceivers from "./receivers/robot-receiver"; 5 | 6 | type Receivers = { 7 | [key in SubTopic]: ( 8 | data: Go1State, 9 | message: Buffer, 10 | dataView: DataView 11 | ) => void; 12 | }; 13 | 14 | const messageReceivers: Receivers = { 15 | ...bmsReceivers, 16 | ...robotReceivers, 17 | }; 18 | 19 | export default function messageHandler( 20 | topic: string, 21 | message: Buffer, 22 | data: Go1State 23 | ) { 24 | const msgTopic = topic as SubTopic; 25 | const dataView = new DataView( 26 | message.buffer, 27 | message.byteOffset, 28 | message.byteLength 29 | ); 30 | const receiver = messageReceivers[msgTopic]; 31 | if (receiver) receiver(data, message, dataView); 32 | //else console.log(message); 33 | } 34 | -------------------------------------------------------------------------------- /examples/move-joints.ts: -------------------------------------------------------------------------------- 1 | import { Go1, Go1Mode } from "../src/go1"; 2 | 3 | let dog = new Go1(); 4 | dog.init(); 5 | dog.setMode(Go1Mode.stand); 6 | 7 | async function run() { 8 | // Use direct methods for pose 9 | for (let i = 0; i < 2; i++) { 10 | console.log("extend up"); 11 | await dog.extendUp(1, 2000); 12 | console.log("squat down"); 13 | await dog.squatDown(1, 2000); 14 | } 15 | 16 | // Use generic pose method 17 | await dog.pose(-1, 0, 0, 0, 2000); // Lean left 18 | await dog.pose(1, 0, 0, 0, 2000); // Lean right 19 | await dog.resetBody(); 20 | await dog.pose(0, -1, 0, 0, 2000); // Twist left 21 | await dog.pose(0, 1, 0, 0, 2000); // Twist right 22 | await dog.resetBody(); 23 | await dog.pose(0, 0, -1, 0, 2000); // Look up 24 | await dog.pose(0, 0, 1, 0, 2000); // Look down 25 | await dog.resetBody(); 26 | await dog.pose(0, 0, 0, -1, 2000); // Squat down 27 | await dog.pose(0, 0, 0, 1, 2000); // Extend up 28 | } 29 | 30 | run(); 31 | -------------------------------------------------------------------------------- /src/mqtt/receivers/bms-receiver.ts: -------------------------------------------------------------------------------- 1 | import { Go1State } from "../go1-state"; 2 | import { BmsSubTopic } from "../topics"; 3 | 4 | type Receivers = { 5 | [key in BmsSubTopic]: ( 6 | data: Go1State, 7 | message: Buffer, 8 | dataView: DataView 9 | ) => void; 10 | }; 11 | 12 | const receivers: Receivers = { 13 | "bms/state": (data, message, dataView) => { 14 | const uint8s = new Uint8Array(message); 15 | //console.log(uint8s); 16 | data.bms.version = uint8s[0] + "." + uint8s[1]; 17 | data.bms.status = uint8s[2]; 18 | data.bms.soc = uint8s[3]; 19 | data.bms.current = dataView.getInt32(4, true); 20 | data.bms.cycle = dataView.getUint16(8, true); 21 | data.bms.temps = [uint8s[10], uint8s[11], uint8s[12], uint8s[13]]; 22 | for (let i = 0; i < 10; i++) { 23 | data.bms.cellVoltages[i] = dataView.getUint16(14 + i * 2, true); 24 | } 25 | data.bms.voltage = data.bms.cellVoltages.reduce((a, c) => a + c); 26 | }, 27 | }; 28 | 29 | export default receivers; 30 | -------------------------------------------------------------------------------- /examples/get-state.ts: -------------------------------------------------------------------------------- 1 | import { Go1, Go1Mode } from "../src/go1"; 2 | import { Go1State } from "../src/mqtt/go1-state"; 3 | 4 | let dog: Go1; 5 | 6 | function handleBattery(state: Go1State): void { 7 | const batt: number = state.bms.soc; 8 | 9 | console.log(`battery is at ${batt}%`); 10 | 11 | if (batt >= 75) { 12 | dog.setLedColor(0, 255, 0); // green 13 | } else if (batt >= 50 && batt < 75) { 14 | dog.setLedColor(255, 127, 0); // orange 15 | } else if (batt < 25) { 16 | dog.setLedColor(255, 0, 0); // red 17 | } 18 | } 19 | 20 | async function main() { 21 | dog = new Go1(); 22 | 23 | dog.on("go1StateChange", (state) => { 24 | handleBattery(state); 25 | }); 26 | 27 | // Normally you would navigate Go1 around 28 | // this infinite loop is just to demonstrate 29 | // the battery handler above 30 | while (true) { 31 | dog.setMode(Go1Mode.standDown); 32 | await dog.wait(2000); 33 | dog.setMode(Go1Mode.standUp); 34 | await dog.wait(2000); 35 | } 36 | } 37 | 38 | main(); 39 | -------------------------------------------------------------------------------- /examples/avoid-obstacle.ts: -------------------------------------------------------------------------------- 1 | import { Go1, Go1Mode } from "../src/go1"; 2 | import { Go1State } from "../src/mqtt/go1-state"; 3 | 4 | let dog: Go1; 5 | 6 | function handleCollisionDetection(state: Go1State): void { 7 | const warningThreshold: number = 0.75; 8 | const frontWarning: number = state.robot.distanceWarning.front; 9 | const backWarning: number = state.robot.distanceWarning.back; 10 | const leftWarning: number = state.robot.distanceWarning.left; 11 | const rightWarning: number = state.robot.distanceWarning.right; 12 | 13 | console.log(`Current forward warning is: ${frontWarning}`); 14 | 15 | if (frontWarning > warningThreshold) { 16 | dog.setLedColor(255, 0, 0); 17 | dog.setMode(Go1Mode.standDown); 18 | } 19 | } 20 | 21 | async function main() { 22 | dog = new Go1(); 23 | dog.init(); 24 | dog.setLedColor(0, 255, 0); 25 | 26 | dog.on("go1StateChange", (state) => { 27 | handleCollisionDetection(state); 28 | }); 29 | 30 | // Manually walk Go1 towards a wall (be careful!) 31 | while (true) { 32 | await dog.wait(500); 33 | } 34 | } 35 | 36 | main(); 37 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Dennis Baldwin, DroneBlocks 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /src/mqtt/go1-state.ts: -------------------------------------------------------------------------------- 1 | export type AiMode = "MNFH" | "cam1" | "cam2" | "cam3" | "cam4" | "cam5"; 2 | 3 | export interface Go1State { 4 | mqttConnected: boolean; 5 | managerOn: boolean; 6 | controllerOn: boolean; 7 | bms: { 8 | version: string; 9 | status: number; 10 | soc: number; 11 | current: number; 12 | cycle: number; 13 | temps: number[]; 14 | voltage: number; 15 | cellVoltages: number[]; 16 | }; 17 | robot: { 18 | sn: { 19 | product: string; 20 | id: string; 21 | }; 22 | version: { 23 | hardware: string; 24 | software: string; 25 | }; 26 | temps: number[]; 27 | mode: number; 28 | gaitType: number; 29 | obstacles: number[]; 30 | state: string; 31 | distanceWarning: { 32 | front: number; 33 | back: number; 34 | left: number; 35 | right: number; 36 | }; 37 | }; 38 | } 39 | 40 | const data: Go1State = { 41 | mqttConnected: false, 42 | managerOn: false, 43 | controllerOn: false, 44 | bms: { 45 | version: "unknown", 46 | status: 0, 47 | soc: 0, 48 | current: 0, 49 | cycle: 0, 50 | temps: new Array(4).fill(0), 51 | voltage: 0, 52 | cellVoltages: new Array(10).fill(0), 53 | }, 54 | robot: { 55 | version: { 56 | hardware: "--", 57 | software: "--", 58 | }, 59 | sn: { 60 | product: "--", 61 | id: "--", 62 | }, 63 | temps: new Array(20).fill(0), 64 | mode: 0, 65 | gaitType: 0, 66 | obstacles: [255, 255, 255, 255], 67 | state: "invalid", 68 | distanceWarning: { 69 | front: 0, 70 | back: 0, 71 | left: 0, 72 | right: 0, 73 | }, 74 | }, 75 | }; 76 | 77 | const dataCopy = JSON.stringify(data); 78 | 79 | export const getGo1StateCopy = () => { 80 | return JSON.parse(dataCopy) as Go1State; 81 | }; 82 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Logs 2 | logs 3 | *.log 4 | npm-debug.log* 5 | yarn-debug.log* 6 | yarn-error.log* 7 | lerna-debug.log* 8 | 9 | # Diagnostic reports (https://nodejs.org/api/report.html) 10 | report.[0-9]*.[0-9]*.[0-9]*.[0-9]*.json 11 | 12 | # Runtime data 13 | pids 14 | *.pid 15 | *.seed 16 | *.pid.lock 17 | 18 | # Directory for instrumented libs generated by jscoverage/JSCover 19 | lib-cov 20 | 21 | # Coverage directory used by tools like istanbul 22 | coverage 23 | *.lcov 24 | 25 | # nyc test coverage 26 | .nyc_output 27 | 28 | # Grunt intermediate storage (https://gruntjs.com/creating-plugins#storing-task-files) 29 | .grunt 30 | 31 | # Bower dependency directory (https://bower.io/) 32 | bower_components 33 | 34 | # node-waf configuration 35 | .lock-wscript 36 | 37 | # Compiled binary addons (https://nodejs.org/api/addons.html) 38 | build/Release 39 | 40 | # Dependency directories 41 | node_modules/ 42 | jspm_packages/ 43 | 44 | # TypeScript v1 declaration files 45 | typings/ 46 | 47 | # TypeScript cache 48 | *.tsbuildinfo 49 | 50 | # Optional npm cache directory 51 | .npm 52 | 53 | # Optional eslint cache 54 | .eslintcache 55 | 56 | # Microbundle cache 57 | .rpt2_cache/ 58 | .rts2_cache_cjs/ 59 | .rts2_cache_es/ 60 | .rts2_cache_umd/ 61 | 62 | # Optional REPL history 63 | .node_repl_history 64 | 65 | # Output of 'npm pack' 66 | *.tgz 67 | 68 | # Yarn Integrity file 69 | .yarn-integrity 70 | 71 | # dotenv environment variables file 72 | .env 73 | .env.test 74 | 75 | # parcel-bundler cache (https://parceljs.org/) 76 | .cache 77 | 78 | # Next.js build output 79 | .next 80 | 81 | # Nuxt.js build / generate output 82 | .nuxt 83 | dist 84 | 85 | # Gatsby files 86 | .cache/ 87 | # Comment in the public line in if your project uses Gatsby and *not* Next.js 88 | # https://nextjs.org/blog/next-9-1#public-directory-support 89 | # public 90 | 91 | # vuepress build output 92 | .vuepress/dist 93 | 94 | # Serverless directories 95 | .serverless/ 96 | 97 | # FuseBox cache 98 | .fusebox/ 99 | 100 | # DynamoDB Local files 101 | .dynamodb/ 102 | 103 | # TernJS port file 104 | .tern-port 105 | -------------------------------------------------------------------------------- /src/mqtt/receivers/robot-receiver.ts: -------------------------------------------------------------------------------- 1 | import { Go1State } from "../go1-state"; 2 | import { FirmwareSubTopic } from "../topics"; 3 | 4 | type Receivers = { 5 | [key in FirmwareSubTopic]: ( 6 | data: Go1State, 7 | message: Buffer, 8 | dataView: DataView 9 | ) => void; 10 | }; 11 | 12 | function distanceToWarning(distance: number) { 13 | if (distance > 30) return 0; 14 | else if (distance < 10) return 1; 15 | else { 16 | return 0.2 + (0.8 * (30 - distance)) / 20; 17 | } 18 | } 19 | 20 | const receivers: Receivers = { 21 | "firmware/version": (data, message, dataView) => { 22 | //console.log(dataView); 23 | data.robot.temps = data.robot.temps.map((v, i) => dataView.getUint8(i + 8)); 24 | if (dataView.byteLength > 28) { 25 | data.robot.mode = dataView.getUint8(28); 26 | data.robot.gaitType = dataView.getUint8(29); 27 | data.robot.obstacles = data.robot.obstacles.map((v, i) => 28 | dataView.getUint8(i + 30) 29 | ); 30 | if (data.robot.mode === 2) { 31 | if (data.robot.gaitType === 2) data.robot.state = "run"; 32 | else if (data.robot.gaitType === 3) data.robot.state = "climb"; 33 | else if (data.robot.gaitType === 1) data.robot.state = "walk"; 34 | } 35 | 36 | data.robot.distanceWarning = { 37 | front: distanceToWarning(data.robot.obstacles[0]), 38 | back: distanceToWarning(data.robot.obstacles[3]), 39 | left: distanceToWarning(data.robot.obstacles[1]), 40 | right: distanceToWarning(data.robot.obstacles[2]), 41 | }; 42 | } 43 | if (dataView.byteLength >= 44) { 44 | let name = ""; 45 | switch (dataView.getUint8(0)) { 46 | case 1: 47 | name = "Laikago"; 48 | break; 49 | case 2: 50 | name = "Aliengo"; 51 | break; 52 | case 3: 53 | name = "A1"; 54 | break; 55 | case 4: 56 | name = "Go1"; 57 | break; 58 | case 5: 59 | name = "B1"; 60 | break; 61 | } 62 | let model = ""; 63 | switch (dataView.getUint8(1)) { 64 | case 1: 65 | model = "AIR"; 66 | break; 67 | case 2: 68 | model = "PRO"; 69 | break; 70 | case 3: 71 | model = "EDU"; 72 | break; 73 | case 4: 74 | model = "PC"; 75 | break; 76 | case 5: 77 | model = "XX"; 78 | break; 79 | } 80 | if (name != "") data.robot.sn.product = name + "_" + model; 81 | 82 | if (dataView.getUint8(2) < 255) 83 | data.robot.sn.id = 84 | dataView.getUint8(2) + 85 | "-" + 86 | dataView.getUint8(3) + 87 | "-" + 88 | dataView.getUint8(4) + 89 | "[" + 90 | dataView.getUint8(5) + 91 | "]"; 92 | if (dataView.getUint8(36) < 255) 93 | data.robot.version.hardware = 94 | dataView.getUint8(36) + 95 | "." + 96 | dataView.getUint8(37) + 97 | "." + 98 | dataView.getUint8(38); 99 | data.robot.version.software = 100 | dataView.getUint8(39) + 101 | "." + 102 | dataView.getUint8(40) + 103 | "." + 104 | dataView.getUint8(41); 105 | } 106 | }, 107 | }; 108 | 109 | export default receivers; 110 | -------------------------------------------------------------------------------- /src/mqtt/go1-mqtt.ts: -------------------------------------------------------------------------------- 1 | import { connect, MqttClient, IClientOptions } from "mqtt"; 2 | import { Go1State, getGo1StateCopy } from "./go1-state"; 3 | import { Go1, Go1Mode } from "../go1"; 4 | import messageHandler from "./message-handler"; 5 | 6 | export class Go1MQTT { 7 | go1: Go1; 8 | client: MqttClient | null; 9 | floats: Float32Array = new Float32Array(4); 10 | connected: boolean = false; 11 | movementTopic: string; 12 | ledTopic: string; 13 | modeTopic: string; 14 | publishFrequency: number; 15 | go1State: Go1State; 16 | iClientOptions: IClientOptions; 17 | 18 | readonly defaultIClientOptions: IClientOptions = { 19 | port: 1883, 20 | host: "192.168.12.1", 21 | clientId: Math.random().toString(16).substring(2, 8), 22 | keepalive: 5, 23 | protocol: "mqtt", 24 | }; 25 | 26 | constructor(go1: Go1, iClientOptions?: IClientOptions) { 27 | this.go1 = go1; 28 | this.iClientOptions = { ...this.defaultIClientOptions, ...iClientOptions }; 29 | this.client = null; 30 | this.floats[0] = 0; // walk left (neg) and right (pos) 31 | this.floats[1] = 0; // turn left (neg) and right (pos) 32 | this.floats[2] = 0; 33 | this.floats[3] = 0; // walk backward (neg) and forward (pos) 34 | this.movementTopic = "controller/stick"; 35 | this.ledTopic = "programming/code"; 36 | this.modeTopic = "controller/action"; 37 | this.publishFrequency = 100; // Send MQTT message every 100ms 38 | this.go1State = getGo1StateCopy(); 39 | } 40 | 41 | connect = () => { 42 | console.log("connecting"); 43 | this.client = connect(this.iClientOptions); 44 | 45 | this.client.on("connect", () => { 46 | console.log("connected"); 47 | this.connected = true; 48 | this.go1.publishConnectionStatus(true); 49 | }); 50 | 51 | this.client.on("close", () => { 52 | this.connected = false; 53 | }); 54 | 55 | this.client.on("disconnect", () => { 56 | console.log("disconnected"); 57 | this.go1.publishConnectionStatus(false); 58 | }); 59 | 60 | this.client.on("offline", () => { 61 | console.log("disconnected"); 62 | this.go1.publishConnectionStatus(false); 63 | }); 64 | 65 | // Handle messages that come from various topics 66 | this.client.on("message", (topic, message) => { 67 | this.go1.publishState(this.go1State); 68 | messageHandler(topic, message, this.go1State); 69 | }); 70 | }; 71 | 72 | // Subscribe to topics for updates 73 | subscribe = () => { 74 | this.client?.subscribe(["bms/state", "firmware/version"]); 75 | }; 76 | 77 | getState = (): Go1State => { 78 | return this.go1State; 79 | }; 80 | 81 | disconnect = () => { 82 | this.client?.end(); 83 | }; 84 | 85 | updateSpeed = ( 86 | leftRight: number, 87 | turnLeftRight: number, 88 | lookUpDown: number, // Only for stand mode 89 | backwardForward: number 90 | ) => { 91 | this.floats[0] = this.clamp(leftRight); 92 | this.floats[1] = this.clamp(turnLeftRight); 93 | this.floats[2] = this.clamp(lookUpDown); 94 | this.floats[3] = this.clamp(backwardForward); 95 | }; 96 | 97 | sendMovementCommand = async (lengthOfTime: number) => { 98 | let interval: ReturnType; 99 | 100 | let zero: Float32Array = new Float32Array(4); 101 | zero[0] = 0; 102 | zero[1] = 0; 103 | zero[2] = 0; 104 | zero[3] = 0; 105 | 106 | // Reset speed from the buffer for a few 107 | this.client?.publish( 108 | this.movementTopic, 109 | new Uint8Array(zero.buffer) as Buffer, 110 | { 111 | qos: 0, 112 | } 113 | ); 114 | 115 | interval = setInterval(() => { 116 | console.log(`sending command ${this.floats}`); 117 | this.client?.publish( 118 | this.movementTopic, 119 | new Uint8Array(this.floats.buffer) as Buffer, 120 | { 121 | qos: 0, 122 | } 123 | ); 124 | }, this.publishFrequency); 125 | 126 | // Stop sending after lengthOfTime 127 | return new Promise((resolve) => { 128 | setTimeout(() => { 129 | resolve(); 130 | clearInterval(interval); 131 | }, lengthOfTime); 132 | }); 133 | }; 134 | 135 | sendLEDCommand = (r: number, g: number, b: number) => { 136 | this.client?.publish( 137 | this.ledTopic, 138 | `child_conn.send('change_light(${r},${g},${b})')`, 139 | { 140 | qos: 0, 141 | } 142 | ); 143 | }; 144 | 145 | sendModeCommand = (mode: Go1Mode) => { 146 | this.client?.publish(this.modeTopic, mode, { 147 | qos: 1, 148 | }); 149 | }; 150 | 151 | clamp = (speed: number) => { 152 | if (speed < -1.0) { 153 | return -1.0; 154 | } else if (speed > 1.0) { 155 | return 1.0; 156 | } else { 157 | return speed; 158 | } 159 | }; 160 | } 161 | -------------------------------------------------------------------------------- /src/go1.ts: -------------------------------------------------------------------------------- 1 | import { EventEmitter } from "events"; 2 | import { Go1MQTT } from "./mqtt/go1-mqtt"; 3 | import { Go1State, getGo1StateCopy } from "./mqtt/go1-state"; 4 | import { IClientOptions } from "mqtt"; 5 | 6 | export enum Go1Mode { 7 | dance1 = "dance1", 8 | dance2 = "dance2", 9 | straightHand1 = "straightHand1", 10 | damping = "damping", 11 | standUp = "standUp", 12 | standDown = "standDown", 13 | recoverStand = "recoverStand", 14 | stand = "stand", 15 | walk = "walk", 16 | run = "run", 17 | climb = "climb", 18 | } 19 | 20 | export class Go1 extends EventEmitter { 21 | mqtt: Go1MQTT; 22 | go1State: Go1State; 23 | 24 | constructor(iClientOptions?: IClientOptions) { 25 | super(); 26 | this.mqtt = new Go1MQTT(this, iClientOptions); 27 | this.go1State = getGo1StateCopy(); 28 | } 29 | 30 | init = () => { 31 | this.mqtt.connect(); 32 | this.mqtt.subscribe(); 33 | }; 34 | /** 35 | * 36 | * @param state 37 | */ 38 | publishState = (state: Go1State) => { 39 | this.emit("go1StateChange", state); 40 | }; 41 | 42 | /** 43 | * 44 | * @param connected 45 | */ 46 | publishConnectionStatus = (connected: boolean) => { 47 | this.emit("go1ConnectionStatus", connected); 48 | }; 49 | 50 | /** 51 | * Move forward based on speed and time 52 | * 53 | * @param speed - A value from 0 to 1 54 | * @param lengthOfTime - Length of time for movement in milliseconds 55 | */ 56 | goForward = async (speed: number, lengthOfTime: number) => { 57 | this.mqtt.updateSpeed(0, 0, 0, speed); 58 | await this.mqtt.sendMovementCommand(lengthOfTime); 59 | }; 60 | 61 | /** 62 | * Move backward based on speed and time 63 | * 64 | * @param speed - A value from 0 to 1 65 | * @param lengthOfTime - Length of time for movement in milliseconds 66 | */ 67 | goBackward = async (speed: number, lengthOfTime: number) => { 68 | this.mqtt.updateSpeed(0, 0, 0, speed * -1.0); 69 | await this.mqtt.sendMovementCommand(lengthOfTime); 70 | }; 71 | 72 | /** 73 | * Move left based on speed and time 74 | * 75 | * @param speed - A value from 0 to 1 76 | * @param lengthOfTime - Length of time for movement in milliseconds 77 | */ 78 | goLeft = async (speed: number, lengthOfTime: number) => { 79 | this.mqtt.updateSpeed(speed * -1, 0, 0, 0); 80 | await this.mqtt.sendMovementCommand(lengthOfTime); 81 | }; 82 | 83 | /** 84 | * Move right based on speed and time 85 | * 86 | * @param speed - A value from 0 to 1 87 | * @param lengthOfTime - Length of time for movement in milliseconds 88 | */ 89 | goRight = async (speed: number, lengthOfTime: number) => { 90 | this.mqtt.updateSpeed(speed, 0, 0, 0); 91 | await this.mqtt.sendMovementCommand(lengthOfTime); 92 | }; 93 | 94 | /** 95 | * 96 | * Go left/right, turn left/right, and go forward/backward based on speed and time 97 | * 98 | * @param leftRightSpeed - A value from -1 to 1 99 | * @param turnLeftRightSpeed - A value from -1 to 1 100 | * @param backwardForwardSpeed - A value from -1 to 1 101 | * @param lengthOfTime - Length of time for movement in milliseconds 102 | */ 103 | go = async ( 104 | leftRightSpeed: number, 105 | turnLeftRightSpeed: number, 106 | backwardForwardSpeed: number, 107 | lengthOfTime: number 108 | ) => { 109 | this.mqtt.updateSpeed( 110 | leftRightSpeed, 111 | turnLeftRightSpeed, 112 | 0, 113 | backwardForwardSpeed 114 | ); 115 | await this.mqtt.sendMovementCommand(lengthOfTime); 116 | }; 117 | 118 | /** 119 | * Rotate left based on speed and time 120 | * 121 | * @param speed - A value from 0 to 1 122 | * @param lengthOfTime - Length of time for movement in milliseconds 123 | */ 124 | turnLeft = async (speed: number, lengthOfTime: number) => { 125 | this.mqtt.updateSpeed(0, speed * -1, 0, 0); 126 | await this.mqtt.sendMovementCommand(lengthOfTime); 127 | }; 128 | 129 | /** 130 | * Rotate right based on speed and time 131 | * 132 | * @param speed - A value from 0 to 1 133 | * @param lengthOfTime - Length of time for movement in milliseconds 134 | */ 135 | turnRight = async (speed: number, lengthOfTime: number) => { 136 | this.mqtt.updateSpeed(0, speed, 0, 0); 137 | await this.mqtt.sendMovementCommand(lengthOfTime); 138 | }; 139 | 140 | /** 141 | * Raw pose method for accessing all of the axes together. Requires stand mode to be set first. 142 | * 143 | * @param leanLeftRightAmount - A value from -1 to 1 144 | * @param twistLeftRightAmount - A value from -1 to 1 145 | * @param lookUpDownAmount - A value from -1 to 1 (only for stand mode) 146 | * @param extendSquatAmount - A value from -1 to 1 147 | * @param lengthOfTime - Length of time for movement in milliseconds 148 | */ 149 | pose = async ( 150 | leanLeftRightAmount: number, 151 | twistLeftRightAmount: number, 152 | lookUpDownAmount: number, 153 | extendSquatAmount: number, 154 | lengthOfTime: number 155 | ) => { 156 | this.mqtt.updateSpeed( 157 | leanLeftRightAmount, 158 | twistLeftRightAmount, 159 | lookUpDownAmount, 160 | extendSquatAmount 161 | ); 162 | await this.mqtt.sendMovementCommand(lengthOfTime); 163 | }; 164 | 165 | /** 166 | * Extend up - requires setMode(Go1Mode.stand) to be set 167 | * 168 | * @param speed - A value from 0 to 1 169 | * @param lengthOfTime - Length of time for movement in milliseconds 170 | */ 171 | extendUp = async (speed: number, lengthOfTime: number) => { 172 | this.mqtt.updateSpeed(0, 0, 0, speed); 173 | await this.mqtt.sendMovementCommand(lengthOfTime); 174 | }; 175 | 176 | /** 177 | * Squat up - requires setMode(Go1Mode.stand) to be set 178 | * 179 | * @param speed - A value from 0 to 1 180 | * @param lengthOfTime - Length of time for movement in milliseconds 181 | */ 182 | squatDown = async (speed: number, lengthOfTime: number) => { 183 | this.mqtt.updateSpeed(0, 0, 0, speed * -1); 184 | await this.mqtt.sendMovementCommand(lengthOfTime); 185 | }; 186 | 187 | /** 188 | * Leans body to the left - requires setMode(Go1Mode.stand) to be set 189 | * 190 | * @param speed - A value from 0 to 1 191 | * @param lengthOfTime - Length of time for movement in milliseconds 192 | */ 193 | leanLeft = async (speed: number, lengthOfTime: number) => { 194 | this.mqtt.updateSpeed(speed * -1, 0, 0, 0); 195 | await this.mqtt.sendMovementCommand(lengthOfTime); 196 | }; 197 | 198 | /** 199 | * Leans body to the right - requires setMode(Go1Mode.stand) to be set 200 | * 201 | * @param speed - A value from 0 to 1 202 | * @param lengthOfTime - Length of time for movement in milliseconds 203 | */ 204 | leanRight = async (speed: number, lengthOfTime: number) => { 205 | this.mqtt.updateSpeed(speed, 0, 0, 0); 206 | await this.mqtt.sendMovementCommand(lengthOfTime); 207 | }; 208 | 209 | /** 210 | * Twists body to the left - requires setMode(Go1Mode.stand) to be set 211 | * 212 | * @param speed - A value from 0 to 1 213 | * @param lengthOfTime - Length of time for movement in milliseconds 214 | */ 215 | twistLeft = async (speed: number, lengthOfTime: number) => { 216 | this.mqtt.updateSpeed(0, speed * -1, 0, 0); 217 | await this.mqtt.sendMovementCommand(lengthOfTime); 218 | }; 219 | 220 | /** 221 | * Twists body to the right - requires setMode(Go1Mode.stand) to be set 222 | * 223 | * @param speed - A value from 0 to 1 224 | * @param lengthOfTime - Length of time for movement in milliseconds 225 | */ 226 | twistRight = async (speed: number, lengthOfTime: number) => { 227 | this.mqtt.updateSpeed(0, speed, 0, 0); 228 | await this.mqtt.sendMovementCommand(lengthOfTime); 229 | }; 230 | 231 | /** 232 | * Leans body down - requires setMode(Go1Mode.stand) to be set 233 | * 234 | * @param speed - A value from 0 to 1 235 | * @param lengthOfTime - Length of time for movement in milliseconds 236 | */ 237 | lookDown = async (speed: number, lengthOfTime: number) => { 238 | this.mqtt.updateSpeed(0, 0, speed, 0); 239 | await this.mqtt.sendMovementCommand(lengthOfTime); 240 | }; 241 | 242 | /** 243 | * Leans body up - requires setMode(Go1Mode.stand) to be set 244 | * 245 | * @param speed - A value from 0 to 1 246 | * @param lengthOfTime - Length of time for movement in milliseconds 247 | */ 248 | lookUp = async (speed: number, lengthOfTime: number) => { 249 | this.mqtt.updateSpeed(0, 0, speed * -1, 0); 250 | await this.mqtt.sendMovementCommand(lengthOfTime); 251 | }; 252 | 253 | /** 254 | * Helper function to clear out previous queued movements 255 | */ 256 | resetBody = async () => { 257 | this.mqtt.updateSpeed(0, 0, 0, 0); 258 | await this.mqtt.sendMovementCommand(1000); 259 | }; 260 | 261 | /** 262 | * Wait for a period of time 263 | * 264 | * @param lengthOfTime - Length of time for wait in milliseconds 265 | */ 266 | wait = (lengthOfTime: number) => { 267 | return new Promise((resolve) => setTimeout(resolve, lengthOfTime)); 268 | }; 269 | 270 | /** 271 | * Change Go1's LED color 272 | * 273 | * @param r - A red value from 0 to 255 274 | * @param g - A green value from 0 to 255 275 | * @param b - A blue value from 0 to 255 276 | */ 277 | setLedColor = (r: number, g: number, b: number) => { 278 | this.mqtt.sendLEDCommand(r, g, b); 279 | }; 280 | 281 | /** 282 | * Set Go1's operation mode 283 | * 284 | * @param mode 285 | * Go1Mode.dance1 286 | * Go1Mode.dance2 287 | * Go1Mode.straightHand1 288 | * Go1Mode.damping 289 | * Go1Mode.standUp, 290 | * Go1Mode.standDown 291 | * Go1Mode.recoverStand 292 | * Go1Mode.stand 293 | * Go1Mode.walk 294 | * Go1Mode.run 295 | * Go1Mode.climb 296 | */ 297 | setMode = (mode: Go1Mode) => { 298 | this.mqtt.sendModeCommand(mode); 299 | }; 300 | } 301 | 302 | /** 303 | * stand 304 | * 0, 0, 0, 1 = stand tall (W) 305 | * 0, 0, 0, -1 = stand short (S) 306 | * 307 | * -1, 0, 0, 0 - tilt left (A) 308 | * 1, 0, 0, 0 - tilt right (D) 309 | * 310 | * 0, -1, 0, 0 - look left (left arrow) 311 | * 0, 1, 0, 0 - look right (right arrow) 312 | * 313 | * 0, 0, -1, 0 - look down (up arrow) 314 | * 0, 0, 1, 0 - look up (down arrow) 315 | */ 316 | --------------------------------------------------------------------------------