1use cached_path::{Cache, Error as CacheError, Options, ProgressBar};
2use candle_core::{D, DType, Device, IndexOp, Module, Result, Shape, Tensor};
3use candle_nn::rnn::LSTMState;
4use candle_nn::{
5 Conv2d, Conv2dConfig, LSTM, LSTMConfig, LayerNorm, Linear, RNN, VarBuilder, conv2d, layer_norm,
6 linear,
7};
8use std::{
9 fs, io,
10 path::{Path, PathBuf},
11};
12
13pub const INPUT_HEIGHT: usize = 60;
14pub const INPUT_WIDTH: usize = 90;
15pub const DEPTH_SHAPE: [usize; 4] = [1, 1, INPUT_HEIGHT, INPUT_WIDTH];
16pub const DESIRED_VELOCITY_SHAPE: [usize; 2] = [1, 1];
17pub const ATTITUDE_SHAPE: [usize; 2] = [1, 4];
18pub const OUTPUT_SHAPE: [usize; 2] = [1, 3];
19pub const RECURRENT_SHAPE: [usize; 2] = [3, 128];
20
21const MODEL_NAME: &str = "vitfly-vitlstm-f32.safetensors";
22const MODEL_URL: &str = "https://cdn.copper-robotics.com/models/vitfly-vitlstm-f32.safetensors";
23const MODEL_CACHE_DIR: &str = ".download-cache";
24const MODEL_SIZE_BYTES: u64 = 14_264_732;
25
26fn model_root() -> PathBuf {
27 PathBuf::from(env!("CARGO_MANIFEST_DIR")).join("weights")
28}
29
30fn link_or_copy_cached_model(src: &Path, dst: &Path) -> io::Result<()> {
31 if fs::symlink_metadata(dst).is_ok() {
32 fs::remove_file(dst)?;
33 }
34
35 #[cfg(unix)]
36 {
37 match std::os::unix::fs::symlink(src, dst) {
38 Ok(()) => Ok(()),
39 Err(symlink_err) => fs::copy(src, dst).map(|_| ()).map_err(|copy_err| {
40 io::Error::new(
41 copy_err.kind(),
42 format!("failed to symlink ({symlink_err}) or copy ({copy_err})"),
43 )
44 }),
45 }
46 }
47
48 #[cfg(windows)]
49 {
50 match std::os::windows::fs::symlink_file(src, dst) {
51 Ok(()) => Ok(()),
52 Err(symlink_err) => fs::copy(src, dst).map(|_| ()).map_err(|copy_err| {
53 io::Error::new(
54 copy_err.kind(),
55 format!("failed to symlink ({symlink_err}) or copy ({copy_err})"),
56 )
57 }),
58 }
59 }
60
61 #[cfg(not(any(unix, windows)))]
62 {
63 fs::copy(src, dst).map(|_| ())
64 }
65}
66
67fn cached_model_path(
68 online_cache: &Cache,
69 offline_cache: &Cache,
70) -> std::result::Result<PathBuf, CacheError> {
71 match offline_cache.cached_path(MODEL_URL) {
72 Ok(path) if model_file_is_valid(&path) => Ok(path),
73 Ok(_) => {
74 eprintln!("{MODEL_NAME}: cached file is incomplete; downloading it again");
75 let path =
76 online_cache.cached_path_with_options(MODEL_URL, &Options::default().force())?;
77 validate_model_file(path)
78 }
79 Err(err) => {
80 if matches!(
81 err,
82 CacheError::NoCachedVersions(_) | CacheError::CacheCorrupted(_)
83 ) {
84 eprintln!("{MODEL_NAME}: cache miss; downloading from {MODEL_URL}");
85 let path = online_cache.cached_path(MODEL_URL)?;
86 validate_model_file(path)
87 } else {
88 Err(err)
89 }
90 }
91 }
92}
93
94fn model_file_is_valid(path: &Path) -> bool {
95 fs::metadata(path)
96 .map(|metadata| metadata.len() == MODEL_SIZE_BYTES)
97 .unwrap_or(false)
98}
99
100fn validate_model_file(path: PathBuf) -> std::result::Result<PathBuf, CacheError> {
101 if model_file_is_valid(&path) {
102 Ok(path)
103 } else {
104 Err(CacheError::CacheCorrupted(format!(
105 "{MODEL_NAME} must be {MODEL_SIZE_BYTES} bytes"
106 )))
107 }
108}
109
110fn prepare_model_weights() -> Result<PathBuf> {
111 let weights_root = model_root();
112 prepare_model_weights_in(&weights_root)
113}
114
115fn prepare_model_weights_in(weights_root: &Path) -> Result<PathBuf> {
116 fs::create_dir_all(weights_root).map_err(candle_core::Error::msg)?;
117 let setup_lock = fs::OpenOptions::new()
121 .read(true)
122 .write(true)
123 .create(true)
124 .truncate(false)
125 .open(weights_root.join(".model-setup.lock"))
126 .map_err(candle_core::Error::msg)?;
127 setup_lock.lock().map_err(candle_core::Error::msg)?;
128 let model_path = weights_root.join(MODEL_NAME);
129 if model_file_is_valid(&model_path) {
130 return Ok(model_path);
131 }
132
133 if fs::symlink_metadata(&model_path).is_ok() {
134 fs::remove_file(&model_path).map_err(candle_core::Error::msg)?;
135 }
136
137 let cache_root = weights_root.join(MODEL_CACHE_DIR);
138 let online_cache = Cache::builder()
139 .dir(cache_root.clone())
140 .progress_bar(Some(ProgressBar::Full))
141 .build()
142 .map_err(candle_core::Error::msg)?;
143 let offline_cache = Cache::builder()
144 .dir(cache_root)
145 .offline(true)
146 .progress_bar(None)
147 .build()
148 .map_err(candle_core::Error::msg)?;
149 let cached_path =
150 cached_model_path(&online_cache, &offline_cache).map_err(candle_core::Error::msg)?;
151 link_or_copy_cached_model(&cached_path, &model_path).map_err(candle_core::Error::msg)?;
152 Ok(model_path)
153}
154
155#[derive(Debug, Clone)]
156struct PatchMerge {
157 conv: Conv2d,
158 norm: LayerNorm,
159}
160
161impl PatchMerge {
162 fn load(
163 in_channels: usize,
164 out_channels: usize,
165 kernel_size: usize,
166 stride: usize,
167 padding: usize,
168 vb: VarBuilder<'_>,
169 ) -> Result<Self> {
170 let conv = conv2d(
171 in_channels,
172 out_channels,
173 kernel_size,
174 Conv2dConfig {
175 stride,
176 padding,
177 ..Default::default()
178 },
179 vb.pp("cn1"),
180 )?;
181 let norm = layer_norm(out_channels, 1e-5, vb.pp("layerNorm"))?;
182 Ok(Self { conv, norm })
183 }
184
185 fn forward(&self, input: &Tensor) -> Result<(Tensor, usize, usize)> {
186 let output = self.conv.forward(input)?;
187 let (_, _, height, width) = output.dims4()?;
188 let output = output.flatten_from(2)?.transpose(1, 2)?.contiguous()?;
189 let output = self.norm.forward(&output)?;
190 Ok((output, height, width))
191 }
192}
193
194#[derive(Debug, Clone)]
195struct EfficientSelfAttention {
196 reduction: Conv2d,
197 reduction_norm: LayerNorm,
198 key_value: Linear,
199 query: Linear,
200 output: Linear,
201 heads: usize,
202}
203
204impl EfficientSelfAttention {
205 fn load(
206 channels: usize,
207 reduction_ratio: usize,
208 heads: usize,
209 vb: VarBuilder<'_>,
210 ) -> Result<Self> {
211 let reduction = conv2d(
212 channels,
213 channels,
214 reduction_ratio,
215 Conv2dConfig {
216 stride: reduction_ratio,
217 ..Default::default()
218 },
219 vb.pp("cn1"),
220 )?;
221 let reduction_norm = layer_norm(channels, 1e-5, vb.pp("ln1"))?;
222 let key_value = linear(channels, channels * 2, vb.pp("keyValueExtractor"))?;
223 let query = linear(channels, channels, vb.pp("query"))?;
224 let output = linear(channels, channels, vb.pp("finalLayer"))?;
225 Ok(Self {
226 reduction,
227 reduction_norm,
228 key_value,
229 query,
230 output,
231 heads,
232 })
233 }
234
235 fn forward(&self, input: &Tensor, height: usize, width: usize) -> Result<Tensor> {
236 let (batch, tokens, channels) = input.dims3()?;
237 let reduced = input
238 .transpose(1, 2)?
239 .contiguous()?
240 .reshape((batch, channels, height, width))?;
241 let reduced = self.reduction.forward(&reduced)?;
242 let reduced = reduced.flatten_from(2)?.transpose(1, 2)?.contiguous()?;
243 let reduced = self.reduction_norm.forward(&reduced)?;
244
245 let key_value = self.key_value.forward(&reduced)?;
246 let reduced_tokens = key_value.dim(1)?;
247 let head_dim = channels / self.heads;
248 let key_value = key_value
249 .reshape((batch, reduced_tokens, 2, self.heads, head_dim))?
250 .permute((2, 0, 3, 1, 4))?
251 .contiguous()?;
252 let key = key_value.i(0)?;
253 let value = key_value.i(1)?;
254 let query = self
255 .query
256 .forward(input)?
257 .reshape((batch, tokens, self.heads, head_dim))?
258 .permute((0, 2, 1, 3))?
259 .contiguous()?;
260
261 let scores = (query.matmul(&key.transpose(2, 3)?)? / (head_dim as f64).sqrt())?;
262 let attention = candle_nn::ops::softmax(&scores, D::Minus1)?;
263 let output = attention
264 .matmul(&value)?
265 .transpose(1, 2)?
266 .contiguous()?
267 .reshape((batch, tokens, channels))?;
268 self.output.forward(&output)
269 }
270}
271
272#[derive(Debug, Clone)]
273struct MixFfn {
274 expand: Linear,
275 depthwise: Conv2d,
276 project: Linear,
277}
278
279impl MixFfn {
280 fn load(channels: usize, expansion_factor: usize, vb: VarBuilder<'_>) -> Result<Self> {
281 let expanded_channels = channels * expansion_factor;
282 let expand = linear(channels, expanded_channels, vb.pp("mlp1"))?;
283 let depthwise = conv2d(
286 expanded_channels,
287 expanded_channels,
288 3,
289 Conv2dConfig {
290 padding: 1,
291 groups: channels,
292 ..Default::default()
293 },
294 vb.pp("depthwise"),
295 )?;
296 let project = linear(expanded_channels, channels, vb.pp("mlp2"))?;
297 Ok(Self {
298 expand,
299 depthwise,
300 project,
301 })
302 }
303
304 fn forward(&self, input: &Tensor, height: usize, width: usize) -> Result<Tensor> {
305 let expanded = self.expand.forward(input)?;
306 let (batch, _, channels) = expanded.dims3()?;
307 let expanded = expanded
308 .transpose(1, 2)?
309 .contiguous()?
310 .reshape((batch, channels, height, width))?;
311 let expanded = self.depthwise.forward(&expanded)?.gelu_erf()?;
312 let expanded = expanded.flatten_from(2)?.transpose(1, 2)?.contiguous()?;
313 self.project.forward(&expanded)
314 }
315}
316
317#[derive(Debug, Clone)]
318struct EncoderStage {
319 patch_merge: PatchMerge,
320 attentions: Vec<EfficientSelfAttention>,
321 ffns: Vec<MixFfn>,
322 norms: Vec<LayerNorm>,
323}
324
325impl EncoderStage {
326 #[allow(clippy::too_many_arguments)]
327 fn load(
328 in_channels: usize,
329 out_channels: usize,
330 patch_size: usize,
331 stride: usize,
332 padding: usize,
333 layers: usize,
334 reduction_ratio: usize,
335 heads: usize,
336 expansion_factor: usize,
337 vb: VarBuilder<'_>,
338 ) -> Result<Self> {
339 let patch_merge = PatchMerge::load(
340 in_channels,
341 out_channels,
342 patch_size,
343 stride,
344 padding,
345 vb.pp("patchMerge"),
346 )?;
347 let mut attentions = Vec::with_capacity(layers);
348 let mut ffns = Vec::with_capacity(layers);
349 let mut norms = Vec::with_capacity(layers);
350 for index in 0..layers {
351 attentions.push(EfficientSelfAttention::load(
352 out_channels,
353 reduction_ratio,
354 heads,
355 vb.pp(format!("_attn.{index}")),
356 )?);
357 ffns.push(MixFfn::load(
358 out_channels,
359 expansion_factor,
360 vb.pp(format!("_ffn.{index}")),
361 )?);
362 norms.push(layer_norm(
363 out_channels,
364 1e-5,
365 vb.pp(format!("_lNorm.{index}")),
366 )?);
367 }
368 Ok(Self {
369 patch_merge,
370 attentions,
371 ffns,
372 norms,
373 })
374 }
375
376 fn forward(
377 &self,
378 input: &Tensor,
379 trace: &mut Option<&mut ForwardTrace>,
380 prefix: &str,
381 ) -> Result<Tensor> {
382 let (mut output, height, width) = self.patch_merge.forward(input)?;
383 record(trace, format!("{prefix}.patch"), &output);
384 for index in 0..self.attentions.len() {
385 let attention = self.attentions[index].forward(&output, height, width)?;
386 output = (&output + attention)?;
387 record(trace, format!("{prefix}.attention{index}"), &output);
388 let ffn = self.ffns[index].forward(&output, height, width)?;
389 output = (&output + ffn)?;
390 record(trace, format!("{prefix}.ffn{index}"), &output);
391 output = self.norms[index].forward(&output)?;
392 record(trace, format!("{prefix}.norm{index}"), &output);
393 }
394 let batch = output.dim(0)?;
395 let channels = output.dim(2)?;
396 let output = output
397 .reshape((batch, height, width, channels))?
398 .permute((0, 3, 1, 2))?
399 .contiguous()?;
400 record(trace, format!("{prefix}.output"), &output);
401 Ok(output)
402 }
403}
404
405#[derive(Debug, Clone)]
407pub struct VitFlyState {
408 layers: Vec<LSTMState>,
409}
410
411impl VitFlyState {
412 pub fn from_tensors(hidden: &Tensor, cell: &Tensor) -> Result<Self> {
414 validate_shape("hidden", hidden.shape(), &RECURRENT_SHAPE)?;
415 validate_shape("cell", cell.shape(), &RECURRENT_SHAPE)?;
416 let mut layers = Vec::with_capacity(RECURRENT_SHAPE[0]);
417 for index in 0..RECURRENT_SHAPE[0] {
418 let hidden = hidden.narrow(0, index, 1)?.contiguous()?;
419 let cell = cell.narrow(0, index, 1)?.contiguous()?;
420 layers.push(LSTMState::new(hidden, cell));
421 }
422 Ok(Self { layers })
423 }
424
425 pub fn hidden_tensor(&self) -> Result<Tensor> {
427 let layers = self
428 .layers
429 .iter()
430 .map(|state| state.h())
431 .collect::<Vec<_>>();
432 Tensor::cat(&layers, 0)
433 }
434
435 pub fn cell_tensor(&self) -> Result<Tensor> {
437 let layers = self
438 .layers
439 .iter()
440 .map(|state| state.c())
441 .collect::<Vec<_>>();
442 Tensor::cat(&layers, 0)
443 }
444
445 pub fn layer(&self, index: usize) -> Option<&LSTMState> {
446 self.layers.get(index)
447 }
448
449 pub fn len(&self) -> usize {
450 self.layers.len()
451 }
452
453 pub fn is_empty(&self) -> bool {
454 self.layers.is_empty()
455 }
456}
457
458#[derive(Debug, Default)]
460pub struct ForwardTrace {
461 tensors: Vec<(String, Tensor)>,
462}
463
464impl ForwardTrace {
465 pub fn get(&self, name: &str) -> Option<&Tensor> {
466 self.tensors
467 .iter()
468 .find_map(|(candidate, tensor)| (candidate == name).then_some(tensor))
469 }
470
471 pub fn iter(&self) -> impl Iterator<Item = (&str, &Tensor)> {
472 self.tensors
473 .iter()
474 .map(|(name, tensor)| (name.as_str(), tensor))
475 }
476}
477
478fn record(trace: &mut Option<&mut ForwardTrace>, name: String, tensor: &Tensor) {
479 if let Some(trace) = trace.as_deref_mut() {
480 trace.tensors.push((name, tensor.clone()));
481 }
482}
483
484#[derive(Debug, Clone)]
486pub struct VitFly {
487 encoder_stages: [EncoderStage; 2],
488 down_sample: Conv2d,
489 decoder: Linear,
490 lstm_layers: [LSTM; 3],
491 output: Linear,
492}
493
494impl VitFly {
495 pub fn load(device: &Device) -> Result<Self> {
500 let weights_path = prepare_model_weights()?;
501 let vb =
504 unsafe { VarBuilder::from_mmaped_safetensors(&[weights_path], DType::F32, device)? };
505 Self::load_from_var_builder(vb)
506 }
507
508 fn load_from_var_builder(vb: VarBuilder<'_>) -> Result<Self> {
509 let encoder0 = EncoderStage::load(1, 32, 7, 4, 3, 2, 8, 1, 8, vb.pp("encoder_blocks.0"))?;
510 let encoder1 = EncoderStage::load(32, 64, 3, 2, 1, 2, 4, 2, 8, vb.pp("encoder_blocks.1"))?;
511 let down_sample = conv2d(
512 48,
513 12,
514 3,
515 Conv2dConfig {
516 padding: 1,
517 ..Default::default()
518 },
519 vb.pp("down_sample"),
520 )?;
521 let decoder = linear(4608, 512, vb.pp("decoder"))?;
522 let load_lstm = |index, input_size| {
523 candle_nn::lstm(
524 input_size,
525 128,
526 LSTMConfig {
527 layer_idx: index,
528 ..Default::default()
529 },
530 vb.pp("lstm"),
531 )
532 };
533 let lstm_layers = [load_lstm(0, 517)?, load_lstm(1, 128)?, load_lstm(2, 128)?];
534 let output = linear(128, 3, vb.pp("nn_fc2"))?;
535 Ok(Self {
536 encoder_stages: [encoder0, encoder1],
537 down_sample,
538 decoder,
539 lstm_layers,
540 output,
541 })
542 }
543
544 pub fn zero_state(&self) -> Result<VitFlyState> {
546 let layers = self
547 .lstm_layers
548 .iter()
549 .map(|layer| layer.zero_state(1))
550 .collect::<Result<Vec<_>>>()?;
551 Ok(VitFlyState { layers })
552 }
553
554 pub fn forward(
556 &self,
557 depth: &Tensor,
558 desired_velocity: &Tensor,
559 attitude: &Tensor,
560 state: Option<&VitFlyState>,
561 ) -> Result<(Tensor, VitFlyState)> {
562 self.forward_impl(depth, desired_velocity, attitude, state, None)
563 }
564
565 pub fn forward_tensors(
568 &self,
569 depth: &Tensor,
570 desired_velocity: &Tensor,
571 attitude: &Tensor,
572 recurrent: Option<(&Tensor, &Tensor)>,
573 ) -> Result<(Tensor, Tensor, Tensor)> {
574 let state = recurrent
575 .map(|(hidden, cell)| VitFlyState::from_tensors(hidden, cell))
576 .transpose()?;
577 let (output, state) = self.forward(depth, desired_velocity, attitude, state.as_ref())?;
578 Ok((output, state.hidden_tensor()?, state.cell_tensor()?))
579 }
580
581 pub fn forward_traced(
583 &self,
584 depth: &Tensor,
585 desired_velocity: &Tensor,
586 attitude: &Tensor,
587 state: Option<&VitFlyState>,
588 ) -> Result<(Tensor, VitFlyState, ForwardTrace)> {
589 let mut trace = ForwardTrace::default();
590 let (output, state) =
591 self.forward_impl(depth, desired_velocity, attitude, state, Some(&mut trace))?;
592 Ok((output, state, trace))
593 }
594
595 fn forward_impl(
596 &self,
597 depth: &Tensor,
598 desired_velocity: &Tensor,
599 attitude: &Tensor,
600 state: Option<&VitFlyState>,
601 mut trace: Option<&mut ForwardTrace>,
602 ) -> Result<(Tensor, VitFlyState)> {
603 validate_shape("depth", depth.shape(), &DEPTH_SHAPE)?;
604 validate_shape(
605 "desired_velocity",
606 desired_velocity.shape(),
607 &DESIRED_VELOCITY_SHAPE,
608 )?;
609 validate_shape("attitude", attitude.shape(), &ATTITUDE_SHAPE)?;
610 if let Some(state) = state
611 && state.layers.len() != self.lstm_layers.len()
612 {
613 candle_core::bail!(
614 "state has {} layers, expected {}",
615 state.layers.len(),
616 self.lstm_layers.len()
617 )
618 }
619
620 record(&mut trace, "input.depth".into(), depth);
621 let encoder0 = self.encoder_stages[0].forward(depth, &mut trace, "encoder0")?;
622 let encoder1 = self.encoder_stages[1].forward(&encoder0, &mut trace, "encoder1")?;
623
624 let shuffled = candle_nn::ops::pixel_shuffle(&encoder1, 2)?;
625 record(&mut trace, "decoder.pixel_shuffle".into(), &shuffled);
626 let upsampled = encoder0.upsample_bilinear2d(16, 24, true)?;
627 record(&mut trace, "decoder.upsample".into(), &upsampled);
628 let decoded = Tensor::cat(&[&shuffled, &upsampled], 1)?;
629 record(&mut trace, "decoder.concat".into(), &decoded);
630 let decoded = self.down_sample.forward(&decoded)?;
631 record(&mut trace, "decoder.down_sample".into(), &decoded);
632 let decoded = self.decoder.forward(&decoded.flatten_from(1)?)?;
633 record(&mut trace, "decoder.linear".into(), &decoded);
634
635 let scaled_velocity = (desired_velocity / 10.0)?;
636 let mut recurrent = Tensor::cat(&[&decoded, &scaled_velocity, attitude], 1)?;
637 record(&mut trace, "metadata.concat".into(), &recurrent);
638
639 let mut next_layers = Vec::with_capacity(self.lstm_layers.len());
640 for (index, layer) in self.lstm_layers.iter().enumerate() {
641 let current = match state {
642 Some(state) => state.layers[index].clone(),
643 None => layer.zero_state(1)?,
644 };
645 let next = layer.step(&recurrent, ¤t)?;
646 recurrent = next.h().clone();
647 record(&mut trace, format!("lstm{index}.h"), next.h());
648 record(&mut trace, format!("lstm{index}.c"), next.c());
649 next_layers.push(next);
650 }
651 let output = self.output.forward(&recurrent)?;
652 record(&mut trace, "output".into(), &output);
653 Ok((
654 output,
655 VitFlyState {
656 layers: next_layers,
657 },
658 ))
659 }
660}
661
662fn validate_shape(name: &str, actual: &Shape, expected: &[usize]) -> Result<()> {
663 if actual.dims() != expected {
664 candle_core::bail!(
665 "{name} shape {:?} does not match required shape {expected:?}",
666 actual.dims()
667 )
668 }
669 Ok(())
670}
671
672#[cfg(test)]
673mod tests {
674 use super::*;
675 use std::sync::mpsc;
676 use std::time::Duration;
677
678 #[test]
679 fn test_model_setup_waits_for_publication() {
680 let root = tempfile::tempdir_in(model_root().parent().unwrap()).unwrap();
681 let lock = fs::File::create(root.path().join(".model-setup.lock")).unwrap();
682 lock.lock().unwrap();
683 let weights_root = root.path().to_path_buf();
684 let (started_tx, started_rx) = mpsc::channel();
685 let (done_tx, done_rx) = mpsc::channel();
686 let worker = std::thread::spawn(move || {
687 started_tx.send(()).unwrap();
688 done_tx
689 .send(prepare_model_weights_in(&weights_root))
690 .unwrap();
691 });
692 started_rx.recv().unwrap();
693 assert!(matches!(
694 done_rx.recv_timeout(Duration::from_millis(100)),
695 Err(mpsc::RecvTimeoutError::Timeout)
696 ));
697
698 let model_path = root.path().join(MODEL_NAME);
700 fs::File::create(&model_path)
701 .unwrap()
702 .set_len(MODEL_SIZE_BYTES)
703 .unwrap();
704 drop(lock);
705 assert_eq!(
706 done_rx
707 .recv_timeout(Duration::from_secs(5))
708 .unwrap()
709 .unwrap(),
710 model_path
711 );
712 worker.join().unwrap();
713 }
714}