40 #include <boost/math/constants/constants.hpp>
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)
57 variable_names_.push_back(
name_ +
"/" + local_variable_names_[i]);
58 variable_index_map_[variable_names_.back()] = i;
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;
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>();
73 computeVariableBoundsMsg();
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_;
90 for (
unsigned int i = 0; i < 2; ++i)
93 if (bounds[i].min_position_ <= 0.0 && bounds[i].max_position_ >= 0.0)
96 values[i] = (bounds[i].min_position_ + bounds[i].max_position_) / 2.0;
102 const Bounds& bounds)
const
104 if (bounds[0].max_position_ >= std::numeric_limits<double>::infinity() ||
105 bounds[0].min_position_ <= -std::numeric_limits<double>::infinity())
109 if (bounds[1].max_position_ >= std::numeric_limits<double>::infinity() ||
110 bounds[1].min_position_ <= -std::numeric_limits<double>::infinity())
118 const Bounds& bounds,
const double* near,
121 if (bounds[0].max_position_ >= std::numeric_limits<double>::infinity() ||
122 bounds[0].min_position_ <= -std::numeric_limits<double>::infinity())
126 std::min(bounds[0].max_position_, near[0] +
distance));
127 if (bounds[1].max_position_ >= std::numeric_limits<double>::infinity() ||
128 bounds[1].min_position_ <= -std::numeric_limits<double>::infinity())
132 std::min(bounds[1].max_position_, near[1] +
distance));
136 if (da > boost::math::constants::pi<double>())
137 da = boost::math::constants::pi<double>();
145 state[0] = from[0] + (to[0] - from[0]) *
t;
146 state[1] = from[1] + (to[1] - from[1]) *
t;
149 double diff = to[2] - from[2];
150 if (fabs(diff) <= boost::math::constants::pi<double>())
151 state[2] = from[2] + diff * t;
155 diff = 2.0 * boost::math::constants::pi<double>() - diff;
157 diff = -2.0 * boost::math::constants::pi<double>() - diff;
158 state[2] = from[2] - diff *
t;
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>();
169 double dx = values1[0] - values2[0];
170 double dy = values1[1] - values2[1];
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;
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)
188 if (v >= -boost::math::constants::pi<double>() && v <= boost::math::constants::pi<double>())
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>();
201 for (
unsigned int i = 0; i < 2; ++i)
203 if (values[i] < bounds[i].min_position_)
205 values[i] = bounds[i].min_position_;
208 else if (values[i] > bounds[i].max_position_)
210 values[i] = bounds[i].max_position_;
219 transf = Eigen::Isometry3d(Eigen::Translation3d(joint_values[0], joint_values[1], 0.0) *
220 Eigen::AngleAxisd(joint_values[2], Eigen::Vector3d::UnitZ()));
225 joint_values[0] = transf.translation().x();
226 joint_values[1] = transf.translation().y();
229 Eigen::Quaterniond q(transf.linear());
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;
236 double s = 1.0 / sqrt(s_squared);
237 joint_values[2] = (acos(q.w()) * 2.0f) * (q.z() *
s);