Skip to content

Commit 0d2c4f0

Browse files
rmburgrmburg
committed
--wip--
1 parent 2f681b2 commit 0d2c4f0

8 files changed

Lines changed: 353 additions & 189 deletions

File tree

‎crates/approx_derive/src/lib.rs‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,7 @@ fn derive_abs_diff_eq(input: DeriveInput) -> Result<TokenStream> {
2626
};
2727
let epsilon_type = extract_epsilon_type(&input.attrs)?;
2828
let name = input.ident;
29+
let (impl_generics, ty_generics, where_clause) = input.generics.split_for_impl();
2930

3031
let conditions = fields.into_iter().map(|field| {
3132
let identifier = field
@@ -38,7 +39,7 @@ fn derive_abs_diff_eq(input: DeriveInput) -> Result<TokenStream> {
3839
});
3940

4041
Ok(quote! {
41-
impl approx::AbsDiffEq for #name {
42+
impl #impl_generics approx::AbsDiffEq for #name #ty_generics #where_clause {
4243
type Epsilon = #epsilon_type;
4344

4445
fn default_epsilon() -> Self::Epsilon {

‎crates/step_planning/src/cost_fields/path_distance.rs‎

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -34,6 +34,7 @@ mod tests {
3434

3535
use geometry::{arc::Arc, circle::Circle, direction::Direction, line_segment::LineSegment};
3636
use linear_algebra::{point, vector, Orientation2, Vector2};
37+
use proptest::proptest;
3738
use types::planned_path::{Path, PathSegment};
3839

3940
use crate::cost_fields::path_distance::PathDistanceField;
@@ -119,9 +120,9 @@ mod tests {
119120
assert_abs_diff_eq!(grad_7, vector![2.0 - SQRT_2, -(2.0 - SQRT_2)]);
120121
}
121122

122-
proptest::proptest! {
123+
proptest! {
123124
#[test]
124-
fn verify_gradient(x in -2.0f32..2.0, y in -2.0f32..2.0) {
125+
fn verify_gradient(x in -2.0f32..5.0, y in -2.0f32..5.0) {
125126
let cost_field = PathDistanceField {
126127
path: Path {
127128
segments: &test_path(),
@@ -130,10 +131,10 @@ mod tests {
130131

131132
let point = point![x, y];
132133

133-
crate::verify_gradient::verify_gradient(
134+
crate::test_utils::verify_gradient::verify_gradient(
134135
&|p| cost_field.cost(p),
135136
&|p| cost_field.grad(p),
136-
0.1,
137+
0.05,
137138
point,
138139
)
139140
}

‎crates/step_planning/src/cost_fields/path_progress.rs‎

Lines changed: 25 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -36,28 +36,12 @@ mod tests {
3636
use std::f32::consts::{FRAC_PI_2, FRAC_PI_4};
3737

3838
use approx::assert_abs_diff_eq;
39+
use proptest::proptest;
3940

40-
use geometry::{arc::Arc, circle::Circle, direction::Direction, line_segment::LineSegment};
41-
use linear_algebra::{point, vector, Orientation2};
42-
use types::planned_path::{Path, PathSegment};
41+
use linear_algebra::{point, vector};
42+
use types::planned_path::Path;
4343

44-
use crate::cost_fields::path_progress::PathProgressField;
45-
46-
fn test_path() -> Vec<PathSegment> {
47-
vec![
48-
PathSegment::LineSegment(LineSegment(point![0.0, 0.0], point![3.0, 0.0])),
49-
PathSegment::Arc(Arc {
50-
circle: Circle {
51-
center: point![3.0, 1.0],
52-
radius: 1.0,
53-
},
54-
start: Orientation2::new(3.0 * FRAC_PI_2),
55-
end: Orientation2::new(0.0),
56-
direction: Direction::Counterclockwise,
57-
}),
58-
PathSegment::LineSegment(LineSegment(point![4.0, 1.0], point![4.0, 4.0])),
59-
]
60-
}
44+
use crate::{cost_fields::path_progress::PathProgressField, test_utils::test_path};
6145

6246
#[test]
6347
fn test_path_progress() {
@@ -123,4 +107,25 @@ mod tests {
123107
assert_abs_diff_eq!(cost_7, cost_3 - FRAC_PI_4, epsilon = 1e-6);
124108
assert_abs_diff_eq!(grad_7, vector![-0.5, -0.5]);
125109
}
110+
111+
proptest!(
112+
#[test]
113+
fn verify_gradient(x in -2.0f32..5.0, y in -2.0f32..5.0) {
114+
let cost_field = PathProgressField {
115+
path: Path {
116+
segments: &test_path(),
117+
},
118+
smoothness: 0.5,
119+
};
120+
121+
let point = point![x, y];
122+
123+
crate::test_utils::verify_gradient::verify_gradient(
124+
&|p| cost_field.cost(p),
125+
&|p| cost_field.grad(p),
126+
0.05,
127+
point,
128+
)
129+
}
130+
);
126131
}

‎crates/step_planning/src/cost_fields/target_orientation.rs‎

Lines changed: 44 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -65,3 +65,47 @@ impl TargetOrientationField<'_> {
6565
}
6666
}
6767
}
68+
69+
#[cfg(test)]
70+
mod tests {
71+
use std::f32::consts::{PI, TAU};
72+
73+
use linear_algebra::point;
74+
use proptest::proptest;
75+
use types::planned_path::Path;
76+
77+
use crate::{
78+
cost_fields::target_orientation::TargetOrientationField,
79+
geometry::{angle::Angle, Pose},
80+
test_utils::test_path,
81+
};
82+
83+
proptest!(
84+
#[test]
85+
fn verify_gradient(x in -2.0f32..5.0, y in -2.0f32..5.0, orientation in 0.0..TAU) {
86+
let cost_field = TargetOrientationField {
87+
target_orientation: Angle(PI),
88+
path: Path {
89+
segments: &test_path(),
90+
},
91+
alignment_start_distance: 1.0,
92+
ramp_width: 0.5,
93+
};
94+
95+
let position = point![x, y];
96+
let orientation = Angle(orientation);
97+
98+
let pose = Pose {
99+
position,
100+
orientation,
101+
};
102+
103+
crate::test_utils::verify_gradient::verify_gradient(
104+
&|p| cost_field.cost(p),
105+
&|p| cost_field.grad(p),
106+
0.05,
107+
pose,
108+
)
109+
}
110+
);
111+
}

‎crates/step_planning/src/geometry/pose.rs‎

Lines changed: 34 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,7 @@ use std::{
33
ops::{Add, AddAssign, Mul},
44
};
55

6+
use approx::{AbsDiffEq, RelativeEq};
67
use nalgebra::{RealField, Scalar};
78
use num_traits::Euclid;
89

@@ -18,7 +19,7 @@ pub struct Pose<T: Scalar> {
1819
pub orientation: Angle<T>,
1920
}
2021

21-
#[derive(Clone, Debug)]
22+
#[derive(Clone, Debug, PartialEq)]
2223
pub struct PoseGradient<T: Scalar> {
2324
pub position: Vector2<Ground, T>,
2425
pub orientation: T,
@@ -30,6 +31,38 @@ impl<T: RealField + Euclid> PartialEq for Pose<T> {
3031
}
3132
}
3233

34+
impl<T: AbsDiffEq + Euclid + RealField> AbsDiffEq for PoseGradient<T> {
35+
type Epsilon = T::Epsilon;
36+
37+
fn default_epsilon() -> Self::Epsilon {
38+
T::default_epsilon()
39+
}
40+
41+
fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
42+
self.position.abs_diff_eq(&other.position, epsilon.clone())
43+
&& self.orientation.abs_diff_eq(&other.orientation, epsilon)
44+
}
45+
}
46+
47+
impl<T: RelativeEq + Euclid + RealField> RelativeEq for PoseGradient<T> {
48+
fn default_max_relative() -> Self::Epsilon {
49+
T::default_max_relative()
50+
}
51+
52+
fn relative_eq(
53+
&self,
54+
other: &Self,
55+
epsilon: Self::Epsilon,
56+
max_relative: Self::Epsilon,
57+
) -> bool {
58+
self.position
59+
.relative_eq(&other.position, epsilon.clone(), max_relative.clone())
60+
&& self
61+
.orientation
62+
.relative_eq(&other.orientation, epsilon, max_relative)
63+
}
64+
}
65+
3366
impl<T: Scalar> Pose<T> {
3467
pub fn with_support_foot(self, support_foot: Side) -> PoseAndSupportFoot<T> {
3568
PoseAndSupportFoot {

‎crates/step_planning/src/lib.rs‎

Lines changed: 85 additions & 61 deletions
Original file line numberDiff line numberDiff line change
@@ -5,76 +5,100 @@ pub mod traits;
55
pub mod utils;
66

77
#[cfg(test)]
8-
pub mod verify_gradient {
9-
use std::fmt::Debug;
10-
11-
use approx::{assert_relative_eq, AbsDiffEq, RelativeEq};
12-
use num_traits::{real::Real, NumAssignOps};
13-
14-
use crate::traits::{
15-
decompose::Decompose,
16-
gradient_type::{Gradient, GradientType},
17-
};
18-
19-
pub fn verify_gradient<
20-
A: Clone + Debug + RelativeEq + Decompose<F> + GradientType,
21-
F: Real + NumAssignOps,
22-
>(
23-
func: &impl Fn(A) -> F,
24-
grad: &impl Fn(A) -> Gradient<A>,
25-
epsilon: <Gradient<A> as AbsDiffEq>::Epsilon,
26-
x: A,
27-
) where
28-
Gradient<A>: Debug + RelativeEq + Decompose<F>,
29-
<Gradient<A> as AbsDiffEq>::Epsilon: From<f32>,
30-
{
31-
let real_gradient = grad(x.clone());
32-
let numerical_gradient = numerical_grad(func, x);
33-
34-
assert_relative_eq!(real_gradient, numerical_gradient, epsilon = epsilon);
35-
}
8+
pub mod test_utils {
9+
use std::f32::consts::FRAC_PI_2;
10+
11+
use geometry::{arc::Arc, circle::Circle, direction::Direction, line_segment::LineSegment};
12+
use linear_algebra::{point, Orientation2};
13+
use types::planned_path::PathSegment;
3614

37-
fn numerical_grad<A: Clone + Decompose<F> + GradientType, F: Real + NumAssignOps>(
38-
func: &impl Fn(A) -> F,
39-
x: A,
40-
) -> Gradient<A>
41-
where
42-
Gradient<A>: Decompose<F>,
43-
{
44-
let decomposed = (0..A::N)
45-
.map(|i| numerical_nth_derivative(func, i, x.clone()))
46-
.collect();
47-
48-
Gradient::<A>::compose(decomposed)
15+
pub fn test_path() -> Vec<PathSegment> {
16+
vec![
17+
PathSegment::LineSegment(LineSegment(point![0.0, 0.0], point![3.0, 0.0])),
18+
PathSegment::Arc(Arc {
19+
circle: Circle {
20+
center: point![3.0, 1.0],
21+
radius: 1.0,
22+
},
23+
start: Orientation2::new(3.0 * FRAC_PI_2),
24+
end: Orientation2::new(0.0),
25+
direction: Direction::Counterclockwise,
26+
}),
27+
PathSegment::LineSegment(LineSegment(point![4.0, 1.0], point![4.0, 4.0])),
28+
]
4929
}
5030

51-
fn numerical_nth_derivative<A: Decompose<F>, F: Real + NumAssignOps>(
52-
func: &impl Fn(A) -> F,
53-
n: usize,
54-
x: A,
55-
) -> F {
56-
let eps = F::from(1e-5).unwrap();
31+
pub mod verify_gradient {
32+
use std::fmt::Debug;
5733

58-
let middle = x.decompose();
59-
let above = {
60-
let mut above = middle.clone();
61-
above[n] += eps;
34+
use approx::{assert_relative_eq, AbsDiffEq, RelativeEq};
35+
use num_traits::{real::Real, NumAssignOps};
6236

63-
A::compose(above)
37+
use crate::traits::{
38+
decompose::Decompose,
39+
gradient_type::{Gradient, GradientType},
6440
};
65-
let below = {
66-
let mut below = middle.clone();
67-
below[n] -= eps;
6841

69-
A::compose(below)
70-
};
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+
};
7194

72-
let sample_above = func(above);
73-
let sample_below = func(below);
95+
let sample_above = func(above);
96+
let sample_below = func(below);
7497

75-
let difference = sample_above - sample_below;
76-
let sample_distance = F::from(2.0).unwrap() * eps;
98+
let difference = sample_above - sample_below;
99+
let sample_distance = F::from(2.0).unwrap() * eps;
77100

78-
difference / sample_distance
101+
difference / sample_distance
102+
}
79103
}
80104
}

0 commit comments

Comments
 (0)