motor_manager.h

00001 #pragma once
00002 
00003 #include "catchrobo_sim/position_control.h"
00004 // #include "catchrobo_sim/position_control_with_CBF.h"
00005 // #include "catchrobo_sim/direct_control.h"
00006 #include "catchrobo_sim/velocity_control.h"
00007 #include "catchrobo_sim/safe_control.h"
00008 #include "catchrobo_sim/go_origin_control.h"
00009 #include "motor_driver_bridge/motor_driver_struct.h"
00010 
00011 #include <catchrobo_msgs/MyRosCmd.h>
00012 
00013 class MotorManager
00014 {
00015 public:
00016     MotorManager() : offset_(0), t_(0), dt_(0), error_limit_(0.5)
00017     {
00018         old_command_.p_des = 0;
00019         old_command_.v_des = 0;
00020         old_command_.torque_feed_forward = 0;
00021         old_command_.kp = 0;
00022         old_command_.kd = 0;
00023 
00024         current_state_.position = 0;
00025         current_state_.velocity = 0;
00026         current_state_.torque = 0;
00027 
00028         old_state_.position = 0;
00029         old_state_.velocity = 0;
00030         old_state_.torque = 0;
00031 
00032         double cbf_params = 1;
00033         safe_control_.setCBFparams(cbf_params);
00034 
00035         estimate_state_.position = 0;
00036         estimate_state_.velocity = 0;
00037         estimate_state_.torque = 0;
00038     };
00039 
00040     //低Hz (1 Hzとか)で呼ばれる
00041     void setRosCmd(const catchrobo_msgs::MyRosCmd &cmd)
00042     {
00043         ros_cmd_ = cmd;
00044         switch (ros_cmd_.mode)
00045         {
00046         case catchrobo_msgs::MyRosCmd::POSITION_CTRL_MODE:
00047             position_control_.setRosCmd(ros_cmd_, current_state_);
00048             break;
00049         case catchrobo_msgs::MyRosCmd::VELOCITY_CTRL_MODE:
00050             // direct_control_.setRosCmd(ros_cmd_, current_state_);
00051             velocity_control_.setRosCmd(ros_cmd_, current_state_);
00052             break;
00053         // case catchrobo_msgs::MyRosCmd::DIRECT_CTRL_MODE:
00054         //     // direct_control_.setRosCmd(ros_cmd_, current_state_);
00055         //     // direct_control_.setRosCmd(ros_cmd_, current_state_);
00056         //     break;
00057         case catchrobo_msgs::MyRosCmd::GO_ORIGIN_MODE:
00058             go_origin_control_.setOffset(no_offset_state_, ros_cmd_, offset_);
00059 
00060             {
00061                 //// [WARN] 面倒なので再帰関数
00062                 catchrobo_msgs::MyRosCmd command;
00063                 command.mode = catchrobo_msgs::MyRosCmd::DIRECT_CTRL_MODE;
00064                 command.kp = 0;
00065                 command.kd = 0;
00066                 command.effort = 0;
00067                 setRosCmd(command);
00068             }
00069             break;
00070 
00071         default:
00072             //            ROS_ERROR("error : No mode in MyRosCmd");
00073             break;
00074         }
00075     };
00076 
00077     // dt間隔で呼ばれる. servo classではoverrideされる。
00078     void getCmd(ControlStruct &offset_command, ControlResult::ControlResult &result)
00079     {
00080         ControlStruct command;
00081         // ros::Time t_start = ros::Time::now();
00082 
00083         switch (ros_cmd_.mode)
00084         {
00085         case catchrobo_msgs::MyRosCmd::POSITION_CTRL_MODE:
00086 
00087             // position_control_.getCmd(t_, estimate_state_, old_command_, command, result);
00088             position_control_.getCmd(t_, current_state_, old_command_, command, result);
00089             safe_control_.getSafeCmd(current_state_, ros_cmd_, old_command_, command);
00090             break;
00091             // case catchrobo_msgs::MyRosCmd::DIRECT_CTRL_MODE:
00092             //     direct_control_.getCmd(current_state_, old_command_, command, result);
00093             //     safe_control_.getSafeCmd(current_state_, ros_cmd_, old_command_, command);
00094             //     break;
00095 
00096         case catchrobo_msgs::MyRosCmd::VELOCITY_CTRL_MODE:
00097             velocity_control_.getCmd(estimate_state_, old_command_, command, result);
00098             safe_control_.getSafeCmd(current_state_, ros_cmd_, old_command_, command);
00099             break;
00100 
00101             // case catchrobo_msgs::MyRosCmd::DIRECT_CTRL_MODE:
00102             //     direct_control_.getCmd(current_state_, old_command_, command, result);
00103             //     safe_control_.getSafeCmd(current_state_, ros_cmd_, old_command_, command);
00104             //     break;
00105 
00106             // case catchrobo_msgs::MyRosCmd::GO_ORIGIN_MODE:
00107             //     ////原点だしではoffset無しの値がほしい
00108             //     {
00109             //         StateStruct no_offset_state = current_state_;
00110             //         no_offset_state.position += offset_;
00111             //         go_origin_control_.getCmd(no_offset_state, old_command_, command, result, offset_);
00112             //     }
00113             //     break;
00114 
00115         default:
00116             command = old_command_;
00117             //            ROS_ERROR("error : No mode in MyRosCmd");
00118             break;
00119         }
00120         // ros::Time now = ros::Time::now();
00121         // ros::Duration ros_dt = now - t_start;
00122         // double dt = ros_dt.toSec();
00123         // if (command.id == 0)
00124         //     ROS_INFO_STREAM(dt);
00125         setEstimateState(current_state_, command, estimate_state_);
00126         //// 基本はros座標系で行う
00127         old_command_ = command;
00128         // 最後の最後にモタドラ座標系に変換
00129         offset_command = command;
00130         offset_command.p_des += offset_;
00131     };
00132 
00133     //高Hz (500Hz)で呼ばれる
00134     void setCurrentState(const StateStruct &state)
00135     {
00136         no_offset_state_ = state;
00137         current_state_ = no_offset_state_;
00138         current_state_.position -= offset_;
00139         current_state_.velocity = (no_offset_state_.position - old_state_.position) / dt_;
00140 
00141         // if (current_state_.velocity > 200)
00142         // ROS_INFO_STREAM("dt: " << dt_ << " p: " << no_offset_state_.position << " old_p: " << old_state_.position << " v: " << current_state_.velocity);
00143         old_state_ = no_offset_state_;
00144     }
00145 
00146     //低Hz (50Hz)で呼ばれる
00147     void getState(StateStruct &state)
00148     {
00149         state = current_state_;
00150     }
00151 
00152     void getRosCmd(catchrobo_msgs::MyRosCmd &ros_cmd)
00153     {
00154         ros_cmd = ros_cmd_;
00155     }
00156     void nextStep(float dt)
00157     {
00158         dt_ = dt;
00159         t_ += dt;
00160         velocity_control_.setDt(dt);
00161         position_control_.setDt(dt);
00162     };
00163     bool IsPegInHoleMode()
00164     {
00165         return (ros_cmd_.mode == catchrobo_msgs::MyRosCmd::PEG_IN_HOLE_MODE);
00166     }
00167 
00168     void setObstacleInfo(bool enable, bool is_min, float limit)
00169     {
00170         safe_control_.setObstacleInfo(enable, is_min, limit);
00171     }
00172 
00173     void changeTarget(bool temp_mode, float target_position)
00174     {
00175         position_control_.changeTarget(temp_mode, target_position, current_state_);
00176     }
00177 
00178     // void changePositionMax(float max_rad)
00179     // {
00180     //     ros_cmd_.position_max = max_rad;
00181     // }
00182     void resetT()
00183     {
00184         t_ = 0;
00185     };
00186 
00187     void init(float arrive_threshold, float estimate_error_limit, float friction)
00188     {
00189         position_control_.init(arrive_threshold, friction);
00190         error_limit_ = estimate_error_limit;
00191     }
00192 
00193     bool isArrived()
00194     {
00195         bool ret = false;
00196         if (ros_cmd_.mode == catchrobo_msgs::MyRosCmd::POSITION_CTRL_MODE)
00197         {
00198             ret = position_control_.isArrived(current_state_);
00199         }
00200         return ret;
00201     }
00202 
00203 protected:
00204     float t_;
00205 
00206 private:
00207     StateStruct current_state_;
00208     StateStruct no_offset_state_;
00209     StateStruct old_state_;
00210     StateStruct estimate_state_;
00211 
00212     ControlStruct old_command_;
00213 
00214     catchrobo_msgs::MyRosCmd ros_cmd_;
00215 
00216     //// motorの値 - offset_ = ros内での値
00217     float offset_;
00218     float dt_;
00219     float target_velocity_;
00220     float error_limit_; //追従誤差の上限
00221 
00222     PositionControl position_control_;
00223     // PositionControlWithCBF position_control_;
00224     // DirectControl direct_control_;
00225     VelocityControl velocity_control_;
00226     SafeControl safe_control_;
00227     GoOriginControl go_origin_control_;
00228 
00229     void setEstimateState(const StateStruct observed_state, const ControlStruct &command, StateStruct &estimate_state)
00230     {
00231         if (command.kd > 0)
00232         {
00233             estimate_state.velocity = command.v_des;
00234         }
00235         else
00236         {
00237             estimate_state.velocity = observed_state.velocity;
00238         }
00239 
00240         float observed_position = observed_state.position;
00241         float command_position = command.p_des;
00242 
00243         if (command.kp > 0 && fabs(observed_position - command_position) < error_limit_)
00244         {
00245             estimate_state.position = command_position;
00246         }
00247         else
00248         {
00249             estimate_state.position = observed_position;
00250         }
00251     }
00252 };