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

asked 2012-01-19 01:29:27 -0600

maruchi gravatar image

updated 2014-01-28 17:11:09 -0600

ngrennan gravatar image

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 ...
(more)
edit retag flag offensive reopen merge delete

Closed for the following reason question is not relevant or outdated by tfoote
close date 2012-05-01 22:32:18

Comments

The question is quite vague, and the answer is quite likely to be ultra specific to your particular code. You'll need to provide much more information.
Asomerville gravatar imageAsomerville ( 2012-01-19 02:45:29 -0600 )edit
You should write some tests and check if the results of both implementations are the same for identical input data. Of course, depending on the scenario, timing issues might also play a role, e.g. if you try to run your C++ code on a real system with (unsatisfied) hard real-time constraints.
Stefan Kohlbrecher gravatar imageStefan Kohlbrecher ( 2012-01-19 04:09:41 -0600 )edit
Without seeing your code, it's hard to say what's wrong.
sglaser gravatar imagesglaser ( 2012-01-19 04:20:29 -0600 )edit
Hi, Stuart, I put the controller code. I check the values of every variables for the first few steps. They are similar to the values from Matlab simulation, but the trend of values are different from each other, between ROS and Matlab.
maruchi gravatar imagemaruchi ( 2012-01-19 09:50:03 -0600 )edit
I know when I was porting things from MATLAB, I would save inputs and outputs to functions as .MAT files. You can then read from these MAT files to create unit tests. This way you can insure that your math is ported over correctly, with no uncertainty.
mjcarroll gravatar imagemjcarroll ( 2012-01-19 10:07:30 -0600 )edit
Which simulation environment are you usingin Matlab? As Stefan already pointed out, timing issues can lead to big differences in the behaviour/performance of the controller. Do you use a real-time environment?
bit-pirate gravatar imagebit-pirate ( 2012-01-19 10:48:39 -0600 )edit
Furthermore, I assume you built a model of the hovercraft for your gazebo simulation, too. This model might differ from the one you are using in your Matlab simulation and hence you might need to adjust your parameters accordingly.
bit-pirate gravatar imagebit-pirate ( 2012-01-19 10:53:14 -0600 )edit
Marcus, there are differences in the open loop test both in gazebo and Matlab. When I apply unit force or moment both in gazebo and Matlab, the outputs of positions and velocities from gazebo are much small than those from Matlab. Is this difference related to time problem in gazebo ?
maruchi gravatar imagemaruchi ( 2012-01-23 03:46:47 -0600 )edit