diff --git a/src/omnidirectional_controllers/src/kinematics.cpp b/src/omnidirectional_controllers/src/kinematics.cpp index 058e830..76151d6 100644 --- a/src/omnidirectional_controllers/src/kinematics.cpp +++ b/src/omnidirectional_controllers/src/kinematics.cpp @@ -61,9 +61,9 @@ std::vector Kinematics::getWheelsAngularVelocities(RobotVelocity vel) { double wl = vel.omega * robot_params_.robot_radius; angular_vel_vec_[0] = (wl + vy) / robot_params_.wheel_radius; - angular_vel_vec_[1] = (wl - vx) / robot_params_.wheel_radius; + angular_vel_vec_[1] = -(wl - vx) / robot_params_.wheel_radius; angular_vel_vec_[2] = (wl - vy) / robot_params_.wheel_radius; - angular_vel_vec_[3] = (wl + vx) / robot_params_.wheel_radius; + angular_vel_vec_[3] = -(wl + vx) / robot_params_.wheel_radius; return angular_vel_vec_; }