Loading...
Searching...
No Matches
PlanarManipulatorTSRRTConfig.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 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_TSRRT_CONFIG_H_
38#define PLANAR_MANIPULATOR_TSRRT_CONFIG_H_
39
40#include "PlanarManipulator.h"
41
42#include <ompl/geometric/planners/rrt/TSRRT.h>
43#include <ompl/util/RandomNumbers.h>
44
46{
47public:
49 : manip_(manip), task_space_bounds_(bounds)
50 {
51 }
52
53 virtual int getDimension() const
54 {
55 return 2;
56 }
57
58 virtual void project(const ompl::base::State *state, Eigen::Ref<Eigen::VectorXd> ts_proj) const
59 {
60 project(state->as<PlanarManipulatorStateSpace::StateType>()->values, ts_proj);
61 }
62
63 virtual void sample(Eigen::Ref<Eigen::VectorXd> ts_proj) const
64 {
65 // Sample point uniformly in task-space
66 for (unsigned int dim = 0; dim < task_space_bounds_.low.size(); ++dim)
67 {
68 ts_proj[dim] = rng_.uniformReal(task_space_bounds_.low[dim], task_space_bounds_.high[dim]);
69 }
70 }
71
72 // Given a point in task-space, generate a configuraton space state
73 // that projects to this point. seed is the nearest task-space neighbor.
74 // Returns false if a lifted configuration could not be generated.
75 virtual bool lift(const Eigen::Ref<Eigen::VectorXd> &ts_proj, const ompl::base::State *seed,
76 ompl::base::State *state) const
77 {
79 std::vector<double> seed_angles(manip_->getNumLinks());
80 for (unsigned int ix = 0; ix < seed_angles.size(); ++ix)
81 {
82 seed_angles[ix] = (*pm_seed)[ix];
83 }
84
85 // Allow any angle.
86 Eigen::Affine2d end_frame(Eigen::Rotation2Dd(0.0) * Eigen::Translation2d(ts_proj[0], ts_proj[1]));
87
88 std::vector<double> lifted_angles;
89 if (!manip_->FABRIK(lifted_angles, seed_angles, end_frame, /*xyTol=*/1e-4,
90 /*thetaTol=*/2.0 * M_PI))
91 {
92 return false;
93 }
94
96 for (unsigned int ix = 0; ix < manip_->getNumLinks(); ++ix)
97 {
98 (*pm_state)[ix] = lifted_angles[ix];
99 }
100 return true;
101 }
102
103private:
104 // Forward kinematics to find the end-effector position (proj) in the task space.
105 void project(const double *angles, Eigen::Ref<Eigen::VectorXd> proj) const
106 {
107 Eigen::Affine2d frame;
108 manip_->FK(angles, frame);
109 proj[0] = frame.translation()(0);
110 proj[1] = frame.translation()(1);
111 }
112
113 const PlanarManipulator *manip_;
114 const ompl::base::RealVectorBounds task_space_bounds_;
115 mutable ompl::RNG rng_;
116};
117
118#endif
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
double uniformReal(double lower_bound, double upper_bound)
Generate a random real within given bounds: [lower_bound, upper_bound)
The lower and upper bounds for an Rn space.
std::vector< double > high
Upper bound.
std::vector< double > low
Lower bound.
double * values
The value of the actual vector in Rn
Definition of an abstract state.
Definition State.h:50
const T * as() const
Cast this instance to a desired type.
Definition State.h:66