pose_model_state_space.h
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2012, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #ifndef MOVEIT_OMPL_INTERFACE_PARAMETERIZATION_WORK_SPACE_POSE_MODEL_STATE_SPACE_
38 #define MOVEIT_OMPL_INTERFACE_PARAMETERIZATION_WORK_SPACE_POSE_MODEL_STATE_SPACE_
39 
41 #include <ompl/base/spaces/SE3StateSpace.h>
42 
43 namespace ompl_interface
44 {
46 {
47 public:
48  static const std::string PARAMETERIZATION_TYPE;
49 
51  {
52  public:
53  enum
54  {
57  };
58 
60  {
62  }
63 
64  bool jointsComputed() const
65  {
66  return flags & JOINTS_COMPUTED;
67  }
68 
69  bool poseComputed() const
70  {
71  return flags & POSE_COMPUTED;
72  }
73 
74  void setJointsComputed(bool value)
75  {
76  if (value)
78  else
80  }
81 
82  void setPoseComputed(bool value)
83  {
84  if (value)
86  else
87  flags &= ~POSE_COMPUTED;
88  }
89 
90  ompl::base::SE3StateSpace::StateType** poses;
91  };
92 
94  virtual ~PoseModelStateSpace();
95 
96  virtual ompl::base::State* allocState() const;
97  virtual void freeState(ompl::base::State* state) const;
98  virtual void copyState(ompl::base::State* destination, const ompl::base::State* source) const;
99  virtual void interpolate(const ompl::base::State* from, const ompl::base::State* to, const double t,
100  ompl::base::State* state) const;
101  virtual double distance(const ompl::base::State* state1, const ompl::base::State* state2) const;
102  virtual double getMaximumExtent() const;
103 
104  virtual ompl::base::StateSamplerPtr allocDefaultStateSampler() const;
105 
106  bool computeStateFK(ompl::base::State* state) const;
107  bool computeStateIK(ompl::base::State* state) const;
108  bool computeStateK(ompl::base::State* state) const;
109 
110  virtual void setPlanningVolume(double minX, double maxX, double minY, double maxY, double minZ, double maxZ);
111  virtual void copyToOMPLState(ompl::base::State* state, const robot_state::RobotState& rstate) const;
112  virtual void sanityChecks() const;
113 
114 private:
116  {
117  PoseComponent(const robot_model::JointModelGroup* subgroup,
118  const robot_model::JointModelGroup::KinematicsSolver& k);
119 
120  bool computeStateFK(StateType* full_state, unsigned int idx) const;
121  bool computeStateIK(StateType* full_state, unsigned int idx) const;
122 
123  bool operator<(const PoseComponent& o) const
124  {
125  return subgroup_->getName() < o.subgroup_->getName();
126  }
127 
128  const robot_model::JointModelGroup* subgroup_;
129  kinematics::KinematicsBasePtr kinematics_solver_;
130  std::vector<unsigned int> bijection_;
131  ompl::base::StateSpacePtr state_space_;
132  std::vector<std::string> fk_link_;
133  };
134 
135  std::vector<PoseComponent> poses_;
136  double jump_factor_;
137 };
138 }
139 
140 #endif
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.
virtual ompl::base::StateSamplerPtr allocDefaultStateSampler() const
static const std::string PARAMETERIZATION_TYPE
bool computeStateK(ompl::base::State *state) const
The MoveIt interface to OMPL.
ompl::base::SE3StateSpace::StateType ** poses
virtual void copyState(ompl::base::State *destination, const ompl::base::State *source) const
virtual ompl::base::State * allocState() const
bool computeStateIK(ompl::base::State *state) 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.
PoseModelStateSpace(const ModelBasedStateSpaceSpecification &spec)
bool computeStateFK(ompl::base::State *state) const
virtual void interpolate(const ompl::base::State *from, const ompl::base::State *to, const double t, ompl::base::State *state) const
virtual void freeState(ompl::base::State *state) const


ompl
Author(s): Ioan Sucan
autogenerated on Sat Apr 21 2018 02:55:34