Problem with velocity from micro drivers

[jviereck]

As reported by @wxmerkt in #89:

When standing perfectly still (zero-order hold), we have been seeing (1) significant outliers in the velocity signal (that are different from the position signal going to zero when the SPI frequency is too high) and (2) non-zero velocity even when the joint isn't moving (the raw uint is 1 and never decrements below).

For (1), we are using filtering that includes outlier rejection and this works well. For (2), this issue impacts state estimation quite a bit and we have a work-around that if the uint is 1, we consider it to be 0. Those two changes made state estimation quite stable.

We should look into this and try to fix it on the firmware level (either master board or udrivers).

[jviereck]

@wxmerkt, hi, what do you mean by "when the SPI frequency is too high"? Do you see these problems only when the SPI connection speed between the master board and udriver is at 6/8 Mhz and not at 4 Mhz or what do you mean?

[wxmerkt]

When the SPI frequency is at 6/8 MHz, we see the position signal frequently dropping to zero.

With the SPI frequency at 4 MHz, the position signal looks great. However, we still see outliers in the velocity signal when commanding a zero-order hold. Since the joint isn't moving at this time, this might be something different to #91.

I am currently not on hardware so won't be able to get newer logs to check

[thomasfla]

Hi, this is actually an issue related to the udriver firmware https://github.com/open-dynamic-robot-initiative/udriver_firmware

The velocity estimation is computed by a TI library and we observed two bug with it:
First, the velocity has a jump when crossing the position, as reported in #91
Second, the velocity is rounded to 1LSB when coming to a complete stop.

I tried to address this two problems by implementing my own velocity estimation from encoder reading but quickly realize that the CPU budget left on the microcontroller is close to 0, and my computation interfered with the communication.

About communication, with SPI, we need to react to an asynchronous incoming communication and provide the SPI fifo with sensor data and reading command data. Due to the limited SPi fifo buffer size of the MCU used in the udriver, we need to fill it with the CPU core on the go and consume cpu time on spi interupt.
Some of the TI FOC code is non interuptible and if a communication happen during it, we miss an spi transaction.
To mitigate the error prone SPI transfer, the master board will reiterate the transfer several time.

If SPI fails for other reason, like noisy lines, it is possible that no transfer happens during all the trials.
In this case, the master board will send a packet with all fields null except for the error code field (0xF). This explains why you sometime read zero as position values.

Ye are actively preparing the next version of the udriver that will use a much capable MCU with much more communication bandwidth, proper SPI with DMA, and 3 CPU cores. One will be dedicated to the FOC including sensor reading and state estimation, one dedicated to the communication, and one left for user code or future development. The new design is hosted here: https://github.com/open-dynamic-robot-initiative/open-motor-driver-initiative

I know this is not very satisfying since we will need to update the hardware, but I have no firmware solution to those issues.

[wxmerkt]

Thank you for the very detailed response @thomasfla and explaining the symptoms we've observed.

Regarding:

Second, the velocity is rounded to 1LSB when coming to a complete stop.

We have observed this precisely and are now considering 1 as 0, i.e. if the reported velocity is +/-0.006 radian after considering the conversion factor and gear ratio, we are setting it to zero. This works well with the current hardware :-)

[jviereck]

Thanks for the detailed explanation @thomasfla and the insight on how to fix it @wxmerkt !

We have observed this precisely and are now considering 1 as 0, i.e. if the reported velocity is +/-0.006 radian after considering the conversion factor and gear ratio, we are setting it to zero. This works well with the current hardware :-)

Do you think this is something we should bake into the official software library? I wonder if we should have something like a minimal velocity in the odri_control_interface and if the velocity is below this threshold, set the velocity to zero as you describe.

What do you think?

[jviereck]

With the SPI frequency at 4 MHz, the position signal looks great.

Should we change the default communication speed from 6 Mhz down to 4 Mhz? What do you think @thomasfla ? As @wxmerkt says, using the speed of 4 Mhz we are able to control the robot quite well so I am not sure if a higher speed is buying us much but rather cause us troubles.

[thomasfla]

Thanks for the detailed explanation @thomasfla and the insight on how to fix it @wxmerkt !

We have observed this precisely and are now considering 1 as 0, i.e. if the reported velocity is +/-0.006 radian after considering the conversion factor and gear ratio, we are setting it to zero. This works well with the current hardware :-)

Do you think this is something we should bake into the official software library? I wonder if we should have something like a minimal velocity in the odri_control_interface and if the velocity is below this threshold, set the velocity to zero as you describe.

What do you think?

Yes, that's a good idea. Maybe implementing it in the SDK directly is better since the LSB is registered here but abstracted on higher layer.

Should we change the default communication speed from 6 Mhz down to 4 Mhz? What do you think @thomasfla ? As @wxmerkt says, using the speed of 4 Mhz we are able to control the robot quite well so I am not sure if a higher speed is buying us much but rather cause us troubles.

Yes, I think it worth investigating it. I propose to test it at LAAS and take some measurement with a scope.
With lower frequency, the SPI transfer takes more time, and we need to communicate with all the devices in less than 1ms. If not, we might degrade the refresh rate of the sensors/commands. Also, with the oncoming powerboard we will need to have com time budget left for communicating the battery voltage, current, and energy.
At best, one driver message takes 35us at 8Mhz, 70us at 4Mhz but there are sometimes needed in between transfers

Could we wait for a month or so that I finish the power board integration and test it in 4Mhz?

[jviereck]

Yes, that's a good idea. Maybe implementing it in the SDK directly is better since the LSB is registered here but abstracted on higher layer.

Good point about implementing it in the master-board-sdk instead. I will look into this next week.

Could we wait for a month or so that I finish the power board integration and test it in 4Mhz?

Waiting for the power board to be ready and do a proper investigation of the entire system makes sense totally sense.

About 6 Mhz to 4 Mhz: While 4 Mhz might be slower, we might be overall still faster if we can avoid the SPI retries that we had to introduce to get a stable 6 Mhz working.