Go to the documentation of this file.00001
00026 #ifndef CONSTRAINED_IK_UTILS_H
00027 #define CONSTRAINED_IK_UTILS_H
00028
00029 #include <XmlRpc.h>
00030 #include <eigen3/Eigen/Core>
00031 #include <constrained_ik/CLIKDynamicConfig.h>
00032
00033 namespace constrained_ik
00034 {
00036 struct ConstrainedIKConfiguration
00037 {
00038 bool debug_mode;
00039 bool allow_joint_convergence;
00040 bool allow_primary_normalization;
00041 bool allow_auxiliary_nomalization;
00042 bool limit_primary_motion;
00043 bool limit_auxiliary_motion;
00044 bool limit_auxiliary_interations;
00045 int solver_max_iterations;
00046 int solver_min_iterations;
00047 int auxiliary_max_iterations;
00048 double primary_max_motion;
00049 double auxiliary_max_motion;
00050 double primary_norm;
00051 double auxiliary_norm;
00052 double primary_gain;
00053 double auxiliary_gain;
00054 double joint_convergence_tol;
00055 };
00056
00062 ConstrainedIKConfiguration convertToConstrainedIKConfiguration(CLIKDynamicConfig &config);
00063
00064 template<typename T>
00065 void validateConstrainedIKConfiguration(T &config)
00066 {
00067 if (config.limit_auxiliary_motion)
00068 {
00069 if (config.auxiliary_norm > config.auxiliary_max_motion)
00070 {
00071 config.auxiliary_norm = config.auxiliary_max_motion;
00072 }
00073 else if (config.auxiliary_norm < config.auxiliary_max_motion)
00074 {
00075 unsigned int divisor = floor(config.auxiliary_max_motion/config.auxiliary_norm) + 1;
00076 config.auxiliary_norm = config.auxiliary_max_motion/divisor;
00077 }
00078 }
00079 }
00080
00088 bool getParam(XmlRpc::XmlRpcValue& config, const std::string& key, double& value);
00089
00097 bool getParam(XmlRpc::XmlRpcValue& config, const std::string& key, std::vector<double>& double_array);
00098
00106 bool getParam(XmlRpc::XmlRpcValue& config, const std::string& key, Eigen::VectorXd& eigen_vector);
00107
00115 bool getParam(XmlRpc::XmlRpcValue& config, const std::string& key, bool& value);
00116
00124 bool getParam(XmlRpc::XmlRpcValue& config, const std::string& key, std::vector<std::string>& string_array);
00125
00126 }
00127 #endif // CONSTRAINED_IK_UTILS_H
00128