enable_manager.h

00001 #pragma once
00002 
00003 #include "catchrobo_sim/accel_curve.h"
00004 #include "catchrobo_sim/safe_control.h"
00005 #include "catchrobo_sim/control_result.h"
00006 #include "motor_driver_bridge/motor_driver_struct.h"
00007 #include "catchrobo_sim/define.h"
00008 
00009 #include <catchrobo_msgs/MyRosCmd.h>
00010 #include <catchrobo_msgs/EnableCmd.h>
00011 #include <catchrobo_msgs/ErrorCode.h>
00012 #include <sensor_msgs/JointState.h>
00013 
00014 //// ブラシレスしかチェックしない
00015 
00016 // struct EnableParams
00017 // {
00018 // public:
00019 //     float position_min[4];
00020 //     float position_max[4];
00021 //     float velocity_limit[4];
00022 //     float torque_limit[4];
00023 //     float trajectory_error_limit[4];
00024 //     EnableParams(){};
00025 // }
00026 
00027 class CheckBound
00028 {
00029 public:
00030     CheckBound() : min_(0), max_(0){};
00031     void init(float min, float max)
00032     {
00033         min_ = min;
00034         max_ = max;
00035     }
00036     bool check(float current)
00037     {
00038         bool result = true;
00039         if (current < min_ || max_ < current)
00040         {
00041             result = false;
00042         }
00043         return result;
00044     };
00045 
00046 private:
00047     float min_, max_;
00048 };
00049 
00050 class EnableManager
00051 {
00052 public:
00053     EnableManager() : current_enable_(false), motor_num_(N_MOTORS)
00054     {
00055 
00056         // const float pi = 3.141592653589;
00057         // pulley_radius_ = 0.002 * 54.0 / (2.0 * pi);
00058         const float KT_OUT = 0.08;
00059         const float current_limit = 20;
00060         float epsilon = 0.1; // ジャストenableに引っかかることがあるので、ちょっと多めに取る
00061         float position_max_rad[] = {78.68526044, 25.67088441, 8.435758051};
00062         float position_min_rad[] = {0, -25.67088441, 0 - epsilon};
00063         // float velocity_limit_rad[] = {180, 180, 180};
00064         // float torque_limit_rad[] = {KT_OUT * current_limit, KT_OUT * current_limit, KT_OUT * 30};
00065         for (size_t i = 0; i < motor_num_; i++)
00066         {
00067             // float pos_min = m2rad(i, position_min_m[i]);
00068             // float pos_max = m2rad(i, position_max_m[i]);
00069             // float vel_min = m2rad(i, -velocity_limit_m[i]);
00070             // float vel_max = m2rad(i, velocity_limit_m[i]);
00071             // float torque_min = m2rad(i, -torque_limit_m[i]);
00072             // float torque_max = m2rad(i, torque_limit_m[i]);
00073 
00074             check_position[i].init(position_min_rad[i], position_max_rad[i]);
00075             // check_velocity[i].init(-velocity_limit_rad[i], velocity_limit_rad[i]);
00076             // check_torque[i].init(-torque_limit_rad[i], torque_limit_rad[i]);
00077         }
00078     };
00079     void setCmd(const catchrobo_msgs::EnableCmd &command)
00080     {
00081         cmd_ = command;
00082     };
00083     //// motor driver bridgeで指示を変更した際にはこの関数を読んで情報を同期させること
00084     void setCurrentEnable(bool is_enable)
00085     {
00086         current_enable_ = is_enable;
00087     };
00088 
00089     void check(const sensor_msgs::JointState &state, catchrobo_msgs::ErrorCode &error)
00090     {
00091         //// default
00092         error.error_code = catchrobo_msgs::ErrorCode::NONE;
00093 
00094         //// disable or enable_checkがF なら何もしない
00095         if (!current_enable_ || !cmd_.enable_check)
00096         {
00097             return;
00098         }
00099 
00100         //// enable状態なら制約チェック.一番最初に引っかかったものだけをreturn.
00101 
00102         for (size_t i = 0; i < motor_num_; i++)
00103         {
00104             error.id = i;
00105             if (!check_position[i].check(state.position[i]))
00106             {
00107                 error.error_code = catchrobo_msgs::ErrorCode::OVER_POSITION;
00108                 return;
00109             }
00110 
00111             // if (!check_velocity[i].check(state.velocity[i]))
00112             // {
00113             //     error.error_code = catchrobo_msgs::ErrorCode::OVER_VELOCITY;
00114             //     //                ROS_INFO_STREAM("id : " << i << "vel : " << state.velocity[i]);
00115             //     return;
00116             // }
00117             // if (!check_torque[i].check(state.effort[i]))
00118             // {
00119             //     error.error_code = catchrobo_msgs::ErrorCode::OVER_TORQUE;
00120             //     return;
00121             // }
00122         }
00123 
00124         // checkCollision(state, cmd_, error);
00125         //     checkTargetPosition(state, cmd, cmd_, error);
00126         //     checkOverTorque(state, cmd_, error);
00127         //     checkOverVelocity(state, cmd_, error);
00128         //     checkOverPosition(state, cmd_, error);
00129     }
00130     bool getEnable()
00131     {
00132         return current_enable_;
00133     }
00134 
00135 private:
00136     catchrobo_msgs::EnableCmd cmd_;
00137     CheckBound check_position[N_MOTORS];
00138     CheckBound check_velocity[N_MOTORS];
00139     CheckBound check_torque[N_MOTORS];
00140     bool current_enable_;
00141     // float pulley_radius_;
00142     const int motor_num_;
00143     //// robot座標系での[m] -> motor回転角度[rad]に変換. gripperは入力をそのまま返す
00144     // float m2rad(int motor_id, float position)
00145     // {
00146     //     float ret = position / pulley_radius_;
00147     //     if (motor_id == 1)
00148     //     {
00149     //         ret *= 0.5;
00150     //     }
00151     //     return ret;
00152     // }
00153 };