Compute different kinds of metrics for kinematics evaluation. Currently includes manipulability. More...
#include <kinematics_metrics.h>
Public Member Functions | |
bool | getManipulability (const robot_state::RobotState &state, const std::string &group_name, double &condition_number, bool translation=false) const |
Get the manipulability = sigma_min/sigma_max where sigma_min and sigma_max are the smallest and largest singular values of the Jacobian matrix J. | |
bool | getManipulability (const robot_state::RobotState &state, const robot_model::JointModelGroup *joint_model_group, double &condition_number, bool translation=false) const |
Get the manipulability = sigma_min/sigma_max where sigma_min and sigma_max are the smallest and largest singular values of the Jacobian matrix J. | |
bool | getManipulabilityEllipsoid (const robot_state::RobotState &state, const std::string &group_name, Eigen::MatrixXcd &eigen_values, Eigen::MatrixXcd &eigen_vectors) const |
Get the (translation) manipulability ellipsoid for a given group at a given joint configuration. | |
bool | getManipulabilityEllipsoid (const robot_state::RobotState &state, const robot_model::JointModelGroup *joint_model_group, Eigen::MatrixXcd &eigen_values, Eigen::MatrixXcd &eigen_vectors) const |
Get the (translation) manipulability ellipsoid for a given group at a given joint configuration. | |
bool | getManipulabilityIndex (const robot_state::RobotState &state, const std::string &group_name, double &manipulability_index, bool translation=false) const |
Get the manipulability for a given group at a given joint configuration. | |
bool | getManipulabilityIndex (const robot_state::RobotState &state, const robot_model::JointModelGroup *joint_model_group, double &manipulability_index, bool translation=false) const |
Get the manipulability for a given group at a given joint configuration. | |
const double & | getPenaltyMultiplier () const |
KinematicsMetrics (const robot_model::RobotModelConstPtr &robot_model) | |
Construct a KinematicsMetricss from a RobotModel. | |
void | setPenaltyMultiplier (double multiplier) |
Protected Attributes | |
robot_model::RobotModelConstPtr | robot_model_ |
Private Member Functions | |
double | getJointLimitsPenalty (const robot_state::RobotState &state, const robot_model::JointModelGroup *joint_model_group) const |
Defines a multiplier for the manipulabilty = 1 - exp ( -penalty_multipler_ * product_{i=1}{n} (distance_to_lower_limit * distance_to_higher_limit/(joint_range*joint_range))) where n is the number of joints in the group. Floating joints are ignored in this computation. Planar joints with finite bounds are considered. Set penalty_multiplier_ to 0 if you don't want this multiplier to have any effect on the manipulability measures. See "Workspace Geometric Characterization and Manipulability of Industrial Robots", Ming-June, Tsia, PhD Thesis, Ohio State University, 1986, for more details. | |
Private Attributes | |
double | penalty_multiplier_ |
Compute different kinds of metrics for kinematics evaluation. Currently includes manipulability.
Definition at line 51 of file kinematics_metrics.h.
kinematics_metrics::KinematicsMetrics::KinematicsMetrics | ( | const robot_model::RobotModelConstPtr & | robot_model | ) | [inline] |
Construct a KinematicsMetricss from a RobotModel.
Definition at line 56 of file kinematics_metrics.h.
double kinematics_metrics::KinematicsMetrics::getJointLimitsPenalty | ( | const robot_state::RobotState & | state, |
const robot_model::JointModelGroup * | joint_model_group | ||
) | const [private] |
Defines a multiplier for the manipulabilty = 1 - exp ( -penalty_multipler_ * product_{i=1}{n} (distance_to_lower_limit * distance_to_higher_limit/(joint_range*joint_range))) where n is the number of joints in the group. Floating joints are ignored in this computation. Planar joints with finite bounds are considered. Set penalty_multiplier_ to 0 if you don't want this multiplier to have any effect on the manipulability measures. See "Workspace Geometric Characterization and Manipulability of Industrial Robots", Ming-June, Tsia, PhD Thesis, Ohio State University, 1986, for more details.
Definition at line 44 of file kinematics_metrics.cpp.
bool kinematics_metrics::KinematicsMetrics::getManipulability | ( | const robot_state::RobotState & | state, |
const std::string & | group_name, | ||
double & | condition_number, | ||
bool | translation = false |
||
) | const |
Get the manipulability = sigma_min/sigma_max where sigma_min and sigma_max are the smallest and largest singular values of the Jacobian matrix J.
state | Complete kinematic state for the robot |
group_name | The group name (e.g. "arm") |
condition_number | Condition number for JJ^T |
Definition at line 194 of file kinematics_metrics.cpp.
bool kinematics_metrics::KinematicsMetrics::getManipulability | ( | const robot_state::RobotState & | state, |
const robot_model::JointModelGroup * | joint_model_group, | ||
double & | condition_number, | ||
bool | translation = false |
||
) | const |
Get the manipulability = sigma_min/sigma_max where sigma_min and sigma_max are the smallest and largest singular values of the Jacobian matrix J.
state | Complete kinematic state for the robot |
joint_model_group | A pointer to the desired joint model group |
condition_number | Condition number for JJ^T |
Definition at line 206 of file kinematics_metrics.cpp.
bool kinematics_metrics::KinematicsMetrics::getManipulabilityEllipsoid | ( | const robot_state::RobotState & | state, |
const std::string & | group_name, | ||
Eigen::MatrixXcd & | eigen_values, | ||
Eigen::MatrixXcd & | eigen_vectors | ||
) | const |
Get the (translation) manipulability ellipsoid for a given group at a given joint configuration.
state | Complete kinematic state for the robot |
group_name | The group name (e.g. "arm") |
eigen_values | The eigen values for the translation part of JJ^T |
eigen_vectors | The eigen vectors for the translation part of JJ^T |
Definition at line 163 of file kinematics_metrics.cpp.
bool kinematics_metrics::KinematicsMetrics::getManipulabilityEllipsoid | ( | const robot_state::RobotState & | state, |
const robot_model::JointModelGroup * | joint_model_group, | ||
Eigen::MatrixXcd & | eigen_values, | ||
Eigen::MatrixXcd & | eigen_vectors | ||
) | const |
Get the (translation) manipulability ellipsoid for a given group at a given joint configuration.
state | Complete kinematic state for the robot |
joint_model_group | A pointer to the desired joint model group |
eigen_values | The eigen values for the translation part of JJ^T |
eigen_vectors | The eigen vectors for the translation part of JJ^T |
Definition at line 175 of file kinematics_metrics.cpp.
bool kinematics_metrics::KinematicsMetrics::getManipulabilityIndex | ( | const robot_state::RobotState & | state, |
const std::string & | group_name, | ||
double & | manipulability_index, | ||
bool | translation = false |
||
) | const |
Get the manipulability for a given group at a given joint configuration.
state | Complete kinematic state for the robot |
group_name | The group name (e.g. "arm") |
manipulability_index | The computed manipulability = sqrt(det(JJ^T)) |
Definition at line 89 of file kinematics_metrics.cpp.
bool kinematics_metrics::KinematicsMetrics::getManipulabilityIndex | ( | const robot_state::RobotState & | state, |
const robot_model::JointModelGroup * | joint_model_group, | ||
double & | manipulability_index, | ||
bool | translation = false |
||
) | const |
Get the manipulability for a given group at a given joint configuration.
state | Complete kinematic state for the robot |
joint_model_group | A pointer to the desired joint model group |
manipulability_index | The computed manipulability = sqrt(det(JJ^T)) |
Definition at line 101 of file kinematics_metrics.cpp.
const double& kinematics_metrics::KinematicsMetrics::getPenaltyMultiplier | ( | ) | const [inline] |
Definition at line 144 of file kinematics_metrics.h.
void kinematics_metrics::KinematicsMetrics::setPenaltyMultiplier | ( | double | multiplier | ) | [inline] |
Definition at line 139 of file kinematics_metrics.h.
double kinematics_metrics::KinematicsMetrics::penalty_multiplier_ [private] |
Definition at line 167 of file kinematics_metrics.h.
robot_model::RobotModelConstPtr kinematics_metrics::KinematicsMetrics::robot_model_ [protected] |
Definition at line 151 of file kinematics_metrics.h.