00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #ifndef COB_TWIST_CONTROLLER_COB_TWIST_CONTROLLER_DATA_TYPES_H
00019 #define COB_TWIST_CONTROLLER_COB_TWIST_CONTROLLER_DATA_TYPES_H
00020
00021 #include <string>
00022 #include <vector>
00023 #include <map>
00024 #include <stdint.h>
00025 #include <Eigen/Core>
00026 #include <Eigen/LU>
00027 #include <kdl/jntarray.hpp>
00028 #include <kdl/chainjnttojacsolver.hpp>
00029 #include <cob_twist_controller/TwistControllerConfig.h>
00030
00031 #define MAX_CRIT true
00032 #define MIN_CRIT false
00033 #define DEFAULT_CYCLE 0.02
00034 #define ZERO_THRESHOLD 1.0e-9 /// used for numerical 0.0 threshold
00035 #define DIV0_SAFE 1.0e-6 /// used for division in case of DIV/0
00036
00037 enum DampingMethodTypes
00038 {
00039 NO_DAMPING = cob_twist_controller::TwistController_NO_DAMPING,
00040 CONSTANT = cob_twist_controller::TwistController_CONSTANT,
00041 MANIPULABILITY = cob_twist_controller::TwistController_MANIPULABILITY,
00042 LEAST_SINGULAR_VALUE = cob_twist_controller::TwistController_LEAST_SINGULAR_VALUE,
00043 SIGMOID = cob_twist_controller::TwistController_SIGMOID,
00044 };
00045
00046 enum KinematicExtensionTypes
00047 {
00048 NO_EXTENSION = cob_twist_controller::TwistController_NO_EXTENSION,
00049 BASE_COMPENSATION = cob_twist_controller::TwistController_BASE_COMPENSATION,
00050 BASE_ACTIVE = cob_twist_controller::TwistController_BASE_ACTIVE,
00051 COB_TORSO = cob_twist_controller::TwistController_COB_TORSO,
00052 LOOKAT = cob_twist_controller::TwistController_LOOKAT
00053 };
00054
00055 enum SolverTypes
00056 {
00057 DEFAULT_SOLVER = cob_twist_controller::TwistController_DEFAULT_SOLVER,
00058 WLN = cob_twist_controller::TwistController_WLN,
00059 GPM = cob_twist_controller::TwistController_GPM,
00060 STACK_OF_TASKS = cob_twist_controller::TwistController_STACK_OF_TASKS,
00061 TASK_2ND_PRIO = cob_twist_controller::TwistController_TASK_2ND_PRIO,
00062 UNIFIED_JLA_SA = cob_twist_controller::TwistController_UNIFIED_JLA_SA,
00063 };
00064
00065 enum ConstraintTypesCA
00066 {
00067 CA_OFF = cob_twist_controller::TwistController_CA_OFF,
00068 CA_ON = cob_twist_controller::TwistController_CA,
00069 };
00070
00071 enum ConstraintTypesJLA
00072 {
00073 JLA_OFF = cob_twist_controller::TwistController_JLA_OFF,
00074 JLA_ON = cob_twist_controller::TwistController_JLA,
00075 JLA_MID_ON = cob_twist_controller::TwistController_JLA_MID,
00076 JLA_INEQ_ON = cob_twist_controller::TwistController_JLA_INEQ,
00077 };
00078
00079 enum LookatAxisTypes
00080 {
00081 X_POSITIVE,
00082 Y_POSITIVE,
00083 Z_POSITIVE,
00084 X_NEGATIVE,
00085 Y_NEGATIVE,
00086 Z_NEGATIVE,
00087 };
00088
00089 struct LookatOffset
00090 {
00091 LookatOffset() :
00092 lookat_axis_type(X_POSITIVE),
00093 translation_x(0.0),
00094 translation_y(0.0),
00095 translation_z(0.0),
00096 rotation_x(0.0),
00097 rotation_y(0.0),
00098 rotation_z(0.0),
00099 rotation_w(1.0)
00100 {}
00101
00102 LookatAxisTypes lookat_axis_type;
00103 double translation_x;
00104 double translation_y;
00105 double translation_z;
00106 double rotation_x;
00107 double rotation_y;
00108 double rotation_z;
00109 double rotation_w;
00110 };
00111
00112 struct JointStates
00113 {
00114 KDL::JntArray current_q_;
00115 KDL::JntArray last_q_;
00116 KDL::JntArray current_q_dot_;
00117 KDL::JntArray last_q_dot_;
00118 };
00119
00120 struct ActiveCartesianDimension
00121 {
00122 ActiveCartesianDimension():
00123 lin_x(0.0), lin_y(0.0), lin_z(0.0), rot_x(0.0), rot_y(0.0), rot_z(0.0) {}
00124
00125 double lin_x;
00126 double lin_y;
00127 double lin_z;
00128 double rot_x;
00129 double rot_y;
00130 double rot_z;
00131 };
00132
00133 struct ObstacleDistanceData
00134 {
00135 double min_distance;
00136 Eigen::Vector3d frame_vector;
00137 Eigen::Vector3d nearest_point_frame_vector;
00138 Eigen::Vector3d nearest_point_obstacle_vector;
00139 };
00140
00141 struct ConstraintThresholds
00142 {
00143 double activation;
00144 double activation_with_buffer;
00145 double critical;
00146 };
00147
00148 struct ConstraintParams
00149 {
00150 uint32_t priority;
00151 double k_H;
00152 double damping;
00153 ConstraintThresholds thresholds;
00154 };
00155
00156 enum ConstraintTypes
00157 {
00158 CA,
00159 JLA,
00160 };
00161
00162 struct LimiterParams
00163 {
00164 LimiterParams() :
00165 keep_direction(true),
00166 enforce_input_limits(true),
00167 enforce_pos_limits(true),
00168 enforce_vel_limits(true),
00169 enforce_acc_limits(false),
00170 limits_tolerance(5.0),
00171 max_lin_twist(0.5),
00172 max_rot_twist(0.5),
00173 max_vel_lin_base(0.5),
00174 max_vel_rot_base(0.5)
00175 {}
00176
00177 bool keep_direction;
00178 bool enforce_input_limits;
00179 bool enforce_pos_limits;
00180 bool enforce_vel_limits;
00181 bool enforce_acc_limits;
00182 double limits_tolerance;
00183 double max_lin_twist;
00184 double max_rot_twist;
00185 double max_vel_lin_base;
00186 double max_vel_rot_base;
00187
00188 std::vector<double> limits_max;
00189 std::vector<double> limits_min;
00190 std::vector<double> limits_vel;
00191 std::vector<double> limits_acc;
00192 };
00193
00194 struct UJSSolverParams
00195 {
00196 UJSSolverParams() :
00197 sigma(0.05),
00198 sigma_speed(0.005),
00199 delta_pos(0.5),
00200 delta_speed(1.0)
00201 {}
00202
00203 double sigma;
00204 double sigma_speed;
00205 double delta_pos;
00206 double delta_speed;
00207 };
00208
00209 struct TwistControllerParams
00210 {
00211 TwistControllerParams() :
00212 dof(0),
00213 controller_interface(""),
00214 integrator_smoothing(0.2),
00215
00216 numerical_filtering(false),
00217 damping_method(SIGMOID),
00218 damping_factor(0.01),
00219 lambda_max(0.001),
00220 w_threshold(0.001),
00221 beta(0.005),
00222 slope_damping(0.05),
00223 eps_damping(0.003),
00224 eps_truncation(0.001),
00225
00226 solver(GPM),
00227 priority_main(500),
00228 k_H(1.0),
00229
00230 constraint_jla(JLA_ON),
00231 constraint_ca(CA_ON),
00232
00233 kinematic_extension(NO_EXTENSION),
00234 extension_ratio(0.0)
00235 {
00236 ConstraintParams cp_ca;
00237 cp_ca.priority = 100;
00238 cp_ca.k_H = 2.0;
00239 cp_ca.damping = 0.000001;
00240 cp_ca.thresholds.activation = 0.1;
00241 cp_ca.thresholds.critical = 0.025;
00242 cp_ca.thresholds.activation_with_buffer = cp_ca.thresholds.activation * 1.5;
00243 constraint_params.insert(std::pair<ConstraintTypes, ConstraintParams>(CA, cp_ca));
00244
00245 ConstraintParams cp_jla;
00246 cp_jla.priority = 50;
00247 cp_jla.k_H = -10.0;
00248 cp_jla.damping = 0.000001;
00249 cp_jla.thresholds.activation = 0.1;
00250 cp_jla.thresholds.critical = 0.05;
00251 cp_jla.thresholds.activation_with_buffer = cp_jla.thresholds.activation * 4.0;
00252 constraint_params.insert(std::pair<ConstraintTypes, ConstraintParams>(JLA, cp_jla));
00253 }
00254
00255 uint8_t dof;
00256 std::string chain_base_link;
00257 std::string chain_tip_link;
00258
00259 std::string controller_interface;
00260 double integrator_smoothing;
00261
00262 bool numerical_filtering;
00263 DampingMethodTypes damping_method;
00264 double damping_factor;
00265 double lambda_max;
00266 double w_threshold;
00267 double beta;
00268 double slope_damping;
00269 double eps_damping;
00270 double eps_truncation;
00271
00272 SolverTypes solver;
00273 uint32_t priority_main;
00274 double k_H;
00275
00276 ConstraintTypesCA constraint_ca;
00277 ConstraintTypesJLA constraint_jla;
00278 std::map<ConstraintTypes, ConstraintParams> constraint_params;
00279
00280 UJSSolverParams ujs_solver_params;
00281 LimiterParams limiter_params;
00282
00283 KinematicExtensionTypes kinematic_extension;
00284 LookatOffset lookat_offset;
00285 double extension_ratio;
00286
00287 std::vector<std::string> frame_names;
00288 std::vector<std::string> joints;
00289
00290
00291 std::vector<std::string> collision_check_links;
00292 };
00293
00294 enum EN_ConstraintStates
00295 {
00296 NORMAL = 0,
00297 DANGER,
00298 CRITICAL,
00299 };
00300
00301 class ConstraintState
00302 {
00303 public:
00304 ConstraintState()
00305 : current_(NORMAL), previous_(NORMAL), transition_(false)
00306 {}
00307
00308 inline void setState(EN_ConstraintStates next_state)
00309 {
00310 this->transition_ = this->current_ != next_state;
00311 this->previous_ = this->current_;
00312 this->current_ = next_state;
00313 }
00314
00315 inline EN_ConstraintStates getCurrent() const
00316 {
00317 return this->current_;
00318 }
00319
00320 inline EN_ConstraintStates getPrevious() const
00321 {
00322 return this->previous_;
00323 }
00324
00325 inline bool isTransition() const
00326 {
00327 return this->transition_;
00328 }
00329
00330 private:
00331 EN_ConstraintStates current_;
00332 EN_ConstraintStates previous_;
00333 bool transition_;
00334 };
00335
00336 typedef Eigen::Matrix<double, 6, Eigen::Dynamic> Matrix6Xd_t;
00337 typedef Eigen::Matrix<double, 6, 1> Vector6d_t;
00338 typedef std::map<std::string, std::vector<ObstacleDistanceData> > ObstacleDistancesInfo_t;
00339
00340 #endif // COB_TWIST_CONTROLLER_COB_TWIST_CONTROLLER_DATA_TYPES_H