cob_twist_controller: cob_twist_controller_data

Go to the documentation of this file.
00001 /*
00002  * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *   http://www.apache.org/licenses/LICENSE-2.0
00009 
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
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;  // best experienced value
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;  // best experienced value
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     // vector of links of the chain to be considered for collision avoidance
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