Go to the documentation of this file.
40 #include <ompl/base/spaces/SE3StateSpace.h>
44 class PoseModelStateSpace :
public ModelBasedStateSpace
49 class StateType :
public ModelBasedStateSpace::StateType
89 ompl::base::SE3StateSpace::StateType**
poses;
95 ompl::base::State*
allocState()
const override;
96 void freeState(ompl::base::State* state)
const override;
97 void copyState(ompl::base::State* destination,
const ompl::base::State* source)
const override;
98 void interpolate(
const ompl::base::State* from,
const ompl::base::State* to,
const double t,
99 ompl::base::State* state)
const override;
100 double distance(
const ompl::base::State* state1,
const ompl::base::State* state2)
const override;
109 void setPlanningVolume(
double minX,
double maxX,
double minY,
double maxY,
double minZ,
double maxZ)
override;
139 std::vector<PoseComponent>
poses_;
double distance(const ompl::base::State *state1, const ompl::base::State *state2) const override
const moveit::core::JointModelGroup * subgroup_
bool computeStateK(ompl::base::State *state) const
bool computeStateFK(ompl::base::State *state) const
bool operator<(const PoseComponent &o) const
static const std::string PARAMETERIZATION_TYPE
void setPoseComputed(bool value)
ompl::base::SE3StateSpace::StateType ** poses
ompl::base::StateSamplerPtr allocDefaultStateSampler() const override
bool jointsComputed() const
The MoveIt interface to OMPL.
std::vector< std::string > fk_link_
bool computeStateIK(ompl::base::State *state) const
bool poseComputed() const
const std::string & getParameterizationType() const override
ompl::base::State * allocState() const override
~PoseModelStateSpace() override
const std::string & getName() const
kinematics::KinematicsBasePtr kinematics_solver_
void freeState(ompl::base::State *state) const override
void interpolate(const ompl::base::State *from, const ompl::base::State *to, const double t, ompl::base::State *state) const override
void copyState(ompl::base::State *destination, const ompl::base::State *source) const override
std::vector< PoseComponent > poses_
std::vector< unsigned int > bijection_
PoseModelStateSpace(const ModelBasedStateSpaceSpecification &spec)
ModelBasedStateSpace(ModelBasedStateSpaceSpecification spec)
void copyToOMPLState(ompl::base::State *state, const moveit::core::RobotState &rstate) const override
Copy the data from a set of joint states to an OMPL state.
double getMaximumExtent() const override
void sanityChecks() const override
PoseComponent(const moveit::core::JointModelGroup *subgroup, const moveit::core::JointModelGroup::KinematicsSolver &k)
bool computeStateIK(StateType *full_state, unsigned int idx) const
void setJointsComputed(bool value)
bool computeStateFK(StateType *full_state, unsigned int idx) const
void setPlanningVolume(double minX, double maxX, double minY, double maxY, double minZ, double maxZ) override
Set the planning volume for the possible SE2 and/or SE3 components of the state space.
ompl::base::StateSpacePtr state_space_
ompl
Author(s): Ioan Sucan
autogenerated on Fri May 3 2024 02:29:39