position_control.h

00001 #pragma once
00002 
00003 #include "catchrobo_sim/accel_designer.h"
00004 #include "catchrobo_sim/safe_control.h"
00005 #include "catchrobo_sim/control_result.h"
00006 
00007 #include "motor_driver_bridge/motor_driver_struct.h"
00008 #include <catchrobo_msgs/MyRosCmd.h>
00009 
00010 #include <limits>
00011 
00012 class PositionControl
00013 {
00014 public:
00015     PositionControl() : no_target_flag_(true), finish_already_notified_(false),
00016                         temp_target_flag_(false), final_target_position_(0), threshold_(0){};
00017 
00018     void init(float threshold, float friction)
00019     {
00020         threshold_ = threshold;
00021         friction_ = 0;
00022     }
00023     void setRosCmd(const catchrobo_msgs::MyRosCmd &cmd, const StateStruct &joint_state)
00024     {
00025         target_ = cmd;
00026         final_target_position_ = cmd.position;
00027         setAccelDesigner(cmd, joint_state);
00028         no_target_flag_ = false;
00029         finish_already_notified_ = false;
00030         temp_target_flag_ = false;
00031     };
00032 
00033     // dt間隔で呼ばれる想定. except_command : 例外時に返す値。
00034     void getCmd(float t, const StateStruct &state, const ControlStruct &except_command, ControlStruct &command, ControlResult::ControlResult &finished)
00035     {
00036         finished = ControlResult::RUNNING;
00037         if (no_target_flag_)
00038         {
00039             // まだ目標値が与えられていないとき
00040             command = except_command;
00041             return;
00042         }
00043 
00044         if (finish_already_notified_)
00045         {
00046             //// 収束後
00047             packAfterFinish(target_, command);
00048             return;
00049         }
00050 
00051         if (fabs(target_.position - state.position) > threshold_)
00052         {
00053             //収束していないとき
00054             packResult2Cmd(t, accel_designer_, target_, command);
00055         }
00056         else
00057         {
00058             //収束後
00059             // packAfterFinish(target_, command);
00060             command = except_command;
00061             if (temp_target_flag_)
00062             {
00063                 //// 一時目標変換していたとき
00064                 return;
00065             }
00066             finished = ControlResult::FINISH;
00067             finish_already_notified_ = true;
00068         }
00069     };
00070 
00071     void changeTarget(bool temp_mode, float target_position, const StateStruct &joint_state)
00072     {
00073         if (temp_mode && !temp_target_flag_)
00074         {
00075             //// temp modeがONに切り替わったとき
00076             target_.position = target_position;
00077             setAccelDesigner(target_, joint_state);
00078         }
00079         else if (!temp_mode && temp_target_flag_)
00080         {
00081             //// temp modeがOFFに切り替わったとき
00082             target_.position = final_target_position_;
00083             setAccelDesigner(target_, joint_state);
00084         }
00085         temp_target_flag_ = temp_mode;
00086     }
00087     void setDt(float dt) {}
00088 
00089     bool isArrived(const StateStruct &state)
00090     {
00091         bool ret = fabs(target_.position - state.position) < threshold_;
00092         return ret;
00093     }
00094 
00095 private:
00096     bool no_target_flag_;
00097     bool finish_already_notified_;
00098     bool temp_target_flag_;
00099 
00100     ctrl::AccelDesigner accel_designer_;
00101     catchrobo_msgs::MyRosCmd target_;
00102     float final_target_position_;
00103     float threshold_; //収束判定しきい値
00104     float friction_;
00105 
00106     void setAccelDesigner(const catchrobo_msgs::MyRosCmd &cmd, const StateStruct &joint_state)
00107     {
00108         float start_posi = joint_state.position;
00109         float target_position = cmd.position;
00110         // if (target_position > cmd.position_max)
00111         // {
00112         //     target_position = cmd.position_max;
00113         // }
00114         // else if (target_position < cmd.position_min)
00115         // {
00116         //     target_position = cmd.position_min;
00117         // }
00118 
00119         float dist = target_position - start_posi; //移動距離
00120 
00121         accel_designer_.reset(cmd.jerk_limit, cmd.acceleration_limit, cmd.velocity_limit,
00122                               0, cmd.velocity, dist,
00123                               start_posi, 0);
00124     }
00125 
00126     void packResult2Cmd(double t, const ctrl::AccelDesigner &accel_designer, const catchrobo_msgs::MyRosCmd &target, ControlStruct &cmd)
00127     {
00128         cmd.id = target.id;
00129         cmd.p_des = accel_designer.x(t);
00130         cmd.v_des = accel_designer.v(t);
00131         float friction = sign(cmd.v_des) * friction_;
00132 
00133         cmd.torque_feed_forward = target.net_inertia * accel_designer.a(t) + target.effort + friction;
00134         cmd.kp = target.kp;
00135         cmd.kd = target.kd;
00136     }
00137 
00138     void packAfterFinish(const catchrobo_msgs::MyRosCmd &target, ControlStruct &cmd)
00139     {
00140         cmd.id = target.id;
00141         cmd.p_des = target.position;
00142         cmd.v_des = 0; // target.velocity;
00143         cmd.torque_feed_forward = target.net_inertia * 0 + target.effort;
00144         cmd.kp = target.kp;
00145         cmd.kd = target.kd;
00146     }
00147     int sign(float val)
00148     {
00149         return (val > 0) - (val < 0);
00150     }
00151 
00152     // void packBeforeSetTargetCmd(int id, ControlStruct &cmd){
00153     //     cmd.id = id;
00154     //     cmd.p_des = 0;
00155     //     cmd.v_des = 0;
00156     //     cmd.i_ff = 0;
00157     //     cmd.kp = 0;
00158     //     cmd.kd = 0;
00159     // }
00160 };