Skip to content

Commit 198e726

Browse files
jgsimardjgsimard
committed
update deps + russel ->faer
1 parent 965cb64 commit 198e726

File tree

15 files changed

+1372
-1037
lines changed

15 files changed

+1372
-1037
lines changed

‎Cargo.lock‎

Lines changed: 1059 additions & 769 deletions
Some generated files are not rendered by default. Learn more about customizing how changed files appear on GitHub.

‎Cargo.toml‎

Lines changed: 7 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -12,26 +12,20 @@ exclude = ["dataset/*"]
1212
# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
1313

1414
[dependencies]
15-
nalgebra = {version = "0.32", features=["rand-no-std"]}
15+
nalgebra = {version = "0.34", features=["rand-no-std"]}
1616
plotters = "0.3"
1717
csv = "1.3"
1818
serde = { version = "1.0", features = ["derive"] }
19-
rustc-hash = "1.1"
20-
rand = "0.8"
21-
rand_distr = "0.4"
22-
russell_lab = "0.7"
23-
russell_sparse = "0.7"
24-
plotpy = "0.5"
19+
rustc-hash = "2.1"
20+
rand = "0.9"
21+
rand_distr = "0.5"
2522
rayon = "1.7"
26-
faer = "0.18" # fast linear algebra
27-
# # python
28-
# pyo3 = { version = "0.18", features = ["extension-module"] }
29-
# numpy = {version = "0.18", features = ["nalgebra"] }
23+
faer = "0.24"
3024

3125
[dev-dependencies]
32-
criterion = "0.5"
26+
criterion = "0.8"
3327
approx = "0.5"
34-
dialoguer = "0.11"
28+
dialoguer = "0.12"
3529

3630
[profile.dev.package.faer]
3731
opt-level = 3

‎README.md‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
# RustRobotics
22

3-
This package is a rust implementation of robotics algorithms. So far, the main source is the book [Probabilistic Robotics](https://mitpress.mit.edu/9780262201629/probabilistic-robotics/). I plan to have algorithms implementations in the `src` folder and the algorithms use cases in the `examples` folder. I plan to have python bindings using [pyo3](https://github.com/PyO3/pyo3)/[maturin](https://github.com/PyO3/maturin). I am also implementing the algorithms in python using [JAX](https://jax.readthedocs.io/en/latest/) in this [repo](https://github.com/jgsimard/jaxrobot).
3+
This package is a rust implementation of robotics algorithms. So far, the main source is the book [Probabilistic Robotics](https://mitpress.mit.edu/9780262201629/probabilistic-robotics/). Algorithms implementations are in the `src` folder and use cases in the `examples` folder.
44

55
## Table of Contents
66

‎examples/control/inverted_pendulum.rs‎

Lines changed: 74 additions & 42 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
use dialoguer::{theme::ColorfulTheme, Select};
22
use nalgebra::{Const, Matrix1, Matrix4, Matrix4x1, Vector4};
3-
use plotpy::{Curve, Plot};
3+
use plotters::prelude::*;
44
use std::error::Error;
55

66
extern crate robotics;
@@ -88,55 +88,87 @@ fn main() -> Result<(), Box<dyn Error>> {
8888
// let algo = algos[algo_idx];
8989

9090
// get data
91-
let (states, commands) = run().expect("unable to run");
91+
let (states, _commands) = run().expect("unable to run");
9292

9393
// Create output directory if it didnt exist
9494
std::fs::create_dir_all("./img")?;
9595

96-
let path = match algo_idx {
96+
let path_prefix = match algo_idx {
9797
0 => "lqr",
9898
1 => "mpc",
9999
_ => unreachable!(),
100100
};
101101

102-
let time = (0..states.len())
103-
.map(|i| (i as f64) / (states.len() as f64) * 5.0)
104-
.collect::<Vec<f64>>();
105-
106-
let mut curve_x = Curve::new();
107-
curve_x
108-
.set_label("x")
109-
.draw(&time, &states.iter().map(|s| s.x).collect());
110-
111-
let mut curve_theta = Curve::new();
112-
curve_theta
113-
.set_label("theta")
114-
.draw(&time, &states.iter().map(|s| s.z).collect());
115-
116-
let mut curve_x_dot = Curve::new();
117-
curve_x_dot
118-
.set_label("x dot")
119-
.draw(&time, &states.iter().map(|s| s.y).collect());
120-
121-
let mut curve_theta_dot = Curve::new();
122-
curve_theta_dot
123-
.set_label("theta dot")
124-
.draw(&time, &states.iter().map(|s| s.w).collect());
125-
126-
let mut curve_u = Curve::new();
127-
curve_u.set_label("u").draw(&time, &commands);
128-
129-
// add features to plot
130-
let mut plot = Plot::new();
131-
132-
plot.add(&curve_x)
133-
.add(&curve_theta)
134-
.add(&curve_x_dot)
135-
.add(&curve_theta_dot)
136-
// .add(&curve_u)
137-
.legend()
138-
.set_equal_axes(true) // save figure
139-
.set_figure_size_points(1000.0, 1000.0)
140-
.save(format!("img/{}-{}.svg", path, "done").as_str())?;
102+
let full_path = format!("img/{}-{}.svg", path_prefix, "done");
103+
104+
let root = SVGBackend::new(&full_path, (1000, 1000)).into_drawing_area();
105+
root.fill(&WHITE)?;
106+
107+
let time: Vec<f64> = (0..states.len())
108+
.map(|i| (i as f64) * 0.01) // dt = 0.01
109+
.collect();
110+
111+
let mut min_y = states.iter().map(|s| s.min()).fold(f64::INFINITY, f64::min);
112+
let mut max_y = states
113+
.iter()
114+
.map(|s| s.max())
115+
.fold(f64::NEG_INFINITY, f64::max);
116+
117+
min_y -= 0.1;
118+
max_y += 0.1;
119+
120+
let mut chart = ChartBuilder::on(&root)
121+
.caption(
122+
format!("Inverted Pendulum - {}", algos[algo_idx]),
123+
("sans-serif", 40),
124+
)
125+
.margin(20)
126+
.x_label_area_size(40)
127+
.y_label_area_size(40)
128+
.build_cartesian_2d(0f64..5f64, min_y..max_y)?;
129+
130+
chart
131+
.configure_mesh()
132+
.x_desc("Time [s]")
133+
.y_desc("State Value")
134+
.draw()?;
135+
136+
let state_configs = vec![
137+
(states.iter().map(|s| s.x).collect::<Vec<_>>(), &BLUE, "x"),
138+
(
139+
states.iter().map(|s| s.y).collect::<Vec<_>>(),
140+
&CYAN,
141+
"x dot",
142+
),
143+
(
144+
states.iter().map(|s| s.z).collect::<Vec<_>>(),
145+
&RED,
146+
"theta",
147+
),
148+
(
149+
states.iter().map(|s| s.w).collect::<Vec<_>>(),
150+
&GREEN,
151+
"theta dot",
152+
),
153+
];
154+
155+
for (data, color, label) in state_configs {
156+
chart
157+
.draw_series(LineSeries::new(
158+
time.iter().zip(data.iter()).map(|(&t, &v)| (t, v)),
159+
color.stroke_width(2),
160+
))?
161+
.label(label)
162+
.legend(move |(x, y)| PathElement::new(vec![(x, y), (x + 20, y)], color.clone()));
163+
}
164+
165+
chart
166+
.configure_series_labels()
167+
.background_style(&WHITE.mix(0.8))
168+
.border_style(&BLACK)
169+
.draw()?;
170+
171+
root.present()?;
172+
141173
Ok(())
142174
}

‎examples/localization/bayesian_filter.rs‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -34,7 +34,7 @@ impl SimpleProblem {
3434
u: &Vector2<f64>,
3535
dt: f64,
3636
) -> (Vector4<f64>, Vector2<f64>, Vector4<f64>, Vector2<f64>) {
37-
let mut rng = rand::thread_rng();
37+
let mut rng = rand::rng();
3838
let normal = Normal::new(0., 1.).unwrap();
3939

4040
let x_true_next = self.motion_model.prediction(x_true, u, dt);

‎src/control/lqr.rs‎

Lines changed: 7 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -5,8 +5,7 @@ use nalgebra::{allocator::Allocator, DefaultAllocator, Dim, OMatrix, RealField};
55

66
pub struct LinearTimeInvariantModel<T: RealField, S: Dim, U: Dim>
77
where
8-
DefaultAllocator:
9-
Allocator<T, S, S> + Allocator<T, S, U> + Allocator<T, U, S> + Allocator<T, U, U>,
8+
DefaultAllocator: Allocator<S, S> + Allocator<S, U> + Allocator<U, S> + Allocator<U, U>,
109
{
1110
pub A: OMatrix<T, S, S>,
1211
pub B: OMatrix<T, S, U>,
@@ -20,12 +19,12 @@ pub fn lqr<T: RealField + Copy, S: Dim, U: Dim>(
2019
epsilon: T,
2120
) -> Option<OMatrix<T, U, S>>
2221
where
23-
DefaultAllocator: Allocator<T, S>
24-
+ Allocator<T, U>
25-
+ Allocator<T, S, S>
26-
+ Allocator<T, S, U>
27-
+ Allocator<T, U, S>
28-
+ Allocator<T, U, U>,
22+
DefaultAllocator: Allocator<S>
23+
+ Allocator<U>
24+
+ Allocator<S, S>
25+
+ Allocator<S, U>
26+
+ Allocator<U, S>
27+
+ Allocator<U, U>,
2928
{
3029
let A = &linear_model.A;
3130
let A_T = &A.transpose();

‎src/localization/bayesian_filter.rs‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,7 @@ use crate::utils::state::GaussianState;
44

55
pub trait BayesianFilter<T: RealField, S: Dim, Z: Dim, U: Dim>
66
where
7-
DefaultAllocator: Allocator<T, S> + Allocator<T, U> + Allocator<T, Z> + Allocator<T, S, S>,
7+
DefaultAllocator: Allocator<S> + Allocator<U> + Allocator<Z> + Allocator<S, S>,
88
{
99
fn update_estimate(&mut self, u: &OVector<T, U>, z: &OVector<T, Z>, dt: T);
1010

@@ -13,7 +13,7 @@ where
1313

1414
pub trait BayesianFilterKnownCorrespondences<T: RealField, S: Dim, Z: Dim, U: Dim>
1515
where
16-
DefaultAllocator: Allocator<T, S> + Allocator<T, U> + Allocator<T, Z> + Allocator<T, S, S>,
16+
DefaultAllocator: Allocator<S> + Allocator<U> + Allocator<Z> + Allocator<S, S>,
1717
{
1818
fn update_estimate(
1919
&mut self,

‎src/localization/extended_kalman_filter.rs‎

Lines changed: 23 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,7 @@ use crate::utils::state::GaussianState;
99
/// S : State Size, Z: Observation Size, U: Input Size
1010
pub struct ExtendedKalmanFilter<T: RealField, S: Dim, Z: Dim, U: Dim>
1111
where
12-
DefaultAllocator: Allocator<T, S> + Allocator<T, S, S> + Allocator<T, Z, Z>,
12+
DefaultAllocator: Allocator<S> + Allocator<S, S> + Allocator<Z, Z>,
1313
{
1414
r: OMatrix<T, S, S>,
1515
q: OMatrix<T, Z, Z>,
@@ -20,7 +20,7 @@ where
2020

2121
impl<T: RealField, S: Dim, Z: Dim, U: Dim> ExtendedKalmanFilter<T, S, Z, U>
2222
where
23-
DefaultAllocator: Allocator<T, S> + Allocator<T, S, S> + Allocator<T, Z, Z>,
23+
DefaultAllocator: Allocator<S> + Allocator<S, S> + Allocator<Z, Z>,
2424
{
2525
pub fn new(
2626
r: OMatrix<T, S, S>,
@@ -42,15 +42,15 @@ where
4242
impl<T: RealField, S: Dim, Z: Dim, U: Dim> BayesianFilter<T, S, Z, U>
4343
for ExtendedKalmanFilter<T, S, Z, U>
4444
where
45-
DefaultAllocator: Allocator<T, S>
46-
+ Allocator<T, U>
47-
+ Allocator<T, Z>
48-
+ Allocator<T, S, S>
49-
+ Allocator<T, Z, Z>
50-
+ Allocator<T, Z, S>
51-
+ Allocator<T, S, U>
52-
+ Allocator<T, U, U>
53-
+ Allocator<T, S, Z>,
45+
DefaultAllocator: Allocator<S>
46+
+ Allocator<U>
47+
+ Allocator<Z>
48+
+ Allocator<S, S>
49+
+ Allocator<Z, Z>
50+
+ Allocator<Z, S>
51+
+ Allocator<S, U>
52+
+ Allocator<U, U>
53+
+ Allocator<S, Z>,
5454
{
5555
fn update_estimate(&mut self, u: &OVector<T, U>, z: &OVector<T, Z>, dt: T) {
5656
// predict
@@ -80,7 +80,7 @@ where
8080
/// S : State Size, Z: Observation Size, U: Input Size
8181
pub struct ExtendedKalmanFilterKnownCorrespondences<T: RealField, S: Dim, Z: Dim, U: Dim>
8282
where
83-
DefaultAllocator: Allocator<T, S> + Allocator<T, S, S> + Allocator<T, Z, Z>,
83+
DefaultAllocator: Allocator<S> + Allocator<S, S> + Allocator<Z, Z>,
8484
{
8585
q: OMatrix<T, Z, Z>,
8686
landmarks: FxHashMap<u32, OVector<T, S>>,
@@ -91,7 +91,7 @@ where
9191

9292
impl<T: RealField, S: Dim, Z: Dim, U: Dim> ExtendedKalmanFilterKnownCorrespondences<T, S, Z, U>
9393
where
94-
DefaultAllocator: Allocator<T, S> + Allocator<T, U> + Allocator<T, S, S> + Allocator<T, Z, Z>,
94+
DefaultAllocator: Allocator<S> + Allocator<U> + Allocator<S, S> + Allocator<Z, Z>,
9595
{
9696
pub fn new(
9797
q: OMatrix<T, Z, Z>,
@@ -113,16 +113,16 @@ where
113113
impl<T: RealField + Copy, S: Dim, Z: Dim, U: Dim> BayesianFilterKnownCorrespondences<T, S, Z, U>
114114
for ExtendedKalmanFilterKnownCorrespondences<T, S, Z, U>
115115
where
116-
DefaultAllocator: Allocator<T, S>
117-
+ Allocator<T, U>
118-
+ Allocator<T, Z>
119-
+ Allocator<T, S, S>
120-
+ Allocator<T, Z, Z>
121-
+ Allocator<T, Z, S>
122-
+ Allocator<T, S, U>
123-
+ Allocator<T, U, U>
124-
+ Allocator<T, S, Z>
125-
+ Allocator<T, U, S>,
116+
DefaultAllocator: Allocator<S>
117+
+ Allocator<U>
118+
+ Allocator<Z>
119+
+ Allocator<S, S>
120+
+ Allocator<Z, Z>
121+
+ Allocator<Z, S>
122+
+ Allocator<S, U>
123+
+ Allocator<U, U>
124+
+ Allocator<S, Z>
125+
+ Allocator<U, S>,
126126
{
127127
fn update_estimate(
128128
&mut self,

0 commit comments

Comments
 (0)