planar_joint_model.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Ioan A. Sucan
5  * Copyright (c) 2008-2013, Willow Garage, Inc.
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of the Willow Garage nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 /* Author: Ioan Sucan */
37 
40 #include <boost/math/constants/constants.hpp>
41 #include <limits>
42 #include <cmath>
43 
44 namespace moveit
45 {
46 namespace core
47 {
48 PlanarJointModel::PlanarJointModel(const std::string& name) : JointModel(name), angular_distance_weight_(1.0)
49 {
50  type_ = PLANAR;
51 
52  local_variable_names_.push_back("x");
53  local_variable_names_.push_back("y");
54  local_variable_names_.push_back("theta");
55  for (int i = 0; i < 3; ++i)
56  {
57  variable_names_.push_back(name_ + "/" + local_variable_names_[i]);
58  variable_index_map_[variable_names_.back()] = i;
59  }
60 
61  variable_bounds_.resize(3);
62  variable_bounds_[0].position_bounded_ = true;
63  variable_bounds_[1].position_bounded_ = true;
64  variable_bounds_[2].position_bounded_ = false;
65 
66  variable_bounds_[0].min_position_ = -std::numeric_limits<double>::infinity();
67  variable_bounds_[0].max_position_ = std::numeric_limits<double>::infinity();
68  variable_bounds_[1].min_position_ = -std::numeric_limits<double>::infinity();
69  variable_bounds_[1].max_position_ = std::numeric_limits<double>::infinity();
70  variable_bounds_[2].min_position_ = -boost::math::constants::pi<double>();
71  variable_bounds_[2].max_position_ = boost::math::constants::pi<double>();
72 
73  computeVariableBoundsMsg();
74 }
75 
76 unsigned int PlanarJointModel::getStateSpaceDimension() const
77 {
78  return 3;
79 }
80 
81 double PlanarJointModel::getMaximumExtent(const Bounds& other_bounds) const
82 {
83  double dx = other_bounds[0].max_position_ - other_bounds[0].min_position_;
84  double dy = other_bounds[1].max_position_ - other_bounds[1].min_position_;
85  return sqrt(dx * dx + dy * dy) + boost::math::constants::pi<double>() * angular_distance_weight_;
86 }
87 
88 void PlanarJointModel::getVariableDefaultPositions(double* values, const Bounds& bounds) const
89 {
90  for (unsigned int i = 0; i < 2; ++i)
91  {
92  // if zero is a valid value
93  if (bounds[i].min_position_ <= 0.0 && bounds[i].max_position_ >= 0.0)
94  values[i] = 0.0;
95  else
96  values[i] = (bounds[i].min_position_ + bounds[i].max_position_) / 2.0;
97  }
98  values[2] = 0.0;
99 }
100 
101 void PlanarJointModel::getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values,
102  const Bounds& bounds) const
103 {
104  if (bounds[0].max_position_ >= std::numeric_limits<double>::infinity() ||
105  bounds[0].min_position_ <= -std::numeric_limits<double>::infinity())
106  values[0] = 0.0;
107  else
108  values[0] = rng.uniformReal(bounds[0].min_position_, bounds[0].max_position_);
109  if (bounds[1].max_position_ >= std::numeric_limits<double>::infinity() ||
110  bounds[1].min_position_ <= -std::numeric_limits<double>::infinity())
111  values[1] = 0.0;
112  else
113  values[1] = rng.uniformReal(bounds[1].min_position_, bounds[1].max_position_);
114  values[2] = rng.uniformReal(bounds[2].min_position_, bounds[2].max_position_);
115 }
116 
117 void PlanarJointModel::getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values,
118  const Bounds& bounds, const double* seed,
119  const double distance) const
120 {
121  if (bounds[0].max_position_ >= std::numeric_limits<double>::infinity() ||
122  bounds[0].min_position_ <= -std::numeric_limits<double>::infinity())
123  values[0] = 0.0;
124  else
125  values[0] = rng.uniformReal(std::max(bounds[0].min_position_, seed[0] - distance),
126  std::min(bounds[0].max_position_, seed[0] + distance));
127  if (bounds[1].max_position_ >= std::numeric_limits<double>::infinity() ||
128  bounds[1].min_position_ <= -std::numeric_limits<double>::infinity())
129  values[1] = 0.0;
130  else
131  values[1] = rng.uniformReal(std::max(bounds[1].min_position_, seed[1] - distance),
132  std::min(bounds[1].max_position_, seed[1] + distance));
133 
134  double da = angular_distance_weight_ * distance;
135  // limit the sampling range to 2pi to work correctly even if the distance is very large
136  if (da > boost::math::constants::pi<double>())
137  da = boost::math::constants::pi<double>();
138  values[2] = rng.uniformReal(seed[2] - da, seed[2] + da);
139  normalizeRotation(values);
140 }
141 
142 void PlanarJointModel::interpolate(const double* from, const double* to, const double t, double* state) const
143 {
144  // interpolate position
145  state[0] = from[0] + (to[0] - from[0]) * t;
146  state[1] = from[1] + (to[1] - from[1]) * t;
147 
148  // interpolate angle
149  double diff = to[2] - from[2];
150  if (fabs(diff) <= boost::math::constants::pi<double>())
151  state[2] = from[2] + diff * t;
152  else
153  {
154  if (diff > 0.0)
155  diff = 2.0 * boost::math::constants::pi<double>() - diff;
156  else
157  diff = -2.0 * boost::math::constants::pi<double>() - diff;
158  state[2] = from[2] - diff * t;
159  // input states are within bounds, so the following check is sufficient
160  if (state[2] > boost::math::constants::pi<double>())
161  state[2] -= 2.0 * boost::math::constants::pi<double>();
162  else if (state[2] < -boost::math::constants::pi<double>())
163  state[2] += 2.0 * boost::math::constants::pi<double>();
164  }
165 }
166 
167 double PlanarJointModel::distance(const double* values1, const double* values2) const
168 {
169  double dx = values1[0] - values2[0];
170  double dy = values1[1] - values2[1];
171 
172  double d = fabs(values1[2] - values2[2]);
173  d = (d > boost::math::constants::pi<double>()) ? 2.0 * boost::math::constants::pi<double>() - d : d;
174  return sqrt(dx * dx + dy * dy) + angular_distance_weight_ * d;
175 }
176 
177 bool PlanarJointModel::satisfiesPositionBounds(const double* values, const Bounds& bounds, double margin) const
178 {
179  for (unsigned int i = 0; i < 3; ++i)
180  if (values[0] < bounds[0].min_position_ - margin || values[0] > bounds[0].max_position_ + margin)
181  return false;
182  return true;
183 }
184 
185 bool PlanarJointModel::normalizeRotation(double* values) const
186 {
187  double& v = values[2];
188  if (v >= -boost::math::constants::pi<double>() && v <= boost::math::constants::pi<double>())
189  return false;
190  v = fmod(v, 2.0 * boost::math::constants::pi<double>());
191  if (v < -boost::math::constants::pi<double>())
192  v += 2.0 * boost::math::constants::pi<double>();
193  else if (v > boost::math::constants::pi<double>())
194  v -= 2.0 * boost::math::constants::pi<double>();
195  return true;
196 }
197 
198 bool PlanarJointModel::enforcePositionBounds(double* values, const Bounds& bounds) const
199 {
200  bool result = normalizeRotation(values);
201  for (unsigned int i = 0; i < 2; ++i)
202  {
203  if (values[i] < bounds[i].min_position_)
204  {
205  values[i] = bounds[i].min_position_;
206  result = true;
207  }
208  else if (values[i] > bounds[i].max_position_)
209  {
210  values[i] = bounds[i].max_position_;
211  result = true;
212  }
213  }
214  return result;
215 }
216 
217 void PlanarJointModel::computeTransform(const double* joint_values, Eigen::Isometry3d& transf) const
218 {
219  transf = Eigen::Isometry3d(Eigen::Translation3d(joint_values[0], joint_values[1], 0.0) *
220  Eigen::AngleAxisd(joint_values[2], Eigen::Vector3d::UnitZ()));
221 }
222 
223 void PlanarJointModel::computeVariablePositions(const Eigen::Isometry3d& transf, double* joint_values) const
224 {
225  joint_values[0] = transf.translation().x();
226  joint_values[1] = transf.translation().y();
227 
228  ASSERT_ISOMETRY(transf) // unsanitized input, could contain a non-isometry
229  Eigen::Quaterniond q(transf.linear());
230  // taken from Bullet
231  double s_squared = 1.0 - (q.w() * q.w());
232  if (s_squared < 10.0 * std::numeric_limits<double>::epsilon())
233  joint_values[2] = 0.0;
234  else
235  {
236  double s = 1.0 / sqrt(s_squared);
237  joint_values[2] = (acos(q.w()) * 2.0f) * (q.z() * s);
238  }
239 }
240 
241 } // end of namespace core
242 } // end of namespace moveit
s
XmlRpcServer s
check_isometry.h
random_numbers::RandomNumberGenerator::uniformReal
double uniformReal(double lower_bound, double upper_bound)
planar_joint_model.h
name
std::string name
ASSERT_ISOMETRY
#define ASSERT_ISOMETRY(transform)
moveit::core::PlanarJointModel::PlanarJointModel
PlanarJointModel(const std::string &name)
Definition: planar_joint_model.cpp:114
d
d
random_numbers::RandomNumberGenerator
moveit
Main namespace for MoveIt.
Definition: background_processing.h:46
values
std::vector< double > values
pr2_arm_kinematics::distance
double distance(const urdf::Pose &transform)
Definition: pr2_arm_ik.h:86
t
geometry_msgs::TransformStamped t


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Fri Dec 3 2021 03:23:21