nilseuropa / gazebo_ros_motors

Motor simulation plugins for Gazebo - ROS
116 stars 26 forks source link

Possible bug in DC motor update function when handling the load torque #12

Open pedrofza opened 3 months ago

pedrofza commented 3 months ago

I believe that the DC motor GazeboRosMotor::UpdateChild method is passing the actual_load_torque parameter of GazeboRosMotor::motorModelUpdate` with the wrong sign: https://github.com/nilseuropa/gazebo_ros_motors/blob/2fd0f319c43a2a58ff35f8e440f05093e62e2645/src/gazebo_ros_dc_motor.cpp#L371C71-L371C82

From what I could tell, the calculations in motorModelUpdate exactly match the ODE solution presented in https://github.com/nilseuropa/gazebo_ros_motors/blob/2fd0f319c43a2a58ff35f8e440f05093e62e2645/doc/dc_motor_de.pdf. The ODE assumes that the load torque opposes the torque produced by the motor, for the purposes of the equation "\omega'[t] = (Km i[t] - T - d \omega[t]) / J". Note that "Km i[t]" and "T" have opposing signs. The code doesn't seem to reflect that, as the equivalent of this->link_->RelativeTorque().Z() is passed as an argument, and the sign is never flipped.