# converting Twist to wheel velocities [closed]

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;
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;
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?

edit retag reopen merge delete