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