moveit2
The MoveIt Motion Planning Framework for ROS 2.
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 <utility>
39 
40 namespace ompl_interface
41 {
42 static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ompl_planning.model_based_state_space");
43 } // namespace ompl_interface
44 
46  : ompl::base::StateSpace(), spec_(std::move(spec))
47 {
48  // set the state space name
49  setName(spec_.joint_model_group_->getName());
51  state_values_size_ = variable_count_ * sizeof(double);
53 
54  // make sure we have bounds for every joint stored within the spec (use default bounds if not specified)
55  if (!spec_.joint_bounds_.empty() && spec_.joint_bounds_.size() != joint_model_vector_.size())
56  {
57  RCLCPP_ERROR(LOGGER, "Joint group '%s' has incorrect bounds specified. Using the default bounds instead.",
58  spec_.joint_model_group_->getName().c_str());
59  spec_.joint_bounds_.clear();
60  }
61 
62  // copy the default joint bounds if needed
63  if (spec_.joint_bounds_.empty())
65 
66  // new perform a deep copy of the bounds, in case we need to modify them
68  for (std::size_t i = 0; i < joint_bounds_storage_.size(); ++i)
69  {
72  }
73 
74  // default settings
75  setTagSnapToSegment(0.95);
76 
78  params_.declareParam<double>(
79  "tag_snap_to_segment", [this](double tag_snap_to_segment) { setTagSnapToSegment(tag_snap_to_segment); },
80  [this] { return getTagSnapToSegment(); });
81 }
82 
84 
86 {
87  return tag_snap_to_segment_;
88 }
89 
91 {
92  if (snap < 0.0 || snap > 1.0)
93  RCLCPP_WARN(LOGGER,
94  "Snap to segment for tags is a ratio. It's value must be between 0.0 and 1.0. "
95  "Value remains as previously set (%lf)",
96  tag_snap_to_segment_);
97  else
98  {
99  tag_snap_to_segment_ = snap;
100  tag_snap_to_segment_complement_ = 1.0 - tag_snap_to_segment_;
101  }
102 }
103 
105 {
106  auto* state = new StateType();
107  state->values = new double[variable_count_];
108  return state;
109 }
110 
111 void ompl_interface::ModelBasedStateSpace::freeState(ompl::base::State* state) const
112 {
113  delete[] state->as<StateType>()->values;
114  delete state->as<StateType>();
115 }
116 
117 void ompl_interface::ModelBasedStateSpace::copyState(ompl::base::State* destination,
118  const ompl::base::State* source) const
119 {
120  memcpy(destination->as<StateType>()->values, source->as<StateType>()->values, state_values_size_);
121  destination->as<StateType>()->tag = source->as<StateType>()->tag;
122  destination->as<StateType>()->flags = source->as<StateType>()->flags;
123  destination->as<StateType>()->distance = source->as<StateType>()->distance;
124 }
125 
127 {
128  return state_values_size_ + sizeof(int);
129 }
130 
131 void ompl_interface::ModelBasedStateSpace::serialize(void* serialization, const ompl::base::State* state) const
132 {
133  *reinterpret_cast<int*>(serialization) = state->as<StateType>()->tag;
134  memcpy(reinterpret_cast<char*>(serialization) + sizeof(int), state->as<StateType>()->values, state_values_size_);
135 }
136 
137 void ompl_interface::ModelBasedStateSpace::deserialize(ompl::base::State* state, const void* serialization) const
138 {
139  state->as<StateType>()->tag = *reinterpret_cast<const int*>(serialization);
140  memcpy(state->as<StateType>()->values, reinterpret_cast<const char*>(serialization) + sizeof(int), state_values_size_);
141 }
142 
144 {
145  unsigned int d = 0;
146  for (const moveit::core::JointModel* i : joint_model_vector_)
147  d += i->getStateSpaceDimension();
148  return d;
149 }
150 
152 {
153  return spec_.joint_model_group_->getMaximumExtent(spec_.joint_bounds_);
154 }
155 
157 {
158  double m = 1.0;
159  for (const moveit::core::JointModel::Bounds* bounds : spec_.joint_bounds_)
160  {
161  for (const moveit::core::VariableBounds& bound : *bounds)
162  {
163  m *= bound.max_position_ - bound.min_position_;
164  }
165  }
166  return m;
167 }
168 
169 double ompl_interface::ModelBasedStateSpace::distance(const ompl::base::State* state1,
170  const ompl::base::State* state2) const
171 {
172  if (distance_function_)
173  return distance_function_(state1, state2);
174  else
175  return spec_.joint_model_group_->distance(state1->as<StateType>()->values, state2->as<StateType>()->values);
176 }
177 
178 bool ompl_interface::ModelBasedStateSpace::equalStates(const ompl::base::State* state1,
179  const ompl::base::State* state2) const
180 {
181  for (unsigned int i = 0; i < variable_count_; ++i)
182  if (fabs(state1->as<StateType>()->values[i] - state2->as<StateType>()->values[i]) >
183  std::numeric_limits<double>::epsilon())
184  return false;
185  return true;
186 }
187 
188 void ompl_interface::ModelBasedStateSpace::enforceBounds(ompl::base::State* state) const
189 {
190  spec_.joint_model_group_->enforcePositionBounds(state->as<StateType>()->values, spec_.joint_bounds_);
191 }
192 
193 bool ompl_interface::ModelBasedStateSpace::satisfiesBounds(const ompl::base::State* state) const
194 {
195  return spec_.joint_model_group_->satisfiesPositionBounds(state->as<StateType>()->values, spec_.joint_bounds_,
196  std::numeric_limits<double>::epsilon());
197 }
198 
199 void ompl_interface::ModelBasedStateSpace::interpolate(const ompl::base::State* from, const ompl::base::State* to,
200  const double t, ompl::base::State* state) const
201 {
202  // clear any cached info (such as validity known or not)
203  state->as<StateType>()->clearKnownInformation();
204 
205  if (!interpolation_function_ || !interpolation_function_(from, to, t, state))
206  {
207  // perform the actual interpolation
208  spec_.joint_model_group_->interpolate(from->as<StateType>()->values, to->as<StateType>()->values, t,
209  state->as<StateType>()->values);
210 
211  // compute tag
212  if (from->as<StateType>()->tag >= 0 && t < 1.0 - tag_snap_to_segment_)
213  state->as<StateType>()->tag = from->as<StateType>()->tag;
214  else if (to->as<StateType>()->tag >= 0 && t > tag_snap_to_segment_)
215  state->as<StateType>()->tag = to->as<StateType>()->tag;
216  else
217  state->as<StateType>()->tag = -1;
218  }
219 }
220 
222  const unsigned int index) const
223 {
224  if (index >= variable_count_)
225  return nullptr;
226  return state->as<StateType>()->values + index;
227 }
228 
229 void ompl_interface::ModelBasedStateSpace::setPlanningVolume(double minX, double maxX, double minY, double maxY,
230  double minZ, double maxZ)
231 {
232  for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
233  if (joint_model_vector_[i]->getType() == moveit::core::JointModel::PLANAR)
234  {
235  joint_bounds_storage_[i][0].min_position_ = minX;
236  joint_bounds_storage_[i][0].max_position_ = maxX;
237  joint_bounds_storage_[i][1].min_position_ = minY;
238  joint_bounds_storage_[i][1].max_position_ = maxY;
239  }
240  else if (joint_model_vector_[i]->getType() == moveit::core::JointModel::FLOATING)
241  {
242  joint_bounds_storage_[i][0].min_position_ = minX;
243  joint_bounds_storage_[i][0].max_position_ = maxX;
244  joint_bounds_storage_[i][1].min_position_ = minY;
245  joint_bounds_storage_[i][1].max_position_ = maxY;
246  joint_bounds_storage_[i][2].min_position_ = minZ;
247  joint_bounds_storage_[i][2].max_position_ = maxZ;
248  }
249 }
250 
252 {
253  class DefaultStateSampler : public ompl::base::StateSampler
254  {
255  public:
256  DefaultStateSampler(const ompl::base::StateSpace* space, const moveit::core::JointModelGroup* group,
257  const moveit::core::JointBoundsVector* joint_bounds)
258  : ompl::base::StateSampler(space), joint_model_group_(group), joint_bounds_(joint_bounds)
259  {
260  }
261 
262  void sampleUniform(ompl::base::State* state) override
263  {
264  joint_model_group_->getVariableRandomPositions(moveit_rng_, state->as<StateType>()->values, *joint_bounds_);
265  state->as<StateType>()->clearKnownInformation();
266  }
267 
268  void sampleUniformNear(ompl::base::State* state, const ompl::base::State* near, const double distance) override
269  {
270  joint_model_group_->getVariableRandomPositionsNearBy(moveit_rng_, state->as<StateType>()->values, *joint_bounds_,
271  near->as<StateType>()->values, distance);
272  state->as<StateType>()->clearKnownInformation();
273  }
274 
275  void sampleGaussian(ompl::base::State* state, const ompl::base::State* mean, const double stdDev) override
276  {
277  sampleUniformNear(state, mean, rng_.gaussian(0.0, stdDev));
278  }
279 
280  protected:
281  random_numbers::RandomNumberGenerator moveit_rng_;
282  const moveit::core::JointModelGroup* joint_model_group_;
283  const moveit::core::JointBoundsVector* joint_bounds_;
284  };
285 
286  return ompl::base::StateSamplerPtr(static_cast<ompl::base::StateSampler*>(
287  new DefaultStateSampler(this, spec_.joint_model_group_, &spec_.joint_bounds_)));
288 }
289 
291 {
292  out << "ModelBasedStateSpace '" << getName() << "' at " << this << '\n';
293 }
294 
295 void ompl_interface::ModelBasedStateSpace::printState(const ompl::base::State* state, std::ostream& out) const
296 {
297  for (const moveit::core::JointModel* j : joint_model_vector_)
298  {
299  out << j->getName() << " = ";
300  const int idx = spec_.joint_model_group_->getVariableGroupIndex(j->getName());
301  const int vc = j->getVariableCount();
302  for (int i = 0; i < vc; ++i)
303  out << state->as<StateType>()->values[idx + i] << " ";
304  out << '\n';
305  }
306 
307  if (state->as<StateType>()->isStartState())
308  out << "* start state \n";
309  if (state->as<StateType>()->isGoalState())
310  out << "* goal state \n";
311  if (state->as<StateType>()->isValidityKnown())
312  {
313  if (state->as<StateType>()->isMarkedValid())
314  out << "* valid state \n";
315  else
316  out << "* invalid state \n";
317  }
318  out << "Tag: " << state->as<StateType>()->tag << '\n';
319 }
320 
322  const ompl::base::State* state) const
323 {
324  rstate.setJointGroupPositions(spec_.joint_model_group_, state->as<StateType>()->values);
325  rstate.update();
326 }
327 
329  const moveit::core::RobotState& rstate) const
330 {
331  rstate.copyJointGroupPositions(spec_.joint_model_group_, state->as<StateType>()->values);
332  // clear any cached info (such as validity known or not)
333  state->as<StateType>()->clearKnownInformation();
334 }
335 
337  const moveit::core::RobotState& robot_state,
338  const moveit::core::JointModel* joint_model,
339  int ompl_state_joint_index) const
340 {
341  // Copy one joint (multiple variables possibly)
342  memcpy(getValueAddressAtIndex(state, ompl_state_joint_index),
343  robot_state.getVariablePositions() + joint_model->getFirstVariableIndex(),
344  joint_model->getVariableCount() * sizeof(double));
345 
346  // clear any cached info (such as validity known or not)
347  state->as<StateType>()->clearKnownInformation();
348 }
const std::vector< const JointModel * > & getActiveJointModels() const
Get the active joints in this group (that have controllable DOF). This does not include mimic joints.
const std::string & getName() const
Get the name of the joint group.
const JointBoundsVector & getActiveJointModelsBounds() const
Get the bounds for all the active joints.
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
Definition: joint_model.h:117
std::vector< VariableBounds > Bounds
The datatype for the joint bounds.
Definition: joint_model.h:131
size_t getFirstVariableIndex() const
Get the index of this joint's first variable within the full robot state.
Definition: joint_model.h:216
std::size_t getVariableCount() const
Get the number of variables that describe this joint.
Definition: joint_model.h:210
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:90
void setJointGroupPositions(const std::string &joint_group_name, const double *gstate)
Given positions for the variables that make up a group, in the order found in the group (including va...
Definition: robot_state.h:605
void copyJointGroupPositions(const std::string &joint_group_name, std::vector< double > &gstate) const
For a given group, copy the position values of the variables that make up the group into another loca...
Definition: robot_state.h:691
double * getVariablePositions()
Get a raw pointer to the positions of the variables stored in this state. Use carefully....
Definition: robot_state.h:147
void update(bool force=false)
Update all transforms.
unsigned int getDimension() const override
bool satisfiesBounds(const ompl::base::State *state) const override
virtual void copyJointToOMPLState(ompl::base::State *state, const moveit::core::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 OM...
bool equalStates(const ompl::base::State *state1, const ompl::base::State *state2) const override
void serialize(void *serialization, const ompl::base::State *state) const override
double * getValueAddressAtIndex(ompl::base::State *state, const unsigned int index) const override
std::vector< const moveit::core::JointModel * > joint_model_vector_
void deserialize(ompl::base::State *state, const void *serialization) const override
std::vector< moveit::core::JointModel::Bounds > joint_bounds_storage_
ModelBasedStateSpaceSpecification spec_
unsigned int getSerializationLength() const override
ompl::base::State * allocState() const override
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.
ompl::base::StateSamplerPtr allocDefaultStateSampler() const override
virtual void copyToRobotState(moveit::core::RobotState &rstate, const ompl::base::State *state) const
Copy the data from an OMPL state to a set of joint states.
void interpolate(const ompl::base::State *from, const ompl::base::State *to, const double t, ompl::base::State *state) const override
void printState(const ompl::base::State *state, std::ostream &out) const override
ModelBasedStateSpace(ModelBasedStateSpaceSpecification spec)
void freeState(ompl::base::State *state) const override
void copyState(ompl::base::State *destination, const ompl::base::State *source) const override
virtual void copyToOMPLState(ompl::base::State *state, const moveit::core::RobotState &rstate) const
Copy the data from a set of joint states to an OMPL state.
void printSettings(std::ostream &out) const override
void enforceBounds(ompl::base::State *state) const override
double distance(const ompl::base::State *state1, const ompl::base::State *state2) const override
std::vector< const JointModel::Bounds * > JointBoundsVector
The MoveIt interface to OMPL.
double distance(const urdf::Pose &transform)
Definition: pr2_arm_ik.h:55
d
Definition: setup.py:4
const moveit::core::JointModelGroup * joint_model_group_