coordinate transform problem between childlink and world frame

asked 2017-12-31 14:04:47 -0600

gq38071112 gravatar image

I have a drone and i defined a local reference coordinate for every blade. The local coordinate will rotate with the blade around the z axis . Because the air flow speed is in world frame and i want to get the flow speed in local frame so that i can
calculate aerodynamic.

   public: void OnlinkMsg(const gazebo_msgs::LinkStatesConstPtr &msg)
{   int i;
   double x1,y1,z1,w1,roll1,pitch1,yaw1,Vzz1;
   x1=msg->pose[2].orientation.x;
   y1=msg->pose[2].orientation.y;
   z1=msg->pose[2].orientation.z;
   w1=msg->pose[2].orientation.w;
   //Transform quaternion positions in euler angles
tf::Quaternion q(x1, y1, z1, w1);
   tf::Matrix3x3 m(q);
    m.getRPY(roll1, pitch1, yaw1);
    Vzz1=-Vx*sin(pitch1)+Vy*cos(pitch1)*sin(roll1)+Vz*cos(yaw1)*cos(pitch1);`

Vzz1 is the air flow speed in local blade coordinate, Vx,Vy,Vz are air flow speed in world frame if the theory is right, wenn drone does not move,and the blade rotates,that means Vx=Vy=0, i shoule get a const Vzz1,but in practise the Vzz1 changes while the blade is rotating. What is wrong with my programm?

edit retag flag offensive close merge delete