pose_model_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 <ompl/base/spaces/SE3StateSpace.h>
40 
42 
44  : ModelBasedStateSpace(spec)
45 {
46  jump_factor_ = 3; // \todo make this a param
47 
48  if (spec.joint_model_group_->getGroupKinematics().first)
49  poses_.push_back(PoseComponent(spec.joint_model_group_, spec.joint_model_group_->getGroupKinematics().first));
50  else if (!spec.joint_model_group_->getGroupKinematics().second.empty())
51  {
52  const robot_model::JointModelGroup::KinematicsSolverMap& m = spec.joint_model_group_->getGroupKinematics().second;
53  for (robot_model::JointModelGroup::KinematicsSolverMap::const_iterator it = m.begin(); it != m.end(); ++it)
54  poses_.push_back(PoseComponent(it->first, it->second));
55  }
56  if (poses_.empty())
57  logError("No kinematics solvers specified. Unable to construct a PoseModelStateSpace");
58  else
59  std::sort(poses_.begin(), poses_.end());
60  setName(getName() + "_" + PARAMETERIZATION_TYPE);
61 }
62 
64 {
65 }
66 
67 double ompl_interface::PoseModelStateSpace::distance(const ompl::base::State* state1,
68  const ompl::base::State* state2) const
69 {
70  double total = 0;
71  for (std::size_t i = 0; i < poses_.size(); ++i)
72  total += poses_[i].state_space_->distance(state1->as<StateType>()->poses[i], state2->as<StateType>()->poses[i]);
73  return total;
74 }
75 
77 {
78  double total = 0.0;
79  for (std::size_t i = 0; i < poses_.size(); ++i)
80  total += poses_[i].state_space_->getMaximumExtent();
81  return total;
82 }
83 
85 {
86  StateType* state = new StateType();
87  state->values =
88  new double[variable_count_]; // need to allocate this here since ModelBasedStateSpace::allocState() is not called
89  state->poses = new ompl::base::SE3StateSpace::StateType*[poses_.size()];
90  for (std::size_t i = 0; i < poses_.size(); ++i)
91  state->poses[i] = poses_[i].state_space_->allocState()->as<ompl::base::SE3StateSpace::StateType>();
92  return state;
93 }
94 
95 void ompl_interface::PoseModelStateSpace::freeState(ompl::base::State* state) const
96 {
97  for (std::size_t i = 0; i < poses_.size(); ++i)
98  poses_[i].state_space_->freeState(state->as<StateType>()->poses[i]);
99  delete[] state->as<StateType>()->poses;
101 }
102 
103 void ompl_interface::PoseModelStateSpace::copyState(ompl::base::State* destination,
104  const ompl::base::State* source) const
105 {
106  // copy the state data
107  ModelBasedStateSpace::copyState(destination, source);
108 
109  for (std::size_t i = 0; i < poses_.size(); ++i)
110  poses_[i].state_space_->copyState(destination->as<StateType>()->poses[i], source->as<StateType>()->poses[i]);
111 
112  // compute additional stuff if needed
113  computeStateK(destination);
114 }
115 
117 {
118  ModelBasedStateSpace::sanityChecks(std::numeric_limits<double>::epsilon(), std::numeric_limits<float>::epsilon(),
119  ~ompl::base::StateSpace::STATESPACE_TRIANGLE_INEQUALITY);
120 }
121 
122 void ompl_interface::PoseModelStateSpace::interpolate(const ompl::base::State* from, const ompl::base::State* to,
123  const double t, ompl::base::State* state) const
124 {
125  // moveit::Profiler::ScopedBlock sblock("interpolate");
126 
127  // we want to interpolate in Cartesian space; we do not have a guarantee that from and to
128  // have their poses computed, but this is very unlikely to happen (depends how the planner gets its input states)
129 
130  // interpolate in joint space
131  ModelBasedStateSpace::interpolate(from, to, t, state);
132 
133  // interpolate SE3 components
134  for (std::size_t i = 0; i < poses_.size(); ++i)
135  poses_[i].state_space_->interpolate(from->as<StateType>()->poses[i], to->as<StateType>()->poses[i], t,
136  state->as<StateType>()->poses[i]);
137 
138  // the call above may reset all flags for state; but we know the pose we want flag should be set
139  state->as<StateType>()->setPoseComputed(true);
140 
141  /*
142  std::cout << "*********** interpolate\n";
143  printState(from, std::cout);
144  printState(to, std::cout);
145  printState(state, std::cout);
146  std::cout << "\n\n";
147  */
148 
149  // after interpolation we cannot be sure about the joint values (we use them as seed only)
150  // so we recompute IK if needed
151  if (computeStateIK(state))
152  {
153  double dj = jump_factor_ * ModelBasedStateSpace::distance(from, to);
154  double d_from = ModelBasedStateSpace::distance(from, state);
155  double d_to = ModelBasedStateSpace::distance(state, to);
156 
157  // if the joint value jumped too much
158  if (d_from + d_to > std::max(0.2, dj)) // \todo make 0.2 a param
159  state->as<StateType>()->markInvalid();
160  }
161 }
162 
163 void ompl_interface::PoseModelStateSpace::setPlanningVolume(double minX, double maxX, double minY, double maxY,
164  double minZ, double maxZ)
165 {
166  ModelBasedStateSpace::setPlanningVolume(minX, maxX, minY, maxY, minZ, maxZ);
167  ompl::base::RealVectorBounds b(3);
168  b.low[0] = minX;
169  b.low[1] = minY;
170  b.low[2] = minZ;
171  b.high[0] = maxX;
172  b.high[1] = maxY;
173  b.high[2] = maxZ;
174  for (std::size_t i = 0; i < poses_.size(); ++i)
175  poses_[i].state_space_->as<ompl::base::SE3StateSpace>()->setBounds(b);
176 }
177 
179  const robot_model::JointModelGroup* subgroup, const robot_model::JointModelGroup::KinematicsSolver& k)
180  : subgroup_(subgroup), kinematics_solver_(k.allocator_(subgroup)), bijection_(k.bijection_)
181 {
182  state_space_.reset(new ompl::base::SE3StateSpace());
183  state_space_->setName(subgroup_->getName() + "_Workspace");
184  fk_link_.resize(1, kinematics_solver_->getTipFrame());
185  if (!fk_link_[0].empty() && fk_link_[0][0] == '/')
186  fk_link_[0] = fk_link_[0].substr(1);
187 }
188 
190 {
191  // read the values from the joint state, in the order expected by the kinematics solver
192  std::vector<double> values(bijection_.size());
193  for (unsigned int i = 0; i < bijection_.size(); ++i)
194  values[i] = full_state->values[bijection_[i]];
195 
196  // compute forward kinematics for the link of interest
197  std::vector<geometry_msgs::Pose> poses;
198  if (!kinematics_solver_->getPositionFK(fk_link_, values, poses))
199  return false;
200 
201  // copy the resulting data to the desired location in the state
202  ompl::base::SE3StateSpace::StateType* se3_state = full_state->poses[idx];
203  se3_state->setXYZ(poses[0].position.x, poses[0].position.y, poses[0].position.z);
204  ompl::base::SO3StateSpace::StateType& so3_state = se3_state->rotation();
205  so3_state.x = poses[0].orientation.x;
206  so3_state.y = poses[0].orientation.y;
207  so3_state.z = poses[0].orientation.z;
208  so3_state.w = poses[0].orientation.w;
209 
210  return true;
211 }
212 
214 {
215  // read the values from the joint state, in the order expected by the kinematics solver; use these as the seed
216  std::vector<double> seed_values(bijection_.size());
217  for (std::size_t i = 0; i < bijection_.size(); ++i)
218  seed_values[i] = full_state->values[bijection_[i]];
219 
220  /*
221  std::cout << "seed: ";
222  for (std::size_t i = 0 ; i < seed_values.size() ; ++i)
223  std::cout << seed_values[i] << " ";
224  std::cout << std::endl;
225  */
226 
227  // construct the pose
228  geometry_msgs::Pose pose;
229  const ompl::base::SE3StateSpace::StateType* se3_state = full_state->poses[idx];
230  pose.position.x = se3_state->getX();
231  pose.position.y = se3_state->getY();
232  pose.position.z = se3_state->getZ();
233  const ompl::base::SO3StateSpace::StateType& so3_state = se3_state->rotation();
234  pose.orientation.x = so3_state.x;
235  pose.orientation.y = so3_state.y;
236  pose.orientation.z = so3_state.z;
237  pose.orientation.w = so3_state.w;
238 
239  // run IK
240  std::vector<double> solution(bijection_.size());
241  moveit_msgs::MoveItErrorCodes err_code;
242  if (!kinematics_solver_->getPositionIK(pose, seed_values, solution, err_code))
243  {
244  if (err_code.val != moveit_msgs::MoveItErrorCodes::TIMED_OUT ||
245  !kinematics_solver_->searchPositionIK(pose, seed_values, kinematics_solver_->getDefaultTimeout() * 2.0,
246  solution, err_code))
247  return false;
248  }
249 
250  for (std::size_t i = 0; i < bijection_.size(); ++i)
251  full_state->values[bijection_[i]] = solution[i];
252 
253  return true;
254 }
255 
256 bool ompl_interface::PoseModelStateSpace::computeStateFK(ompl::base::State* state) const
257 {
258  if (state->as<StateType>()->poseComputed())
259  return true;
260  for (std::size_t i = 0; i < poses_.size(); ++i)
261  if (!poses_[i].computeStateFK(state->as<StateType>(), i))
262  {
263  state->as<StateType>()->markInvalid();
264  return false;
265  }
266  state->as<StateType>()->setPoseComputed(true);
267  return true;
268 }
269 
270 bool ompl_interface::PoseModelStateSpace::computeStateIK(ompl::base::State* state) const
271 {
272  if (state->as<StateType>()->jointsComputed())
273  return true;
274  for (std::size_t i = 0; i < poses_.size(); ++i)
275  if (!poses_[i].computeStateIK(state->as<StateType>(), i))
276  {
277  state->as<StateType>()->markInvalid();
278  return false;
279  }
280  state->as<StateType>()->setJointsComputed(true);
281  return true;
282 }
283 
284 bool ompl_interface::PoseModelStateSpace::computeStateK(ompl::base::State* state) const
285 {
286  if (state->as<StateType>()->jointsComputed() && !state->as<StateType>()->poseComputed())
287  return computeStateFK(state);
288  if (!state->as<StateType>()->jointsComputed() && state->as<StateType>()->poseComputed())
289  return computeStateIK(state);
290  if (state->as<StateType>()->jointsComputed() && state->as<StateType>()->poseComputed())
291  return true;
292  state->as<StateType>()->markInvalid();
293  return false;
294 }
295 
297 {
298  class PoseModelStateSampler : public ompl::base::StateSampler
299  {
300  public:
301  PoseModelStateSampler(const ompl::base::StateSpace* space, const ompl::base::StateSamplerPtr& sampler)
302  : ompl::base::StateSampler(space), sampler_(sampler)
303  {
304  }
305 
306  virtual void sampleUniform(ompl::base::State* state)
307  {
308  sampler_->sampleUniform(state);
309  afterStateSample(state);
310  }
311 
312  virtual void sampleUniformNear(ompl::base::State* state, const ompl::base::State* near, const double distance)
313  {
314  sampler_->sampleUniformNear(state, near, distance);
315  afterStateSample(state);
316  }
317 
318  virtual void sampleGaussian(ompl::base::State* state, const ompl::base::State* mean, const double stdDev)
319  {
320  sampler_->sampleGaussian(state, mean, stdDev);
321  afterStateSample(state);
322  }
323 
324  protected:
325  void afterStateSample(ompl::base::State* sample) const
326  {
327  sample->as<StateType>()->setJointsComputed(true);
328  sample->as<StateType>()->setPoseComputed(false);
329  space_->as<PoseModelStateSpace>()->computeStateFK(sample);
330  }
331 
332  ompl::base::StateSamplerPtr sampler_;
333  };
334 
335  return ompl::base::StateSamplerPtr(static_cast<ompl::base::StateSampler*>(
336  new PoseModelStateSampler(this, ModelBasedStateSpace::allocDefaultStateSampler())));
337 }
338 
340  const robot_state::RobotState& rstate) const
341 {
343  state->as<StateType>()->setJointsComputed(true);
344  state->as<StateType>()->setPoseComputed(false);
345  computeStateFK(state);
346  /*
347  std::cout << "COPY STATE IN:\n";
348  printState(state, std::cout);
349  std::cout << "---------- COPY STATE IN\n"; */
350 }
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
PoseComponent(const robot_model::JointModelGroup *subgroup, const robot_model::JointModelGroup::KinematicsSolver &k)
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 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.
std::vector< double > values
bool computeStateFK(StateType *full_state, unsigned int idx) const
std::string getName(void *handle)
virtual double distance(const ompl::base::State *state1, const ompl::base::State *state2) const
static const std::string PARAMETERIZATION_TYPE
bool computeStateK(ompl::base::State *state) const
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 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
bool computeStateIK(StateType *full_state, unsigned int idx) const
virtual ompl::base::StateSamplerPtr allocDefaultStateSampler() 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 void copyState(ompl::base::State *destination, const ompl::base::State *source) const
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 Mon Jan 15 2018 03:51:38