Vel_integrator_torque parameter + motors keep draining current at zero velocity

Hi,
can someone please explain why my motors drain current (3-10A) even if they are not moving and input_velocity is set to 0? What is the purpose of that?

I’m using 2 big hybrid servos with ABZ encoder powering it from battery, and controllers are in velocity control mode. I’m also using velocity ramps.

I’ve noticed parameter named vel_integrator_torque which is exact value as current set point + I can zero it manually. After that current drops to zero. What should I do ? I like the option of using vel ramps and the plan is to drive it using ASCII protocol.

Thank you in advance !

Sounds as if vel_integrator_gain is too high.

Also, it could be that you have very coggy motors. Some current may be required to “hold position” (ie keep velocity at 0) when the motor is halfway down a cogging detent. Reducing vel_integrator_gain should help to reduce that effect. You can even set it to zero if it is OK for your application for the motor to run at slightly less than the commanded velocity when under heavy load.

Thanks for your reply!

You are right, my vel_integrator_gain value is 8. If I remember correctly the higher value helped to run the motor at low speeds and the measured vel doesn’t oscillate around the setpoint.d the measured vel doesn’t oscillate around the setpoint.

errrrr yes iirc the default value is something like 0.0001 for that. :stuck_out_tongue_closed_eyes:

Out of interest, what’s your encoder resolution? Also what ‘low speeds’ are you running at?
If your motors are extremely coggy and you need smooth and efficient constant-speed operation, it may be worth adding some mechanical inertia i.e. a flywheel. What’s your application?

The motors are used to drive differential robot with tracks.
It uses 10:1 worm gearbox and total weight with payload is around 200kgs
(as you can imagine it is really difficult and dangerous to tune the motors)
I lifted the robot and set the gains so the motors would work “in the air” and then I just bumped the gains a bit with robot moving on the ground.
I will try to write down my configs.
The motors are unable to overcome dry friction with less than 25A.

config for motor:

current_meas_phC = -0.16823065280914307 (float)
effective_current_lim = 90.0 (float)
DC_calib_phB = -0.26156526803970337 (float)
config:
pole_pairs = 5 (int)
current_lim_margin = 8.0 (float)
motor_type = 0 (int)
acim_autoflux_decay_gain = 1.0 (float)
acim_autoflux_enable = False (bool)
pre_calibrated = True (bool)
current_control_bandwidth = 100.0 (float)
phase_resistance = 0.028385797515511513 (float)
torque_lim = inf (float)
acim_slip_velocity = 14.706000328063965 (float)
R_wL_FF_enable = False (bool)
current_lim = 90.0 (float)
acim_gain_min_flux = 10.0 (float)
calibration_current = 40.0 (float)
phase_inductance = 3.152631325065158e-05 (float)
acim_autoflux_attack_gain = 10.0 (float)
torque_constant = 0.12530303001403809 (float)
resistance_calib_max_voltage = 4.0 (float)
acim_autoflux_min_Id = 10.0 (float)
inverter_temp_limit_upper = 120.0 (float)
bEMF_FF_enable = False (bool)
inverter_temp_limit_lower = 100.0 (float)
requested_current_range = 90.0 (float)
direction = -1 (int)
current_control:
max_allowed_current = 121.5 (float)
Ibus = 0.8225415945053101 (float)
final_v_alpha = 4.910243988037109 (float)
Id_measured = 0.7967747449874878 (float)
async_phase_vel = 0.0 (float)
Id_setpoint = 0.0 (float)
final_v_beta = -10.641430854797363 (float)
overcurrent_trip_level = 135.0 (float)
Iq_setpoint = -79.80652618408203 (float)
Iq_measured = -1.1903678178787231 (float)
v_current_control_integral_q = -11.499893188476562 (float)
async_phase_offset = 0.0 (float)
I_measured_report_filter_k = 1.0 (float)
p_gain = 0.0031526312232017517 (float)
v_current_control_integral_d = -0.008218998089432716 (float)
i_gain = 2.8385796546936035 (float)
acim_rotor_flux = 0.0 (float)
error = 0x0000 (int)
current_meas_phB = -0.38439610600471497 (float)
is_calibrated = True (bool)
phase_current_rev_gain = 0.05000000074505806 (float)
armed_state = 0 (int)
DC_calib_phC = -1.12149178981781 (float)
timing_log:
meas_r = 0 (int)
sample_now = 0 (int)
foc_current = 8938 (int)
idx_search = 0 (int)
meas_l = 0 (int)
adc_cb_dc = 12794 (int)
enc_calib = 0 (int)
foc_voltage = 0 (int)
adc_cb_i = 2606 (int)
spi_start = 0 (int)
spi_end = 0 (int)
general = 0 (int)

encoder config:

config:
mode = 0 (int)
use_index = True (bool)
calib_range = 0.5 (float)
offset_float = -0.43089061975479126 (float)
calib_scan_distance = 50.26548385620117 (float)
calib_scan_omega = 12.566370964050293 (float)
cpr = 10000 (int)
abs_spi_cs_gpio_pin = 8 (int)
sincos_gpio_pin_cos = 4 (int)
pre_calibrated = True (bool)
enable_phase_interpolation = True (bool)
zero_count_on_find_idx = True (bool)
ignore_illegal_hall_state = False (bool)
find_idx_on_lockin_only = False (bool)
bandwidth = 100.0 (float)
idx_search_unidirectional = False (bool)
sincos_gpio_pin_sin = 3 (int)
offset = -8150 (int)

controller config:

config:
enable_vel_limit = True (bool)
load_encoder_axis = 1 (int)
control_mode = 2 (int)
vel_gain = 2.0 (float)
torque_ramp_rate = 0.009999999776482582 (float)
anticogging: …
vel_ramp_rate = 15.0 (float)
input_filter_bandwidth = 10.0 (float)
mirror_ratio = 1.0 (float)
axis_to_mirror = 255 (int)
circular_setpoints = False (bool)
enable_current_mode_vel_limit = True (bool)
inertia = 0.0 (float)
vel_limit = 3000.0 (float)
enable_gain_scheduling = False (bool)
circular_setpoint_range = 1.0 (float)
gain_scheduling_width = 10.0 (float)
enable_overspeed_error = True (bool)
vel_integrator_gain = 20.0 (float)
pos_gain = 1.0 (float)
homing_speed = 0.25 (float)
input_mode = 2 (int)
vel_limit_tolerance = 1.2000000476837158 (float)

It looks that my vel_integrator_gain is waaay higher :stuck_out_tongue_winking_eye:
Any ideas how to tune it safely and the proper way?

As long as you have a functioning Emergency Stop, it should be safe to tune it on the ground. As you have seen, on the ground it has a very different set of dynamics so motor tuning in the air isn’t really valid.

However, the integrator can be dangerous because of integral windup, so I would advise to set vel_integrator_gain to zero, and tune vel_gain only.
This should be very simple to just bring up from near zero, increasing by a factor of 10 a few times until you see a tiny amount of movement, and tune from there. Maximum motor torque (at stall) will be proportional to vel_setpoint * vel_gain, and will not ramp up to current_lim as it does if you have any integral gain.
You could also change your power supply fuse to something that limits the power to around 50W (which is considered a safe power limit for collaborative robots) - so if your robot takes off for any reason, it will blow the fuse.
At 0.03 ohms motor resistance, 25A ~= 20W, so in theory it should pull less than 1A from the power supply if it’s not moving very fast.
That’s a lot of current to overcome friction though, so it’s no wonder it’s difficult to tune with such a nonlinear plant. Have you tried oiling it? :stuck_out_tongue_closed_eyes:

Thanks! will definitely try it out.

The worm gearbox itself contains some sort of oil in it.
How about bandwith parameters of encoder and current control? Is the 100 range OK?

Leave all that at default (1000?) unless you have a lot of noise on your encoder for some reason (which is very unlikely). If you lower the encoder bandwidth too much, you could screw up the FOC torque controller, i think.

If you get oscillation with the velocity controller, you could drive in torque mode with a velocity limit instead, and set both from your application. That way you might be able to model out some of that friction in your high level controller, if you have one.
Or alternatively send velocity commands with a torque feed-forward value coming from your model.

I think I screwed up.

tried to tune axis0 motor on the ground, vel_integrator_gain = 0, vel_gain=1, input_vel = 3 no movement.
increasing the vel_gain to 10 still no movement live plotter was plotting 90A Iq_measured.
After that I’m getting MOTOR_ERROR_DRV_FAULT on axis0 the same motor works fine on axis1 :frowning_face:

I’m guessing you didn’t use the fuse then :frowning:

I have never needed to go more than 0.01 for my gains, I don’t understand why you have to set them so high (unless the units have changed - how fast are you expecting a speed of “3” to be? 3 encoder counts/sec or 3 revs/sec?) - I see you have set up the torque constant for your motor, so there’s a factor of 10 difference there too. I normally leave it at 1 and measure torque in “amps”.

It does sound like you need some feedforward for this motor though! Although a simple linear feedforward might not be enough - you might need to add a constant depending on direction.

Does the DRV_FAULT persist after you power-cycle the drive?

I guess It would be easier to start the configuration again with new board and proper wiring and protections.

Yes the error persists even after recovering my saved configuration.
input_vel = 3 [3 revs/s of motor] that means 0.3 rev/s on the output shaft.
(I’m using the newer one firmware)

Can you link to the motor spec and/or post a photo? I don’t see how any of this could have damaged your board so far. :confused:

Unfortunately I don’t have any kind of documentation. All I know is that the motors are rated at 24V and it is NEMA 32 housing. I had to find out the pinout by myself.

Here is the photo:

riiight. Ok, I think I may know what your original problem is.
I’m guessing you did not remove the motors from their gearboxes before calibration?
Really, this needs to be done or else ODrive will get a poor calibration, and very inefficient torque production. But in your case that’s difficult to do.

However, I see the motor also has Hall sensors. These do not require a precise calibration and will always give a “good enough” commutation signal, so you would not need to disassemble your motors to use those.

Can you explain and write formula for calculate torque feed-forward, when use VELOCITY_CONTROL and send velocity command with torque_FF?

This simply applies a torque proportional to the velocity command (whereas the vel_gain applies a torque proportional to the error between the velocity command and the actual velocity)
If you use both feedforward and feedback gains, their effects are added together to produce a torque command.
so

T_{cmd} = k_{vff} V_{cmd} + k_{vp}(V_{cmd}-V_{act})

I omit the integral term, assuming k_vi (i.e. vel_integrator_gain) = 0

Velocity feedforward gain is useful when you have a lot of friction in the system which is roughly proportional to velocity. If your friction does not increase with velocity then it is not so useful.

I use CAN interface and send command set_input_vel(float velocity, float torque_FF).
Confused about parameter - torque_FF. I think, that this parametr can help me solving error between velocity_command and velocity_actual.

That parameter can be used to directly add a torque offset, so instead of being a function of velocity, it adds whatever torque you supply.
You could for example, use the jacobian of your robot to transform the load vector at the end-effector into torque FF commands for each joint

thank for answer
send velocity command and try to automatic control velocity by torque_ff, but so far to no avail.