robot_manager.h

00001 #pragma once
00002 
00003 #include "catchrobo_sim/define.h"
00004 #include "catchrobo_sim/motor_manager.h"
00005 #include "catchrobo_sim/servo_manager.h"
00006 #include "catchrobo_sim/enable_manager.h"
00007 // #include "catchrobo_sim/obstacle_avoidance.h"
00008 #include "motor_driver_bridge/motor_driver_struct.h"
00009 
00010 #include <std_msgs/Float32MultiArray.h>
00011 #include <sensor_msgs/JointState.h>
00012 #include <catchrobo_msgs/MyRosCmd.h>
00013 #include <catchrobo_msgs/MyRosCmdArray.h>
00014 #include <catchrobo_msgs/EnableCmd.h>
00015 #include <catchrobo_msgs/ErrorCode.h>
00016 
00017 #include <vector>
00018 #include <string>
00019 
00020 class RobotManager
00021 {
00022 public:
00023     RobotManager() : actuator_num_(JOINT_NUM), motor_num_(N_MOTORS)
00024     {
00025         ////[TODO] 面倒なので直打ち
00026         char *joint_name[JOINT_NUM] = {"arm/joint1", "arm/joint2", "arm/joint3", "gripper/joint1"};
00027         resetJointState(actuator_num_, joint_name);
00028         resetJointRad(actuator_num_);
00029     }
00030 
00031 #ifdef USE_MBED
00032 
00033     ~RobotManager()
00034     {
00035         if (joint_state_.position != NULL)
00036             delete[] joint_state_.position;
00037         if (joint_state_.velocity != NULL)
00038             delete[] joint_state_.position;
00039         if (joint_state_.effort != NULL)
00040             delete[] joint_state_.position;
00041         if (joint_rad_.data != NULL)
00042             delete[] joint_rad_.data;
00043     }
00044     void resetJointState(int joint_num, char *joint_name[])
00045     {
00046 
00047         joint_state_.name_length = joint_num;
00048         joint_state_.position_length = joint_num;
00049         joint_state_.velocity_length = joint_num;
00050         joint_state_.effort_length = joint_num;
00051 
00052         joint_state_.name = joint_name;
00053         joint_state_.position = new double[joint_num]; //(double *)malloc(sizeof(double)*joint_num)
00054         joint_state_.velocity = new double[joint_num];
00055         joint_state_.effort = new double[joint_num];
00056 
00057         for (int i = 0; i < joint_num; i++)
00058         {
00059             joint_state_.position[i] = 0;
00060             joint_state_.velocity[i] = 0;
00061             joint_state_.effort[i] = 0;
00062         }
00063     }
00064 
00065     void resetJointRad(int joint_num)
00066     {
00067         int num = joint_num;
00068         num = num * 2;
00069         joint_rad_.data_length = num;
00070         joint_rad_.data = new float[num];
00071 
00072         for (int i = 0; i < num; i++)
00073         {
00074             joint_rad_.data[i] = 0;
00075         }
00076     }
00077 #endif
00078 
00079 #ifndef USE_MBED
00080     void resetJointState(int joint_num, char *joint_name[])
00081     {
00082         std::vector<std::string> name(joint_num);
00083         for (size_t i = 0; i < joint_num; i++)
00084         {
00085             name[i] = joint_name[i];
00086         }
00087         joint_state_.name = name;
00088         joint_state_.position = std::vector<double>(joint_num, 0);
00089         joint_state_.velocity = std::vector<double>(joint_num, 0);
00090         joint_state_.effort = std::vector<double>(joint_num, 0);
00091     }
00092     void resetJointRad(int joint_num)
00093     {
00094         int num = joint_num;
00095         num = num * 2;
00096         joint_rad_.data.resize(num);
00097     }
00098 #endif
00099     ////本当はMyRosCmdArrayを受け取るのがキレイだが、配列要素数を取得する計算がPCとmbedで変わってしまうため、MyRosCmdで受け取るようにしている。
00100     void setRosCmd(const catchrobo_msgs::MyRosCmd &command)
00101     {
00102         if (command.id < motor_num_)
00103         {
00104             motor_manager_[command.id].setRosCmd(command);
00105             motor_manager_[command.id].resetT();
00106         }
00107     };
00108 
00109     void getMotorDrivesCommand(ControlStruct (&cmd)[N_MOTORS], ControlResult::ControlResult (&result)[N_MOTORS])
00110     {
00111         for (size_t i = 0; i < motor_num_; i++)
00112         {
00113             motor_manager_[i].getCmd(cmd[i], result[i]);
00114         }
00115         ///// cmdのidを上書き。初期値である0になっている場合があるため
00116         for (size_t i = 0; i < motor_num_; i++)
00117         {
00118             cmd[i].id = i;
00119         }
00120     };
00121 
00122     void nextStep(float dt)
00123     {
00124         for (size_t i = 0; i < motor_num_; i++)
00125         {
00126             motor_manager_[i].nextStep(dt);
00127         }
00128     }
00129 
00130     void setCurrentState(const StateStruct &state)
00131     {
00132         if (state.id < motor_num_)
00133         {
00134             motor_manager_[state.id].setCurrentState(state);
00135         }
00136     };
00137     void getJointState(sensor_msgs::JointState &joint_state)
00138     {
00139 
00140         for (size_t i = 0; i < motor_num_; i++)
00141         {
00142             StateStruct state;
00143             motor_manager_[i].getState(state);
00144             setJointState(i, state, joint_state_);
00145         }
00146         joint_state = joint_state_;
00147     };
00148 
00149     void getJointRad(std_msgs::Float32MultiArray &joint_rad)
00150     {
00151         sensor_msgs::JointState joint_state;
00152         getJointState(joint_state);
00153         for (size_t i = 0; i < motor_num_; i++)
00154         {
00155             joint_rad_.data[i] = joint_state.position[i];
00156             joint_rad_.data[i + actuator_num_] = joint_state.effort[i];
00157         }
00158         joint_rad = joint_rad_;
00159     };
00160 
00161     void disable()
00162     {
00163         catchrobo_msgs::MyRosCmd command;
00164         command.mode = catchrobo_msgs::MyRosCmd::DIRECT_CTRL_MODE;
00165 
00166         command.kp = 0;
00167         command.kd = 0;
00168         command.effort = 0;
00169         for (size_t i = 0; i < motor_num_; i++)
00170         {
00171             /* code */
00172             StateStruct state;
00173             motor_manager_[i].getState(state);
00174             command.id = i;
00175             command.position = state.position;
00176             setRosCmd(command);
00177         }
00178     }
00179 
00180     void init(const float (&arrive_threshold)[N_MOTORS], const float (&friction)[N_MOTORS], float estimate_error_limit)
00181     {
00182         for (size_t i = 0; i < motor_num_; i++)
00183         {
00184             motor_manager_[i].init(arrive_threshold[i], estimate_error_limit, friction[i]);
00185         }
00186     }
00187 
00188     void arriveCheck(bool (&is_arrived)[N_MOTORS])
00189     {
00190         ControlStruct cmd[N_MOTORS];
00191         for (size_t i = 0; i < motor_num_; i++)
00192         {
00193             is_arrived[i] = motor_manager_[i].isArrived();
00194         }
00195     }
00196 
00197 private:
00198     MotorManager motor_manager_[N_MOTORS];
00199     sensor_msgs::JointState joint_state_;   // JointState型
00200     std_msgs::Float32MultiArray joint_rad_; // joint_state_から変換される。joint_state_が更新された後に更新される必要がある
00201 
00202     int actuator_num_;
00203     const int motor_num_;
00204 
00205     void setJointState(int i, const StateStruct &state, sensor_msgs::JointState &joint_state)
00206     {
00207         joint_state.position[i] = state.position;
00208         joint_state.velocity[i] = state.velocity;
00209         joint_state.effort[i] = state.torque;
00210     }
00211 
00212     // void independentControl(ControlStruct (&cmd)[JOINT_NUM], ControlResult::ControlResult (&result)[JOINT_NUM])
00213     // {
00214     //     for (size_t i = 0; i < actuator_num_; i++)
00215     //     {
00216     //         motor_manager_[i]->getCmd(cmd[i], result[i]);
00217     //     }
00218     // }
00219 };