~sertac/smp_doc/dubins__double__integrator_8h_source.html

00001
00011 #ifndef _SMP_SYSTEM_DUBINS_DOUBLE_INTEGRATOR_H_
00012 #define _SMP_SYSTEM_DUBINS_DOUBLE_INTEGRATOR_H_
00013
00014
00015 #include <smp/components/extenders/state_array_double.h>
00016 #include <smp/components/extenders/input_array_double.h>
00017 #include <smp/components/extenders/base.h>
00018
00019
00020 #include <list>
00021
00022
00023 using namespace std;
00024
00025
00026 namespace smp {
00027
00028
00030
00036     class state_dubins_double_integrator : public state_array_double<5> {
00037
00038     };
00039
00040
00041
00043
00049     class input_dubins_double_integrator : public input_array_double<3> {
00050
00051     };
00052
00053
00054
00056
00061     template < class typeparams >
00062     class extender_dubins_double_integrator : public extender_base<typeparams> {
00063
00064
00065         typedef typename typeparams::state state_t;
00066         typedef typename typeparams::input input_t;
00067         typedef typename typeparams::vertex_data vertex_data_t;
00068         typedef typename typeparams::edge_data edge_data_t;
00069
00070         typedef vertex<typeparams> vertex_t;
00071         typedef edge<typeparams> edge_t;
00072
00073         typedef trajectory< typeparams > trajectory_t;
00074
00075
00076
00077         // // TODO: get back to appropriate velocity constraints.
00078
00079
00080         double extend_dubins_spheres_return_params (double x_s1, double y_s1, double t_s1,
00081                                                     double x_s2, double y_s2, double t_s2, int comb_no,
00082                                                     double *t_increment_s1_out,
00083                                                     double *s_increment_out,
00084                                                     double *t_increment_s2_out);
00085
00086
00087         double extend_with_time_optimal_control_one_axis (double s_ini[2], double s_fin[2], double u_max,
00088                                                           int *direction, int *traj_saturated,
00089                                                           double *x_intersect_beg,
00090                                                           double *x_intersect_end,
00091                                                           double *v_intersect);
00092
00093
00094         int extend_with_effort_optimal_control_one_axis (double s_ini[2], double s_fin[2], double u_max,
00095                                                          double t_min, double t_goal, double t_eps,
00096                                                          int *dir, int *traj_saturated, double *max_control,
00097                                                          double *x_intersect_beg, double *x_intersect_end,
00098                                                          double *v_intersect);
00099
00100
00101         int extend_dubins_di (state_t* state_ini, state_t* state_fin,
00102                               int* fully_extends,  list<state_t*> *list_states_out, list<input_t*> *list_inputs_out);
00103
00104
00105
00106     public :
00107
00108         extender_dubins_double_integrator ();
00109         ~extender_dubins_double_integrator ();
00110
00111
00112         int ex_update_insert_vertex (vertex_t *vertex_in) {return 1;}
00113
00114
00115         int ex_update_insert_edge (edge_t *edge_in)  {return 1;}
00116
00117
00118         int ex_update_delete_vertex (vertex_t *vertex_in) {return 1;}
00119
00120
00121         int ex_update_delete_edge (edge_t *edge_in) {return 1;}
00122
00123
00124
00125         int extend (state_t *state_from_in, state_t *state_towards_in,
00126                     int *exact_connection_out, trajectory_t *trajectory_out,
00127                     list<state_t*> *intermediate_vertices_out);
00128
00129     };
00130
00131
00132 }
00133
00134 #endif