catchrobo2022 / Mbed 2 deprecated catchrobo2022_mbed

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

catchrobo_sim/motor_manager.h@0:803105042c95, 2022-09-26 (annotated)

Committer:
shimizuta
Date:
Mon Sep 26 13:45:05 2022 +0000
Revision:
0:803105042c95
catchrobo2022 finish

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimizuta 0:803105042c95 1 #pragma once
shimizuta 0:803105042c95 2
shimizuta 0:803105042c95 3 #include "catchrobo_sim/position_control.h"
shimizuta 0:803105042c95 4 // #include "catchrobo_sim/position_control_with_CBF.h"
shimizuta 0:803105042c95 5 // #include "catchrobo_sim/direct_control.h"
shimizuta 0:803105042c95 6 #include "catchrobo_sim/velocity_control.h"
shimizuta 0:803105042c95 7 #include "catchrobo_sim/safe_control.h"
shimizuta 0:803105042c95 8 #include "catchrobo_sim/go_origin_control.h"
shimizuta 0:803105042c95 9 #include "motor_driver_bridge/motor_driver_struct.h"
shimizuta 0:803105042c95 10
shimizuta 0:803105042c95 11 #include <catchrobo_msgs/MyRosCmd.h>
shimizuta 0:803105042c95 12
shimizuta 0:803105042c95 13 class MotorManager
shimizuta 0:803105042c95 14 {
shimizuta 0:803105042c95 15 public:
shimizuta 0:803105042c95 16 MotorManager() : offset_(0), t_(0), dt_(0), error_limit_(0.5)
shimizuta 0:803105042c95 17 {
shimizuta 0:803105042c95 18 old_command_.p_des = 0;
shimizuta 0:803105042c95 19 old_command_.v_des = 0;
shimizuta 0:803105042c95 20 old_command_.torque_feed_forward = 0;
shimizuta 0:803105042c95 21 old_command_.kp = 0;
shimizuta 0:803105042c95 22 old_command_.kd = 0;
shimizuta 0:803105042c95 23
shimizuta 0:803105042c95 24 current_state_.position = 0;
shimizuta 0:803105042c95 25 current_state_.velocity = 0;
shimizuta 0:803105042c95 26 current_state_.torque = 0;
shimizuta 0:803105042c95 27
shimizuta 0:803105042c95 28 old_state_.position = 0;
shimizuta 0:803105042c95 29 old_state_.velocity = 0;
shimizuta 0:803105042c95 30 old_state_.torque = 0;
shimizuta 0:803105042c95 31
shimizuta 0:803105042c95 32 double cbf_params = 1;
shimizuta 0:803105042c95 33 safe_control_.setCBFparams(cbf_params);
shimizuta 0:803105042c95 34
shimizuta 0:803105042c95 35 estimate_state_.position = 0;
shimizuta 0:803105042c95 36 estimate_state_.velocity = 0;
shimizuta 0:803105042c95 37 estimate_state_.torque = 0;
shimizuta 0:803105042c95 38 };
shimizuta 0:803105042c95 39
shimizuta 0:803105042c95 40 //低Hz (1 Hzとか)で呼ばれる
shimizuta 0:803105042c95 41 void setRosCmd(const catchrobo_msgs::MyRosCmd &cmd)
shimizuta 0:803105042c95 42 {
shimizuta 0:803105042c95 43 ros_cmd_ = cmd;
shimizuta 0:803105042c95 44 switch (ros_cmd_.mode)
shimizuta 0:803105042c95 45 {
shimizuta 0:803105042c95 46 case catchrobo_msgs::MyRosCmd::POSITION_CTRL_MODE:
shimizuta 0:803105042c95 47 position_control_.setRosCmd(ros_cmd_, current_state_);
shimizuta 0:803105042c95 48 break;
shimizuta 0:803105042c95 49 case catchrobo_msgs::MyRosCmd::VELOCITY_CTRL_MODE:
shimizuta 0:803105042c95 50 // direct_control_.setRosCmd(ros_cmd_, current_state_);
shimizuta 0:803105042c95 51 velocity_control_.setRosCmd(ros_cmd_, current_state_);
shimizuta 0:803105042c95 52 break;
shimizuta 0:803105042c95 53 // case catchrobo_msgs::MyRosCmd::DIRECT_CTRL_MODE:
shimizuta 0:803105042c95 54 // // direct_control_.setRosCmd(ros_cmd_, current_state_);
shimizuta 0:803105042c95 55 // // direct_control_.setRosCmd(ros_cmd_, current_state_);
shimizuta 0:803105042c95 56 // break;
shimizuta 0:803105042c95 57 case catchrobo_msgs::MyRosCmd::GO_ORIGIN_MODE:
shimizuta 0:803105042c95 58 go_origin_control_.setOffset(no_offset_state_, ros_cmd_, offset_);
shimizuta 0:803105042c95 59
shimizuta 0:803105042c95 60 {
shimizuta 0:803105042c95 61 //// [WARN] 面倒なので再帰関数
shimizuta 0:803105042c95 62 catchrobo_msgs::MyRosCmd command;
shimizuta 0:803105042c95 63 command.mode = catchrobo_msgs::MyRosCmd::DIRECT_CTRL_MODE;
shimizuta 0:803105042c95 64 command.kp = 0;
shimizuta 0:803105042c95 65 command.kd = 0;
shimizuta 0:803105042c95 66 command.effort = 0;
shimizuta 0:803105042c95 67 setRosCmd(command);
shimizuta 0:803105042c95 68 }
shimizuta 0:803105042c95 69 break;
shimizuta 0:803105042c95 70
shimizuta 0:803105042c95 71 default:
shimizuta 0:803105042c95 72 // ROS_ERROR("error : No mode in MyRosCmd");
shimizuta 0:803105042c95 73 break;
shimizuta 0:803105042c95 74 }
shimizuta 0:803105042c95 75 };
shimizuta 0:803105042c95 76
shimizuta 0:803105042c95 77 // dt間隔で呼ばれる. servo classではoverrideされる。
shimizuta 0:803105042c95 78 void getCmd(ControlStruct &offset_command, ControlResult::ControlResult &result)
shimizuta 0:803105042c95 79 {
shimizuta 0:803105042c95 80 ControlStruct command;
shimizuta 0:803105042c95 81 // ros::Time t_start = ros::Time::now();
shimizuta 0:803105042c95 82
shimizuta 0:803105042c95 83 switch (ros_cmd_.mode)
shimizuta 0:803105042c95 84 {
shimizuta 0:803105042c95 85 case catchrobo_msgs::MyRosCmd::POSITION_CTRL_MODE:
shimizuta 0:803105042c95 86
shimizuta 0:803105042c95 87 // position_control_.getCmd(t_, estimate_state_, old_command_, command, result);
shimizuta 0:803105042c95 88 position_control_.getCmd(t_, current_state_, old_command_, command, result);
shimizuta 0:803105042c95 89 safe_control_.getSafeCmd(current_state_, ros_cmd_, old_command_, command);
shimizuta 0:803105042c95 90 break;
shimizuta 0:803105042c95 91 // case catchrobo_msgs::MyRosCmd::DIRECT_CTRL_MODE:
shimizuta 0:803105042c95 92 // direct_control_.getCmd(current_state_, old_command_, command, result);
shimizuta 0:803105042c95 93 // safe_control_.getSafeCmd(current_state_, ros_cmd_, old_command_, command);
shimizuta 0:803105042c95 94 // break;
shimizuta 0:803105042c95 95
shimizuta 0:803105042c95 96 case catchrobo_msgs::MyRosCmd::VELOCITY_CTRL_MODE:
shimizuta 0:803105042c95 97 velocity_control_.getCmd(estimate_state_, old_command_, command, result);
shimizuta 0:803105042c95 98 safe_control_.getSafeCmd(current_state_, ros_cmd_, old_command_, command);
shimizuta 0:803105042c95 99 break;
shimizuta 0:803105042c95 100
shimizuta 0:803105042c95 101 // case catchrobo_msgs::MyRosCmd::DIRECT_CTRL_MODE:
shimizuta 0:803105042c95 102 // direct_control_.getCmd(current_state_, old_command_, command, result);
shimizuta 0:803105042c95 103 // safe_control_.getSafeCmd(current_state_, ros_cmd_, old_command_, command);
shimizuta 0:803105042c95 104 // break;
shimizuta 0:803105042c95 105
shimizuta 0:803105042c95 106 // case catchrobo_msgs::MyRosCmd::GO_ORIGIN_MODE:
shimizuta 0:803105042c95 107 // ////原点だしではoffset無しの値がほしい
shimizuta 0:803105042c95 108 // {
shimizuta 0:803105042c95 109 // StateStruct no_offset_state = current_state_;
shimizuta 0:803105042c95 110 // no_offset_state.position += offset_;
shimizuta 0:803105042c95 111 // go_origin_control_.getCmd(no_offset_state, old_command_, command, result, offset_);
shimizuta 0:803105042c95 112 // }
shimizuta 0:803105042c95 113 // break;
shimizuta 0:803105042c95 114
shimizuta 0:803105042c95 115 default:
shimizuta 0:803105042c95 116 command = old_command_;
shimizuta 0:803105042c95 117 // ROS_ERROR("error : No mode in MyRosCmd");
shimizuta 0:803105042c95 118 break;
shimizuta 0:803105042c95 119 }
shimizuta 0:803105042c95 120 // ros::Time now = ros::Time::now();
shimizuta 0:803105042c95 121 // ros::Duration ros_dt = now - t_start;
shimizuta 0:803105042c95 122 // double dt = ros_dt.toSec();
shimizuta 0:803105042c95 123 // if (command.id == 0)
shimizuta 0:803105042c95 124 // ROS_INFO_STREAM(dt);
shimizuta 0:803105042c95 125 setEstimateState(current_state_, command, estimate_state_);
shimizuta 0:803105042c95 126 //// 基本はros座標系で行う
shimizuta 0:803105042c95 127 old_command_ = command;
shimizuta 0:803105042c95 128 // 最後の最後にモタドラ座標系に変換
shimizuta 0:803105042c95 129 offset_command = command;
shimizuta 0:803105042c95 130 offset_command.p_des += offset_;
shimizuta 0:803105042c95 131 };
shimizuta 0:803105042c95 132
shimizuta 0:803105042c95 133 //高Hz (500Hz)で呼ばれる
shimizuta 0:803105042c95 134 void setCurrentState(const StateStruct &state)
shimizuta 0:803105042c95 135 {
shimizuta 0:803105042c95 136 no_offset_state_ = state;
shimizuta 0:803105042c95 137 current_state_ = no_offset_state_;
shimizuta 0:803105042c95 138 current_state_.position -= offset_;
shimizuta 0:803105042c95 139 current_state_.velocity = (no_offset_state_.position - old_state_.position) / dt_;
shimizuta 0:803105042c95 140
shimizuta 0:803105042c95 141 // if (current_state_.velocity > 200)
shimizuta 0:803105042c95 142 // ROS_INFO_STREAM("dt: " << dt_ << " p: " << no_offset_state_.position << " old_p: " << old_state_.position << " v: " << current_state_.velocity);
shimizuta 0:803105042c95 143 old_state_ = no_offset_state_;
shimizuta 0:803105042c95 144 }
shimizuta 0:803105042c95 145
shimizuta 0:803105042c95 146 //低Hz (50Hz)で呼ばれる
shimizuta 0:803105042c95 147 void getState(StateStruct &state)
shimizuta 0:803105042c95 148 {
shimizuta 0:803105042c95 149 state = current_state_;
shimizuta 0:803105042c95 150 }
shimizuta 0:803105042c95 151
shimizuta 0:803105042c95 152 void getRosCmd(catchrobo_msgs::MyRosCmd &ros_cmd)
shimizuta 0:803105042c95 153 {
shimizuta 0:803105042c95 154 ros_cmd = ros_cmd_;
shimizuta 0:803105042c95 155 }
shimizuta 0:803105042c95 156 void nextStep(float dt)
shimizuta 0:803105042c95 157 {
shimizuta 0:803105042c95 158 dt_ = dt;
shimizuta 0:803105042c95 159 t_ += dt;
shimizuta 0:803105042c95 160 velocity_control_.setDt(dt);
shimizuta 0:803105042c95 161 position_control_.setDt(dt);
shimizuta 0:803105042c95 162 };
shimizuta 0:803105042c95 163 bool IsPegInHoleMode()
shimizuta 0:803105042c95 164 {
shimizuta 0:803105042c95 165 return (ros_cmd_.mode == catchrobo_msgs::MyRosCmd::PEG_IN_HOLE_MODE);
shimizuta 0:803105042c95 166 }
shimizuta 0:803105042c95 167
shimizuta 0:803105042c95 168 void setObstacleInfo(bool enable, bool is_min, float limit)
shimizuta 0:803105042c95 169 {
shimizuta 0:803105042c95 170 safe_control_.setObstacleInfo(enable, is_min, limit);
shimizuta 0:803105042c95 171 }
shimizuta 0:803105042c95 172
shimizuta 0:803105042c95 173 void changeTarget(bool temp_mode, float target_position)
shimizuta 0:803105042c95 174 {
shimizuta 0:803105042c95 175 position_control_.changeTarget(temp_mode, target_position, current_state_);
shimizuta 0:803105042c95 176 }
shimizuta 0:803105042c95 177
shimizuta 0:803105042c95 178 // void changePositionMax(float max_rad)
shimizuta 0:803105042c95 179 // {
shimizuta 0:803105042c95 180 // ros_cmd_.position_max = max_rad;
shimizuta 0:803105042c95 181 // }
shimizuta 0:803105042c95 182 void resetT()
shimizuta 0:803105042c95 183 {
shimizuta 0:803105042c95 184 t_ = 0;
shimizuta 0:803105042c95 185 };
shimizuta 0:803105042c95 186
shimizuta 0:803105042c95 187 void init(float arrive_threshold, float estimate_error_limit, float friction)
shimizuta 0:803105042c95 188 {
shimizuta 0:803105042c95 189 position_control_.init(arrive_threshold, friction);
shimizuta 0:803105042c95 190 error_limit_ = estimate_error_limit;
shimizuta 0:803105042c95 191 }
shimizuta 0:803105042c95 192
shimizuta 0:803105042c95 193 bool isArrived()
shimizuta 0:803105042c95 194 {
shimizuta 0:803105042c95 195 bool ret = false;
shimizuta 0:803105042c95 196 if (ros_cmd_.mode == catchrobo_msgs::MyRosCmd::POSITION_CTRL_MODE)
shimizuta 0:803105042c95 197 {
shimizuta 0:803105042c95 198 ret = position_control_.isArrived(current_state_);
shimizuta 0:803105042c95 199 }
shimizuta 0:803105042c95 200 return ret;
shimizuta 0:803105042c95 201 }
shimizuta 0:803105042c95 202
shimizuta 0:803105042c95 203 protected:
shimizuta 0:803105042c95 204 float t_;
shimizuta 0:803105042c95 205
shimizuta 0:803105042c95 206 private:
shimizuta 0:803105042c95 207 StateStruct current_state_;
shimizuta 0:803105042c95 208 StateStruct no_offset_state_;
shimizuta 0:803105042c95 209 StateStruct old_state_;
shimizuta 0:803105042c95 210 StateStruct estimate_state_;
shimizuta 0:803105042c95 211
shimizuta 0:803105042c95 212 ControlStruct old_command_;
shimizuta 0:803105042c95 213
shimizuta 0:803105042c95 214 catchrobo_msgs::MyRosCmd ros_cmd_;
shimizuta 0:803105042c95 215
shimizuta 0:803105042c95 216 //// motorの値 - offset_ = ros内での値
shimizuta 0:803105042c95 217 float offset_;
shimizuta 0:803105042c95 218 float dt_;
shimizuta 0:803105042c95 219 float target_velocity_;
shimizuta 0:803105042c95 220 float error_limit_; //追従誤差の上限
shimizuta 0:803105042c95 221
shimizuta 0:803105042c95 222 PositionControl position_control_;
shimizuta 0:803105042c95 223 // PositionControlWithCBF position_control_;
shimizuta 0:803105042c95 224 // DirectControl direct_control_;
shimizuta 0:803105042c95 225 VelocityControl velocity_control_;
shimizuta 0:803105042c95 226 SafeControl safe_control_;
shimizuta 0:803105042c95 227 GoOriginControl go_origin_control_;
shimizuta 0:803105042c95 228
shimizuta 0:803105042c95 229 void setEstimateState(const StateStruct observed_state, const ControlStruct &command, StateStruct &estimate_state)
shimizuta 0:803105042c95 230 {
shimizuta 0:803105042c95 231 if (command.kd > 0)
shimizuta 0:803105042c95 232 {
shimizuta 0:803105042c95 233 estimate_state.velocity = command.v_des;
shimizuta 0:803105042c95 234 }
shimizuta 0:803105042c95 235 else
shimizuta 0:803105042c95 236 {
shimizuta 0:803105042c95 237 estimate_state.velocity = observed_state.velocity;
shimizuta 0:803105042c95 238 }
shimizuta 0:803105042c95 239
shimizuta 0:803105042c95 240 float observed_position = observed_state.position;
shimizuta 0:803105042c95 241 float command_position = command.p_des;
shimizuta 0:803105042c95 242
shimizuta 0:803105042c95 243 if (command.kp > 0 && fabs(observed_position - command_position) < error_limit_)
shimizuta 0:803105042c95 244 {
shimizuta 0:803105042c95 245 estimate_state.position = command_position;
shimizuta 0:803105042c95 246 }
shimizuta 0:803105042c95 247 else
shimizuta 0:803105042c95 248 {
shimizuta 0:803105042c95 249 estimate_state.position = observed_position;
shimizuta 0:803105042c95 250 }
shimizuta 0:803105042c95 251 }
shimizuta 0:803105042c95 252 };