PlanarManipulatorXXLDecomposition.h
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2015, Rice University
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 the Rice University 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: Ryan Luna */
36 
37 #ifndef PLANAR_MANIPULATOR_XXL_DECOMPOSITION_
38 #define PLANAR_MANIPULATOR_XXL_DECOMPOSITION_
39 
40 #include <eigen3/Eigen/Dense>
41 #include <ompl/base/SpaceInformation.h>
42 #include <ompl/geometric/planners/xxl/XXLPlanarDecomposition.h>
43 #include "PlanarManipulator.h"
44 #include "PlanarManipulatorStateSpace.h"
45 
46 // An XXL decomposition for a planar manipulator
48 {
49 public:
53  const ompl::base::RealVectorBounds &xyBounds, const std::vector<int> &xySlices,
54  const int thetaSlices, std::vector<int> &projectedJoints, bool diagonalEdges)
55  : ompl::geometric::XXLPlanarDecomposition(xyBounds, xySlices, thetaSlices, diagonalEdges)
56  , si_(si)
57  , manip_(manip)
58  , projectedJoints_(projectedJoints)
59  {
60  if (projectedJoints_.size() == 0)
61  {
62  OMPL_WARN("No projected joints. Assuming end effector projection");
63  projectedJoints_.push_back(manip_->getNumLinks() - 1);
64  }
65  // Construct partial manipulators for projections
66  int startLink = 0;
67  const std::vector<double> &linkLengths = manip_->getLinkLengths();
68  for (size_t i = 0; i < projectedJoints_.size(); ++i)
69  {
70  if (projectedJoints_[i] >= (int)manip_->getNumLinks())
71  throw ompl::Exception("Projected joint is out of range");
72  if (projectedJoints_[i] < startLink)
73  throw ompl::Exception("Projected joints must be unique and sorted in ascending order");
74 
75  int numLinks = projectedJoints_[i] - startLink + 1;
76  OMPL_DEBUG("PMXXLDecomposition: Constructing partial manipulator %lu with %d links [from links %d to %d]",
77  i, numLinks, startLink, startLink + numLinks);
78  std::vector<double> lengths(numLinks);
79  for (int j = startLink; j < startLink + numLinks; ++j)
80  lengths[j - startLink] = linkLengths[j];
81  partialManips_.push_back(PlanarManipulator(numLinks, lengths));
82  startLink = projectedJoints_[i] + 1;
83  }
84 
85  partialManips_[0].setBaseFrame(manip_->getBaseFrame());
86  }
87 
88  virtual ~PMXXLDecomposition()
89  {
90  }
91 
93  virtual int numLayers() const
94  {
95  return (int)projectedJoints_.size();
96  }
97 
98  static void frameToPose(const Eigen::Affine2d &frame, Eigen::VectorXd &pose)
99  {
100  pose = Eigen::VectorXd(3);
101  pose(0) = frame.translation()(0);
102  pose(1) = frame.translation()(1);
103  pose(2) = acos(frame.matrix()(0, 0));
104  }
105 
106  virtual bool steerToRegion(int r, int layer, const ompl::base::State *start,
107  std::vector<ompl::base::State *> &states) const
108  {
109  if (layer >= (int)partialManips_.size())
110  throw ompl::Exception("Layer is out of range");
111 
112  if (!start)
113  throw ompl::Exception("Start state must be non-nullptr");
114 
115  // Get a handle to the manipulator
116  PlanarManipulator &partialManip = partialManips_[layer];
117 
118  if (layer > 0)
119  {
120  std::vector<Eigen::Affine2d> frames;
121  manip_->FK(start->as<PlanarManipulatorStateSpace::StateType>()->values, frames);
122 
123  // Set the base frame of the manipulator
124  partialManip.setBaseFrame(frames[projectedJoints_[layer - 1]]);
125  }
126 
127  // Current joint angles
128  Eigen::VectorXd angles(partialManip.getNumLinks());
129 
130  // Extracting current joint angles.
131  // For sublayers, the joint seed is a suffix of the seed
132  if (layer > 0)
133  memcpy(&angles[0],
134  &start->as<PlanarManipulatorStateSpace::StateType>()->values[projectedJoints_[layer - 1] + 1],
135  partialManip.getNumLinks() * sizeof(double));
136  else
137  memcpy(&angles[0], start->as<PlanarManipulatorStateSpace::StateType>()->values,
138  partialManip.getNumLinks() * sizeof(double));
139 
140  // Sample a pose in the desired region
141  Eigen::VectorXd desired(3);
142  sampleCoordinateFromRegion(r, &desired(0));
143 
144  // Create the end effector pose for partialManip. This is the coordinate we just sampled
145  Eigen::Affine2d link_pose = Eigen::Affine2d::Identity();
146  link_pose.rotate(desired[2]);
147  link_pose.translation()(0) = desired[0];
148  link_pose.translation()(1) = desired[1];
149 
150  Eigen::Affine2d current_frame;
151  partialManip.FK(angles, current_frame);
152  Eigen::VectorXd current;
153  frameToPose(current_frame, current);
154 
155  std::vector<int> projection;
156  project(start, projection);
157 
158  // Trivially done. Start is in the correct region
159  if (projection[layer] == r)
160  {
161  return true;
162  }
163 
164  // Nudge factor for the joint angles
165  double alpha = 0.5; // big-ish
166 
167  Eigen::VectorXd e(desired - current);
168 
169  unsigned int iter = 0;
170  unsigned int maxIter = 20;
171 
172  Eigen::MatrixXd jac = Eigen::MatrixXd::Zero(3, partialManip.getNumLinks());
173  Eigen::JacobiSVD<Eigen::MatrixXd> svd(3, partialManip.getNumLinks(), Eigen::ComputeFullU | Eigen::ComputeFullV);
174  Eigen::MatrixXd D = Eigen::MatrixXd::Zero(partialManip.getNumLinks(), 3);
175 
176  unsigned int precedingVals = 0;
177  if (layer > 0)
178  precedingVals = projectedJoints_[layer - 1] + 1;
179 
180  bool valid = true;
181  while (projection[layer] != r && iter++ < maxIter)
182  {
183  partialManip.Jacobian(angles, jac);
184 
185  // Invert the jacobian
186  // Moore-Penrose Pseudoinverse
187  svd.compute(jac);
188  const Eigen::JacobiSVD<Eigen::MatrixXd>::SingularValuesType &d = svd.singularValues();
189 
190  // "Invert" the singular value matrix
191  for (int i = 0; i < d.size(); ++i)
192  if (d(i) > 1e-6)
193  D(i, i) = 1.0 / d(i);
194  else
195  D(i, i) = 0.0;
196 
197  // Inverse is V*D^-1*U^t
198  Eigen::MatrixXd jac_inv = svd.matrixV() * D * svd.matrixU().transpose();
199  Eigen::VectorXd delta_theta = jac_inv * e;
200 
201  if (delta_theta(0) != delta_theta(0)) // nan
202  {
203  OMPL_ERROR("EPIC FAIL IN STEER");
204  return false;
205  }
206 
207  // New joint angles
208  angles = angles + (alpha * delta_theta);
209  partialManip.FK(angles, current_frame);
210  frameToPose(current_frame, current);
211 
212  // New error
213  e = desired - current;
214 
215  // Constructing state
216  ompl::base::State *newState = si_->allocState();
217  double *values = newState->as<PlanarManipulatorStateSpace::StateType>()->values;
218 
219  // Upper layer joints will not change
220  if (precedingVals > 0)
221  memcpy(values, start->as<PlanarManipulatorStateSpace::StateType>()->values,
222  precedingVals * sizeof(double));
223 
224  // Copying solution for this layer
225  memcpy(&values[precedingVals], &angles[0], partialManip.getNumLinks() * sizeof(double));
226 
227  // Keep any remaining links the same. Perturb them if they are in collision.
228  valid = true;
229  if (precedingVals + partialManip.getNumLinks() < manip_->getNumLinks())
230  {
231  valid = false;
232  unsigned int index = precedingVals + partialManip.getNumLinks();
233  unsigned int attempts = 5;
234  const double *seed = start->as<PlanarManipulatorStateSpace::StateType>()->values;
235 
236  double variance = 0.025;
237  for (unsigned int att = 0; att < attempts && !valid; ++att)
238  {
239  // for(unsigned int i = index; i < manip_->getNumLinks(); ++i)
240  // values[i] = rng_.uniformReal(-PI, PI);
241  for (unsigned int i = index; i < manip_->getNumLinks(); ++i)
242  values[i] = rng_.gaussian(seed[i], variance);
243 
244  variance += variance;
245 
246  // Make sure all values are in the requested range
247  si_->getStateSpace()->enforceBounds(newState);
248 
249  // Check validity
250  valid = si_->isValid(newState);
251  }
252  }
253 
254  // Next state is valid. Now see if the motion is valid
255  const ompl::base::State *previous = (states.size() == 0 ? start : states.back());
256  if (valid && si_->checkMotion(previous, newState))
257  {
258  project(newState, projection);
259  states.push_back(newState);
260  }
261  else
262  {
263  // free allocated memory
264  for (size_t i = 0; i < states.size(); ++i)
265  si_->freeState(states[i]);
266  states.clear();
267 
268  return false;
269  }
270  }
271 
272  // Return true if state projects to the correct region
273  if (projection[layer] == r)
274  {
275  return true;
276  }
277  return false;
278  }
279 
281  // todo: remove this
282  virtual bool sampleFromRegion(int r, ompl::base::State *s, const ompl::base::State *seed = nullptr) const
283  {
284  return sampleFromRegion(r, s, seed, 0);
285  }
286 
287  void initializePartialManipulator(int layer, const double *seedAngles) const
288  {
289  if (layer > 0) // must set the base frame of the manipulator based on seed angles
290  {
291  PlanarManipulator &partialManip = partialManips_[layer];
292 
293  std::vector<Eigen::Affine2d> frames;
294  manip_->FK(seedAngles, frames);
295  partialManip.setBaseFrame(frames[projectedJoints_[layer - 1]]);
296  }
297  }
298 
299  void sampleEndEffectorPose(int region, Eigen::Affine2d &pose) const
300  {
301  // Sample a pose in the desired region
302  std::vector<double> coord;
303  sampleCoordinateFromRegion(region, coord);
304 
305  // Create the end effector pose for partialManip. This is the coordinate we just sampled
306  pose = Eigen::Affine2d::Identity();
307  pose.rotate(coord[2]);
308  pose.translation()(0) = coord[0];
309  pose.translation()(1) = coord[1];
310  }
311 
312  void initializePartialSeed(int layer, const double *seedAngles, std::vector<double> &partialSeed) const
313  {
314  const PlanarManipulator &partialManip = partialManips_[layer];
315 
316  partialSeed.resize(partialManip.getNumLinks());
317  if (seedAngles) // use seed angles through the partial manipulator
318  {
319  memcpy(&partialSeed[0], seedAngles, partialManip.getNumLinks() * sizeof(double));
320  }
321  else // Set all joints randomly
322  {
323  for (size_t i = 0; i < partialSeed.size(); ++i)
324  partialSeed[i] = rng_.uniformReal(-PI, PI);
325  }
326  }
327 
328  bool sampleRemainingJoints(int layer, ompl::base::State *s, const double *const seedVals,
329  const std::vector<double> &partialSln) const
330  {
331  double *values = s->as<PlanarManipulatorStateSpace::StateType>()->values;
332 
333  unsigned int precedingVals =
334  (layer > 0 ? projectedJoints_[layer - 1] + 1 : 0); // number of joints in the partial solution?
335 
336  // Copying seed values for immutable joints
337  if (precedingVals > 0)
338  memcpy(values, seedVals, precedingVals * sizeof(double));
339 
340  // Copying solution for this layer
341  PlanarManipulator &partialManip = partialManips_[layer];
342  memcpy(&values[precedingVals], &partialSln[0], partialManip.getNumLinks() * sizeof(double));
343 
344  precedingVals += partialManip.getNumLinks();
345 
346  // deal with the rest of the links after partialManip
347  if (precedingVals < manip_->getNumLinks())
348  {
349  int maxAttempts = 10; // try this many joint configurations
350  double variance = 0.05;
351  for (int att = 0; att < maxAttempts; ++att)
352  {
353  bool random = rng_.uniform01() < 0.50;
354  for (unsigned int i = precedingVals; i < manip_->getNumLinks(); ++i)
355  {
356  if (!seedVals || random)
357  values[i] = rng_.uniformReal(-PI, PI);
358  else
359  values[i] = rng_.gaussian(seedVals[i], variance);
360  }
361 
362  variance += variance; // double the variance after every attempt
363 
364  if (si_->isValid(s))
365  return true;
366  }
367  }
368  else // nothing to sample. Just check validity
369  {
370  return si_->isValid(s);
371  }
372 
373  return false; // failed to find valid state
374  }
375 
376  virtual bool sampleFromRegion(int r, ompl::base::State *s, const ompl::base::State *seed, int layer) const
377  {
378  if (layer >= (int)partialManips_.size())
379  throw ompl::Exception("Layer is out of range");
380  if (!seed && layer > 0)
381  throw ompl::Exception("You must set the seed value to sample from a layer > 0");
382 
383  const double *seedVals = seed ? seed->as<PlanarManipulatorStateSpace::StateType>()->values : nullptr;
384  initializePartialManipulator(layer, seedVals);
385  PlanarManipulator &partialManip = partialManips_[layer];
386 
387  int maxPoses = 3 * (layer + 1); // try up to this many IK poses. try harder in lower levels
388  for (int npose = 0; npose < maxPoses; ++npose)
389  {
390  Eigen::Affine2d link_pose;
391  sampleEndEffectorPose(r, link_pose);
392 
393  if (layer > 0 && rng_.uniform01() < 0.5) // bias end effector orientation
394  {
395  Eigen::Affine2d frame;
396  manip_->FK(seedVals, frame);
397  double theta = rng_.gaussian(acos(frame.matrix()(0, 0)), 0.2);
398 
399  double diff = theta - acos(link_pose.matrix()(0, 0));
400  link_pose.rotate(diff);
401  }
402 
403  std::vector<double> joint_seed;
404  initializePartialSeed(layer, seedVals, joint_seed);
405 
406  // IK to reach end effector pose
407  std::vector<double> solution;
408  if (partialManip.FABRIK(solution, joint_seed, link_pose)) // about 2 orders of magnitude faster than
409  // traditional ik. Better success rate too
410  {
411  if (sampleRemainingJoints(layer, s, seedVals, solution))
412  {
413  si_->getStateSpace()->enforceBounds(s);
414  return true;
415  }
416  }
417  }
418 
419  return false;
420  }
421 
423  virtual void project(const ompl::base::State *s, std::vector<double> &coord, int layer = 0) const
424  {
425  std::vector<Eigen::Affine2d> frames;
426  manip_->FK(s->as<PlanarManipulatorStateSpace::StateType>()->values, frames);
427 
428  coord.resize(3);
429  coord[0] = frames[projectedJoints_[layer]].translation()[0];
430  coord[1] = frames[projectedJoints_[layer]].translation()[1];
431  coord[2] =
432  atan2(frames[projectedJoints_[layer]].matrix()(1, 0), frames[projectedJoints_[layer]].matrix()(1, 1));
433  }
434 
436  virtual void project(const ompl::base::State *s, std::vector<int> &layers) const
437  {
438  std::vector<Eigen::Affine2d> frames;
439  manip_->FK(s->as<PlanarManipulatorStateSpace::StateType>()->values, frames);
440 
441  layers.resize(projectedJoints_.size());
442  std::vector<double> coord(3);
443  for (size_t i = 0; i < projectedJoints_.size(); ++i)
444  {
445  coord[0] = frames[projectedJoints_[i]].translation()[0];
446  coord[1] = frames[projectedJoints_[i]].translation()[1];
447  coord[2] = atan2(frames[projectedJoints_[i]].matrix()(1, 0), frames[projectedJoints_[i]].matrix()(1, 1));
448  layers[i] = coordToRegion(coord);
449  }
450  }
451 
452 protected:
454  const PlanarManipulator *manip_;
455  mutable std::vector<PlanarManipulator> partialManips_;
456  std::vector<int> projectedJoints_;
457 };
458 
459 #endif
virtual int numLayers() const
Return the number of layers possible in this decomposition. Must be at least 1.
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:114
double * values
The value of the actual vector in Rn
const T * as() const
Cast this instance to a desired type.
Definition: State.h:162
virtual bool sampleFromRegion(int r, ompl::base::State *s, const ompl::base::State *seed=nullptr) const
Sample a valid state s from region r in layer 0.
virtual void project(const ompl::base::State *s, std::vector< double > &coord, int layer=0) const
Project the given State into the XXLDecomposition.
double gaussian(double mean, double stddev)
Generate a random real using a normal distribution with given mean and variance.
int coordToRegion(const std::vector< double > &coord) const
Return the region id of the given coordinate in the decomposition.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
double uniformReal(double lower_bound, double upper_bound)
Generate a random real within given bounds: [lower_bound, upper_bound)
PMXXLDecomposition(ompl::base::SpaceInformationPtr si, const PlanarManipulator *manip, const ompl::base::RealVectorBounds &xyBounds, const std::vector< int > &xySlices, const int thetaSlices, std::vector< int > &projectedJoints, bool diagonalEdges)
Constructor. The end of the links in projectedJoints constitute the different layers to this decompos...
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
double uniform01()
Generate a random real between 0 and 1.
The exception type for ompl.
Definition: Exception.h:79
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
The lower and upper bounds for an Rn space.