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/floating_joint_model.h>
00038 #include <boost/math/constants/constants.hpp>
00039 #include <limits>
00040 #include <cmath>
00041
00042 robot_model::FloatingJointModel::FloatingJointModel(const std::string& name) : JointModel(name), angular_distance_weight_(1.0)
00043 {
00044 type_ = FLOATING;
00045 local_variable_names_.push_back("trans_x");
00046 local_variable_names_.push_back("trans_y");
00047 local_variable_names_.push_back("trans_z");
00048 local_variable_names_.push_back("rot_x");
00049 local_variable_names_.push_back("rot_y");
00050 local_variable_names_.push_back("rot_z");
00051 local_variable_names_.push_back("rot_w");
00052 for (int i = 0 ; i < 7 ; ++i)
00053 variable_names_.push_back(name_ + "/" + local_variable_names_[i]);
00054 variable_bounds_.resize(7);
00055 variable_bounds_[0] = std::make_pair(-std::numeric_limits<double>::max(), std::numeric_limits<double>::max());
00056 variable_bounds_[1] = std::make_pair(-std::numeric_limits<double>::max(), std::numeric_limits<double>::max());
00057 variable_bounds_[2] = std::make_pair(-std::numeric_limits<double>::max(), std::numeric_limits<double>::max());
00058 variable_bounds_[3] = std::make_pair(-1.0, 1.0);
00059 variable_bounds_[4] = std::make_pair(-1.0, 1.0);
00060 variable_bounds_[5] = std::make_pair(-1.0, 1.0);
00061 variable_bounds_[6] = std::make_pair(-1.0, 1.0);
00062 }
00063
00064 double robot_model::FloatingJointModel::getMaximumExtent(const Bounds &other_bounds) const
00065 {
00066 double dx = other_bounds[0].first - other_bounds[0].second;
00067 double dy = other_bounds[1].first - other_bounds[1].second;
00068 double dz = other_bounds[2].first - other_bounds[2].second;
00069 return sqrt(dx*dx + dy*dy + dz*dz) + boost::math::constants::pi<double>() * 0.5 * angular_distance_weight_;
00070 }
00071
00072 double robot_model::FloatingJointModel::distance(const std::vector<double> &values1, const std::vector<double> &values2) const
00073 {
00074 assert(values1.size() == 7);
00075 assert(values2.size() == 7);
00076 double dx = values1[0] - values2[0];
00077 double dy = values1[1] - values2[1];
00078 double dz = values1[2] - values2[2];
00079 double dq = fabs(values1[3] * values2[3] + values1[4] * values2[4] + values1[5] * values2[5] + values1[6] * values2[6]);
00080 if (dq + std::numeric_limits<double>::epsilon() >= 1.0)
00081 return sqrt(dx*dx + dy*dy + dz * dz);
00082 else
00083 return angular_distance_weight_ * acos(dq) + sqrt(dx*dx + dy*dy + dz * dz);
00084 }
00085
00086 double robot_model::FloatingJointModel::distanceTranslation(const std::vector<double> &values1, const std::vector<double> &values2) const
00087 {
00088 assert(values1.size() == 7);
00089 assert(values2.size() == 7);
00090 double dx = values1[0] - values2[0];
00091 double dy = values1[1] - values2[1];
00092 double dz = values1[2] - values2[2];
00093 return sqrt(dx*dx + dy*dy + dz * dz);
00094 }
00095
00096 double robot_model::FloatingJointModel::distanceRotation(const std::vector<double> &values1, const std::vector<double> &values2) const
00097 {
00098 double dq = fabs(values1[3] * values2[3] + values1[4] * values2[4] + values1[5] * values2[5] + values1[6] * values2[6]);
00099 if (dq + std::numeric_limits<double>::epsilon() >= 1.0)
00100 return 0.0;
00101 else
00102 return acos(dq);
00103 }
00104
00105
00106 void robot_model::FloatingJointModel::interpolate(const std::vector<double> &from, const std::vector<double> &to, const double t, std::vector<double> &state) const
00107 {
00108
00109 state[0] = from[0] + (to[0] - from[0]) * t;
00110 state[1] = from[1] + (to[1] - from[1]) * t;
00111 state[2] = from[2] + (to[2] - from[2]) * t;
00112
00113 double dq = fabs(from[3] * to[3] + from[4] * to[4] + from[5] * to[5] + from[6] * to[6]);
00114 double theta = (dq + std::numeric_limits<double>::epsilon() >= 1.0) ? 0.0 : acos(dq);
00115 if (theta > std::numeric_limits<double>::epsilon())
00116 {
00117 double d = 1.0 / sin(theta);
00118 double s0 = sin((1.0 - t) * theta);
00119 double s1 = sin(t * theta);
00120 if (dq < 0)
00121 s1 = -s1;
00122 state[3] = (from[3] * s0 + to[3] * s1) * d;
00123 state[4] = (from[4] * s0 + to[4] * s1) * d;
00124 state[5] = (from[5] * s0 + to[5] * s1) * d;
00125 state[6] = (from[6] * s0 + to[6] * s1) * d;
00126 }
00127 else
00128 {
00129 state[3] = from[3];
00130 state[4] = from[4];
00131 state[5] = from[5];
00132 state[6] = from[6];
00133 }
00134 }
00135
00136 bool robot_model::FloatingJointModel::satisfiesBounds(const std::vector<double> &values, const Bounds &bounds, double margin) const
00137 {
00138 assert(bounds.size() > 2);
00139
00140 if (values[0] < bounds[0].first - margin || values[0] > bounds[0].second + margin)
00141 return false;
00142 if (values[1] < bounds[1].first - margin || values[1] > bounds[1].second + margin)
00143 return false;
00144 if (values[2] < bounds[2].first - margin || values[2] > bounds[2].second + margin)
00145 return false;
00146 double normSqr = values[3] * values[3] + values[4] * values[4] + values[5] * values[5] + values[6] * values[6];
00147 if (fabs(normSqr - 1.0) > std::numeric_limits<float>::epsilon() * 10.0)
00148 return false;
00149 return true;
00150 }
00151
00152 bool robot_model::FloatingJointModel::normalizeRotation(std::vector<double> &values) const
00153 {
00154
00155 double normSqr = values[3] * values[3] + values[4] * values[4] + values[5] * values[5] + values[6] * values[6];
00156 if (fabs(normSqr - 1.0) > std::numeric_limits<double>::epsilon() * 100.0)
00157 {
00158 double norm = sqrt(normSqr);
00159 if (norm < std::numeric_limits<double>::epsilon() * 100.0)
00160 {
00161 logWarn("Quaternion is zero in RobotState *representation. Setting to identity");
00162 values[3] = 0.0;
00163 values[4] = 0.0;
00164 values[5] = 0.0;
00165 values[6] = 1.0;
00166 }
00167 else
00168 {
00169 values[3] /= norm;
00170 values[4] /= norm;
00171 values[5] /= norm;
00172 values[6] /= norm;
00173 }
00174 return true;
00175 }
00176 else
00177 return false;
00178 }
00179
00180 unsigned int robot_model::FloatingJointModel::getStateSpaceDimension() const
00181 {
00182 return 6;
00183 }
00184
00185 void robot_model::FloatingJointModel::enforceBounds(std::vector<double> &values, const Bounds &bounds) const
00186 {
00187 normalizeRotation(values);
00188 for (unsigned int i = 0 ; i < 3 ; ++i)
00189 {
00190 const std::pair<double, double> &b = bounds[i];
00191 if (values[i] < b.first)
00192 values[i] = b.first;
00193 else
00194 if (values[i] > b.second)
00195 values[i] = b.second;
00196 }
00197 }
00198
00199 void robot_model::FloatingJointModel::computeTransform(const std::vector<double>& joint_values, Eigen::Affine3d &transf) const
00200 {
00201 updateTransform(joint_values, transf);
00202 }
00203
00204 void robot_model::FloatingJointModel::updateTransform(const std::vector<double>& joint_values, Eigen::Affine3d &transf) const
00205 {
00206 transf = Eigen::Affine3d(Eigen::Translation3d(joint_values[0], joint_values[1], joint_values[2])
00207 *Eigen::Quaterniond(joint_values[6],joint_values[3], joint_values[4], joint_values[5]).toRotationMatrix());
00208 }
00209
00210 void robot_model::FloatingJointModel::computeJointStateValues(const Eigen::Affine3d& transf, std::vector<double> &joint_values) const
00211 {
00212 joint_values.resize(7);
00213 joint_values[0] = transf.translation().x();
00214 joint_values[1] = transf.translation().y();
00215 joint_values[2] = transf.translation().z();
00216 Eigen::Quaterniond q(transf.rotation());
00217 joint_values[3] = q.x();
00218 joint_values[4] = q.y();
00219 joint_values[5] = q.z();
00220 joint_values[6] = q.w();
00221 }
00222
00223 void robot_model::FloatingJointModel::getVariableDefaultValues(std::vector<double>& values, const Bounds &bounds) const
00224 {
00225 assert(bounds.size() > 2);
00226 for (unsigned int i = 0 ; i < 3 ; ++i)
00227 {
00228
00229 if (bounds[i].first <= 0.0 && bounds[i].second >= 0.0)
00230 values.push_back(0.0);
00231 else
00232 values.push_back((bounds[i].first + bounds[i].second)/2.0);
00233 }
00234
00235 values.push_back(0.0);
00236 values.push_back(0.0);
00237 values.push_back(0.0);
00238 values.push_back(1.0);
00239 }
00240
00241 void robot_model::FloatingJointModel::getVariableRandomValues(random_numbers::RandomNumberGenerator &rng, std::vector<double> &values, const Bounds &bounds) const
00242 {
00243 std::size_t s = values.size();
00244 values.resize(s + 7);
00245
00246 if (bounds[0].second >= std::numeric_limits<double>::max() || bounds[0].first <= -std::numeric_limits<double>::max())
00247 values[s] = 0.0;
00248 else
00249 values[s] = rng.uniformReal(bounds[0].first, bounds[0].second);
00250 if (bounds[1].second >= std::numeric_limits<double>::max() || bounds[1].first <= -std::numeric_limits<double>::max())
00251 values[s + 1] = 0.0;
00252 else
00253 values[s + 1] = rng.uniformReal(bounds[1].first, bounds[1].second);
00254 if (bounds[2].second >= std::numeric_limits<double>::max() || bounds[2].first <= -std::numeric_limits<double>::max())
00255 values[s + 2] = 0.0;
00256 else
00257 values[s + 2] = rng.uniformReal(bounds[2].first, bounds[2].second);
00258
00259 double q[4]; rng.quaternion(q);
00260 values[s + 3] = q[0];
00261 values[s + 4] = q[1];
00262 values[s + 5] = q[2];
00263 values[s + 6] = q[3];
00264 }
00265
00266 void robot_model::FloatingJointModel::getVariableRandomValuesNearBy(random_numbers::RandomNumberGenerator &rng, std::vector<double> &values, const Bounds &bounds,
00267 const std::vector<double> &near, const double distance) const
00268 {
00269 std::size_t s = values.size();
00270 values.resize(s + 7);
00271
00272 if (bounds[0].second >= std::numeric_limits<double>::max() || bounds[0].first <= -std::numeric_limits<double>::max())
00273 values[s] = 0.0;
00274 else
00275 values[s] = rng.uniformReal(std::max(bounds[0].first, near[s] - distance),
00276 std::min(bounds[0].second, near[s] + distance));
00277 if (bounds[1].second >= std::numeric_limits<double>::max() || bounds[1].first <= -std::numeric_limits<double>::max())
00278 values[s + 1] = 0.0;
00279 else
00280 values[s + 1] = rng.uniformReal(std::max(bounds[1].first, near[s + 1] - distance),
00281 std::min(bounds[1].second, near[s + 1] + distance));
00282 if (bounds[2].second >= std::numeric_limits<double>::max() || bounds[2].first <= -std::numeric_limits<double>::max())
00283 values[s + 2] = 0.0;
00284 else
00285 values[s + 2] = rng.uniformReal(std::max(bounds[2].first, near[s + 2] - distance),
00286 std::min(bounds[2].second, near[s + 2] + distance));
00287
00288 double da = angular_distance_weight_ * distance;
00289 if (da >= .25 * boost::math::constants::pi<double>())
00290 {
00291 double q[4]; rng.quaternion(q);
00292 values[s + 3] = q[0];
00293 values[s + 4] = q[1];
00294 values[s + 5] = q[2];
00295 values[s + 6] = q[3];
00296 }
00297 else
00298 {
00299
00300
00301 double ax = rng.gaussian01();
00302 double ay = rng.gaussian01();
00303 double az = rng.gaussian01();
00304 double angle = 2.0 * pow(rng.uniform01(), 1.0/3.0) * da;
00305
00306 double q[4];
00307 double norm = sqrt(ax * ax + ay * ay + az * az);
00308 if (norm < 1e-6)
00309 {
00310 q[0] = q[1] = q[2] = 0.0;
00311 q[3] = 1.0;
00312 }
00313 else
00314 {
00315 double s = sin(angle / 2.0);
00316 q[0] = s * ax / norm;
00317 q[1] = s * ay / norm;
00318 q[2] = s * az / norm;
00319 q[3] = cos(angle / 2.0);
00320 }
00321
00322 values[s + 3] = near[s + 6]*q[0] + near[s + 3]*q[3] + near[s + 4]*q[2] - near[s + 5]*q[1];
00323 values[s + 4] = near[s + 6]*q[1] + near[s + 4]*q[3] + near[s + 5]*q[0] - near[s + 3]*q[2];
00324 values[s + 5] = near[s + 6]*q[2] + near[s + 5]*q[3] + near[s + 3]*q[1] - near[s + 4]*q[0];
00325 values[s + 6] = near[s + 6]*q[3] - near[s + 3]*q[0] - near[s + 4]*q[1] - near[s + 5]*q[2];
00326 }
00327 }