#include <model_based_state_space.h>
Classes | |
class | StateType |
Public Member Functions | |
virtual ompl::base::StateSamplerPtr | allocDefaultStateSampler () const |
virtual ompl::base::State * | allocState () const |
virtual void | copyJointToOMPLState (ompl::base::State *state, const robot_state::RobotState &robot_state, const moveit::core::JointModel *joint_model, int ompl_state_joint_index) const |
Copy a single joint's values (which might have multiple variables) from a MoveIt! robot_state to an OMPL state. More... | |
virtual void | copyState (ompl::base::State *destination, const ompl::base::State *source) const |
virtual void | copyToOMPLState (ompl::base::State *state, const robot_state::RobotState &rstate) const |
Copy the data from a set of joint states to an OMPL state. More... | |
virtual void | copyToRobotState (robot_state::RobotState &rstate, const ompl::base::State *state) const |
Copy the data from an OMPL state to a set of joint states. More... | |
virtual void | deserialize (ompl::base::State *state, const void *serialization) const |
virtual double | distance (const ompl::base::State *state1, const ompl::base::State *state2) const |
virtual void | enforceBounds (ompl::base::State *state) const |
virtual bool | equalStates (const ompl::base::State *state1, const ompl::base::State *state2) const |
virtual void | freeState (ompl::base::State *state) const |
virtual unsigned int | getDimension () const |
const robot_model::JointModelGroup * | getJointModelGroup () const |
const std::string & | getJointModelGroupName () const |
const robot_model::JointBoundsVector & | getJointsBounds () const |
virtual double | getMaximumExtent () const |
virtual double | getMeasure () const |
const robot_model::RobotModelConstPtr & | getRobotModel () const |
virtual unsigned int | getSerializationLength () const |
const ModelBasedStateSpaceSpecification & | getSpecification () const |
double | getTagSnapToSegment () const |
virtual double * | getValueAddressAtIndex (ompl::base::State *state, const unsigned int index) const |
virtual void | interpolate (const ompl::base::State *from, const ompl::base::State *to, const double t, ompl::base::State *state) const |
ModelBasedStateSpace (const ModelBasedStateSpaceSpecification &spec) | |
virtual void | printSettings (std::ostream &out) const |
virtual void | printState (const ompl::base::State *state, std::ostream &out) const |
virtual bool | satisfiesBounds (const ompl::base::State *state) const |
virtual void | serialize (void *serialization, const ompl::base::State *state) const |
void | setDistanceFunction (const DistanceFunction &fun) |
void | setInterpolationFunction (const InterpolationFunction &fun) |
virtual void | setPlanningVolume (double minX, double maxX, double minY, double maxY, double minZ, double maxZ) |
Set the planning volume for the possible SE2 and/or SE3 components of the state space. More... | |
void | setTagSnapToSegment (double snap) |
virtual | ~ModelBasedStateSpace () |
Protected Attributes | |
DistanceFunction | distance_function_ |
InterpolationFunction | interpolation_function_ |
std::vector< robot_model::JointModel::Bounds > | joint_bounds_storage_ |
std::vector< const robot_model::JointModel * > | joint_model_vector_ |
ModelBasedStateSpaceSpecification | spec_ |
size_t | state_values_size_ |
double | tag_snap_to_segment_ |
double | tag_snap_to_segment_complement_ |
unsigned int | variable_count_ |
Definition at line 74 of file model_based_state_space.h.
ompl_interface::ModelBasedStateSpace::ModelBasedStateSpace | ( | const ModelBasedStateSpaceSpecification & | spec | ) |
expose parameters
Definition at line 40 of file model_based_state_space.cpp.
|
virtual |
Definition at line 78 of file model_based_state_space.cpp.
|
virtual |
Reimplemented in ompl_interface::PoseModelStateSpace.
Definition at line 250 of file model_based_state_space.cpp.
|
virtual |
Reimplemented in ompl_interface::PoseModelStateSpace.
Definition at line 101 of file model_based_state_space.cpp.
|
virtual |
Copy a single joint's values (which might have multiple variables) from a MoveIt! robot_state to an OMPL state.
state | - output OMPL state with single joint modified |
robot_state | - input MoveIt! state to get the joint value from |
joint_model | - the joint to copy values of |
ompl_state_joint_index | - the index of the joint in the ompl state (passed in for efficiency, you should cache this index) e.g. ompl_state_joint_index = joint_model_group_->getVariableGroupIndex("virtual_joint"); |
Definition at line 335 of file model_based_state_space.cpp.
|
virtual |
Reimplemented in ompl_interface::PoseModelStateSpace.
Definition at line 114 of file model_based_state_space.cpp.
|
virtual |
Copy the data from a set of joint states to an OMPL state.
Reimplemented in ompl_interface::PoseModelStateSpace.
Definition at line 327 of file model_based_state_space.cpp.
|
virtual |
Copy the data from an OMPL state to a set of joint states.
Definition at line 320 of file model_based_state_space.cpp.
|
virtual |
Definition at line 134 of file model_based_state_space.cpp.
|
virtual |
Reimplemented in ompl_interface::PoseModelStateSpace.
Definition at line 168 of file model_based_state_space.cpp.
|
virtual |
Definition at line 187 of file model_based_state_space.cpp.
|
virtual |
Definition at line 177 of file model_based_state_space.cpp.
|
virtual |
Reimplemented in ompl_interface::PoseModelStateSpace.
Definition at line 108 of file model_based_state_space.cpp.
|
virtual |
Definition at line 141 of file model_based_state_space.cpp.
|
inline |
Definition at line 208 of file model_based_state_space.h.
|
inline |
Definition at line 213 of file model_based_state_space.h.
|
inline |
Definition at line 229 of file model_based_state_space.h.
|
virtual |
Reimplemented in ompl_interface::PoseModelStateSpace.
Definition at line 149 of file model_based_state_space.cpp.
|
virtual |
Definition at line 154 of file model_based_state_space.cpp.
|
inline |
Definition at line 203 of file model_based_state_space.h.
|
virtual |
Definition at line 123 of file model_based_state_space.cpp.
|
inline |
Definition at line 218 of file model_based_state_space.h.
double ompl_interface::ModelBasedStateSpace::getTagSnapToSegment | ( | ) | const |
Definition at line 82 of file model_based_state_space.cpp.
|
virtual |
Definition at line 220 of file model_based_state_space.cpp.
|
virtual |
Reimplemented in ompl_interface::PoseModelStateSpace.
Definition at line 198 of file model_based_state_space.cpp.
|
virtual |
Definition at line 289 of file model_based_state_space.cpp.
|
virtual |
Definition at line 294 of file model_based_state_space.cpp.
|
virtual |
Definition at line 192 of file model_based_state_space.cpp.
|
virtual |
Definition at line 128 of file model_based_state_space.cpp.
|
inline |
Definition at line 177 of file model_based_state_space.h.
|
inline |
Definition at line 172 of file model_based_state_space.h.
|
virtual |
Set the planning volume for the possible SE2 and/or SE3 components of the state space.
Reimplemented in ompl_interface::PoseModelStateSpace.
Definition at line 228 of file model_based_state_space.cpp.
void ompl_interface::ModelBasedStateSpace::setTagSnapToSegment | ( | double | snap | ) |
Definition at line 87 of file model_based_state_space.cpp.
|
protected |
Definition at line 266 of file model_based_state_space.h.
|
protected |
Definition at line 265 of file model_based_state_space.h.
|
protected |
Definition at line 260 of file model_based_state_space.h.
|
protected |
Definition at line 261 of file model_based_state_space.h.
|
protected |
Definition at line 259 of file model_based_state_space.h.
|
protected |
Definition at line 263 of file model_based_state_space.h.
|
protected |
Definition at line 268 of file model_based_state_space.h.
|
protected |
Definition at line 269 of file model_based_state_space.h.
|
protected |
Definition at line 262 of file model_based_state_space.h.