Skip to content

Commit 4329b12

Browse files
rmburgrmburg
committed
Refactor module layout
1 parent f118bd2 commit 4329b12

5 files changed

Lines changed: 136 additions & 78 deletions

File tree

‎crates/step_planning/src/lib.rs‎

Lines changed: 4 additions & 74 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,10 @@ pub mod utils;
66

77
#[cfg(test)]
88
pub mod test_utils {
9+
pub mod decompose;
10+
pub mod gradient_type;
11+
pub mod verify_gradient;
12+
913
use std::f32::consts::FRAC_PI_2;
1014

1115
use geometry::{arc::Arc, circle::Circle, direction::Direction, line_segment::LineSegment};
@@ -27,78 +31,4 @@ pub mod test_utils {
2731
PathSegment::LineSegment(LineSegment(point![4.0, 1.0], point![4.0, 4.0])),
2832
]
2933
}
30-
31-
pub mod verify_gradient {
32-
use std::fmt::Debug;
33-
34-
use approx::{assert_relative_eq, AbsDiffEq, RelativeEq};
35-
use num_traits::{real::Real, NumAssignOps};
36-
37-
use crate::traits::{
38-
decompose::Decompose,
39-
gradient_type::{Gradient, GradientType},
40-
};
41-
42-
pub fn verify_gradient<
43-
A: Clone + Debug + Decompose<F> + GradientType,
44-
F: Real + NumAssignOps,
45-
>(
46-
func: &impl Fn(A) -> F,
47-
grad: &impl Fn(A) -> Gradient<A>,
48-
epsilon: <Gradient<A> as AbsDiffEq>::Epsilon,
49-
x: A,
50-
) where
51-
Gradient<A>: Debug + RelativeEq + Decompose<F>,
52-
<Gradient<A> as AbsDiffEq>::Epsilon: From<f32>,
53-
{
54-
let real_gradient = grad(x.clone());
55-
let numerical_gradient = numerical_grad(func, x);
56-
57-
assert_relative_eq!(real_gradient, numerical_gradient, epsilon = epsilon);
58-
}
59-
60-
fn numerical_grad<A: Clone + Decompose<F> + GradientType, F: Real + NumAssignOps>(
61-
func: &impl Fn(A) -> F,
62-
x: A,
63-
) -> Gradient<A>
64-
where
65-
Gradient<A>: Decompose<F>,
66-
{
67-
let decomposed = (0..A::N)
68-
.map(|i| numerical_nth_derivative(func, i, x.clone()))
69-
.collect();
70-
71-
Gradient::<A>::compose(decomposed)
72-
}
73-
74-
fn numerical_nth_derivative<A: Decompose<F>, F: Real + NumAssignOps>(
75-
func: &impl Fn(A) -> F,
76-
n: usize,
77-
x: A,
78-
) -> F {
79-
let eps = F::from(1e-4).unwrap();
80-
81-
let middle = x.decompose();
82-
let above = {
83-
let mut above = middle.clone();
84-
above[n] += eps;
85-
86-
A::compose(above)
87-
};
88-
let below = {
89-
let mut below = middle.clone();
90-
below[n] -= eps;
91-
92-
A::compose(below)
93-
};
94-
95-
let sample_above = func(above);
96-
let sample_below = func(below);
97-
98-
let difference = sample_above - sample_below;
99-
let sample_distance = F::from(2.0).unwrap() * eps;
100-
101-
difference / sample_distance
102-
}
103-
}
10434
}

crates/step_planning/src/traits/decompose.rs renamed to crates/step_planning/src/test_utils/decompose.rs

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -135,7 +135,7 @@ where
135135
mod tests {
136136
use nalgebra::{matrix, OMatrix, U2};
137137

138-
use crate::traits::decompose::Decompose;
138+
use crate::test_utils::decompose::Decompose;
139139

140140
#[test]
141141
fn decompose_matrix() {

‎crates/step_planning/src/test_utils/gradient_type.rs‎

Lines changed: 63 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,63 @@
1+
use nalgebra::{allocator::Allocator, DefaultAllocator, DimName, OPoint, OVector, Scalar};
2+
3+
use linear_algebra::Framed;
4+
use types::step::Step;
5+
6+
use crate::geometry::{pose::PoseGradient, Pose};
7+
8+
pub trait GradientType {
9+
type Gradient;
10+
}
11+
12+
pub type Gradient<T> = <T as GradientType>::Gradient;
13+
14+
macro_rules! impl_gradient {
15+
($a:ident, $b:ident) => {
16+
impl GradientType for $a {
17+
type Gradient = $b;
18+
}
19+
};
20+
(
21+
<$(
22+
$t:ident $(: $bound:ident $(+ $bound2:ident)* )?
23+
),+> ;
24+
$a:ty, $b:ty
25+
) => {
26+
impl<$($t $(: $bound $(+ $bound2)* )?),*> GradientType for $a {
27+
type Gradient = $b;
28+
}
29+
};
30+
(
31+
<$(
32+
$t:ident $(: $bound:ident $(+ $bound2:ident)* )?
33+
),+> ;
34+
where $($where_type:ident : $where_bound:path),+ ;
35+
$a:ty, $b:ty
36+
) => {
37+
impl<$($t $(: $bound $(+ $bound2)* )?),*> GradientType for $a
38+
where
39+
$($where_type: $where_bound)+
40+
{
41+
type Gradient = $b;
42+
}
43+
};
44+
}
45+
46+
impl_gradient!(f32, f32);
47+
impl_gradient!(
48+
<T: Scalar, D: DimName>;
49+
where DefaultAllocator: Allocator<D>;
50+
OPoint<T, D>, OVector<T, D>
51+
);
52+
impl_gradient!(
53+
<T>;
54+
Step<T>, Step<T>
55+
);
56+
impl_gradient!(
57+
<T: Scalar>;
58+
Pose<T>, PoseGradient<T>
59+
);
60+
impl_gradient!(
61+
<Frame, Inner: GradientType>;
62+
Framed<Frame, Inner>, Framed<Frame, Gradient<Inner>>
63+
);

‎crates/step_planning/src/test_utils/verify_gradient.rs‎

Lines changed: 68 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,68 @@
1+
use std::fmt::Debug;
2+
3+
use approx::{assert_relative_eq, AbsDiffEq, RelativeEq};
4+
use num_traits::{real::Real, NumAssignOps};
5+
6+
use crate::{
7+
test_utils::decompose::Decompose,
8+
traits::gradient_type::{Gradient, GradientType},
9+
};
10+
11+
pub fn verify_gradient<A: Clone + Debug + Decompose<F> + GradientType, F: Real + NumAssignOps>(
12+
func: &impl Fn(A) -> F,
13+
grad: &impl Fn(A) -> Gradient<A>,
14+
epsilon: <Gradient<A> as AbsDiffEq>::Epsilon,
15+
x: A,
16+
) where
17+
Gradient<A>: Debug + RelativeEq + Decompose<F>,
18+
<Gradient<A> as AbsDiffEq>::Epsilon: From<f32>,
19+
{
20+
let real_gradient = grad(x.clone());
21+
let numerical_gradient = numerical_grad(func, x);
22+
23+
assert_relative_eq!(real_gradient, numerical_gradient, epsilon = epsilon);
24+
}
25+
26+
fn numerical_grad<A: Clone + Decompose<F> + GradientType, F: Real + NumAssignOps>(
27+
func: &impl Fn(A) -> F,
28+
x: A,
29+
) -> Gradient<A>
30+
where
31+
Gradient<A>: Decompose<F>,
32+
{
33+
let decomposed = (0..A::N)
34+
.map(|i| numerical_nth_derivative(func, i, x.clone()))
35+
.collect();
36+
37+
Gradient::<A>::compose(decomposed)
38+
}
39+
40+
fn numerical_nth_derivative<A: Decompose<F>, F: Real + NumAssignOps>(
41+
func: &impl Fn(A) -> F,
42+
n: usize,
43+
x: A,
44+
) -> F {
45+
let eps = F::from(1e-4).unwrap();
46+
47+
let middle = x.decompose();
48+
let above = {
49+
let mut above = middle.clone();
50+
above[n] += eps;
51+
52+
A::compose(above)
53+
};
54+
let below = {
55+
let mut below = middle.clone();
56+
below[n] -= eps;
57+
58+
A::compose(below)
59+
};
60+
61+
let sample_above = func(above);
62+
let sample_below = func(below);
63+
64+
let difference = sample_above - sample_below;
65+
let sample_distance = F::from(2.0).unwrap() * eps;
66+
67+
difference / sample_distance
68+
}

‎crates/step_planning/src/traits.rs‎

Lines changed: 0 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -78,6 +78,3 @@ pub mod gradient_type {
7878
Framed<Frame, Inner>, Framed<Frame, Gradient<Inner>>
7979
);
8080
}
81-
82-
#[cfg(test)]
83-
pub mod decompose;

0 commit comments

Comments
 (0)