amino
Lightweight Robot Utility Library
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
scene_ompl.h
Go to the documentation of this file.
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 
64 namespace amino {
65 
72 class sgStateSpace : public ompl::base::RealVectorStateSpace {
73 public:
74 
78  sgStateSpace( const struct aa_rx_sg_sub *sub_sg ) :
79  scene_graph(sub_sg->scenegraph),
80  sub_scene_graph(sub_sg),
81  ompl::base::RealVectorStateSpace((unsigned)aa_rx_sg_sub_config_count(sub_sg)),
82  allowed(aa_rx_cl_set_create(scene_graph)) {
83 
84  aa_mem_region_init(&reg, 4000);
85 
86  // TODO: get actual bounds
87  size_t n_configs = config_count_subset();
88  ompl::base::RealVectorBounds vb( (unsigned int)n_configs );
89  for( unsigned i = 0; i < (unsigned)n_configs; i ++ ) {
90  double min,max;
91  aa_rx_config_id cid = aa_rx_sg_sub_config(sub_scene_graph, i);
92  int r = aa_rx_sg_get_limit_pos(scene_graph, cid, &min, &max);
93  if(r) {
94  fprintf(stderr, "ERROR: no position limits for %s\n",
95  aa_rx_sg_config_name(scene_graph, cid));
96  /* This seems as good as anything */
97  min = -M_PI;
98  max = M_PI;
99  }
100 
101  vb.setLow(i,min);
102  vb.setHigh(i,max);
103 
104  }
105  setBounds(vb);
106  // // allowed
107  // size_t n_q = config_count_all();
108  // double q[n_q];
109  // AA_MEM_ZERO(q, n_q); // TODO: give good config
110  // allow_config(q);
111 
112  // Load allowable configs from scenegraph
113  aa_rx_sg_cl_set_copy(scene_graph, allowed);
114  }
115 
119  virtual ~sgStateSpace() {
120  aa_mem_region_destroy(&reg);
121  aa_rx_cl_set_destroy(allowed);
122  }
123 
127  const aa_rx_sg *get_scene_graph() const {
128  return scene_graph;
129  }
130 
134  size_t config_count_all() const {
136  }
137 
141  size_t config_count_subset() const {
142  return getDimension();
143  }
144 
148  size_t frame_count() const {
150  }
151 
155  void allow_config( double *q ) {
156  aa_rx_sg_get_collision(scene_graph, q, allowed);
157  }
158 
163  void extract_state( const double *q_all, double *q_set ) const {
164  aa_rx_sg_sub_config_get( sub_scene_graph,
165  config_count_all(), q_all,
166  config_count_subset(), q_set );
167  }
168 
169  void extract_state( const double *q_all, StateType *state ) const {
170  extract_state( q_all, state->values );
171  }
172 
173  void insert_state( const double *q_set, double *q_all ) const {
174  aa_rx_sg_sub_config_set( sub_scene_graph,
175  config_count_subset(), q_set,
176  config_count_all(), q_all );
177  }
178 
179  void insert_state( const StateType *state, double *q_all ) const {
180  insert_state( state->values, q_all );
181  }
182 
183  void copy_state( const double *q_set, StateType *state ) {
184  std::copy( q_set, q_set + config_count_subset(), state->values );
185  }
186 
187 
188  const aa_rx_sg *scene_graph;
189  const aa_rx_sg_sub *sub_scene_graph;
190  struct aa_rx_cl_set *allowed;
191  struct aa_mem_region reg;
192 };
193 
194 typedef ::ompl::base::TypedSpaceInformation<amino::sgStateSpace> sgSpaceInformation;
195 
196 
197 class sgStateValidityChecker : public ::ompl::base::TypedStateValidityChecker<sgStateSpace> {
198 public:
199  sgStateValidityChecker(sgSpaceInformation *si,
200  const double *q_initial ) :
201  TypedStateValidityChecker(si),
202  q_all(new double[getTypedStateSpace()->config_count_all()]) {
203  size_t n_all = getTypedStateSpace()->config_count_all();
204  std::copy( q_initial, q_initial + n_all, q_all );
205  }
206 
208  delete [] q_all;
209  }
210 
211  virtual bool isValid(const ompl::base::State *state_) const
212  {
213  const sgSpaceInformation::StateType *state = state_as(state_);
214  sgStateSpace *space = getTypedStateSpace();
215  size_t n_q = space->config_count_all();
216  size_t n_s = space->config_count_subset();
217  size_t n_f = space->frame_count();
218 
219  // Set configs
220 
221  double q[n_q];
222  std::copy( q_all, q_all + n_q, q );
223  space->insert_state(state, q);
224  //aa_dump_vec(stdout, q, n_q);
225 
226  // Find TFs
227  double TF_rel[7*n_f];
228  double TF_abs[7*n_f];
229  aa_rx_sg_tf( space->scene_graph, n_q, q,
230  n_f,
231  TF_rel, 7,
232  TF_abs, 7 );
233 
234  // check collision
235  struct aa_rx_cl *cl = aa_rx_cl_create( space->scene_graph );
236  aa_rx_cl_allow_set( cl, space->allowed );
237  int col = aa_rx_cl_check( cl, n_f, TF_abs, 7, NULL );
238  aa_rx_cl_destroy(cl);
239 
240  bool valid = !col;
241  return valid;
242  }
243  double *q_all;
244 };
245 
246 
247 } /* namespace amino */
248 
249 struct aa_rx_mp {
250  aa_rx_mp( const struct aa_rx_sg_sub *sub_sg ) :
251  config_start(NULL),
252  space_information(
253  new amino::sgSpaceInformation(
254  amino::sgSpaceInformation::SpacePtr(
255  new amino::sgStateSpace (sub_sg)))),
256  problem_definition(new ompl::base::ProblemDefinition(space_information)),
257  simplify(0)
258  { }
259  ~aa_rx_mp();
260 
261  void set_planner( ompl::base::Planner *p ) {
262  this->planner.reset(p);
263  }
264 
265  amino::sgSpaceInformation::Ptr space_information;
266  ompl::base::ProblemDefinitionPtr problem_definition;
267 
268  ompl::base::PlannerPtr planner;
269 
270  double *config_start;
271 
272  unsigned simplify : 1;
273 };
274 
275 #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.
size_t frame_count() const
Return the number of frames in the full scenegraph.
Definition: scene_ompl.h:148
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.
Collision checking.
sgStateSpace(const struct aa_rx_sg_sub *sub_sg)
Create a state space for the sub-scenegraph `sub_sg'.
Definition: scene_ompl.h:78
size_t config_count_subset() const
Return the number of configuration variables in the sub-scenegraph.
Definition: scene_ompl.h:141
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.
The scenegraph data structure.
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:198
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.
Error codes and functions.
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.
void allow_config(double *q)
Mark configuration q as allowed.
Definition: scene_ompl.h:155
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
size_t config_count_all() const
Return the number of configuration variables in the full scenegraph.
Definition: scene_ompl.h:134
virtual ~sgStateSpace()
Destroy the state space.
Definition: scene_ompl.h:119
void extract_state(const double *q_all, double *q_set) const
Retrieve the sub-scenegraph configuration `q_set' from the full scenegraph array `q_all'.
Definition: scene_ompl.h:163
Scenegraph-related type declarations.
const aa_rx_sg * get_scene_graph() const
Return the scene graph for the state space.
Definition: scene_ompl.h:127
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.
Motion Planning.
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.
An OMPL state space for an amino scene graph.
Definition: scene_ompl.h:72
signed long aa_rx_config_id
Type for configuration indices.
Definition: scenegraph.h:54
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.