Bullet position tracking could be closer to moteus behavior

[stephane-caron]

Currently Bullet position tracking is implemented as $\tau = \tau_f + K_p (p^* - p) + K_v (v^* - v),$ while moteus position control is implemented as:

acceleration = trajectory_follower(command_position, command_velocity)
control_velocity = command_velocity OR control_velocity + acceleration * dt OR 0.0
control_position = command_position OR control_position + control_velocity * dt
position_error = control_position - feedback_position
velocity_error = control_velocity - feedback_velocity
position_integrator = limit(position_integrator + ki * position_error * dt, ilimit)
torque = position_integrator +
         kp * kp_scale * position_error +
         kd * kd_scale * velocity_error +
         command_torque

The simulation would be improved by adding the internal control_position and control_velocity for each motor to model more accurately the moteus behavior.