amino
Lightweight Robot Utility Library
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
scene_ompl.h
1 /* -*- mode: C++; c-basic-offset: 4; -*- */
2 /* ex: set shiftwidth=4 tabstop=4 expandtab: */
3 /*
4  * Copyright (c) 2015, Rice University
5  * All rights reserved.
6  *
7  * Author(s): Neil T. Dantam <ntd@rice.edu>
8  *
9  * Redistribution and use in source and binary forms, with or
10  * without modification, are permitted provided that the following
11  * conditions are met:
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of copyright holder the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
23  * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
24  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
25  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
26  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
27  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
28  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
29  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
30  * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
31  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  */
37 
38 #ifndef AMINO_RX_SCENE_OMPL_H
39 #define AMINO_RX_SCENE_OMPL_H
40 
41 #include "rxerr.h"
42 #include "rxtype.h"
43 #include "scenegraph.h"
44 #include "scene_kin_internal.h"
45 #include "scene_collision.h"
46 #include "scene_planning.h"
47 
48 #include <ompl/base/StateValidityChecker.h>
49 #include <ompl/base/spaces/RealVectorStateSpace.h>
50 #include <ompl/base/ProblemDefinition.h>
51 #include <ompl/base/spaces/RealVectorBounds.h>
52 
53 #include <ompl/base/ScopedState.h>
54 #include <ompl/base/TypedSpaceInformation.h>
55 #include <ompl/base/TypedStateValidityChecker.h>
56 #include <ompl/base/Planner.h>
57 
58 namespace amino {
59 
60 
61 class sgStateSpace : public ompl::base::RealVectorStateSpace {
62 public:
63  sgStateSpace( const struct aa_rx_sg_sub *sub_sg ) :
64  scene_graph(sub_sg->scenegraph),
65  sub_scene_graph(sub_sg),
66  ompl::base::RealVectorStateSpace((unsigned)aa_rx_sg_sub_config_count(sub_sg)),
67  allowed(aa_rx_cl_set_create(scene_graph)) {
68 
69  aa_mem_region_init(&reg, 4000);
70 
71  // TODO: get actual bounds
72  size_t n_configs = config_count_subset();
73  ompl::base::RealVectorBounds vb( (unsigned int)n_configs );
74  for( unsigned i = 0; i < (unsigned)n_configs; i ++ ) {
75  double min,max;
76  aa_rx_config_id cid = aa_rx_sg_sub_config(sub_scene_graph, i);
77  int r = aa_rx_sg_get_limit_pos(scene_graph, cid, &min, &max);
78  if(r) {
79  fprintf(stderr, "ERROR: no position limits for %s\n",
80  aa_rx_sg_config_name(scene_graph, cid));
81  /* This seems as good as anything */
82  min = -M_PI;
83  max = M_PI;
84  }
85 
86  vb.setLow(i,min);
87  vb.setHigh(i,max);
88 
89  }
90  setBounds(vb);
91  // // allowed
92  // size_t n_q = config_count_all();
93  // double q[n_q];
94  // AA_MEM_ZERO(q, n_q); // TODO: give good config
95  // allow_config(q);
96 
97  // Load allowable configs from scenegraph
98  aa_rx_sg_cl_set_copy(scene_graph, allowed);
99  }
100 
101  virtual ~sgStateSpace() {
102  aa_mem_region_destroy(&reg);
103  aa_rx_cl_set_destroy(allowed);
104  }
105 
106  const aa_rx_sg *get_scene_graph() const {
107  return scene_graph;
108  }
109 
110  size_t config_count_all() const {
111  return aa_rx_sg_config_count(get_scene_graph());
112  }
113  size_t config_count_subset() const {
114  return getDimension();
115  }
116 
117  size_t frame_count() const {
118  return aa_rx_sg_frame_count(get_scene_graph());
119  }
120 
121  void allow_config( double *q ) {
122  aa_rx_sg_get_collision(scene_graph, q, allowed);
123  }
124 
125  void extract_state( const double *q_all, double *q_set ) const {
126  aa_rx_sg_sub_config_get( sub_scene_graph,
127  config_count_all(), q_all,
128  config_count_subset(), q_set );
129  }
130 
131  void extract_state( const double *q_all, StateType *state ) const {
132  extract_state( q_all, state->values );
133  }
134 
135  void insert_state( const double *q_set, double *q_all ) const {
136  aa_rx_sg_sub_config_set( sub_scene_graph,
137  config_count_subset(), q_set,
138  config_count_all(), q_all );
139  }
140 
141  void insert_state( const StateType *state, double *q_all ) const {
142  insert_state( state->values, q_all );
143  }
144 
145  void copy_state( const double *q_set, StateType *state ) {
146  std::copy( q_set, q_set + config_count_subset(), state->values );
147  }
148 
149 
150  const aa_rx_sg *scene_graph;
151  const aa_rx_sg_sub *sub_scene_graph;
152  struct aa_rx_cl_set *allowed;
153  struct aa_mem_region reg;
154 };
155 
156 typedef ::ompl::base::TypedSpaceInformation<amino::sgStateSpace> sgSpaceInformation;
157 
158 
159 class sgStateValidityChecker : public ::ompl::base::TypedStateValidityChecker<sgStateSpace> {
160 public:
161  sgStateValidityChecker(sgSpaceInformation *si,
162  const double *q_initial ) :
163  TypedStateValidityChecker(si),
164  q_all(new double[getTypedStateSpace()->config_count_all()]) {
165  size_t n_all = getTypedStateSpace()->config_count_all();
166  std::copy( q_initial, q_initial + n_all, q_all );
167  }
168 
170  delete [] q_all;
171  }
172 
173  virtual bool isValid(const ompl::base::State *state_) const
174  {
175  const sgSpaceInformation::StateType *state = state_as(state_);
176  sgStateSpace *space = getTypedStateSpace();
177  size_t n_q = space->config_count_all();
178  size_t n_s = space->config_count_subset();
179  size_t n_f = space->frame_count();
180 
181  // Set configs
182 
183  double q[n_q];
184  std::copy( q_all, q_all + n_q, q );
185  space->insert_state(state, q);
186  //aa_dump_vec(stdout, q, n_q);
187 
188  // Find TFs
189  double TF_rel[7*n_f];
190  double TF_abs[7*n_f];
191  aa_rx_sg_tf( space->scene_graph, n_q, q,
192  n_f,
193  TF_rel, 7,
194  TF_abs, 7 );
195 
196  // check collision
197  struct aa_rx_cl *cl = aa_rx_cl_create( space->scene_graph );
198  aa_rx_cl_allow_set( cl, space->allowed );
199  int col = aa_rx_cl_check( cl, n_f, TF_abs, 7, NULL );
200  aa_rx_cl_destroy(cl);
201 
202  bool valid = !col;
203  return valid;
204  }
205  double *q_all;
206 };
207 
208 
209 } /* namespace amino */
210 
211 struct aa_rx_mp {
212  aa_rx_mp( const struct aa_rx_sg_sub *sub_sg ) :
213  config_start(NULL),
214  space_information(
215  new amino::sgSpaceInformation(
216  amino::sgSpaceInformation::SpacePtr(
217  new amino::sgStateSpace (sub_sg)))),
218  problem_definition(new ompl::base::ProblemDefinition(space_information)),
219  simplify(0)
220  { }
221  ~aa_rx_mp();
222 
223  void set_planner( ompl::base::Planner *p ) {
224  this->planner.reset(p);
225  }
226 
227  amino::sgSpaceInformation::Ptr space_information;
228  ompl::base::ProblemDefinitionPtr problem_definition;
229 
230  ompl::base::PlannerPtr planner;
231 
232  double *config_start;
233 
234  unsigned simplify : 1;
235 };
236 
237 #endif /*AMINO_RX_SCENE_OMPL_H*/
AA_API struct aa_rx_cl_set * aa_rx_cl_set_create(const struct aa_rx_sg *sg)
Create a collision set.
AA_API void aa_rx_cl_set_destroy(struct aa_rx_cl_set *cl_set)
Destroy a collision set.
AA_API const char * aa_rx_sg_config_name(const struct aa_rx_sg *scene_graph, aa_rx_config_id config_id)
Return the config of the given frame.
AA_API void aa_mem_region_init(aa_mem_region_t *region, size_t size)
Initialize memory region with an initial chunk of size bytes.
AA_API void aa_rx_sg_tf(const struct aa_rx_sg *scene_graph, size_t n_q, const double *q, size_t n_tf, double *TF_rel, size_t ld_rel, double *TF_abs, size_t ld_abs)
Compute transforms for the scene graph.
AA_API void aa_rx_cl_destroy(struct aa_rx_cl *cl)
Destroy a collision detection context.
AA_API struct aa_rx_cl * aa_rx_cl_create(const struct aa_rx_sg *scene_graph)
Create a new collision detection context for scene_graph.
Data Structure for Region-Based memory allocation.
Definition: mem.h:197
AA_API size_t aa_rx_sg_sub_config_count(const struct aa_rx_sg_sub *sg_sub)
Return the number of configuration variables in the scenegraph subset.
Opaque type for a scene_graph.
AA_API size_t aa_rx_sg_frame_count(const struct aa_rx_sg *scene_graph)
Return the number of frames in scene_graph.
AA_API int aa_rx_sg_get_limit_pos(const struct aa_rx_sg *scenegraph, aa_rx_config_id config_id, double *min, double *max)
Get position limit values.
amino namespace
Definition: amino.hpp:60
AA_API size_t aa_rx_sg_config_count(const struct aa_rx_sg *scene_graph)
Return the number of configuration variables in scene_graph.
AA_API void aa_rx_cl_allow_set(struct aa_rx_cl *cl, const struct aa_rx_cl_set *set)
Allow collisions between all frame pairs in set.
AA_API void aa_mem_region_destroy(aa_mem_region_t *region)
Destroy memory region freeing all chunks.
signed long aa_rx_config_id
Type for configuration indices.
Definition: scenegraph.h:53
AA_API aa_rx_config_id aa_rx_sg_sub_config(const struct aa_rx_sg_sub *sg_sub, size_t i)
Return the full scenegraph config id for the i'th configuration of the sub-scenegraph.
AA_API int aa_rx_cl_check(struct aa_rx_cl *cl, size_t n_tf, const double *TF, size_t ldTF, struct aa_rx_cl_set *cl_set)
Detect collisions.