Any experience about difference between controller design in ROS and Matlab [closed]

edit

Hi,

I have designed a backstepping controller and verified that it worked well in Matlab. The goal of controller is to make the hovercraft follow the given circular trajectory in 2D. But the controller implemented in C++ does not work well in ROS. The hovercraft mostly rotates in gazebo simulation wheares it follows the circular trajectory in Matlab. I think I have checked every variables, time step, and etc. If there is anyone who has those experience, could you let me know something which I have to check to make my controller implemented in C++ work in ROS ?

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Wrench.h>
#include <algorithm>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/LU>

//#include <LinearMath/btQuaternion.h> // Needed to convert rotation ...
//#include <LinearMath/btMatrix3x3.h>  // ... quaternion into Euler angles


class Controller
{
public:
  Controller();

private:
  void controllerCallback(const nav_msgs::Odometry::ConstPtr& controller);

  ros::NodeHandle nh_;
  ros::Publisher left_fan_force_pub_;
  ros::Publisher right_fan_force_pub_;
  ros::Subscriber odom_sub_;

  double goal_x;
  double goal_y;
  double old_time;
  double old_x;
  double old_y;
  Eigen::Vector2d old_alpha;
  bool init_odom_;
  double init_odom_time_;
  geometry_msgs::Wrench left_fan_force_;
  geometry_msgs::Wrench right_fan_force_;
};


Controller::Controller()
{
  init_odom_ = true;
  old_time = ros::Time::now().toSec();
  old_x = 0;
  old_y = 0;
  Eigen::Vector2d old_alpha(0, 0);
  goal_x = 5.0;
  goal_y = 0.0;
  psi = 0;
  left_fan_force_pub_ = nh_.advertise<geometry_msgs::Wrench>("left_fan_force", 1);
  right_fan_force_pub_ = nh_.advertise<geometry_msgs::Wrench>("right_fan_force", 1);
  odom_sub_ = nh_.subscribe<nav_msgs::Odometry>("base_pose_ground_truth", 10, &Controller::controllerCallback, this);
}

void Controller::controllerCallback(const nav_msgs::Odometry::ConstPtr& odom)
{
  if (init_odom_){
    init_odom_time_ = odom->header.stamp.toSec();
    init_odom_ = false;
  }
  double m = 15.67;
  double d = 0.2;
  double Izz = 0.952;
  double u_des = 0.2;
  double omega_des = 0.1;
  double time = odom->header.stamp.toSec() - init_odom_time_;
  double dt = time - old_time;
  double psi_des = omega_des*time;


//  std::cout << 2*sin(psi_des) << " " << -2*cos(psi_des) + 2 << std::endl;
//  std::cout << psi_des << std::endl;

  double q0 = odom->pose.pose.orientation.w;
  double q1 = odom->pose.pose.orientation.x;
  double q2 = odom->pose.pose.orientation.y;
  double q3 = odom->pose.pose.orientation.z;

  double vx = odom->twist.twist.linear.x;
  double vy = odom->twist.twist.linear.y;
  double omega = odom->twist.twist.angular.z;

  double psi = atan2(2*(q0*q3+q1*q2),1-2*(q2*q2+q3*q3));

//  std::cout << psi << std::endl;

  Eigen::Vector2d vel(vx, vy);
  Eigen::Matrix2d R; // This R is actaully the transposed R.
  R << cos(psi), sin(psi),
      -sin(psi), cos(psi); 

  Eigen::Vector2d delta(-0.05, 0);
  Eigen::Vector2d nu = R*vel;
  Eigen::Vector2d Pdot_des(u_des*cos(psi_des), u_des*sin(psi_des));
  Eigen::Vector2d P2dot_des(-u_des*omega_des*sin(psi_des),
                             u_des*omega_des*cos(psi_des));

  double dv = 0.45; //translational damping coeffcient ;
  double Ke = 4; // This is controller parameter
  Eigen::Matrix2d Dv;
  Dv << dv, 0, 0, dv;
  Eigen::Matrix2d Kphi;
  Kphi << 2, 0, 0, 2; // This is controller parameter
  Eigen::Matrix2d M;
  M << m, 0, 0, m;
  Eigen::Matrix2d Minv;
  Minv << 1/m, 0, 0, 1/m;

  Eigen::Vector2d P(odom->pose.pose.position.x, odom->pose.pose.position.y);
  Eigen::Vector2d Pdes(2*sin(psi_des), -2*cos(psi_des) + 2);
  Eigen::Vector2d ...