model_based_state_space.cpp
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 
38 #include <boost/bind.hpp>
39 
41  : ompl::base::StateSpace(), spec_(spec)
42 {
43  // set the state space name
44  setName(spec_.joint_model_group_->getName());
45  variable_count_ = spec_.joint_model_group_->getVariableCount();
46  state_values_size_ = variable_count_ * sizeof(double);
47  joint_model_vector_ = spec_.joint_model_group_->getActiveJointModels();
48 
49  // make sure we have bounds for every joint stored within the spec (use default bounds if not specified)
50  if (!spec_.joint_bounds_.empty() && spec_.joint_bounds_.size() != joint_model_vector_.size())
51  {
52  logError("Joint group '%s' has incorrect bounds specified. Using the default bounds instead.",
53  spec_.joint_model_group_->getName().c_str());
54  spec_.joint_bounds_.clear();
55  }
56 
57  // copy the default joint bounds if needed
58  if (spec_.joint_bounds_.empty())
59  spec_.joint_bounds_ = spec_.joint_model_group_->getActiveJointModelsBounds();
60 
61  // new perform a deep copy of the bounds, in case we need to modify them
63  for (std::size_t i = 0; i < joint_bounds_storage_.size(); ++i)
64  {
67  }
68 
69  // default settings
70  setTagSnapToSegment(0.95);
71 
73  params_.declareParam<double>("tag_snap_to_segment", boost::bind(&ModelBasedStateSpace::setTagSnapToSegment, this, _1),
74  boost::bind(&ModelBasedStateSpace::getTagSnapToSegment, this));
75 }
76 
78 {
79 }
80 
82 {
83  return tag_snap_to_segment_;
84 }
85 
87 {
88  if (snap < 0.0 || snap > 1.0)
89  logWarn("Snap to segment for tags is a ratio. It's value must be between 0.0 and 1.0. Value remains as previously "
90  "set (%lf)",
92  else
93  {
94  tag_snap_to_segment_ = snap;
96  }
97 }
98 
100 {
101  StateType* state = new StateType();
102  state->values = new double[variable_count_];
103  return state;
104 }
105 
106 void ompl_interface::ModelBasedStateSpace::freeState(ompl::base::State* state) const
107 {
108  delete[] state->as<StateType>()->values;
109  delete state->as<StateType>();
110 }
111 
112 void ompl_interface::ModelBasedStateSpace::copyState(ompl::base::State* destination,
113  const ompl::base::State* source) const
114 {
115  memcpy(destination->as<StateType>()->values, source->as<StateType>()->values, state_values_size_);
116  destination->as<StateType>()->tag = source->as<StateType>()->tag;
117  destination->as<StateType>()->flags = source->as<StateType>()->flags;
118  destination->as<StateType>()->distance = source->as<StateType>()->distance;
119 }
120 
122 {
123  return state_values_size_ + sizeof(int);
124 }
125 
126 void ompl_interface::ModelBasedStateSpace::serialize(void* serialization, const ompl::base::State* state) const
127 {
128  *reinterpret_cast<int*>(serialization) = state->as<StateType>()->tag;
129  memcpy(reinterpret_cast<char*>(serialization) + sizeof(int), state->as<StateType>()->values, state_values_size_);
130 }
131 
132 void ompl_interface::ModelBasedStateSpace::deserialize(ompl::base::State* state, const void* serialization) const
133 {
134  state->as<StateType>()->tag = *reinterpret_cast<const int*>(serialization);
135  memcpy(state->as<StateType>()->values, reinterpret_cast<const char*>(serialization) + sizeof(int),
137 }
138 
140 {
141  unsigned int d = 0;
142  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
143  d += joint_model_vector_[i]->getStateSpaceDimension();
144  return d;
145 }
146 
148 {
149  return spec_.joint_model_group_->getMaximumExtent(spec_.joint_bounds_);
150 }
151 
153 {
154  double m = 1.0;
155  for (std::size_t i = 0; i < spec_.joint_bounds_.size(); ++i)
156  {
157  const robot_model::JointModel::Bounds& bounds = *spec_.joint_bounds_[i];
158  for (std::size_t j = 0; j < bounds.size(); ++j)
159  {
160  m *= bounds[j].max_position_ - bounds[j].min_position_;
161  }
162  }
163  return m;
164 }
165 
166 double ompl_interface::ModelBasedStateSpace::distance(const ompl::base::State* state1,
167  const ompl::base::State* state2) const
168 {
169  if (distance_function_)
170  return distance_function_(state1, state2);
171  else
172  return spec_.joint_model_group_->distance(state1->as<StateType>()->values, state2->as<StateType>()->values);
173 }
174 
175 bool ompl_interface::ModelBasedStateSpace::equalStates(const ompl::base::State* state1,
176  const ompl::base::State* state2) const
177 {
178  for (unsigned int i = 0; i < variable_count_; ++i)
179  if (fabs(state1->as<StateType>()->values[i] - state2->as<StateType>()->values[i]) >
180  std::numeric_limits<double>::epsilon())
181  return false;
182  return true;
183 }
184 
185 void ompl_interface::ModelBasedStateSpace::enforceBounds(ompl::base::State* state) const
186 {
187  spec_.joint_model_group_->enforcePositionBounds(state->as<StateType>()->values, spec_.joint_bounds_);
188 }
189 
190 bool ompl_interface::ModelBasedStateSpace::satisfiesBounds(const ompl::base::State* state) const
191 {
192  return spec_.joint_model_group_->satisfiesPositionBounds(state->as<StateType>()->values, spec_.joint_bounds_,
193  std::numeric_limits<double>::epsilon());
194 }
195 
196 void ompl_interface::ModelBasedStateSpace::interpolate(const ompl::base::State* from, const ompl::base::State* to,
197  const double t, ompl::base::State* state) const
198 {
199  // clear any cached info (such as validity known or not)
200  state->as<StateType>()->clearKnownInformation();
201 
202  if (!interpolation_function_ || !interpolation_function_(from, to, t, state))
203  {
204  // perform the actual interpolation
205  spec_.joint_model_group_->interpolate(from->as<StateType>()->values, to->as<StateType>()->values, t,
206  state->as<StateType>()->values);
207 
208  // compute tag
209  if (from->as<StateType>()->tag >= 0 && t < 1.0 - tag_snap_to_segment_)
210  state->as<StateType>()->tag = from->as<StateType>()->tag;
211  else if (to->as<StateType>()->tag >= 0 && t > tag_snap_to_segment_)
212  state->as<StateType>()->tag = to->as<StateType>()->tag;
213  else
214  state->as<StateType>()->tag = -1;
215  }
216 }
217 
219  const unsigned int index) const
220 {
221  if (index >= variable_count_)
222  return NULL;
223  return state->as<StateType>()->values + index;
224 }
225 
226 void ompl_interface::ModelBasedStateSpace::setPlanningVolume(double minX, double maxX, double minY, double maxY,
227  double minZ, double maxZ)
228 {
229  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
230  if (joint_model_vector_[i]->getType() == robot_model::JointModel::PLANAR)
231  {
232  joint_bounds_storage_[i][0].min_position_ = minX;
233  joint_bounds_storage_[i][0].max_position_ = maxX;
234  joint_bounds_storage_[i][1].min_position_ = minY;
235  joint_bounds_storage_[i][1].max_position_ = maxY;
236  }
237  else if (joint_model_vector_[i]->getType() == robot_model::JointModel::FLOATING)
238  {
239  joint_bounds_storage_[i][0].min_position_ = minX;
240  joint_bounds_storage_[i][0].max_position_ = maxX;
241  joint_bounds_storage_[i][1].min_position_ = minY;
242  joint_bounds_storage_[i][1].max_position_ = maxY;
243  joint_bounds_storage_[i][2].min_position_ = minZ;
244  joint_bounds_storage_[i][2].max_position_ = maxZ;
245  }
246 }
247 
249 {
250  class DefaultStateSampler : public ompl::base::StateSampler
251  {
252  public:
253  DefaultStateSampler(const ompl::base::StateSpace* space, const robot_model::JointModelGroup* group,
254  const robot_model::JointBoundsVector* joint_bounds)
255  : ompl::base::StateSampler(space), joint_model_group_(group), joint_bounds_(joint_bounds)
256  {
257  }
258 
259  virtual void sampleUniform(ompl::base::State* state)
260  {
261  joint_model_group_->getVariableRandomPositions(moveit_rng_, state->as<StateType>()->values, *joint_bounds_);
262  state->as<StateType>()->clearKnownInformation();
263  }
264 
265  virtual void sampleUniformNear(ompl::base::State* state, const ompl::base::State* near, const double distance)
266  {
267  joint_model_group_->getVariableRandomPositionsNearBy(moveit_rng_, state->as<StateType>()->values, *joint_bounds_,
268  near->as<StateType>()->values, distance);
269  state->as<StateType>()->clearKnownInformation();
270  }
271 
272  virtual void sampleGaussian(ompl::base::State* state, const ompl::base::State* mean, const double stdDev)
273  {
274  sampleUniformNear(state, mean, rng_.gaussian(0.0, stdDev));
275  }
276 
277  protected:
279  const robot_model::JointModelGroup* joint_model_group_;
280  const robot_model::JointBoundsVector* joint_bounds_;
281  };
282 
283  return ompl::base::StateSamplerPtr(static_cast<ompl::base::StateSampler*>(
284  new DefaultStateSampler(this, spec_.joint_model_group_, &spec_.joint_bounds_)));
285 }
286 
288 {
289  out << "ModelBasedStateSpace '" << getName() << "' at " << this << std::endl;
290 }
291 
292 void ompl_interface::ModelBasedStateSpace::printState(const ompl::base::State* state, std::ostream& out) const
293 {
294  for (std::size_t j = 0; j < joint_model_vector_.size(); ++j)
295  {
296  out << joint_model_vector_[j]->getName() << " = ";
297  const int idx = spec_.joint_model_group_->getVariableGroupIndex(joint_model_vector_[j]->getName());
298  const int vc = joint_model_vector_[j]->getVariableCount();
299  for (int i = 0; i < vc; ++i)
300  out << state->as<StateType>()->values[idx + i] << " ";
301  out << std::endl;
302  }
303 
304  if (state->as<StateType>()->isStartState())
305  out << "* start state" << std::endl;
306  if (state->as<StateType>()->isGoalState())
307  out << "* goal state" << std::endl;
308  if (state->as<StateType>()->isValidityKnown())
309  {
310  if (state->as<StateType>()->isMarkedValid())
311  out << "* valid state" << std::endl;
312  else
313  out << "* invalid state" << std::endl;
314  }
315  out << "Tag: " << state->as<StateType>()->tag << std::endl;
316 }
317 
318 void ompl_interface::ModelBasedStateSpace::copyToRobotState(robot_state::RobotState& rstate,
319  const ompl::base::State* state) const
320 {
321  rstate.setJointGroupPositions(spec_.joint_model_group_, state->as<StateType>()->values);
322  rstate.update();
323 }
324 
326  const robot_state::RobotState& rstate) const
327 {
328  rstate.copyJointGroupPositions(spec_.joint_model_group_, state->as<StateType>()->values);
329  // clear any cached info (such as validity known or not)
330  state->as<StateType>()->clearKnownInformation();
331 }
332 
334  const robot_state::RobotState& robot_state,
335  const moveit::core::JointModel* joint_model,
336  int ompl_state_joint_index) const
337 {
338  // Copy one joint (multiple variables possibly)
339  memcpy(getValueAddressAtIndex(state, ompl_state_joint_index),
340  robot_state.getVariablePositions() + joint_model->getFirstVariableIndex() * sizeof(double),
341  joint_model->getVariableCount() * sizeof(double));
342 
343  // clear any cached info (such as validity known or not)
344  state->as<StateType>()->clearKnownInformation();
345 }
d
std::size_t getVariableCount() const
virtual void enforceBounds(ompl::base::State *state) const
std::vector< const robot_model::JointModel * > joint_model_vector_
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.
const robot_model::JointModelGroup * joint_model_group_
virtual double distance(const ompl::base::State *state1, const ompl::base::State *state2) 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.
virtual bool equalStates(const ompl::base::State *state1, const ompl::base::State *state2) const
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 unsigned int getSerializationLength() const
std::vector< double > values
virtual double * getValueAddressAtIndex(ompl::base::State *state, const unsigned int index) const
std::vector< robot_model::JointModel::Bounds > joint_bounds_storage_
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&#39;s values (which might have multiple variables) from a MoveIt! robot_state to an O...
std::string getName(void *handle)
virtual void deserialize(ompl::base::State *state, const void *serialization) const
virtual void serialize(void *serialization, const ompl::base::State *state) const
virtual ompl::base::State * allocState() const
virtual void printState(const ompl::base::State *state, std::ostream &out) const
virtual void printSettings(std::ostream &out) const
virtual void freeState(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
ModelBasedStateSpaceSpecification spec_
virtual ompl::base::StateSamplerPtr allocDefaultStateSampler() const
ModelBasedStateSpace(const ModelBasedStateSpaceSpecification &spec)
virtual void copyState(ompl::base::State *destination, const ompl::base::State *source) const
int getFirstVariableIndex() const
virtual unsigned int getDimension() const
virtual bool satisfiesBounds(const ompl::base::State *state) const


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