[ROS] Gazebo상에서 joint_position rosservice값으로 로봇 PID제어하기

이전 게시물에서는 rosservice로 로봇 조인트의 position값을 받는 부분까지 알아보았다.

 

이번에는 subscribe한 값을 이용해서 링크가 넘어지지 않고 계속 서있을 수 있도록 로봇을 제어(PID)하는 방법을 다뤄보도록 하겠다.

 

teleop_twist_keyboard.cpp 파일을 다음과 같이 수정한다.

 

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

#include <stdio.h>
#include <unistd.h>
#include <termios.h>

#include <map>

#include <gazebo_msgs/GetJointProperties.h>

#include <unistd.h>


// Init variables

float Kp(50), Ki(1.2), Kd(1.5);
float P_control(0), I_control(0), D_control(0), PID_control(0);
float goal_position(0);
float error(0), pervious_error(0);

int main(int argc, char** argv)
{
  // Init ROS node
  ros::init(argc, argv, "teleop_twist_keyboard");
  ros::NodeHandle nh;

  // Init cmd_vel publisher
  ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);

  // Create Twist message
  geometry_msgs::Twist twist;

  //새로 추가된 부분
  ros::ServiceClient gazebo_Client = nh.serviceClient<gazebo_msgs::GetJointProperties>
  ("gazebo/get_joint_properties");

  gazebo_msgs::GetJointProperties::Request req;
  gazebo_msgs::GetJointProperties::Response resp;

  req.joint_name = "link_joint";


  while(true){
    
    bool success = gazebo_Client.call(req, resp);

    if(success) {
      ROS_INFO("\rJoint position is %f", resp.position[0]);
    } else {
      ROS_ERROR("\rfailed to spawn");
    }
    
    error = goal_position - resp.position[0];
    P_control = Kp * error;
    I_control += Ki * error * 0.013;
    D_control 
    PID_control = P_control + I_control + D_control; 
    twist.linear.x = -PID_control;
    ROS_INFO("\rx value is %f",-PID_control);

    if (resp.position[0] > 0.7 || resp.position[0] < -0.7)  
    {
      twist.linear.x = 0;
      break;
    }
   
    // Publish it and resolve any remaining callbacks
    pub.publish(twist);
    ros::spinOnce();
    ros::Duration(0.01).sleep(); 
  }
  return 0;
}

 

제어하는 데에 사용할 변수들이 추가되었고,

while문 안에 pid제어 코드가 들어갔다.

 

조인트가 똑바로 서있을 때 position값이 0이므로 error는 조인트의 position값 자체이고,

이 error를 비례, 적분, 미분한 값을 합쳐서 제어한다.

 

아직까지는 완벽한 제어가 되지 않는 상태이다.

 

gazebo를 실행하고 조인트가 기울기 전에 pid제어 코드를 실행하면 앞뒤로 몇번 왔다갔다하면서 균형을 잡는 것처럼 보이다가 한 방향으로 쏠리면서 결국 균형을 못 잡고 만다.

 

제어가 잘 되지 않는 원인으로

1. 로봇을 제어 명령에 따라 빠릿빠릿하게 잘 움직일 수 있도록 만들지 못 했다. 그래서 크기, 무게, 조인트들의 토크 등을 바꿔가보면서 고쳐보려고 시도했으나 성공하지 못 했다.

 

2. PID 제어 상수 튜닝을 정확히 하지 못 했다. 제어 시스템에 따라 적절한 PID 상수는 다 다르므로 여러 값들을 넣어보면서 경험적으로 찾는 수 밖에 없다. 제일 먼저 P상수만 넣어서 제어를 하고 어느 정도 되는 거 같으면 다음으로 I, D 순서로 추가를 해나가야 하는데, 나는 로봇을 제어가 가능한 수준으로 만들지 못 해서 P, I까지만 해보다가 일단 멈춘 상태이다.

 

혹시 해결책을 아시는 분은 조언 부탁드립니다 ㅠ