converting Twist to wheel velocities [closed]

edit

I'm writing a new controller for diff drive controller wheel which acts like an omni directional wheel. Here is the algorithm, logic i've implemented till now:

  void computeVel(const geometry_msgs::TwistConstPtr cmd_vel, WheelAngVel &wheel_ang_vel) {
    float v = sqrt(cmd_vel->linear.x*cmd_vel->linear.x + cmd_vel->linear.y*cmd_vel->linear.y);
    float v_y = cmd_vel->linear.y;
    float required_yaw = acos(cmd_vel->linear.y/v);

// Computing right and left wheel ang velocity according to linear y velocity, pure rotation
if(round(required_yaw) != 0) {
  double actual_roll, actual_pitch, actual_yaw;
  tf::Quaternion q(
    filtered_odom->pose.pose.orientation.x,
    filtered_odom->pose.pose.orientation.y,
    filtered_odom->pose.pose.orientation.z,
    filtered_odom->pose.pose.orientation.w
  );
  tf::Matrix3x3(q).getRPY(actual_roll, actual_pitch, actual_yaw);
  while(actual_yaw < required_yaw) {
    float wheel_linear_vel = max_yaw_rate * wheel_to_odom_dist;
    float wheel_angular_vel = wheel_linear_vel/wheel_radi;
    if(wheel_angular_vel>0) {
      wheel_ang_vel.left = wheel_angular_vel;
      wheel_ang_vel.right = -wheel_angular_vel;
    }
    else {
      wheel_ang_vel.left = -wheel_angular_vel;
      wheel_ang_vel.right = wheel_angular_vel;
    }
  }
}
else {
  // Diff drive logic
  // Computing right and left wheel ang velocity according to linear x velocity
  wheel_ang_vel.left = cmd_vel->linear.x;
      wheel_ang_vel.right = cmd_vel->linear.x;
      // Computing right and left wheel ang velocity according to angular z velocity
      float wheel_linear_vel = cmd_vel->angular.z * wheel_to_odom_dist;
      float wheel_angular_vel = wheel_linear_vel/wheel_radi;
      if(wheel_angular_vel>0) {
        wheel_ang_vel.left += wheel_angular_vel;
        wheel_ang_vel.right -= wheel_angular_vel;
      }
      else {
        wheel_ang_vel.left -= wheel_angular_vel;
        wheel_ang_vel.right += wheel_angular_vel;
      }
    }
  }

Here i have devided the logi into two segments one for making the orientation of base to the direction of motion and second one which i tried to use the pure diff drive controller logic. I'm planning to test it latter. Is their any problem in this logic?