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 #ifndef MOVEIT_ROBOT_MODEL_FLOATING_JOINT_MODEL_
00038 #define MOVEIT_ROBOT_MODEL_FLOATING_JOINT_MODEL_
00039
00040 #include <moveit/robot_model/joint_model.h>
00041
00042 namespace robot_model
00043 {
00044
00046 class FloatingJointModel : public JointModel
00047 {
00048 friend class RobotModel;
00049 public:
00050 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00051
00052 FloatingJointModel(const std::string& name);
00053 virtual void getVariableDefaultValues(std::vector<double> &values, const Bounds &other_bounds) const;
00054 virtual void getVariableRandomValues(random_numbers::RandomNumberGenerator &rng, std::vector<double> &values, const Bounds &other_bounds) const;
00055 virtual void getVariableRandomValuesNearBy(random_numbers::RandomNumberGenerator &rng, std::vector<double> &values, const Bounds &other_bounds,
00056 const std::vector<double> &near, const double distance) const;
00057 virtual void enforceBounds(std::vector<double> &values, const Bounds &other_bounds) const;
00058 virtual bool satisfiesBounds(const std::vector<double> &values, const Bounds &other_bounds, double margin) const;
00059
00060 virtual double getMaximumExtent(const Bounds &other_bounds) const;
00061 virtual double distance(const std::vector<double> &values1, const std::vector<double> &values2) const;
00062 virtual void interpolate(const std::vector<double> &from, const std::vector<double> &to, const double t, std::vector<double> &state) const;
00063 virtual unsigned int getStateSpaceDimension() const;
00064 virtual void computeTransform(const std::vector<double>& joint_values, Eigen::Affine3d &transf) const;
00065 virtual void computeJointStateValues(const Eigen::Affine3d& transf, std::vector<double>& joint_values) const;
00066 virtual void updateTransform(const std::vector<double>& joint_values, Eigen::Affine3d &transf) const;
00067
00068 double getAngularDistanceWeight() const
00069 {
00070 return angular_distance_weight_;
00071 }
00072
00073 void setAngularDistanceWeight(double weight)
00074 {
00075 angular_distance_weight_ = weight;
00076 }
00077
00080 bool normalizeRotation(std::vector<double> &values) const;
00081
00082 double distanceRotation(const std::vector<double> &values1, const std::vector<double> &values2) const;
00083
00084 double distanceTranslation(const std::vector<double> &values1, const std::vector<double> &values2) const;
00085
00086 private:
00087
00088 double angular_distance_weight_;
00089 };
00090 }
00091
00092 #endif