Loading...
Searching...
No Matches
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 <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{
49public:
52 PMXXLDecomposition(ompl::base::SpaceInformationPtr si, const PlanarManipulator *manip,
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
452protected:
453 ompl::base::SpaceInformationPtr si_;
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.
virtual void project(const ompl::base::State *s, std::vector< int > &layers) const
Project the state into the decomposition and retrieve the region for all valid layers.
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...
virtual void project(const ompl::base::State *s, std::vector< double > &coord, int layer=0) const
Project the given State into the XXLDecomposition.
virtual bool sampleFromRegion(int r, ompl::base::State *s, const ompl::base::State *seed, int layer) const
Sample a state s from region r in the given layer.
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.
The exception type for ompl.
Definition Exception.h:47
The lower and upper bounds for an Rn space.
double * values
The value of the actual vector in Rn
ompl::base::State StateType
Define the type of state allocated by this space.
Definition StateSpace.h:78
Definition of an abstract state.
Definition State.h:50
const T * as() const
Cast this instance to a desired type.
Definition State.h:66
int coordToRegion(const std::vector< double > &coord) const
Return the region id of the given coordinate in the decomposition.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition Console.h:64
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition Console.h:70
Main namespace. Contains everything in this library.