safe_control.h

00001 #pragma once
00002 
00003 #include "motor_driver_bridge/motor_driver_struct.h"
00004 #include <catchrobo_msgs/MyRosCmd.h>
00005 #include <algorithm>
00006 // #
00007 
00008 class SafeControl
00009 {
00010 public:
00011     SafeControl() : alpha_(0), obstacle_avoidance_limit_(0),
00012                     obstacle_avoidance_enable_(false), obstacle_avoidance_is_min_(false) {}
00013     void setCBFparams(float alpha)
00014     {
00015         alpha_ = alpha;
00016     }
00017     void getSafeCmd(const StateStruct &state, const catchrobo_msgs::MyRosCmd &target,
00018                     const ControlStruct &except_command, ControlStruct &command)
00019     {
00020         nanCheck(except_command, command);
00021 
00022         float position_min = 0;
00023         float position_max = 0;
00024         calcPositionLimit(target, position_min, position_max);
00025         ControlBarrierFunctions(position_min, position_max, state, command);
00026 
00027         positionBound(state.position, position_min, position_max, command);
00028         bound(-target.velocity_limit, target.velocity_limit, command.v_des);
00029 
00030         // boundIn(position_min, position_max, target.velocity_limit, command);
00031         // if (target.id == 2)
00032         // {
00033         //     ROS_INFO_STREAM(position_min);
00034         // }
00035     }
00036     void setObstacleInfo(bool enable, bool is_min, float limit)
00037     {
00038         obstacle_avoidance_enable_ = enable;
00039         obstacle_avoidance_is_min_ = is_min;
00040         obstacle_avoidance_limit_ = limit;
00041     }
00042 
00043 private:
00044     float alpha_;
00045     float obstacle_avoidance_limit_;
00046     bool obstacle_avoidance_enable_;
00047     bool obstacle_avoidance_is_min_;
00048     void nanCheck(const ControlStruct &except_command, ControlStruct &command)
00049     {
00050         // nanチェック
00051         if (isnan(command.p_des) || isnan(command.v_des) || isnan(command.torque_feed_forward))
00052         {
00053             //異常時
00054             command = except_command;
00055         }
00056     }
00057 
00058     void bound(double min, double max, float &target)
00059     {
00060         if (target < min)
00061         {
00062             target = min;
00063         }
00064         if (target > max)
00065         {
00066             target = max;
00067         }
00068     }
00069 
00070     void boundIn(float position_min, float position_max, float velocity_limit, ControlStruct &command)
00071     {
00072         //[min, max] にコマンドを入れる
00073         // bound(position_min, position_max, command.p_des);
00074         // positionBoind(position)
00075         bound(-velocity_limit, velocity_limit, command.v_des);
00076     }
00077 
00078     void positionBound(double position_now, float position_min, float position_max, ControlStruct &command)
00079     {
00080         if (position_now - position_min > 0) //現在値が領域内
00081         {
00082 
00083             if (command.p_des < position_min) //目標値が外なら内側に入れる
00084             {
00085                 command.p_des = position_min;
00086             }
00087         }
00088         else
00089         {
00090             if (position_now - command.p_des > 0) //現在地が領域外
00091             {
00092                 command.p_des = position_now; //外にいく方向の指示はしない
00093             }
00094         }
00095 
00096         if (position_max - position_now > 0) //現在値が領域内
00097         {
00098 
00099             if (command.p_des > position_max) //目標値が外なら内側に入れる
00100             {
00101                 command.p_des = position_max;
00102             }
00103         }
00104         else
00105         {
00106             if (position_now - command.p_des < 0) //現在地が領域外
00107             {
00108                 command.p_des = position_now; //外にいく方向の指示はしない
00109             }
00110         }
00111     }
00112 
00113     void minPositionCBF(double position_now, double position_min, double alpha, float &target_velocity)
00114     {
00115         ////CBF: b>=0 , b:=position_now-position_min
00116         ////<=> \dot{b}+alpha(b)>=0
00117         ////<=> target_velocity >= - alpha(b)
00118         double b = position_now - position_min;
00119 
00120         if (b < 0)
00121         {
00122             b = 0;
00123         }
00124         double temp = -alpha * b;
00125         if (target_velocity < temp)
00126         {
00127             target_velocity = temp;
00128         }
00129     }
00130 
00131     void maxPositionCBF(double position_now, double position_max, double alpha, float &target_velocity)
00132     {
00133         ////CBF: b>=0 , b:=position_max - position_now
00134         ////<=> \dot{b}+alpha(b)>=0
00135         ////<=>  alpha(b) >= target_velocity
00136         double b = position_max - position_now;
00137 
00138         if (b < 0)
00139         {
00140             b = 0;
00141         }
00142         double temp = alpha * b;
00143         if (target_velocity > temp)
00144         {
00145             target_velocity = temp;
00146         }
00147     }
00148 
00149     void ControlBarrierFunctions(float position_min, float position_max, const StateStruct &state, ControlStruct &command)
00150     {
00151         minPositionCBF(state.position, position_min, alpha_, command.v_des);
00152         maxPositionCBF(state.position, position_max, alpha_, command.v_des);
00153     }
00154 
00155     void calcPositionLimit(const catchrobo_msgs::MyRosCmd &target, float &position_min, float &position_max)
00156     {
00157         position_min = target.position_min;
00158         position_max = target.position_max;
00159         if (obstacle_avoidance_enable_)
00160         {
00161             if (obstacle_avoidance_is_min_)
00162             {
00163                 position_min = std::max(obstacle_avoidance_limit_, position_min);
00164             }
00165             else
00166             {
00167                 position_max = std::min(obstacle_avoidance_limit_, position_max);
00168             }
00169         }
00170     }
00171 };