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


ompl
Author(s): Ioan Sucan
autogenerated on Sun Oct 21 2018 10:37:13