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_KINEMATICS_METRICS_KINEMATICS_METRICS_
00038 #define MOVEIT_KINEMATICS_METRICS_KINEMATICS_METRICS_
00039
00040 #include <moveit/robot_state/robot_state.h>
00041 #include <moveit/planning_scene/planning_scene.h>
00042
00044 namespace kinematics_metrics
00045 {
00046
00051 class KinematicsMetrics
00052 {
00053 public:
00054
00056 KinematicsMetrics(const robot_model::RobotModelConstPtr &robot_model) :
00057 robot_model_(robot_model), penalty_multiplier_(0.0)
00058 {
00059 }
00060
00068 bool getManipulabilityIndex(const robot_state::RobotState &state,
00069 const std::string &group_name,
00070 double &manipulability_index,
00071 bool translation = false) const;
00072
00080 bool getManipulabilityIndex(const robot_state::RobotState &state,
00081 const robot_model::JointModelGroup *joint_model_group,
00082 double &manipulability_index,
00083 bool translation = false) const;
00084
00093 bool getManipulabilityEllipsoid(const robot_state::RobotState &state,
00094 const std::string &group_name,
00095 Eigen::MatrixXcd &eigen_values,
00096 Eigen::MatrixXcd &eigen_vectors) const;
00097
00106 bool getManipulabilityEllipsoid(const robot_state::RobotState &state,
00107 const robot_model::JointModelGroup *joint_model_group,
00108 Eigen::MatrixXcd &eigen_values,
00109 Eigen::MatrixXcd &eigen_vectors) const;
00110
00120 bool getManipulability(const robot_state::RobotState &state,
00121 const std::string &group_name,
00122 double &condition_number,
00123 bool translation = false) const;
00124
00134 bool getManipulability(const robot_state::RobotState &state,
00135 const robot_model::JointModelGroup *joint_model_group,
00136 double &condition_number,
00137 bool translation = false) const;
00138
00139 void setPenaltyMultiplier(double multiplier)
00140 {
00141 penalty_multiplier_ = fabs(multiplier);
00142 }
00143
00144 const double& getPenaltyMultiplier() const
00145 {
00146 return penalty_multiplier_;
00147 }
00148
00149 protected:
00150
00151 robot_model::RobotModelConstPtr robot_model_;
00152
00153 private:
00154
00165 double getJointLimitsPenalty(const robot_state::RobotState &state, const robot_model::JointModelGroup *joint_model_group) const;
00166
00167 double penalty_multiplier_;
00168
00169 };
00170
00171 MOVEIT_CLASS_FORWARD(KinematicsMetrics);
00172
00173 }
00174
00175 #endif