Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <moveit/robot_model/planar_joint_model.h>
00038 #include <boost/math/constants/constants.hpp>
00039 #include <limits>
00040 #include <cmath>
00041
00042 robot_model::PlanarJointModel::PlanarJointModel(const std::string& name) : JointModel(name), angular_distance_weight_(1.0)
00043 {
00044 type_ = PLANAR;
00045
00046 local_variable_names_.push_back("x");
00047 local_variable_names_.push_back("y");
00048 local_variable_names_.push_back("theta");
00049 for (int i = 0 ; i < 3 ; ++i)
00050 variable_names_.push_back(name_ + "/" + local_variable_names_[i]);
00051 variable_bounds_.resize(3);
00052 variable_bounds_[0] = std::make_pair(-std::numeric_limits<double>::max(), std::numeric_limits<double>::max());
00053 variable_bounds_[1] = std::make_pair(-std::numeric_limits<double>::max(), std::numeric_limits<double>::max());
00054 variable_bounds_[2] = std::make_pair(-boost::math::constants::pi<double>(), boost::math::constants::pi<double>());
00055 }
00056
00057 unsigned int robot_model::PlanarJointModel::getStateSpaceDimension() const
00058 {
00059 return 3;
00060 }
00061
00062 double robot_model::PlanarJointModel::getMaximumExtent(const Bounds &other_bounds) const
00063 {
00064 double dx = other_bounds[0].first - other_bounds[0].second;
00065 double dy = other_bounds[1].first - other_bounds[1].second;
00066 return sqrt(dx*dx + dy*dy) + boost::math::constants::pi<double>() * angular_distance_weight_;
00067 }
00068
00069 void robot_model::PlanarJointModel::getVariableDefaultValues(std::vector<double>& values, const Bounds &bounds) const
00070 {
00071 assert(bounds.size() > 1);
00072 for (unsigned int i = 0 ; i < 2 ; ++i)
00073 {
00074
00075 if (bounds[i].first <= 0.0 && bounds[i].second >= 0.0)
00076 values.push_back(0.0);
00077 else
00078 values.push_back((bounds[i].first + bounds[i].second)/2.0);
00079 }
00080 values.push_back(0.0);
00081 }
00082
00083 void robot_model::PlanarJointModel::getVariableRandomValues(random_numbers::RandomNumberGenerator &rng, std::vector<double> &values, const Bounds &bounds) const
00084 {
00085 std::size_t s = values.size();
00086 values.resize(s + 3);
00087 if (bounds[0].second >= std::numeric_limits<double>::max() || bounds[0].first <= -std::numeric_limits<double>::max())
00088 values[s] = 0.0;
00089 else
00090 values[s] = rng.uniformReal(bounds[0].first, bounds[0].second);
00091 if (bounds[1].second >= std::numeric_limits<double>::max() || bounds[1].first <= -std::numeric_limits<double>::max())
00092 values[s + 1] = 0.0;
00093 else
00094 values[s + 1] = rng.uniformReal(bounds[1].first, bounds[1].second);
00095 values[s + 2] = rng.uniformReal(bounds[2].first, bounds[2].second);
00096 }
00097
00098 void robot_model::PlanarJointModel::getVariableRandomValuesNearBy(random_numbers::RandomNumberGenerator &rng, std::vector<double> &values, const Bounds &bounds,
00099 const std::vector<double> &near, const double distance) const
00100 {
00101 std::size_t s = values.size();
00102 values.resize(s + 3);
00103 if (bounds[0].second >= std::numeric_limits<double>::max() || bounds[0].first <= -std::numeric_limits<double>::max())
00104 values[s] = 0.0;
00105 else
00106 values[s] = rng.uniformReal(std::max(bounds[0].first, near[s] - distance),
00107 std::min(bounds[0].second, near[s] + distance));
00108 if (bounds[1].second >= std::numeric_limits<double>::max() || bounds[1].first <= -std::numeric_limits<double>::max())
00109 values[s + 1] = 0.0;
00110 else
00111 values[s + 1] = rng.uniformReal(std::max(bounds[1].first, near[s + 1] - distance),
00112 std::min(bounds[1].second, near[s + 1] + distance));
00113
00114 double da = angular_distance_weight_ * distance;
00115 values[s + 2] = rng.uniformReal(near[s + 2] - da, near[s + 2] + da);
00116 normalizeRotation(values);
00117 }
00118
00119 void robot_model::PlanarJointModel::interpolate(const std::vector<double> &from, const std::vector<double> &to, const double t, std::vector<double> &state) const
00120 {
00121
00122 state[0] = from[0] + (to[0] - from[0]) * t;
00123 state[1] = from[1] + (to[1] - from[1]) * t;
00124
00125
00126 double diff = to[2] - from[2];
00127 if (fabs(diff) <= boost::math::constants::pi<double>())
00128 state[2] = from[2] + diff * t;
00129 else
00130 {
00131 if (diff > 0.0)
00132 diff = 2.0 * boost::math::constants::pi<double>() - diff;
00133 else
00134 diff = -2.0 * boost::math::constants::pi<double>() - diff;
00135 state[2] = from[2] - diff * t;
00136
00137 if (state[2] > boost::math::constants::pi<double>())
00138 state[2] -= 2.0 * boost::math::constants::pi<double>();
00139 else
00140 if (state[2] < -boost::math::constants::pi<double>())
00141 state[2] += 2.0 * boost::math::constants::pi<double>();
00142 }
00143 }
00144
00145 double robot_model::PlanarJointModel::distance(const std::vector<double> &values1, const std::vector<double> &values2) const
00146 {
00147 assert(values1.size() == 3);
00148 assert(values2.size() == 3);
00149 double dx = values1[0] - values2[0];
00150 double dy = values1[1] - values2[1];
00151
00152 double d = fabs(values1[2] - values2[2]);
00153 d = (d > boost::math::constants::pi<double>()) ? 2.0 * boost::math::constants::pi<double>() - d : d;
00154 return sqrt(dx*dx + dy*dy) + angular_distance_weight_ * d;
00155 }
00156
00157 bool robot_model::PlanarJointModel::satisfiesBounds(const std::vector<double> &values, const Bounds &bounds, double margin) const
00158 {
00159 assert(bounds.size() > 1);
00160 for (unsigned int i = 0 ; i < 3 ; ++i)
00161 if (values[0] < bounds[0].first - margin || values[0] > bounds[0].second + margin)
00162 return false;
00163 return true;
00164 }
00165
00166 bool robot_model::PlanarJointModel::normalizeRotation(std::vector<double> &values) const
00167 {
00168 double &v = values[2];
00169 if (v >= -boost::math::constants::pi<double>() && v <= boost::math::constants::pi<double>())
00170 return false;
00171 v = fmod(v, 2.0 * boost::math::constants::pi<double>());
00172 if (v < -boost::math::constants::pi<double>())
00173 v += 2.0 * boost::math::constants::pi<double>();
00174 else
00175 if (v > boost::math::constants::pi<double>())
00176 v -= 2.0 * boost::math::constants::pi<double>();
00177 return true;
00178 }
00179
00180 void robot_model::PlanarJointModel::enforceBounds(std::vector<double> &values, const Bounds &bounds) const
00181 {
00182 normalizeRotation(values);
00183 for (unsigned int i = 0 ; i < 2 ; ++i)
00184 {
00185 const std::pair<double, double> &b = bounds[i];
00186 if (values[i] < b.first)
00187 values[i] = b.first;
00188 else
00189 if (values[i] > b.second)
00190 values[i] = b.second;
00191 }
00192 }
00193
00194 void robot_model::PlanarJointModel::computeTransform(const std::vector<double>& joint_values, Eigen::Affine3d &transf) const
00195 {
00196 updateTransform(joint_values, transf);
00197 }
00198
00199 void robot_model::PlanarJointModel::updateTransform(const std::vector<double>& joint_values, Eigen::Affine3d &transf) const
00200 {
00201 transf = Eigen::Affine3d(Eigen::Translation3d(joint_values[0], joint_values[1], 0.0) * Eigen::AngleAxisd(joint_values[2], Eigen::Vector3d::UnitZ()));
00202 }
00203
00204 void robot_model::PlanarJointModel::computeJointStateValues(const Eigen::Affine3d& transf, std::vector<double> &joint_values) const
00205 {
00206 joint_values.resize(3);
00207 joint_values[0] = transf.translation().x();
00208 joint_values[1] = transf.translation().y();
00209
00210 Eigen::Quaterniond q(transf.rotation());
00211
00212 double s_squared = 1.0-(q.w()*q.w());
00213 if (s_squared < 10.0 * std::numeric_limits<double>::epsilon())
00214 joint_values[2] = 0.0;
00215 else
00216 {
00217 double s = 1.0/sqrt(s_squared);
00218 joint_values[2] = (acos(q.w())*2.0f)*(q.z()*s);
00219 }
00220 }
00221
00222 std::vector<moveit_msgs::JointLimits> robot_model::PlanarJointModel::getVariableLimits() const
00223 {
00224 std::vector<moveit_msgs::JointLimits> ret_vec = JointModel::getVariableLimits();
00225 ret_vec[2].has_position_limits = false;
00226 return ret_vec;
00227 }