sample_planning 0.0.4

Sample Based Planner
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96

use std::marker::PhantomData;
use std::fmt;

use crate::states::States;
use crate::control::Control;

extern crate mazth;

use zpatial::mazth::i_shape::IShape;
use zpatial::mazth::{rbox::RecBox,triprism::TriPrism};

pub enum StopCondition {
    Iterations(i32),
    TimeMilliSeconds(i32),
}

#[derive(Clone,Debug)]
pub struct Param <T, C, TObs> where T: States, C: Control, TObs: States {
    // pub memory_limit: Option<i32>,
    pub stop_cond: fn(T/*system state*/,TObs/*config state*/,T/*desired state*/)->bool,
    pub states_init: T,
    pub states_goal: T,
    pub dynamics: fn(T,C,f32)->T, //uses state space of system
    pub project_state_to_config: fn(T)->TObs,
    pub sim_delta: f32, //to be used as simulation step size
    pub param_sampler: fn(f32)->C, //sampling in parameter space
    pub ss_sampler: fn()->T, //sampleing in state space
    pub ss_metric: fn(T,T)->f32, //distance function in state space
    pub cs_metric: fn(TObs, TObs) -> f32, //estimated cloness ness in configuration space
    pub iterations_bound: u32,

    //optional, but would ggive error if running motion_primitives feature without providing functions
    pub motion_primitive_xform: Option<fn(T,T)->T>,
    pub motion_primitive_xform_inv: Option<fn(T,T)->T>,

    pub ss_add: fn(T,T)->T,
    pub ss_mul: fn(T,f32)->T,
}

#[derive(Clone,Debug)]
pub enum ObsVariant {
    RBOX(Vec<RecBox>),
    TRIPRISM(Vec<TriPrism>),
}

#[derive(Clone,Debug)]
pub struct ParamObstacles <T> where T: States {
    pub obstacles: ObsVariant,
    pub states_info: PhantomData<T>,
}

impl<T,C,TObs> fmt::Display for Param <T, C, TObs> where T: States, C: Control, TObs: States  {
   
    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {

        f.debug_struct("Param")
            .field("states_init", &format!("{:?}",&self.states_init) )
            .field("states_goal", &format!("{:?}",&self.states_goal) )
            .field("sim_delta", &self.sim_delta )
            .field("iterations_bound", &self.iterations_bound )
            .finish()
    }
}


#[derive(Clone,Debug)]
pub struct ParamTree {
    pub delta_v: f32,
    pub delta_s: f32,
    pub prop_delta_low: f32,
    pub prop_delta_high: f32,
}

impl Default for ParamTree {
    fn default() -> Self {
        ParamTree {
            delta_v: 0.01,
            delta_s: 0.001,
            prop_delta_low: 0.15,
            prop_delta_high: 1.,
        }       
    }
}

impl fmt::Display for ParamTree {
   
    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
        
        f.debug_struct("ParamTree")
            .field("delta_v", &format!("{:?}",&self.delta_v) )
            .field("delta_s", &format!("{:?}",&self.delta_s) )
            .field("prop_delta_low", &format!("{:?}",&self.prop_delta_low) )
            .field("prop_delta_high", &format!("{:?}",&self.prop_delta_high) )
            .finish()
    }
}