velocity_control.h

00001 #pragma once
00002 
00003 #include "catchrobo_sim/accel_curve.h"
00004 #include "catchrobo_sim/safe_control.h"
00005 #include "catchrobo_sim/control_result.h"
00006 #include "motor_driver_bridge/motor_driver_struct.h"
00007 #include <catchrobo_msgs/MyRosCmd.h>
00008 
00009 //#
00010 
00011 class VelocityControl
00012 {
00013 public:
00014     VelocityControl() : dt_(0){};
00015     void setRosCmd(const catchrobo_msgs::MyRosCmd &command, const StateStruct &joint_state)
00016     {
00017         target_ = command;
00018     };
00019 
00020     // dt間隔で呼ばれる想定
00021     void getCmd(const StateStruct &state, const ControlStruct &except_command, ControlStruct &command, ControlResult::ControlResult &result)
00022     {
00023         //// accel_curveを入れると、速度の誤差が大きいため変な動きをする
00024         command.id = target_.id;
00025         command.p_des = state.position + target_.velocity * dt_;
00026         command.v_des = target_.velocity;
00027         command.torque_feed_forward = target_.effort;
00028         command.kp = target_.kp;
00029         command.kd = target_.kd;
00030         result = ControlResult::RUNNING;
00031     };
00032     void setDt(float dt)
00033     {
00034         dt_ = dt;
00035     }
00036 
00037 private:
00038     catchrobo_msgs::MyRosCmd target_;
00039     float dt_;
00040 };