38 #ifndef AMINO_RX_SCENE_OMPL_H
39 #define AMINO_RX_SCENE_OMPL_H
44 #include "scene_kin_internal.h"
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>
53 #include <ompl/base/ScopedState.h>
54 #include <ompl/base/TypedSpaceInformation.h>
55 #include <ompl/base/TypedStateValidityChecker.h>
56 #include <ompl/base/Planner.h>
79 scene_graph(sub_sg->scenegraph),
80 sub_scene_graph(sub_sg),
88 ompl::base::RealVectorBounds vb( (
unsigned int)n_configs );
89 for(
unsigned i = 0; i < (unsigned)n_configs; i ++ ) {
94 fprintf(stderr,
"ERROR: no position limits for %s\n",
113 aa_rx_sg_cl_set_copy(scene_graph, allowed);
142 return getDimension();
156 aa_rx_sg_get_collision(scene_graph, q, allowed);
164 aa_rx_sg_sub_config_get( sub_scene_graph,
169 void extract_state(
const double *q_all, StateType *state )
const {
173 void insert_state(
const double *q_set,
double *q_all )
const {
174 aa_rx_sg_sub_config_set( sub_scene_graph,
179 void insert_state(
const StateType *state,
double *q_all )
const {
180 insert_state( state->values, q_all );
183 void copy_state(
const double *q_set, StateType *state ) {
189 const aa_rx_sg_sub *sub_scene_graph;
190 struct aa_rx_cl_set *allowed;
194 typedef ::ompl::base::TypedSpaceInformation<amino::sgStateSpace> sgSpaceInformation;
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 );
211 virtual bool isValid(
const ompl::base::State *state_)
const
213 const sgSpaceInformation::StateType *state = state_as(state_);
222 std::copy( q_all, q_all + n_q, q );
223 space->insert_state(state, q);
227 double TF_rel[7*n_f];
228 double TF_abs[7*n_f];
250 aa_rx_mp(
const struct aa_rx_sg_sub *sub_sg ) :
253 new amino::sgSpaceInformation(
254 amino::sgSpaceInformation::SpacePtr(
256 problem_definition(
new ompl::base::ProblemDefinition(space_information)),
261 void set_planner( ompl::base::Planner *p ) {
262 this->planner.reset(p);
265 amino::sgSpaceInformation::Ptr space_information;
266 ompl::base::ProblemDefinitionPtr problem_definition;
268 ompl::base::PlannerPtr planner;
270 double *config_start;
272 unsigned simplify : 1;
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.
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.
sgStateSpace(const struct aa_rx_sg_sub *sub_sg)
Create a state space for the sub-scenegraph `sub_sg'.
size_t config_count_subset() const
Return the number of configuration variables in the sub-scenegraph.
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.
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.
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.
size_t config_count_all() const
Return the number of configuration variables in the full scenegraph.
virtual ~sgStateSpace()
Destroy the state space.
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'.
Scenegraph-related type declarations.
const aa_rx_sg * get_scene_graph() const
Return the scene graph for the state space.
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.
An OMPL state space for an amino scene graph.
signed long aa_rx_config_id
Type for configuration indices.
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.