├── .gitignore
├── Cargo.toml
├── LICENSE
├── README.md
├── assets
    ├── ctm2f110
    │   ├── ctm2f110.bin
    │   └── ctm2f110.gltf
    └── ur5
    │   ├── ur5.bin
    │   └── ur5.gltf
├── index.html
├── media
    ├── demo-web.png
    └── demo.png
├── rust-toolchain.toml
└── src
    ├── draw_trail.rs
    ├── gripper_ctm2f110.rs
    ├── main.rs
    └── robot_ur5.rs


/.gitignore:
--------------------------------------------------------------------------------
 1 | # Generated by Cargo
 2 | # will have compiled files and executables
 3 | debug/
 4 | target/
 5 | 
 6 | # Remove Cargo.lock from gitignore if creating an executable, leave it for libraries
 7 | # More information here https://doc.rust-lang.org/cargo/guide/cargo-toml-vs-cargo-lock.html
 8 | Cargo.lock
 9 | 
10 | # These are backup files generated by rustfmt
11 | **/*.rs.bk
12 | 
13 | # MSVC Windows builds of rustc generate these, which store debugging information
14 | *.pdb
15 | 
16 | .idea/
17 | 
18 | dist/


--------------------------------------------------------------------------------
/Cargo.toml:
--------------------------------------------------------------------------------
 1 | [package]
 2 | name = "demo-bevy_robot"
 3 | version = "0.2.0"
 4 | edition = "2021"
 5 | 
 6 | 
 7 | [dependencies]
 8 | nalgebra = "0.33"
 9 | bevy_egui = "0.34"
10 | bevy_panorbit_camera = { version = "0.26", features = [ "bevy_egui" ] }
11 | flume = "0.11"
12 | wasm-bindgen = "0.2"
13 | 
14 | [dependencies.bevy]
15 | version = "0.16"
16 | default-features = false
17 | features = [
18 |     "bevy_asset", "bevy_core_pipeline", "bevy_gizmos", "bevy_gltf", "bevy_pbr", "bevy_render", "bevy_scene",
19 |     "bevy_sprite", "bevy_text", "bevy_ui", "bevy_winit", "ktx2", "tonemapping_luts", "zstd", "default_font",
20 |     "webgl2"
21 | ]
22 | 
23 | [target.wasm32-unknown-unknown.dependencies]
24 | web-sys = { version = "0.3", features = ["Window", "Document", "HtmlElement", "Element", "CustomEvent"] }
25 | serde = { version = "1.0", features = ["derive"] }
26 | serde-wasm-bindgen = "0.6"
27 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | MIT License
 2 | 
 3 | Copyright (c) 2023 sanri
 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 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # demo-bevy_robot
 2 | Display UR5 robots using the bevy engine
 3 | 
 4 | <img src="media/demo.png">
 5 | 
 6 | <img src="media/demo-web.png">
 7 | 
 8 | ## native application
 9 | 
10 | ### build
11 | 
12 | Run cmd
13 | ```shell
14 | cargo build --release
15 | ```
16 | 
17 | ### run
18 | 
19 | Run cmd
20 | ```shell
21 | cargo run --release
22 | ```
23 | 
24 | ## single page web application
25 | 
26 | ### build
27 | 
28 | Compile to wasm, refer to [trunk](https://trunkrs.dev/). the generated files are in the path "./dist". 
29 |  ```shell
30 |  trunk build --release
31 |  ```
32 |    
33 | ### run
34 | 
35 | use [static-web-server](https://static-web-server.net/) or others web-server.
36 | 
37 |  ```shell
38 |  static-web-server -p 8080 --root ./dist/
39 |  ```
40 | 


--------------------------------------------------------------------------------
/assets/ctm2f110/ctm2f110.bin:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/sanri/demo-bevy_robot/018420c83cc85f40344324e82d4cd640a5528fb9/assets/ctm2f110/ctm2f110.bin


--------------------------------------------------------------------------------
/assets/ctm2f110/ctm2f110.gltf:
--------------------------------------------------------------------------------
  1 | {
  2 | 	"asset":{
  3 | 		"generator":"Khronos glTF Blender I/O v3.5.30",
  4 | 		"version":"2.0"
  5 | 	},
  6 | 	"scene":0,
  7 | 	"scenes":[
  8 | 		{ "name":"main", "nodes":[ 0 ] },
  9 | 		{ "name":"driving", "nodes":[ 1 ] },
 10 | 		{ "name":"follower", "nodes":[ 2 ] },
 11 | 		{ "name":"finger", "nodes":[ 3 ] }
 12 | 	],
 13 | 	"nodes":[
 14 | 		{ "mesh":0, "name":"main" },
 15 | 		{ "mesh":1, "name":"driving" },
 16 | 		{ "mesh":2, "name":"follower" },
 17 | 		{ "mesh":3, "name":"finger" }
 18 | 	],
 19 | 	"meshes":[
 20 | 		{
 21 | 			"name":"main",
 22 | 			"primitives":[
 23 | 				{
 24 | 					"attributes":{
 25 | 						"COLOR_0":0,
 26 | 						"POSITION":1,
 27 | 						"TEXCOORD_0":2,
 28 | 						"NORMAL":3
 29 | 					},
 30 | 					"indices":4
 31 | 				}
 32 | 			]
 33 | 		},
 34 | 		{
 35 | 			"name":"driving",
 36 | 			"primitives":[
 37 | 				{
 38 | 					"attributes":{
 39 | 						"COLOR_0":5,
 40 | 						"POSITION":6,
 41 | 						"TEXCOORD_0":7,
 42 | 						"NORMAL":8
 43 | 					},
 44 | 					"indices":9
 45 | 				}
 46 | 			]
 47 | 		},
 48 | 		{
 49 | 			"name":"follower",
 50 | 			"primitives":[
 51 | 				{
 52 | 					"attributes":{
 53 | 						"COLOR_0":10,
 54 | 						"POSITION":11,
 55 | 						"TEXCOORD_0":12,
 56 | 						"NORMAL":13
 57 | 					},
 58 | 					"indices":14
 59 | 				}
 60 | 			]
 61 | 		},
 62 | 		{
 63 | 			"name":"fingertip",
 64 | 			"primitives":[
 65 | 				{
 66 | 					"attributes":{
 67 | 						"COLOR_0":15,
 68 | 						"POSITION":16,
 69 | 						"TEXCOORD_0":17,
 70 | 						"NORMAL":18
 71 | 					},
 72 | 					"indices":19
 73 | 				}
 74 | 			]
 75 | 		}
 76 | 	],
 77 | 	"accessors":[
 78 | 		{
 79 | 			"bufferView":0,
 80 | 			"componentType":5126,
 81 | 			"count":18422,
 82 | 			"type":"VEC4"
 83 | 		},
 84 | 		{
 85 | 			"bufferView":1,
 86 | 			"componentType":5126,
 87 | 			"count":18422,
 88 | 			"max":[
 89 | 				0.06299400329589844,
 90 | 				0.03749829903244972,
 91 | 				0.10074099898338318
 92 | 			],
 93 | 			"min":[
 94 | 				-0.06299400329589844,
 95 | 				-0.03749829903244972,
 96 | 				3.679869919892553e-08
 97 | 			],
 98 | 			"type":"VEC3"
 99 | 		},
100 | 		{
101 | 			"bufferView":2,
102 | 			"componentType":5126,
103 | 			"count":18422,
104 | 			"type":"VEC2"
105 | 		},
106 | 		{
107 | 			"bufferView":3,
108 | 			"componentType":5126,
109 | 			"count":18422,
110 | 			"type":"VEC3"
111 | 		},
112 | 		{
113 | 			"bufferView":4,
114 | 			"componentType":5123,
115 | 			"count":57162,
116 | 			"type":"SCALAR"
117 | 		},
118 | 		{
119 | 			"bufferView":5,
120 | 			"componentType":5126,
121 | 			"count":11322,
122 | 			"type":"VEC4"
123 | 		},
124 | 		{
125 | 			"bufferView":6,
126 | 			"componentType":5126,
127 | 			"count":11322,
128 | 			"max":[
129 | 				0.06099399924278259,
130 | 				0.011994400061666965,
131 | 				0.029997700825333595
132 | 			],
133 | 			"min":[
134 | 				-0.011997600086033344,
135 | 				-0.01199449971318245,
136 | 				-0.029997700825333595
137 | 			],
138 | 			"type":"VEC3"
139 | 		},
140 | 		{
141 | 			"bufferView":7,
142 | 			"componentType":5126,
143 | 			"count":11322,
144 | 			"type":"VEC2"
145 | 		},
146 | 		{
147 | 			"bufferView":8,
148 | 			"componentType":5126,
149 | 			"count":11322,
150 | 			"type":"VEC3"
151 | 		},
152 | 		{
153 | 			"bufferView":9,
154 | 			"componentType":5123,
155 | 			"count":39252,
156 | 			"type":"SCALAR"
157 | 		},
158 | 		{
159 | 			"bufferView":10,
160 | 			"componentType":5126,
161 | 			"count":8070,
162 | 			"type":"VEC4"
163 | 		},
164 | 		{
165 | 			"bufferView":11,
166 | 			"componentType":5126,
167 | 			"count":8070,
168 | 			"max":[
169 | 				0.06099399924278259,
170 | 				0.005999989807605743,
171 | 				0.012597699649631977
172 | 			],
173 | 			"min":[
174 | 				-0.0059939902275800705,
175 | 				-0.006000009831041098,
176 | 				-0.012597699649631977
177 | 			],
178 | 			"type":"VEC3"
179 | 		},
180 | 		{
181 | 			"bufferView":12,
182 | 			"componentType":5126,
183 | 			"count":8070,
184 | 			"type":"VEC2"
185 | 		},
186 | 		{
187 | 			"bufferView":13,
188 | 			"componentType":5126,
189 | 			"count":8070,
190 | 			"type":"VEC3"
191 | 		},
192 | 		{
193 | 			"bufferView":14,
194 | 			"componentType":5123,
195 | 			"count":28812,
196 | 			"type":"SCALAR"
197 | 		},
198 | 		{
199 | 			"bufferView":15,
200 | 			"componentType":5126,
201 | 			"count":4428,
202 | 			"type":"VEC4"
203 | 		},
204 | 		{
205 | 			"bufferView":16,
206 | 			"componentType":5126,
207 | 			"count":4428,
208 | 			"max":[
209 | 				0.050476301461458206,
210 | 				0.010999999940395355,
211 | 				0.011500000022351742
212 | 			],
213 | 			"min":[
214 | 				-0.017983200028538704,
215 | 				-0.03899409994482994,
216 | 				-0.011500000022351742
217 | 			],
218 | 			"type":"VEC3"
219 | 		},
220 | 		{
221 | 			"bufferView":17,
222 | 			"componentType":5126,
223 | 			"count":4428,
224 | 			"type":"VEC2"
225 | 		},
226 | 		{
227 | 			"bufferView":18,
228 | 			"componentType":5126,
229 | 			"count":4428,
230 | 			"type":"VEC3"
231 | 		},
232 | 		{
233 | 			"bufferView":19,
234 | 			"componentType":5123,
235 | 			"count":14880,
236 | 			"type":"SCALAR"
237 | 		}
238 | 	],
239 | 	"bufferViews":[
240 | 		{
241 | 			"buffer":0,
242 | 			"byteLength":294752,
243 | 			"byteOffset":0,
244 | 			"target":34962
245 | 		},
246 | 		{
247 | 			"buffer":0,
248 | 			"byteLength":221064,
249 | 			"byteOffset":294752,
250 | 			"target":34962
251 | 		},
252 | 		{
253 | 			"buffer":0,
254 | 			"byteLength":147376,
255 | 			"byteOffset":515816,
256 | 			"target":34962
257 | 		},
258 | 		{
259 | 			"buffer":0,
260 | 			"byteLength":221064,
261 | 			"byteOffset":663192,
262 | 			"target":34962
263 | 		},
264 | 		{
265 | 			"buffer":0,
266 | 			"byteLength":114324,
267 | 			"byteOffset":884256,
268 | 			"target":34963
269 | 		},
270 | 		{
271 | 			"buffer":0,
272 | 			"byteLength":181152,
273 | 			"byteOffset":998580,
274 | 			"target":34962
275 | 		},
276 | 		{
277 | 			"buffer":0,
278 | 			"byteLength":135864,
279 | 			"byteOffset":1179732,
280 | 			"target":34962
281 | 		},
282 | 		{
283 | 			"buffer":0,
284 | 			"byteLength":90576,
285 | 			"byteOffset":1315596,
286 | 			"target":34962
287 | 		},
288 | 		{
289 | 			"buffer":0,
290 | 			"byteLength":135864,
291 | 			"byteOffset":1406172,
292 | 			"target":34962
293 | 		},
294 | 		{
295 | 			"buffer":0,
296 | 			"byteLength":78504,
297 | 			"byteOffset":1542036,
298 | 			"target":34963
299 | 		},
300 | 		{
301 | 			"buffer":0,
302 | 			"byteLength":129120,
303 | 			"byteOffset":1620540,
304 | 			"target":34962
305 | 		},
306 | 		{
307 | 			"buffer":0,
308 | 			"byteLength":96840,
309 | 			"byteOffset":1749660,
310 | 			"target":34962
311 | 		},
312 | 		{
313 | 			"buffer":0,
314 | 			"byteLength":64560,
315 | 			"byteOffset":1846500,
316 | 			"target":34962
317 | 		},
318 | 		{
319 | 			"buffer":0,
320 | 			"byteLength":96840,
321 | 			"byteOffset":1911060,
322 | 			"target":34962
323 | 		},
324 | 		{
325 | 			"buffer":0,
326 | 			"byteLength":57624,
327 | 			"byteOffset":2007900,
328 | 			"target":34963
329 | 		},
330 | 		{
331 | 			"buffer":0,
332 | 			"byteLength":70848,
333 | 			"byteOffset":2065524,
334 | 			"target":34962
335 | 		},
336 | 		{
337 | 			"buffer":0,
338 | 			"byteLength":53136,
339 | 			"byteOffset":2136372,
340 | 			"target":34962
341 | 		},
342 | 		{
343 | 			"buffer":0,
344 | 			"byteLength":35424,
345 | 			"byteOffset":2189508,
346 | 			"target":34962
347 | 		},
348 | 		{
349 | 			"buffer":0,
350 | 			"byteLength":53136,
351 | 			"byteOffset":2224932,
352 | 			"target":34962
353 | 		},
354 | 		{
355 | 			"buffer":0,
356 | 			"byteLength":29760,
357 | 			"byteOffset":2278068,
358 | 			"target":34963
359 | 		}
360 | 	],
361 | 	"buffers":[
362 | 		{
363 | 			"byteLength":2307828,
364 | 			"uri":"ctm2f110.bin"
365 | 		}
366 | 	]
367 | }
368 | 


--------------------------------------------------------------------------------
/assets/ur5/ur5.bin:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/sanri/demo-bevy_robot/018420c83cc85f40344324e82d4cd640a5528fb9/assets/ur5/ur5.bin


--------------------------------------------------------------------------------
/assets/ur5/ur5.gltf:
--------------------------------------------------------------------------------
  1 | {
  2 | 	"asset":{
  3 | 		"generator":"Khronos glTF Blender I/O v3.5.30",
  4 | 		"version":"2.0"
  5 | 	},
  6 | 	"scene":0,
  7 | 	"scenes":[
  8 | 		{ "name":"Base", "nodes":[ 6 ] },
  9 | 		{ "name":"Arm1", "nodes":[ 4 ] },
 10 | 		{ "name":"Arm2", "nodes":[ 3 ] },
 11 | 		{ "name":"Arm3", "nodes":[ 2 ] },
 12 | 		{ "name":"Arm4", "nodes":[ 5 ] },
 13 | 		{ "name":"Arm5", "nodes":[ 0 ] },
 14 | 		{ "name":"Arm6", "nodes":[ 1 ] }
 15 | 	],
 16 | 	"nodes":[
 17 | 		{
 18 | 			"mesh":0,
 19 | 			"name":"UR5_Arm5"
 20 | 		},
 21 | 		{
 22 | 			"mesh":1,
 23 | 			"name":"UR5_Arm6"
 24 | 		},
 25 | 		{
 26 | 			"mesh":2,
 27 | 			"name":"UR5_Arm3"
 28 | 		},
 29 | 		{
 30 | 			"mesh":3,
 31 | 			"name":"UR5_Arm2"
 32 | 		},
 33 | 		{
 34 | 			"mesh":4,
 35 | 			"name":"UR5_Arm1"
 36 | 		},
 37 | 		{
 38 | 			"mesh":5,
 39 | 			"name":"UR5_Arm4"
 40 | 		},
 41 | 		{
 42 | 			"mesh":6,
 43 | 			"name":"UR5_Base"
 44 | 		}
 45 | 	],
 46 | 	"meshes":[
 47 | 		{
 48 | 			"name":"UR5_Arm5",
 49 | 			"primitives":[
 50 | 				{
 51 | 					"attributes":{
 52 | 						"COLOR_0":0,
 53 | 						"POSITION":1,
 54 | 						"TEXCOORD_0":2,
 55 | 						"NORMAL":3
 56 | 					},
 57 | 					"indices":4
 58 | 				}
 59 | 			]
 60 | 		},
 61 | 		{
 62 | 			"name":"UR5_Arm6",
 63 | 			"primitives":[
 64 | 				{
 65 | 					"attributes":{
 66 | 						"COLOR_0":5,
 67 | 						"POSITION":6,
 68 | 						"TEXCOORD_0":7,
 69 | 						"NORMAL":8
 70 | 					},
 71 | 					"indices":9
 72 | 				}
 73 | 			]
 74 | 		},
 75 | 		{
 76 | 			"name":"UR5_Arm3",
 77 | 			"primitives":[
 78 | 				{
 79 | 					"attributes":{
 80 | 						"COLOR_0":10,
 81 | 						"POSITION":11,
 82 | 						"TEXCOORD_0":12,
 83 | 						"NORMAL":13
 84 | 					},
 85 | 					"indices":14
 86 | 				}
 87 | 			]
 88 | 		},
 89 | 		{
 90 | 			"name":"UR5_Arm2",
 91 | 			"primitives":[
 92 | 				{
 93 | 					"attributes":{
 94 | 						"COLOR_0":15,
 95 | 						"POSITION":16,
 96 | 						"TEXCOORD_0":17,
 97 | 						"NORMAL":18
 98 | 					},
 99 | 					"indices":19
100 | 				}
101 | 			]
102 | 		},
103 | 		{
104 | 			"name":"UR5_Arm1",
105 | 			"primitives":[
106 | 				{
107 | 					"attributes":{
108 | 						"COLOR_0":20,
109 | 						"POSITION":21,
110 | 						"TEXCOORD_0":22,
111 | 						"NORMAL":23
112 | 					},
113 | 					"indices":24
114 | 				}
115 | 			]
116 | 		},
117 | 		{
118 | 			"name":"UR5_Arm4",
119 | 			"primitives":[
120 | 				{
121 | 					"attributes":{
122 | 						"COLOR_0":25,
123 | 						"POSITION":26,
124 | 						"TEXCOORD_0":27,
125 | 						"NORMAL":28
126 | 					},
127 | 					"indices":29
128 | 				}
129 | 			]
130 | 		},
131 | 		{
132 | 			"name":"UR5_Base",
133 | 			"primitives":[
134 | 				{
135 | 					"attributes":{
136 | 						"COLOR_0":30,
137 | 						"POSITION":31,
138 | 						"TEXCOORD_0":32,
139 | 						"NORMAL":33
140 | 					},
141 | 					"indices":34
142 | 				}
143 | 			]
144 | 		}
145 | 	],
146 | 	"accessors":[
147 | 		{
148 | 			"bufferView":0,
149 | 			"componentType":5126,
150 | 			"count":39197,
151 | 			"type":"VEC4"
152 | 		},
153 | 		{
154 | 			"bufferView":1,
155 | 			"componentType":5126,
156 | 			"count":39197,
157 | 			"max":[
158 | 				0.0393499992787838,
159 | 				0.048500001430511475,
160 | 				0.0393357016146183
161 | 			],
162 | 			"min":[
163 | 				-0.0393499992787838,
164 | 				-0.05803060159087181,
165 | 				-0.04625000059604645
166 | 			],
167 | 			"type":"VEC3"
168 | 		},
169 | 		{
170 | 			"bufferView":2,
171 | 			"componentType":5126,
172 | 			"count":39197,
173 | 			"type":"VEC2"
174 | 		},
175 | 		{
176 | 			"bufferView":3,
177 | 			"componentType":5126,
178 | 			"count":39197,
179 | 			"type":"VEC3"
180 | 		},
181 | 		{
182 | 			"bufferView":4,
183 | 			"componentType":5123,
184 | 			"count":114669,
185 | 			"type":"SCALAR"
186 | 		},
187 | 		{
188 | 			"bufferView":5,
189 | 			"componentType":5126,
190 | 			"count":2117,
191 | 			"type":"VEC4"
192 | 		},
193 | 		{
194 | 			"bufferView":6,
195 | 			"componentType":5126,
196 | 			"count":2117,
197 | 			"max":[
198 | 				0.03748060017824173,
199 | 				0.03749990090727806,
200 | 				-4.582820167797763e-08
201 | 			],
202 | 			"min":[
203 | 				-0.03748060017824173,
204 | 				-0.03749990090727806,
205 | 				-0.032999999821186066
206 | 			],
207 | 			"type":"VEC3"
208 | 		},
209 | 		{
210 | 			"bufferView":7,
211 | 			"componentType":5126,
212 | 			"count":2117,
213 | 			"type":"VEC2"
214 | 		},
215 | 		{
216 | 			"bufferView":8,
217 | 			"componentType":5126,
218 | 			"count":2117,
219 | 			"type":"VEC3"
220 | 		},
221 | 		{
222 | 			"bufferView":9,
223 | 			"componentType":5123,
224 | 			"count":5448,
225 | 			"type":"SCALAR"
226 | 		},
227 | 		{
228 | 			"bufferView":10,
229 | 			"componentType":5126,
230 | 			"count":51118,
231 | 			"type":"VEC4"
232 | 		},
233 | 		{
234 | 			"bufferView":11,
235 | 			"componentType":5126,
236 | 			"count":51118,
237 | 			"max":[
238 | 				0.43160000443458557,
239 | 				0.057999998331069946,
240 | 				0.05803050100803375
241 | 			],
242 | 			"min":[
243 | 				-0.05796470120549202,
244 | 				-0.05794690176844597,
245 | 				-0.052549999207258224
246 | 			],
247 | 			"type":"VEC3"
248 | 		},
249 | 		{
250 | 			"bufferView":12,
251 | 			"componentType":5126,
252 | 			"count":51118,
253 | 			"type":"VEC2"
254 | 		},
255 | 		{
256 | 			"bufferView":13,
257 | 			"componentType":5126,
258 | 			"count":51118,
259 | 			"type":"VEC3"
260 | 		},
261 | 		{
262 | 			"bufferView":14,
263 | 			"componentType":5123,
264 | 			"count":130191,
265 | 			"type":"SCALAR"
266 | 		},
267 | 		{
268 | 			"bufferView":15,
269 | 			"componentType":5126,
270 | 			"count":92711,
271 | 			"type":"VEC4"
272 | 		},
273 | 		{
274 | 			"bufferView":16,
275 | 			"componentType":5126,
276 | 			"count":92711,
277 | 			"max":[
278 | 				0.48510000109672546,
279 | 				0.060100000351667404,
280 | 				0.07347720116376877
281 | 			],
282 | 			"min":[
283 | 				-0.060100000351667404,
284 | 				-0.060100000351667404,
285 | 				-0.06639999896287918
286 | 			],
287 | 			"type":"VEC3"
288 | 		},
289 | 		{
290 | 			"bufferView":17,
291 | 			"componentType":5126,
292 | 			"count":92711,
293 | 			"type":"VEC2"
294 | 		},
295 | 		{
296 | 			"bufferView":18,
297 | 			"componentType":5126,
298 | 			"count":92711,
299 | 			"type":"VEC3"
300 | 		},
301 | 		{
302 | 			"bufferView":19,
303 | 			"componentType":5125,
304 | 			"count":236400,
305 | 			"type":"SCALAR"
306 | 		},
307 | 		{
308 | 			"bufferView":20,
309 | 			"componentType":5126,
310 | 			"count":35886,
311 | 			"type":"VEC4"
312 | 		},
313 | 		{
314 | 			"bufferView":21,
315 | 			"componentType":5126,
316 | 			"count":35886,
317 | 			"max":[
318 | 				0.060062799602746964,
319 | 				0.06780000030994415,
320 | 				0.07347720116376877
321 | 			],
322 | 			"min":[
323 | 				-0.060100000351667404,
324 | 				-0.060100000351667404,
325 | 				-0.06639999896287918
326 | 			],
327 | 			"type":"VEC3"
328 | 		},
329 | 		{
330 | 			"bufferView":22,
331 | 			"componentType":5126,
332 | 			"count":35886,
333 | 			"type":"VEC2"
334 | 		},
335 | 		{
336 | 			"bufferView":23,
337 | 			"componentType":5126,
338 | 			"count":35886,
339 | 			"type":"VEC3"
340 | 		},
341 | 		{
342 | 			"bufferView":24,
343 | 			"componentType":5123,
344 | 			"count":99063,
345 | 			"type":"SCALAR"
346 | 		},
347 | 		{
348 | 			"bufferView":25,
349 | 			"componentType":5126,
350 | 			"count":39197,
351 | 			"type":"VEC4"
352 | 		},
353 | 		{
354 | 			"bufferView":26,
355 | 			"componentType":5126,
356 | 			"count":39197,
357 | 			"max":[
358 | 				0.0393499992787838,
359 | 				0.048500001430511475,
360 | 				0.0393357016146183
361 | 			],
362 | 			"min":[
363 | 				-0.0393499992787838,
364 | 				-0.05803050100803375,
365 | 				-0.04625000059604645
366 | 			],
367 | 			"type":"VEC3"
368 | 		},
369 | 		{
370 | 			"bufferView":27,
371 | 			"componentType":5126,
372 | 			"count":39197,
373 | 			"type":"VEC2"
374 | 		},
375 | 		{
376 | 			"bufferView":28,
377 | 			"componentType":5126,
378 | 			"count":39197,
379 | 			"type":"VEC3"
380 | 		},
381 | 		{
382 | 			"bufferView":29,
383 | 			"componentType":5123,
384 | 			"count":114597,
385 | 			"type":"SCALAR"
386 | 		},
387 | 		{
388 | 			"bufferView":30,
389 | 			"componentType":5126,
390 | 			"count":2508,
391 | 			"type":"VEC4"
392 | 		},
393 | 		{
394 | 			"bufferView":31,
395 | 			"componentType":5126,
396 | 			"count":2508,
397 | 			"max":[
398 | 				0.07450000196695328,
399 | 				0.07445459812879562,
400 | 				0.02280000038444996
401 | 			],
402 | 			"min":[
403 | 				-0.07450000196695328,
404 | 				-0.07445459812879562,
405 | 				9.315980200993728e-18
406 | 			],
407 | 			"type":"VEC3"
408 | 		},
409 | 		{
410 | 			"bufferView":32,
411 | 			"componentType":5126,
412 | 			"count":2508,
413 | 			"type":"VEC2"
414 | 		},
415 | 		{
416 | 			"bufferView":33,
417 | 			"componentType":5126,
418 | 			"count":2508,
419 | 			"type":"VEC3"
420 | 		},
421 | 		{
422 | 			"bufferView":34,
423 | 			"componentType":5123,
424 | 			"count":6456,
425 | 			"type":"SCALAR"
426 | 		}
427 | 	],
428 | 	"bufferViews":[
429 | 		{
430 | 			"buffer":0,
431 | 			"byteLength":627152,
432 | 			"byteOffset":0,
433 | 			"target":34962
434 | 		},
435 | 		{
436 | 			"buffer":0,
437 | 			"byteLength":470364,
438 | 			"byteOffset":627152,
439 | 			"target":34962
440 | 		},
441 | 		{
442 | 			"buffer":0,
443 | 			"byteLength":313576,
444 | 			"byteOffset":1097516,
445 | 			"target":34962
446 | 		},
447 | 		{
448 | 			"buffer":0,
449 | 			"byteLength":470364,
450 | 			"byteOffset":1411092,
451 | 			"target":34962
452 | 		},
453 | 		{
454 | 			"buffer":0,
455 | 			"byteLength":229338,
456 | 			"byteOffset":1881456,
457 | 			"target":34963
458 | 		},
459 | 		{
460 | 			"buffer":0,
461 | 			"byteLength":33872,
462 | 			"byteOffset":2110796,
463 | 			"target":34962
464 | 		},
465 | 		{
466 | 			"buffer":0,
467 | 			"byteLength":25404,
468 | 			"byteOffset":2144668,
469 | 			"target":34962
470 | 		},
471 | 		{
472 | 			"buffer":0,
473 | 			"byteLength":16936,
474 | 			"byteOffset":2170072,
475 | 			"target":34962
476 | 		},
477 | 		{
478 | 			"buffer":0,
479 | 			"byteLength":25404,
480 | 			"byteOffset":2187008,
481 | 			"target":34962
482 | 		},
483 | 		{
484 | 			"buffer":0,
485 | 			"byteLength":10896,
486 | 			"byteOffset":2212412,
487 | 			"target":34963
488 | 		},
489 | 		{
490 | 			"buffer":0,
491 | 			"byteLength":817888,
492 | 			"byteOffset":2223308,
493 | 			"target":34962
494 | 		},
495 | 		{
496 | 			"buffer":0,
497 | 			"byteLength":613416,
498 | 			"byteOffset":3041196,
499 | 			"target":34962
500 | 		},
501 | 		{
502 | 			"buffer":0,
503 | 			"byteLength":408944,
504 | 			"byteOffset":3654612,
505 | 			"target":34962
506 | 		},
507 | 		{
508 | 			"buffer":0,
509 | 			"byteLength":613416,
510 | 			"byteOffset":4063556,
511 | 			"target":34962
512 | 		},
513 | 		{
514 | 			"buffer":0,
515 | 			"byteLength":260382,
516 | 			"byteOffset":4676972,
517 | 			"target":34963
518 | 		},
519 | 		{
520 | 			"buffer":0,
521 | 			"byteLength":1483376,
522 | 			"byteOffset":4937356,
523 | 			"target":34962
524 | 		},
525 | 		{
526 | 			"buffer":0,
527 | 			"byteLength":1112532,
528 | 			"byteOffset":6420732,
529 | 			"target":34962
530 | 		},
531 | 		{
532 | 			"buffer":0,
533 | 			"byteLength":741688,
534 | 			"byteOffset":7533264,
535 | 			"target":34962
536 | 		},
537 | 		{
538 | 			"buffer":0,
539 | 			"byteLength":1112532,
540 | 			"byteOffset":8274952,
541 | 			"target":34962
542 | 		},
543 | 		{
544 | 			"buffer":0,
545 | 			"byteLength":945600,
546 | 			"byteOffset":9387484,
547 | 			"target":34963
548 | 		},
549 | 		{
550 | 			"buffer":0,
551 | 			"byteLength":574176,
552 | 			"byteOffset":10333084,
553 | 			"target":34962
554 | 		},
555 | 		{
556 | 			"buffer":0,
557 | 			"byteLength":430632,
558 | 			"byteOffset":10907260,
559 | 			"target":34962
560 | 		},
561 | 		{
562 | 			"buffer":0,
563 | 			"byteLength":287088,
564 | 			"byteOffset":11337892,
565 | 			"target":34962
566 | 		},
567 | 		{
568 | 			"buffer":0,
569 | 			"byteLength":430632,
570 | 			"byteOffset":11624980,
571 | 			"target":34962
572 | 		},
573 | 		{
574 | 			"buffer":0,
575 | 			"byteLength":198126,
576 | 			"byteOffset":12055612,
577 | 			"target":34963
578 | 		},
579 | 		{
580 | 			"buffer":0,
581 | 			"byteLength":627152,
582 | 			"byteOffset":12253740,
583 | 			"target":34962
584 | 		},
585 | 		{
586 | 			"buffer":0,
587 | 			"byteLength":470364,
588 | 			"byteOffset":12880892,
589 | 			"target":34962
590 | 		},
591 | 		{
592 | 			"buffer":0,
593 | 			"byteLength":313576,
594 | 			"byteOffset":13351256,
595 | 			"target":34962
596 | 		},
597 | 		{
598 | 			"buffer":0,
599 | 			"byteLength":470364,
600 | 			"byteOffset":13664832,
601 | 			"target":34962
602 | 		},
603 | 		{
604 | 			"buffer":0,
605 | 			"byteLength":229194,
606 | 			"byteOffset":14135196,
607 | 			"target":34963
608 | 		},
609 | 		{
610 | 			"buffer":0,
611 | 			"byteLength":40128,
612 | 			"byteOffset":14364392,
613 | 			"target":34962
614 | 		},
615 | 		{
616 | 			"buffer":0,
617 | 			"byteLength":30096,
618 | 			"byteOffset":14404520,
619 | 			"target":34962
620 | 		},
621 | 		{
622 | 			"buffer":0,
623 | 			"byteLength":20064,
624 | 			"byteOffset":14434616,
625 | 			"target":34962
626 | 		},
627 | 		{
628 | 			"buffer":0,
629 | 			"byteLength":30096,
630 | 			"byteOffset":14454680,
631 | 			"target":34962
632 | 		},
633 | 		{
634 | 			"buffer":0,
635 | 			"byteLength":12912,
636 | 			"byteOffset":14484776,
637 | 			"target":34963
638 | 		}
639 | 	],
640 | 	"buffers":[
641 | 		{
642 | 			"byteLength":14497688,
643 | 			"uri":"ur5.bin"
644 | 		}
645 | 	]
646 | }
647 | 


--------------------------------------------------------------------------------
/index.html:
--------------------------------------------------------------------------------
  1 | <html>
  2 | <head>
  3 |     <meta charset="UTF-8"/>
  4 |     <title>demo-bevy_robot</title>
  5 |     <link data-trunk rel="rust" data-wasm-opt=s data-integrity="none"/>
  6 |     <link data-trunk rel="copy-dir" href="assets"/>
  7 |     <script defer="defer" type="text/javascript">
  8 |         function input_robot_joint_click(robot, joint) {
  9 |             const element_id = `input_robot${robot}_joint${joint}`;
 10 |             const angle = document.getElementById(element_id).value;
 11 |             window.wasmBindings.set_robot_joint_pos(robot, joint, angle);
 12 |         }
 13 | 
 14 |         function input_robot_finger_click(robot, finger) {
 15 |             const element_id = `input_robot${robot}_finger${finger}`;
 16 |             const pos = document.getElementById(element_id).value;
 17 |             window.wasmBindings.set_robot_finger_pos(robot, finger, pos);
 18 |         }
 19 | 
 20 |         window.addEventListener("joint_changed", function (event) {
 21 |             const robot = event.detail["robot"];
 22 |             const joint = event.detail["joint"];
 23 |             const angle = event.detail["angle"];
 24 |             const element_id = `input_robot${robot}_joint${joint}`;
 25 |             document.getElementById(element_id).value = angle;
 26 |         })
 27 | 
 28 |         window.addEventListener("finger_changed", function (event) {
 29 |             const robot = event.detail["robot"];
 30 |             const finger = event.detail["finger"];
 31 |             const pos = event.detail["pos"];
 32 |             const element_id = `input_robot${robot}_finger${finger}`;
 33 |             document.getElementById(element_id).value = pos;
 34 |         })
 35 |     </script>
 36 | 
 37 |     <style>
 38 |         .par_row {
 39 |             display: flex;
 40 |             justify-content: space-around;
 41 |             margin: 10px;
 42 |         }
 43 | 
 44 |         .input_label {
 45 |             width: 80px;
 46 |         }
 47 | 
 48 |         .input_box {
 49 |             width: 60px;
 50 |             margin-right: 20px;
 51 |         }
 52 |     </style>
 53 | </head>
 54 | 
 55 | <body>
 56 | 
 57 | <h1 style="text-align: center">demo-bevy_robot</h1>
 58 | 
 59 | <div style="width: 80%;height: 60%; margin: auto">
 60 |     <canvas id="demo-bevy_robot"></canvas>
 61 |     <script type="text/javascript">
 62 |         document.getElementById("demo-bevy_robot").oncontextmenu = function () {
 63 |             return false;
 64 |         }
 65 |     </script>
 66 | </div>
 67 | 
 68 | <div style="display: flex; justify-content: space-around">
 69 | 
 70 |     <div id="robot0">
 71 |         <label>Robot0</label>
 72 | 
 73 |         <div class="par_row">
 74 |             <label class="input_label" for="input_robot0_joint1">Joint 1:</label>
 75 |             <input class="input_box" type="number" id="input_robot0_joint1" min="-720" max="720" value="0"/>
 76 |             <button onclick="input_robot_joint_click(0,1)">set</button>
 77 |         </div>
 78 | 
 79 |         <div class="par_row">
 80 |             <label class="input_label" for="input_robot0_joint2">Joint 2:</label>
 81 |             <input class="input_box" type="number" id="input_robot0_joint2" min="-720" max="720" value="0"/>
 82 |             <button onclick="input_robot_joint_click(0,2)">set</button>
 83 |         </div>
 84 | 
 85 |         <div class="par_row">
 86 |             <label class="input_label" for="input_robot0_joint3">Joint 3:</label>
 87 |             <input class="input_box" type="number" id="input_robot0_joint3" min="-720" max="720" value="0"/>
 88 |             <button onclick="input_robot_joint_click(0,3)">set</button>
 89 |         </div>
 90 | 
 91 |         <div class="par_row">
 92 |             <label class="input_label" for="input_robot0_joint4">Joint 4:</label>
 93 |             <input class="input_box" type="number" id="input_robot0_joint4" min="-720" max="720" value="0"/>
 94 |             <button onclick="input_robot_joint_click(0,4)">set</button>
 95 |         </div>
 96 | 
 97 |         <div class="par_row">
 98 |             <label class="input_label" for="input_robot0_joint5">Joint 5:</label>
 99 |             <input class="input_box" type="number" id="input_robot0_joint5" min="-720" max="720" value="0"/>
100 |             <button onclick="input_robot_joint_click(0,5)">set</button>
101 |         </div>
102 | 
103 |         <div class="par_row">
104 |             <label class="input_label" for="input_robot0_joint6">Joint 6:</label>
105 |             <input class="input_box" type="number" id="input_robot0_joint6" min="-720" max="720" value="0"/>
106 |             <button onclick="input_robot_joint_click(0,6)">set</button>
107 |         </div>
108 | 
109 |         <div class="par_row">
110 |             <label class="input_label" for="input_robot0_finger1">Finger 1:</label>
111 |             <input class="input_box" type="number" id="input_robot0_finger1" min="0" max="100" value="0"/>
112 |             <button onclick="input_robot_finger_click(0,1)">set</button>
113 |         </div>
114 | 
115 |         <div class="par_row">
116 |             <label class="input_label" for="input_robot0_finger2">Finger 2:</label>
117 |             <input class="input_box" type="number" id="input_robot0_finger2" min="0" max="100" value="0"/>
118 |             <button onclick="input_robot_finger_click(0,2)">set</button>
119 |         </div>
120 | 
121 |     </div>
122 | 
123 |     <div id="robot1">
124 |         <label>Robot1</label>
125 | 
126 |         <div class="par_row">
127 |             <label class="input_label" for="input_robot1_joint1">Joint 1:</label>
128 |             <input class="input_box" type="number" id="input_robot1_joint1" min="-720" max="720" value="0"/>
129 |             <button onclick="input_robot_joint_click(1,1)">set</button>
130 |         </div>
131 | 
132 |         <div class="par_row">
133 |             <label class="input_label" for="input_robot1_joint2">Joint 2:</label>
134 |             <input class="input_box" type="number" id="input_robot1_joint2" min="-720" max="720" value="0"/>
135 |             <button onclick="input_robot_joint_click(1,2)">set</button>
136 |         </div>
137 | 
138 |         <div class="par_row">
139 |             <label class="input_label" for="input_robot1_joint3">Joint 3:</label>
140 |             <input class="input_box" type="number" id="input_robot1_joint3" min="-720" max="720" value="0"/>
141 |             <button onclick="input_robot_joint_click(1,3)">set</button>
142 |         </div>
143 | 
144 |         <div class="par_row">
145 |             <label class="input_label" for="input_robot1_joint4">Joint 4:</label>
146 |             <input class="input_box" type="number" id="input_robot1_joint4" min="-720" max="720" value="0"/>
147 |             <button onclick="input_robot_joint_click(1,4)">set</button>
148 |         </div>
149 | 
150 |         <div class="par_row">
151 |             <label class="input_label" for="input_robot1_joint5">Joint 5:</label>
152 |             <input class="input_box" type="number" id="input_robot1_joint5" min="-720" max="720" value="0"/>
153 |             <button onclick="input_robot_joint_click(1,5)">set</button>
154 |         </div>
155 | 
156 |         <div class="par_row">
157 |             <label class="input_label" for="input_robot1_joint6">Joint 6:</label>
158 |             <input class="input_box" type="number" id="input_robot1_joint6" min="-720" max="720" value="0"/>
159 |             <button onclick="input_robot_joint_click(1,6)">set</button>
160 |         </div>
161 | 
162 |         <div class="par_row">
163 |             <label class="input_label" for="input_robot1_finger1">Finger 1:</label>
164 |             <input class="input_box" type="number" id="input_robot1_finger1" min="0" max="100" value="0"/>
165 |             <button onclick="input_robot_finger_click(1,1)">set</button>
166 |         </div>
167 | 
168 |         <div class="par_row">
169 |             <label class="input_label" for="input_robot1_finger2">Finger 2:</label>
170 |             <input class="input_box" type="number" id="input_robot1_finger2" min="0" max="100" value="0"/>
171 |             <button onclick="input_robot_finger_click(1,2)">set</button>
172 |         </div>
173 | 
174 |     </div>
175 | 
176 |     <script>
177 |         const joints_default_pos = [90.0, -120.0, 90.0, -60.0, -90.0, 0.0];
178 |         for (let robot = 0; robot < 2; robot++) {
179 |             for (let joint = 0; joint < 6; joint++) {
180 |                 const element_id = `input_robot${robot}_joint${joint + 1}`;
181 |                 document.getElementById(element_id).value = joints_default_pos[joint];
182 |             }
183 |         }
184 |     </script>
185 | 
186 | </div>
187 | 
188 | </body>
189 | </html>


--------------------------------------------------------------------------------
/media/demo-web.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/sanri/demo-bevy_robot/018420c83cc85f40344324e82d4cd640a5528fb9/media/demo-web.png


--------------------------------------------------------------------------------
/media/demo.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/sanri/demo-bevy_robot/018420c83cc85f40344324e82d4cd640a5528fb9/media/demo.png


--------------------------------------------------------------------------------
/rust-toolchain.toml:
--------------------------------------------------------------------------------
1 | [toolchain]
2 | channel = "1.87.0"
3 | 


--------------------------------------------------------------------------------
/src/draw_trail.rs:
--------------------------------------------------------------------------------
 1 | use bevy::prelude::*;
 2 | use std::collections::{BTreeMap, VecDeque};
 3 | 
 4 | pub struct DrawTrailPlugin;
 5 | 
 6 | impl Plugin for DrawTrailPlugin {
 7 |     fn build(&self, app: &mut App) {
 8 |         app.insert_resource(Trails::default())
 9 |             .add_systems(Update, Trails::draw_trails);
10 |     }
11 | }
12 | 
13 | struct Trail {
14 |     duration: f32,
15 |     color: Color,
16 |     data: VecDeque<(f32, Vec3)>, // ( time, point )
17 | }
18 | 
19 | #[derive(Resource)]
20 | pub struct Trails {
21 |     map: BTreeMap<u64, Trail>,
22 | }
23 | 
24 | impl Trails {
25 |     pub fn add_point(&mut self, id: u64, time: f32, point: Vec3, duration: f32, color: Color) {
26 |         let trail = self.map.entry(id).or_insert(Trail {
27 |             duration,
28 |             color,
29 |             data: VecDeque::new(),
30 |         });
31 | 
32 |         trail.duration = duration.abs();
33 |         trail.color = color;
34 |         trail.data.push_back((time.abs(), point));
35 |     }
36 | 
37 |     fn draw_trails(mut gizmos: Gizmos, mut trails: ResMut<Trails>, time: Res<Time>) {
38 |         let time = time.elapsed_secs();
39 |         for trail in trails.map.values_mut() {
40 |             // remove timeout point
41 |             'la: while let Some((t, _)) = trail.data.front() {
42 |                 if (time - *t) > trail.duration {
43 |                     trail.data.pop_front();
44 |                 } else {
45 |                     break 'la;
46 |                 }
47 |             }
48 | 
49 |             // draw line
50 |             let mut point_old: Option<Vec3> = None;
51 |             for (_, point_new) in trail.data.iter() {
52 |                 if let Some(point_old) = point_old {
53 |                     gizmos.line(point_old, point_new.clone(), trail.color);
54 |                 }
55 |                 point_old = Some(point_new.clone());
56 |             }
57 |         }
58 |     }
59 | }
60 | 
61 | impl Default for Trails {
62 |     fn default() -> Self {
63 |         Trails {
64 |             map: BTreeMap::new(),
65 |         }
66 |     }
67 | }
68 | 


--------------------------------------------------------------------------------
/src/gripper_ctm2f110.rs:
--------------------------------------------------------------------------------
  1 | use bevy::prelude::*;
  2 | use std::f32::consts::{FRAC_PI_2, PI};
  3 | 
  4 | const ASSET: &'static str = "ctm2f110/ctm2f110.gltf";
  5 | 
  6 | const DRIVING_POS: [f32; 2] = [24.0 / 1000.0, 80.75 / 1000.0]; // [ x, z ] mm
  7 | const DRIVING_LENGTH: f32 = 55.0 / 1000.0; // mm
  8 | const DRIVING_ANGLE: [f32; 2] = [30.0 / 180.0 * PI, 103.5 / 180.0 * PI]; // rad
  9 | const FOLLOWER_POS: [f32; 2] = [57.0 / 1000.0, 68.75 / 1000.0]; // [ x, z ] mm
 10 | const FINGERTIP_POS: [f32; 2] = [-11.0 / 1000.0, 50.5 / 1000.0]; // mm
 11 | 
 12 | #[derive(Component)]
 13 | pub enum GripperComponent {
 14 |     Main,
 15 |     Driving1,
 16 |     Driving2,
 17 |     Follower1,
 18 |     Follower2,
 19 |     Finger1,
 20 |     Finger2,
 21 |     Fingertip1,
 22 |     Fingertip2,
 23 | }
 24 | 
 25 | #[derive(Eq, PartialEq)]
 26 | pub enum Finger {
 27 |     One,
 28 |     Two,
 29 | }
 30 | 
 31 | #[derive(Component)]
 32 | pub struct GripperFingertip {
 33 |     pub id: u64,
 34 |     pub finger: Finger,
 35 | }
 36 | 
 37 | #[derive(Component)]
 38 | pub struct GripperCtm2f110 {
 39 |     pub id: u64,
 40 |     pub pos1: f32, // range [ 0.0, 1.0 ]
 41 |     pub pos2: f32, // range [ 0.0, 1.0 ]
 42 | }
 43 | 
 44 | #[derive(Resource, Clone)]
 45 | struct GripperPluginRes {
 46 |     main: Handle<Scene>,
 47 |     driving: Handle<Scene>,
 48 |     follower: Handle<Scene>,
 49 |     finger: Handle<Scene>,
 50 | }
 51 | 
 52 | impl FromWorld for GripperPluginRes {
 53 |     fn from_world(world: &mut World) -> Self {
 54 |         let asset_server = world.get_resource::<AssetServer>().unwrap();
 55 |         let main = asset_server.load(GltfAssetLabel::Scene(0).from_asset(ASSET));
 56 |         let driving = asset_server.load(GltfAssetLabel::Scene(1).from_asset(ASSET));
 57 |         let follower = asset_server.load(GltfAssetLabel::Scene(2).from_asset(ASSET));
 58 |         let finger = asset_server.load(GltfAssetLabel::Scene(3).from_asset(ASSET));
 59 |         GripperPluginRes {
 60 |             main,
 61 |             driving,
 62 |             follower,
 63 |             finger,
 64 |         }
 65 |     }
 66 | }
 67 | 
 68 | pub struct GripperPlugin;
 69 | 
 70 | impl GripperPlugin {
 71 |     // (main, fingertip1, fingertip2)
 72 |     pub fn add_gripper(
 73 |         world: &mut World,
 74 |         id: u64,
 75 |         tf: Option<Transform>,
 76 |         finger_pos: Option<[f32; 2]>,
 77 |     ) -> (Entity, Entity, Entity) {
 78 |         let tf = tf.unwrap_or(Transform::default());
 79 |         let [pos1, pos2] = finger_pos.unwrap_or([1.0, 1.0]);
 80 |         let gripper = GripperCtm2f110 { id, pos1, pos2 };
 81 |         let [driving1, follower1, finger1, fingertip1] = compute_finger1(pos1);
 82 |         let [driving2, follower2, finger2, fingertip2] = compute_finger2(pos2);
 83 |         let parent = world.spawn((gripper, tf)).id();
 84 | 
 85 |         let res: GripperPluginRes = world.get_resource::<GripperPluginRes>().unwrap().clone();
 86 | 
 87 |         let child_main = world
 88 |             .spawn((SceneRoot(res.main.clone()), GripperComponent::Main))
 89 |             .id();
 90 | 
 91 |         let child_driving1 = world
 92 |             .spawn((
 93 |                 SceneRoot(res.driving.clone()),
 94 |                 driving1,
 95 |                 GripperComponent::Driving1,
 96 |             ))
 97 |             .id();
 98 |         let child_driving2 = world
 99 |             .spawn((
100 |                 SceneRoot(res.driving.clone()),
101 |                 driving2,
102 |                 GripperComponent::Driving2,
103 |             ))
104 |             .id();
105 | 
106 |         let child_follower1 = world
107 |             .spawn((
108 |                 SceneRoot(res.follower.clone()),
109 |                 follower1,
110 |                 GripperComponent::Follower1,
111 |             ))
112 |             .id();
113 |         let child_follower2 = world
114 |             .spawn((
115 |                 SceneRoot(res.follower.clone()),
116 |                 follower2,
117 |                 GripperComponent::Follower2,
118 |             ))
119 |             .id();
120 | 
121 |         let child_finger1 = world
122 |             .spawn((
123 |                 SceneRoot(res.finger.clone()),
124 |                 finger1,
125 |                 GripperComponent::Finger1,
126 |             ))
127 |             .id();
128 |         let child_finger2 = world
129 |             .spawn((
130 |                 SceneRoot(res.finger.clone()),
131 |                 finger2,
132 |                 GripperComponent::Finger2,
133 |             ))
134 |             .id();
135 |         let child_fingertip1 = world
136 |             .spawn((
137 |                 fingertip1,
138 |                 GripperComponent::Fingertip1,
139 |                 GripperFingertip {
140 |                     id,
141 |                     finger: Finger::One,
142 |                 },
143 |             ))
144 |             .id();
145 |         let child_fingertip2 = world
146 |             .spawn((
147 |                 fingertip2,
148 |                 GripperComponent::Fingertip2,
149 |                 GripperFingertip {
150 |                     id,
151 |                     finger: Finger::Two,
152 |                 },
153 |             ))
154 |             .id();
155 |         let children = [
156 |             child_main,
157 |             child_driving1,
158 |             child_driving2,
159 |             child_follower1,
160 |             child_follower2,
161 |             child_finger1,
162 |             child_finger2,
163 |             child_fingertip1,
164 |             child_fingertip2,
165 |         ];
166 |         world.entity_mut(parent).add_children(&children);
167 |         (parent, child_fingertip1, child_fingertip2)
168 |     }
169 | 
170 |     fn update_component_pos(
171 |         q_parent: Query<(&GripperCtm2f110, &Children), Changed<GripperCtm2f110>>,
172 |         mut q_child: Query<(&GripperComponent, &mut Transform)>,
173 |     ) {
174 |         for (gripper, children) in &q_parent {
175 |             let [driving1, follower1, finger1, fingertip1] = compute_finger1(gripper.pos1);
176 |             let [driving2, follower2, finger2, fingertip2] = compute_finger2(gripper.pos2);
177 |             for child in children {
178 |                 if let Ok((gc, mut tf)) = q_child.get_mut(*child) {
179 |                     match gc {
180 |                         GripperComponent::Main => {}
181 |                         GripperComponent::Driving1 => {
182 |                             *tf = driving1;
183 |                         }
184 |                         GripperComponent::Driving2 => {
185 |                             *tf = driving2;
186 |                         }
187 |                         GripperComponent::Follower1 => {
188 |                             *tf = follower1;
189 |                         }
190 |                         GripperComponent::Follower2 => {
191 |                             *tf = follower2;
192 |                         }
193 |                         GripperComponent::Finger1 => {
194 |                             *tf = finger1;
195 |                         }
196 |                         GripperComponent::Finger2 => {
197 |                             *tf = finger2;
198 |                         }
199 |                         GripperComponent::Fingertip1 => {
200 |                             *tf = fingertip1;
201 |                         }
202 |                         GripperComponent::Fingertip2 => {
203 |                             *tf = fingertip2;
204 |                         }
205 |                     }
206 |                 }
207 |             }
208 |         }
209 |     }
210 | }
211 | 
212 | impl Plugin for GripperPlugin {
213 |     fn build(&self, app: &mut App) {
214 |         app.init_resource::<GripperPluginRes>()
215 |             .add_systems(Update, GripperPlugin::update_component_pos);
216 |     }
217 | }
218 | 
219 | // In: pos range [0.0, 1.0]
220 | // Out: [driving,follower,finger,fingertip]
221 | fn compute_finger2(pos: f32) -> [Transform; 4] {
222 |     let pos = (1.0 - pos).abs();
223 |     let pos = if pos > 1.0 { 1.0 } else { pos };
224 |     let angle = DRIVING_ANGLE[0] + (DRIVING_ANGLE[1] - DRIVING_ANGLE[0]) * pos;
225 | 
226 |     let driving_x = DRIVING_POS[0];
227 |     let driving_z = DRIVING_POS[1];
228 |     let mut driving = Transform::from_xyz(driving_x, 0.0, driving_z);
229 |     driving.rotate_local_x(FRAC_PI_2);
230 |     driving.rotate_local_z(angle);
231 | 
232 |     let mut follower = Transform::from_xyz(FOLLOWER_POS[0], 0.0, FOLLOWER_POS[1]);
233 |     follower.rotate_local_x(FRAC_PI_2);
234 |     follower.rotate_local_z(angle);
235 | 
236 |     let finger_x = driving_x + DRIVING_LENGTH * angle.cos();
237 |     let finger_z = driving_z + DRIVING_LENGTH * angle.sin();
238 |     let mut finger = Transform::from_xyz(finger_x, 0.0, finger_z);
239 |     finger.rotate_local_x(FRAC_PI_2);
240 |     finger.rotate_local_z(FRAC_PI_2);
241 | 
242 |     let fingertip_x = finger_x + FINGERTIP_POS[0];
243 |     let fingertip_z = finger_z + FINGERTIP_POS[1];
244 |     let mut fingertip = Transform::from_xyz(fingertip_x, 0.0, fingertip_z);
245 |     fingertip.rotate_local_x(FRAC_PI_2);
246 |     fingertip.rotate_local_z(FRAC_PI_2);
247 | 
248 |     [driving, follower, finger, fingertip]
249 | }
250 | 
251 | // In: pos range [0.0, 1.0]
252 | // Out: [driving,follower,finger,fingertip]
253 | fn compute_finger1(pos: f32) -> [Transform; 4] {
254 |     let mut tfs = compute_finger2(pos);
255 |     for tf in &mut tfs {
256 |         tf.translation.x = -tf.translation.x;
257 |         tf.rotate_z(PI);
258 |     }
259 |     tfs
260 | }
261 | 


--------------------------------------------------------------------------------
/src/main.rs:
--------------------------------------------------------------------------------
  1 | mod draw_trail;
  2 | mod gripper_ctm2f110;
  3 | mod robot_ur5;
  4 | 
  5 | use bevy::prelude::*;
  6 | use bevy_egui::{egui, EguiContextPass, EguiContexts, EguiPlugin};
  7 | use bevy_panorbit_camera::{PanOrbitCamera, PanOrbitCameraPlugin};
  8 | use flume::{unbounded, Receiver, Sender};
  9 | #[cfg(target_family = "wasm")]
 10 | use serde::{Deserialize, Serialize};
 11 | #[cfg(target_family = "wasm")]
 12 | use wasm_bindgen::prelude::wasm_bindgen;
 13 | 
 14 | use crate::{
 15 |     draw_trail::{DrawTrailPlugin, Trails},
 16 |     gripper_ctm2f110::{Finger, GripperCtm2f110, GripperFingertip, GripperPlugin},
 17 |     robot_ur5::{RobotPluginUr5, RobotUr5, JOINTS_POS},
 18 | };
 19 | 
 20 | static mut SENDER: Option<Sender<Cmd>> = None;
 21 | 
 22 | #[cfg(target_family = "wasm")]
 23 | #[derive(Serialize, Deserialize)]
 24 | pub struct EventJointChanged {
 25 |     robot: u16,
 26 |     joint: u16,
 27 |     angle: f32,
 28 | }
 29 | 
 30 | #[cfg(target_family = "wasm")]
 31 | #[derive(Serialize, Deserialize)]
 32 | pub struct EventFingerChanged {
 33 |     robot: u16,
 34 |     finger: u16,
 35 |     pos: f32,
 36 | }
 37 | 
 38 | #[cfg(target_family = "wasm")]
 39 | #[wasm_bindgen]
 40 | pub fn set_robot_joint_pos(robot: u16, joint: u16, angle: f32) {
 41 |     unsafe {
 42 |         if let Some(sender) = &SENDER {
 43 |             let cmd = Cmd::RobotJointPos {
 44 |                 robot,
 45 |                 joint,
 46 |                 angle,
 47 |             };
 48 |             let _ = sender.send(cmd);
 49 |         }
 50 |     }
 51 | }
 52 | 
 53 | #[cfg(target_family = "wasm")]
 54 | #[wasm_bindgen]
 55 | pub fn set_robot_finger_pos(robot: u16, finger: u16, pos: f32) {
 56 |     unsafe {
 57 |         if let Some(sender) = &SENDER {
 58 |             let cmd = Cmd::RobotFingerPos { robot, finger, pos };
 59 |             let _ = sender.send(cmd);
 60 |         }
 61 |     }
 62 | }
 63 | 
 64 | const ROBOT_KEY_0: u64 = 0;
 65 | const ROBOT_KEY_1: u64 = 1;
 66 | const DARK_GRAY: Color = Color::linear_rgb(0.5, 0.5, 0.5);
 67 | const GREEN: Color = Color::linear_rgb(0.0, 1.0, 0.0);
 68 | 
 69 | fn main() {
 70 |     let (sender, receiver) = unbounded::<Cmd>();
 71 |     unsafe {
 72 |         SENDER = Some(sender);
 73 |     }
 74 |     let cmd_channel = CmdChannel { receiver };
 75 | 
 76 |     App::new()
 77 |         .init_resource::<JointsPos>()
 78 |         .init_resource::<FingerPos>()
 79 |         .insert_resource(cmd_channel)
 80 |         .add_plugins((DefaultPlugins.set(WindowPlugin {
 81 |             primary_window: Some(Window {
 82 |                 fit_canvas_to_parent: true,
 83 |                 canvas: Some("#demo-bevy_robot".to_string()),
 84 |                 ..default()
 85 |             }),
 86 |             ..default()
 87 |         }),))
 88 |         .add_plugins((
 89 |             PanOrbitCameraPlugin,
 90 |             RobotPluginUr5,
 91 |             GripperPlugin,
 92 |             DrawTrailPlugin,
 93 |         ))
 94 |         .add_plugins(EguiPlugin {
 95 |             enable_multipass_for_primary_context: true,
 96 |         })
 97 |         .add_systems(Startup, (setup_camera_light, setup_robot, setup_label))
 98 |         .add_systems(
 99 |             Update,
100 |             (
101 |                 update_joints_pos,
102 |                 update_finger_pos,
103 |                 draw_floor_grids,
104 |                 draw_gripper_trails,
105 |                 update_label_pos,
106 |                 recv_cmd,
107 |             ),
108 |         )
109 |         .add_systems(EguiContextPass, ui)
110 |         .run();
111 | }
112 | 
113 | #[derive(Clone)]
114 | enum Cmd {
115 |     RobotJointPos { robot: u16, joint: u16, angle: f32 },
116 |     RobotFingerPos { robot: u16, finger: u16, pos: f32 },
117 | }
118 | 
119 | #[derive(Resource)]
120 | struct CmdChannel {
121 |     receiver: Receiver<Cmd>,
122 | }
123 | 
124 | #[derive(Resource, Clone)]
125 | struct JointsPos([[f64; 6]; 2]); // deg
126 | 
127 | impl Default for JointsPos {
128 |     fn default() -> Self {
129 |         JointsPos([JOINTS_POS; 2])
130 |     }
131 | }
132 | 
133 | fn recv_cmd(
134 |     cmd_channel: Res<CmdChannel>,
135 |     mut joints_pos: ResMut<JointsPos>,
136 |     mut finger_pos: ResMut<FingerPos>,
137 | ) {
138 |     while let Ok(cmd) = cmd_channel.receiver.try_recv() {
139 |         match cmd {
140 |             Cmd::RobotJointPos {
141 |                 robot,
142 |                 joint,
143 |                 angle,
144 |             } => {
145 |                 let robot: usize = if (robot as u64) == ROBOT_KEY_0 { 0 } else { 1 };
146 |                 let joint: usize = match joint {
147 |                     1 => 0,
148 |                     2 => 1,
149 |                     3 => 2,
150 |                     4 => 3,
151 |                     5 => 4,
152 |                     _ => 5,
153 |                 };
154 |                 let angle = if angle > 720.0 {
155 |                     720.0
156 |                 } else if angle < -720.0 {
157 |                     -720.0
158 |                 } else {
159 |                     angle
160 |                 };
161 |                 joints_pos.0[robot][joint] = angle as f64;
162 |             }
163 |             Cmd::RobotFingerPos { robot, finger, pos } => {
164 |                 let robot: usize = if (robot as u64) == ROBOT_KEY_0 { 0 } else { 1 };
165 |                 let finger: usize = match finger {
166 |                     1 => 0,
167 |                     _ => 1,
168 |                 };
169 |                 let pos = if pos > 100.0 {
170 |                     100.0
171 |                 } else if pos < 0.0 {
172 |                     0.0
173 |                 } else {
174 |                     pos
175 |                 };
176 |                 finger_pos.0[robot][finger] = pos;
177 |             }
178 |         }
179 |     }
180 | }
181 | 
182 | fn update_joints_pos(
183 |     joints: Res<JointsPos>,
184 |     mut query: Query<&mut RobotUr5>,
185 |     mut now_joints: Local<JointsPos>,
186 |     mut last_joints: Local<JointsPos>,
187 | ) {
188 |     #[cfg(target_family = "wasm")]
189 |     {
190 |         use std::ops::Deref;
191 |         use web_sys::CustomEvent;
192 |         for robot in 0..=1 {
193 |             for joint in 0..6 {
194 |                 if last_joints.0[robot][joint] != joints.0[robot][joint] {
195 |                     let custom_event = CustomEvent::new("").unwrap();
196 |                     let data = EventJointChanged {
197 |                         robot: robot as u16,
198 |                         joint: (joint + 1) as u16,
199 |                         angle: joints.0[robot][joint] as f32,
200 |                     };
201 |                     let js_value = serde_wasm_bindgen::to_value(&data).unwrap();
202 |                     custom_event.init_custom_event_with_can_bubble_and_cancelable_and_detail(
203 |                         "joint_changed",
204 |                         true,
205 |                         true,
206 |                         &js_value,
207 |                     );
208 |                     let window = web_sys::window().unwrap();
209 |                     let _ = window.dispatch_event(custom_event.deref());
210 |                 }
211 |             }
212 |         }
213 |     }
214 | 
215 |     for mut robot in query.iter_mut() {
216 |         let id = robot.id as usize;
217 |         let target = joints.0[id];
218 |         let now = now_joints.0[id];
219 |         let pos = ct_robot_joints(&now, &target);
220 |         now_joints.0[id] = pos;
221 |         robot.set_deg(pos)
222 |     }
223 | 
224 |     last_joints.0 = joints.0;
225 | }
226 | 
227 | #[derive(Resource, Clone, Default)]
228 | struct FingerPos([[f32; 2]; 2]); // range [0.0, 100.0]
229 | 
230 | fn update_finger_pos(
231 |     fingers: Res<FingerPos>,
232 |     mut query: Query<&mut GripperCtm2f110>,
233 |     mut now_fingers: Local<FingerPos>,
234 |     mut last_fingers: Local<FingerPos>,
235 | ) {
236 |     #[cfg(target_family = "wasm")]
237 |     {
238 |         use std::ops::Deref;
239 |         use web_sys::CustomEvent;
240 |         for robot in 0..=1 {
241 |             for finger in 0..=1 {
242 |                 if last_fingers.0[robot][finger] != fingers.0[robot][finger] {
243 |                     let custom_event = CustomEvent::new("").unwrap();
244 |                     let data = EventFingerChanged {
245 |                         robot: robot as u16,
246 |                         finger: (finger + 1) as u16,
247 |                         pos: fingers.0[robot][finger] as f32,
248 |                     };
249 |                     let js_value = serde_wasm_bindgen::to_value(&data).unwrap();
250 |                     custom_event.init_custom_event_with_can_bubble_and_cancelable_and_detail(
251 |                         "finger_changed",
252 |                         true,
253 |                         true,
254 |                         &js_value,
255 |                     );
256 |                     let window = web_sys::window().unwrap();
257 |                     let _ = window.dispatch_event(custom_event.deref());
258 |                 }
259 |             }
260 |         }
261 |     }
262 |     for mut gripper in query.iter_mut() {
263 |         let id = gripper.id as usize;
264 |         let target = fingers.0[id];
265 |         let now = now_fingers.0[id];
266 |         let pos = ct_gripper_finger(&now, &target);
267 |         now_fingers.0[id] = pos;
268 |         gripper.pos1 = pos[0] / 100.0;
269 |         gripper.pos2 = pos[1] / 100.0;
270 |     }
271 | 
272 |     last_fingers.0 = fingers.0;
273 | }
274 | 
275 | fn draw_floor_grids(mut gizmos: Gizmos) {
276 |     for i in 0..11 {
277 |         let z = -0.5 + (i as f32) * 0.1;
278 |         gizmos.line(Vec3::new(-1.0, 0.0, z), Vec3::new(1.0, 0.0, z), DARK_GRAY);
279 |     }
280 |     for i in 0..21 {
281 |         let x = -1.0 + (i as f32) * 0.1;
282 |         gizmos.line(Vec3::new(x, 0.0, -0.5), Vec3::new(x, 0.0, 0.5), DARK_GRAY);
283 |     }
284 | }
285 | 
286 | fn draw_gripper_trails(
287 |     time: Res<Time>,
288 |     mut trails: ResMut<Trails>,
289 |     query_gripper_finger: Query<(&GripperFingertip, &GlobalTransform), Changed<GlobalTransform>>,
290 | ) {
291 |     let time = time.elapsed_secs();
292 |     for (fingertip, global_transform) in query_gripper_finger.iter() {
293 |         if fingertip.finger == Finger::Two {
294 |             continue;
295 |         }
296 |         let id = fingertip.id;
297 |         let point = global_transform.translation();
298 |         trails.add_point(id, time, point, 2.0, GREEN);
299 |     }
300 | }
301 | 
302 | fn ui(
303 |     mut contexts: EguiContexts,
304 |     mut joints: ResMut<JointsPos>,
305 |     mut finger_pos: ResMut<FingerPos>,
306 |     mut show_window: Local<[bool; 2]>,
307 | ) {
308 |     let ctx = contexts.ctx_mut();
309 | 
310 |     egui::TopBottomPanel::top("top_panel")
311 |         .resizable(true)
312 |         .show(ctx, |ui| {
313 |             ui.horizontal_centered(|ui| {
314 |                 if ui.selectable_label(!show_window[0], "Robot0").clicked() {
315 |                     show_window[0] = !(show_window[0]);
316 |                 }
317 | 
318 |                 if ui.selectable_label(!show_window[1], "Robot1").clicked() {
319 |                     show_window[1] = !(show_window[1]);
320 |                 }
321 |             });
322 |         });
323 | 
324 |     for i in 0..2 {
325 |         if !show_window[i] {
326 |             egui::Window::new(format!("Robot{}", i)).show(ctx, |ui| {
327 |                 if ui.button("reset").clicked() {
328 |                     joints.0[i] = JOINTS_POS;
329 |                     finger_pos.0[i] = [0.0, 0.0];
330 |                 }
331 | 
332 |                 egui::Grid::new("robot_axis").num_columns(2).show(ui, |ui| {
333 |                     ui.label("Axis1");
334 |                     ui.add(egui::Slider::new(&mut joints.0[i][0], -360.0..=360.0).suffix("°"));
335 |                     ui.end_row();
336 | 
337 |                     ui.label("Axis2");
338 |                     ui.add(egui::Slider::new(&mut joints.0[i][1], -360.0..=360.0).suffix("°"));
339 |                     ui.end_row();
340 | 
341 |                     ui.label("Axis3");
342 |                     ui.add(egui::Slider::new(&mut joints.0[i][2], -360.0..=360.0).suffix("°"));
343 |                     ui.end_row();
344 | 
345 |                     ui.label("Axis4");
346 |                     ui.add(egui::Slider::new(&mut joints.0[i][3], -360.0..=360.0).suffix("°"));
347 |                     ui.end_row();
348 | 
349 |                     ui.label("Axis5");
350 |                     ui.add(egui::Slider::new(&mut joints.0[i][4], -360.0..=360.0).suffix("°"));
351 |                     ui.end_row();
352 | 
353 |                     ui.label("Axis6");
354 |                     ui.add(egui::Slider::new(&mut joints.0[i][5], -360.0..=360.0).suffix("°"));
355 |                     ui.end_row();
356 | 
357 |                     ui.label("Finger1");
358 |                     ui.add(egui::Slider::new(&mut finger_pos.0[i][0], 0.0..=100.0).suffix("%"));
359 |                     ui.end_row();
360 | 
361 |                     ui.label("Finger2");
362 |                     ui.add(egui::Slider::new(&mut finger_pos.0[i][1], 0.0..=100.0).suffix("%"));
363 |                     ui.end_row();
364 |                 });
365 |             });
366 |         }
367 |     }
368 | }
369 | 
370 | fn setup_camera_light(mut commands: Commands) {
371 |     commands.spawn((
372 |         Transform::from_translation(Vec3::new(0.0, 1.5, 3.0)),
373 |         PanOrbitCamera {
374 |             button_orbit: MouseButton::Left,
375 |             button_pan: MouseButton::Right,
376 |             // modifier_orbit: Some(KeyCode::LShift),
377 |             ..default()
378 |         },
379 |     ));
380 | 
381 |     commands.spawn(DirectionalLight {
382 |         shadows_enabled: false,
383 |         illuminance: 10000.0,
384 |         ..default()
385 |     });
386 | 
387 |     let mut tf = Transform::default();
388 |     tf.rotate_local_x(-std::f32::consts::FRAC_PI_2);
389 |     commands.spawn((
390 |         tf,
391 |         DirectionalLight {
392 |             shadows_enabled: false,
393 |             illuminance: 10000.0,
394 |             ..default()
395 |         },
396 |     ));
397 | }
398 | 
399 | fn setup_robot(mut commands: Commands) {
400 |     commands.queue(|world: &mut World| {
401 |         let mut tf = Transform::from_xyz(-0.5, 0.0, 0.0);
402 |         tf.rotate_x(-std::f32::consts::FRAC_PI_2);
403 |         let (_, wrist) = RobotPluginUr5::add_robot(world, ROBOT_KEY_0, Some(tf), None);
404 |         let (gripper, _, _) =
405 |             GripperPlugin::add_gripper(world, ROBOT_KEY_0, None, Some([0.0, 0.0]));
406 |         world.entity_mut(wrist).add_children(&[gripper]);
407 |         world.entity_mut(wrist);
408 |     });
409 | 
410 |     commands.queue(|world: &mut World| {
411 |         let mut tf = Transform::from_xyz(0.5, 0.0, 0.0);
412 |         tf.rotate_x(-std::f32::consts::FRAC_PI_2);
413 |         let (_, wrist) = RobotPluginUr5::add_robot(world, ROBOT_KEY_1, Some(tf), None);
414 |         let (gripper, _, _) =
415 |             GripperPlugin::add_gripper(world, ROBOT_KEY_1, None, Some([0.0, 0.0]));
416 |         world.entity_mut(wrist).add_children(&[gripper]);
417 |     });
418 | }
419 | 
420 | #[derive(Component)]
421 | struct Label(u64);
422 | 
423 | fn setup_label(mut commands: Commands) {
424 |     commands.spawn((
425 |         Text::new("Robot0"),
426 |         TextFont {
427 |             font_size: 24.0,
428 |             ..default()
429 |         },
430 |         Node {
431 |             position_type: PositionType::Absolute,
432 |             ..default()
433 |         },
434 |         Label(ROBOT_KEY_0),
435 |     ));
436 | 
437 |     commands.spawn((
438 |         Text::new("Robot1"),
439 |         TextFont {
440 |             font_size: 24.0,
441 |             ..default()
442 |         },
443 |         Node {
444 |             position_type: PositionType::Absolute,
445 |             ..default()
446 |         },
447 |         Label(ROBOT_KEY_1),
448 |     ));
449 | }
450 | 
451 | fn update_label_pos(
452 |     q_robot: Query<(&RobotUr5, &GlobalTransform)>,
453 |     q_camera: Query<(&Camera, &GlobalTransform)>,
454 |     mut q_label: Query<(&Label, &mut Node)>,
455 | ) {
456 |     if let Ok((camera, camera_gt)) = q_camera.get_single() {
457 |         for (robot, gt) in q_robot.iter() {
458 |             for (label, mut style) in q_label.iter_mut() {
459 |                 if robot.id == label.0 {
460 |                     if let Ok(pos) = camera.world_to_viewport(camera_gt, gt.translation()) {
461 |                         style.left = Val::Px(pos.x - 40.0);
462 |                         style.top = Val::Px(pos.y + 10.0);
463 |                     }
464 |                 }
465 |             }
466 |         }
467 |     }
468 | }
469 | 
470 | // now 当前值
471 | // target 目标值
472 | // k 变化系数
473 | // d_max 最大变化值
474 | // 返回下一帧的值
475 | fn compute_track(now: f64, target: f64, k: f64, d_max: f64) -> f64 {
476 |     let mut delta = (2.0 * k * (target - now)).abs().sqrt();
477 |     if delta > d_max.abs() {
478 |         delta = d_max.abs();
479 |     }
480 |     if target >= now {
481 |         let out = now + delta;
482 |         if out > target {
483 |             target
484 |         } else {
485 |             out
486 |         }
487 |     } else {
488 |         let out = now - delta;
489 |         if out < target {
490 |             target
491 |         } else {
492 |             out
493 |         }
494 |     }
495 | }
496 | 
497 | fn ct_robot_joints(now: &[f64; 6], target: &[f64; 6]) -> [f64; 6] {
498 |     let mut out = [0f64; 6];
499 |     for i in 0..6 {
500 |         out[i] = compute_track(now[i], target[i], 0.5, 16.0);
501 |     }
502 |     out
503 | }
504 | 
505 | fn ct_gripper_finger(now: &[f32; 2], target: &[f32; 2]) -> [f32; 2] {
506 |     let mut out = [0f32; 2];
507 |     for i in 0..2 {
508 |         out[i] = compute_track(now[i] as f64, target[i] as f64, 2.0, 5.0) as f32;
509 |     }
510 |     out
511 | }
512 | 


--------------------------------------------------------------------------------
/src/robot_ur5.rs:
--------------------------------------------------------------------------------
  1 | use bevy::prelude::*;
  2 | use nalgebra::{matrix, Matrix4};
  3 | use std::f64::consts::{FRAC_PI_2, PI};
  4 | 
  5 | const ASSET: &'static str = "ur5/ur5.gltf";
  6 | 
  7 | #[derive(Component)]
  8 | pub enum RobotComponent {
  9 |     Base,
 10 |     Arm1,
 11 |     Arm2,
 12 |     Arm3,
 13 |     Arm4,
 14 |     Arm5,
 15 |     Arm6,
 16 | }
 17 | 
 18 | pub const JOINTS_POS: [f64; 6] = [90.0, -120.0, 90.0, -60.0, -90.0, 0.0];
 19 | 
 20 | #[derive(Component)]
 21 | pub struct RobotUr5 {
 22 |     pub id: u64,
 23 |     joints: [f64; 6], // rad
 24 | }
 25 | 
 26 | #[derive(Component)]
 27 | pub struct RobotWrist(pub u64);
 28 | 
 29 | impl RobotUr5 {
 30 |     fn get_local_tfs(&self) -> [Transform; 7] {
 31 |         let m4s = compute_joint_to_base(self.joints);
 32 |         [
 33 |             Transform::default(),
 34 |             matrix4_to_tf(m4s[0]),
 35 |             matrix4_to_tf(m4s[1]),
 36 |             matrix4_to_tf(m4s[2]),
 37 |             matrix4_to_tf(m4s[3]),
 38 |             matrix4_to_tf(m4s[4]),
 39 |             matrix4_to_tf(m4s[5]),
 40 |         ]
 41 |     }
 42 | 
 43 |     fn default_joints() -> [f64; 6] {
 44 |         let mut out = [0.0; 6];
 45 |         for i in 0..6 {
 46 |             out[i] = d2r(JOINTS_POS[i]);
 47 |         }
 48 |         out
 49 |     }
 50 | 
 51 |     pub fn set_deg(&mut self, j: [f64; 6]) {
 52 |         for i in 0..6 {
 53 |             self.joints[i] = d2r(j[i]);
 54 |         }
 55 |     }
 56 | }
 57 | 
 58 | #[derive(Resource, Clone)]
 59 | struct RobotPluginResUr5 {
 60 |     base: Handle<Scene>,
 61 |     arm1: Handle<Scene>,
 62 |     arm2: Handle<Scene>,
 63 |     arm3: Handle<Scene>,
 64 |     arm4: Handle<Scene>,
 65 |     arm5: Handle<Scene>,
 66 |     arm6: Handle<Scene>,
 67 | }
 68 | 
 69 | impl FromWorld for RobotPluginResUr5 {
 70 |     fn from_world(world: &mut World) -> Self {
 71 |         let asset_server = world.get_resource::<AssetServer>().unwrap();
 72 |         let base = asset_server.load(GltfAssetLabel::Scene(0).from_asset(ASSET));
 73 |         let arm1 = asset_server.load(GltfAssetLabel::Scene(1).from_asset(ASSET));
 74 |         let arm2 = asset_server.load(GltfAssetLabel::Scene(2).from_asset(ASSET));
 75 |         let arm3 = asset_server.load(GltfAssetLabel::Scene(3).from_asset(ASSET));
 76 |         let arm4 = asset_server.load(GltfAssetLabel::Scene(4).from_asset(ASSET));
 77 |         let arm5 = asset_server.load(GltfAssetLabel::Scene(5).from_asset(ASSET));
 78 |         let arm6 = asset_server.load(GltfAssetLabel::Scene(6).from_asset(ASSET));
 79 |         RobotPluginResUr5 {
 80 |             base,
 81 |             arm1,
 82 |             arm2,
 83 |             arm3,
 84 |             arm4,
 85 |             arm5,
 86 |             arm6,
 87 |         }
 88 |     }
 89 | }
 90 | 
 91 | pub struct RobotPluginUr5;
 92 | 
 93 | impl RobotPluginUr5 {
 94 |     // (base,arm6)
 95 |     pub fn add_robot(
 96 |         world: &mut World,
 97 |         id: u64,
 98 |         tf: Option<Transform>,
 99 |         joints: Option<[f64; 6]>, // rad
100 |     ) -> (Entity, Entity) {
101 |         let tf = tf.unwrap_or_else(|| {
102 |             let mut t = Transform::default();
103 |             t.rotate_x(-std::f32::consts::FRAC_PI_2);
104 |             t
105 |         });
106 | 
107 |         let joints = joints.unwrap_or(RobotUr5::default_joints());
108 |         let robot = RobotUr5 { id, joints };
109 |         let component_tfs = robot.get_local_tfs();
110 |         let parent = world.spawn((robot, tf)).id();
111 | 
112 |         let res: RobotPluginResUr5 = world.get_resource::<RobotPluginResUr5>().unwrap().clone();
113 | 
114 |         let child_base = world
115 |             .spawn((
116 |                 SceneRoot(res.base.clone()),
117 |                 component_tfs[0],
118 |                 RobotComponent::Base,
119 |             ))
120 |             .id();
121 | 
122 |         let child_arm1 = world
123 |             .spawn((
124 |                 SceneRoot(res.arm1.clone()),
125 |                 component_tfs[1],
126 |                 RobotComponent::Arm1,
127 |             ))
128 |             .id();
129 | 
130 |         let child_arm2 = world
131 |             .spawn((
132 |                 SceneRoot(res.arm2.clone()),
133 |                 component_tfs[2],
134 |                 RobotComponent::Arm2,
135 |             ))
136 |             .id();
137 | 
138 |         let child_arm3 = world
139 |             .spawn((
140 |                 SceneRoot(res.arm3.clone()),
141 |                 component_tfs[3],
142 |                 RobotComponent::Arm3,
143 |             ))
144 |             .id();
145 | 
146 |         let child_arm4 = world
147 |             .spawn((
148 |                 SceneRoot(res.arm4.clone()),
149 |                 component_tfs[4],
150 |                 RobotComponent::Arm4,
151 |             ))
152 |             .id();
153 | 
154 |         let child_arm5 = world
155 |             .spawn((
156 |                 SceneRoot(res.arm5.clone()),
157 |                 component_tfs[5],
158 |                 RobotComponent::Arm5,
159 |             ))
160 |             .id();
161 | 
162 |         let child_arm6 = world
163 |             .spawn((
164 |                 SceneRoot(res.arm6.clone()),
165 |                 component_tfs[6],
166 |                 RobotComponent::Arm6,
167 |                 RobotWrist(id),
168 |             ))
169 |             .id();
170 | 
171 |         let children = [
172 |             child_base, child_arm1, child_arm2, child_arm3, child_arm4, child_arm5, child_arm6,
173 |         ];
174 | 
175 |         world.entity_mut(parent).add_children(&children);
176 |         (parent, child_arm6)
177 |     }
178 | 
179 |     fn update_component_pos(
180 |         q_parent: Query<(&RobotUr5, &Children), Changed<RobotUr5>>,
181 |         mut q_child: Query<(&RobotComponent, &mut Transform)>,
182 |     ) {
183 |         for (ur5, children) in &q_parent {
184 |             let tfs = ur5.get_local_tfs();
185 |             for child in children {
186 |                 if let Ok((rc, mut tf)) = q_child.get_mut(*child) {
187 |                     match rc {
188 |                         RobotComponent::Base => {
189 |                             *tf = tfs[0];
190 |                         }
191 |                         RobotComponent::Arm1 => {
192 |                             *tf = tfs[1];
193 |                         }
194 |                         RobotComponent::Arm2 => {
195 |                             *tf = tfs[2];
196 |                         }
197 |                         RobotComponent::Arm3 => {
198 |                             *tf = tfs[3];
199 |                         }
200 |                         RobotComponent::Arm4 => {
201 |                             *tf = tfs[4];
202 |                         }
203 |                         RobotComponent::Arm5 => {
204 |                             *tf = tfs[5];
205 |                         }
206 |                         RobotComponent::Arm6 => {
207 |                             *tf = tfs[6];
208 |                         }
209 |                     }
210 |                 }
211 |             }
212 |         }
213 |     }
214 | }
215 | 
216 | impl Plugin for RobotPluginUr5 {
217 |     fn build(&self, app: &mut App) {
218 |         app.init_resource::<RobotPluginResUr5>()
219 |             .add_systems(Update, RobotPluginUr5::update_component_pos);
220 |     }
221 | }
222 | 
223 | fn t_(a: f64, alpha: f64, d: f64, theta: f64) -> Matrix4<f64> {
224 |     let i11 = theta.cos();
225 |     let i12 = -theta.sin();
226 |     let i13 = 0.0;
227 |     let i14 = a;
228 |     let i21 = theta.sin() * alpha.cos();
229 |     let i22 = theta.cos() * alpha.cos();
230 |     let i23 = -alpha.sin();
231 |     let i24 = -alpha.sin() * d;
232 |     let i31 = theta.sin() * alpha.sin();
233 |     let i32 = theta.cos() * alpha.sin();
234 |     let i33 = alpha.cos();
235 |     let i34 = alpha.cos() * d;
236 |     let i41 = 0.0;
237 |     let i42 = 0.0;
238 |     let i43 = 0.0;
239 |     let i44 = 1.0;
240 |     let out = matrix![
241 |         i11,i12,i13,i14;
242 |         i21,i22,i23,i24;
243 |         i31,i32,i33,i34;
244 |         i41,i42,i43,i44
245 |     ];
246 |     out
247 | }
248 | 
249 | struct RobotPar {
250 |     a: f64,
251 |     alpha: f64,
252 |     d: f64,
253 | }
254 | 
255 | const PARS: [RobotPar; 6] = [
256 |     // 0 - 1
257 |     RobotPar {
258 |         a: 0.0,
259 |         alpha: 0.0,
260 |         d: 89.2 / 1000.0,
261 |     },
262 |     // 1 - 2
263 |     RobotPar {
264 |         a: 0.0,
265 |         alpha: -FRAC_PI_2,
266 |         d: 134.2 / 1000.0,
267 |     },
268 |     // 2 - 3
269 |     RobotPar {
270 |         a: 425.0 / 1000.0,
271 |         alpha: PI,
272 |         d: 118.95 / 1000.0,
273 |     },
274 |     // 3 - 4
275 |     RobotPar {
276 |         a: 392.25 / 1000.0,
277 |         alpha: PI,
278 |         d: 94.75 / 1000.0,
279 |     },
280 |     // 4 - 5
281 |     RobotPar {
282 |         a: 0.0,
283 |         alpha: -FRAC_PI_2,
284 |         d: 94.75 / 1000.0,
285 |     },
286 |     // 5 - 6
287 |     RobotPar {
288 |         a: 0.0,
289 |         alpha: -FRAC_PI_2,
290 |         d: 81.5 / 1000.0,
291 |     },
292 | ];
293 | 
294 | fn compute_joint_to_base(joints: [f64; 6]) -> [Matrix4<f64>; 6] {
295 |     // revised data, align with ur5 robot
296 |     let joints_revise = [
297 |         joints[0] + PI,
298 |         joints[1],
299 |         -joints[2],
300 |         joints[3],
301 |         joints[4] + PI,
302 |         joints[5],
303 |     ];
304 |     let mut ts: [Matrix4<f64>; 6] = [Matrix4::zeros(); 6];
305 |     for i in 0..6 {
306 |         ts[i] = t_(PARS[i].a, PARS[i].alpha, PARS[i].d, joints_revise[i]);
307 |     }
308 |     let mut out = [Matrix4::zeros(); 6];
309 |     let mut buf = ts[0];
310 |     out[0] = buf;
311 |     for i in 1..6 {
312 |         buf = buf * ts[i];
313 |         out[i] = buf;
314 |     }
315 |     out
316 | }
317 | 
318 | fn matrix4_to_tf(m: Matrix4<f64>) -> Transform {
319 |     let x_axis = Vec4::new(m.m11 as f32, m.m21 as f32, m.m31 as f32, m.m41 as f32);
320 |     let y_axis = Vec4::new(m.m12 as f32, m.m22 as f32, m.m32 as f32, m.m42 as f32);
321 |     let z_axis = Vec4::new(m.m13 as f32, m.m23 as f32, m.m33 as f32, m.m43 as f32);
322 |     let w_axis = Vec4::new(m.m14 as f32, m.m24 as f32, m.m34 as f32, m.m44 as f32);
323 |     let mat = Mat4::from_cols(x_axis, y_axis, z_axis, w_axis);
324 |     Transform::from_matrix(mat)
325 | }
326 | 
327 | #[inline]
328 | fn d2r(ang: f64) -> f64 {
329 |     ang / 180.0 * PI
330 | }
331 | 


--------------------------------------------------------------------------------