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/prismatic_joint_model.h>
00038 #include <limits>
00039
00040 robot_model::PrismaticJointModel::PrismaticJointModel(const std::string& name) : JointModel(name), axis_(0.0, 0.0, 0.0)
00041 {
00042 type_ = PRISMATIC;
00043 variable_bounds_.push_back(std::make_pair(-std::numeric_limits<double>::max(), std::numeric_limits<double>::max()));
00044 variable_names_.push_back(name_);
00045 }
00046
00047 unsigned int robot_model::PrismaticJointModel::getStateSpaceDimension() const
00048 {
00049 return 1;
00050 }
00051
00052 double robot_model::PrismaticJointModel::getMaximumExtent(const Bounds &other_bounds) const
00053 {
00054 return other_bounds[0].second - other_bounds[0].first;
00055 }
00056
00057 void robot_model::PrismaticJointModel::getVariableDefaultValues(std::vector<double> &values, const Bounds &bounds) const
00058 {
00059
00060 if (bounds[0].first <= 0.0 && bounds[0].second >= 0.0)
00061 values.push_back(0.0);
00062 else
00063 values.push_back((bounds[0].first + bounds[0].second)/2.0);
00064 }
00065
00066 bool robot_model::PrismaticJointModel::satisfiesBounds(const std::vector<double> &values, const Bounds &bounds, double margin) const
00067 {
00068 assert(bounds.size() > 0);
00069 if (values[0] < bounds[0].first - margin || values[0] > bounds[0].second + margin)
00070 return false;
00071 return true;
00072 }
00073
00074 void robot_model::PrismaticJointModel::getVariableRandomValues(random_numbers::RandomNumberGenerator &rng, std::vector<double> &values, const Bounds &bounds) const
00075 {
00076 values.push_back(rng.uniformReal(bounds[0].first, bounds[0].second));
00077 }
00078
00079 void robot_model::PrismaticJointModel::getVariableRandomValuesNearBy(random_numbers::RandomNumberGenerator &rng, std::vector<double> &values, const Bounds &bounds,
00080 const std::vector<double> &near, const double distance) const
00081 {
00082 values.push_back(rng.uniformReal(std::max(bounds[0].first, near[values.size()] - distance),
00083 std::min(bounds[0].second, near[values.size()] + distance)));
00084 }
00085
00086 void robot_model::PrismaticJointModel::enforceBounds(std::vector<double> &values, const Bounds &bounds) const
00087 {
00088 const std::pair<double, double> &b = bounds[0];
00089 if (values[0] < b.first)
00090 values[0] = b.first;
00091 else
00092 if (values[0] > b.second)
00093 values[0] = b.second;
00094 }
00095
00096 double robot_model::PrismaticJointModel::distance(const std::vector<double> &values1, const std::vector<double> &values2) const
00097 {
00098 assert(values1.size() == 1);
00099 assert(values2.size() == 1);
00100 return fabs(values1[0] - values2[0]);
00101 }
00102
00103 void robot_model::PrismaticJointModel::interpolate(const std::vector<double> &from, const std::vector<double> &to, const double t, std::vector<double> &state) const
00104 {
00105 state[0] = from[0] + (to[0] - from[0]) * t;
00106 }
00107
00108 void robot_model::PrismaticJointModel::computeTransform(const std::vector<double>& joint_values, Eigen::Affine3d &transf) const
00109 {
00110 transf.setIdentity();
00111 updateTransform(joint_values, transf);
00112 }
00113
00114 void robot_model::PrismaticJointModel::updateTransform(const std::vector<double>& joint_values, Eigen::Affine3d &transf) const
00115 {
00116 transf.translation() = Eigen::Vector3d(axis_ * joint_values[0]);
00117 }
00118
00119 void robot_model::PrismaticJointModel::computeJointStateValues(const Eigen::Affine3d& transf, std::vector<double> &joint_values) const
00120 {
00121 joint_values.resize(1);
00122 joint_values[0] = transf.translation().dot(axis_);
00123 }