Detecting collision for a wheeled robot.

[TechnicBRICKs]

Hi,

Generally I'm trying to detect collision events for a wheeled robot. It can be a fixed obstacle or in the limit another moving car, but lets keep it simple to start.

I've planned to use the IMU from TechnicHub to detect a deceleration, but the fact is that even considerable speed and high sampling rates, the acceleration readings doesn't seem to stand out from the IMU readings noise level.

Another possibility would be to try detecting the wheels slow down. However my experience tells me me that even in front of an obstacle, a relative light weight car keeps its wheels running as if it was moving freely...
This regardless if it is a varnished parquet, ceramic tiles or ABS like the large 8x16 tiles.
I may try playing with stall_tolerances method in Control Class, but I've some doubts this could successfully solve the problem.
Any ideas you'd like to suggest?

Below the code I'm using to log the IMU readings in the console. For the case of my prototype, the significant readings should appear mostly on Y axis.

while True:
    drive_base.drive(300,0)
    print(hub.imu.acceleration(axis=Axis.X), hub.imu.acceleration(axis=Axis.Y), hub.imu.acceleration(axis=Axis.Z))
    wait (5)

I've tried with larger wait times as it was my intention, trying even to remove the wait to increase the sampling to the maximum so I won't miss the collision event, without success.

BTW, I've also experienced another problem.
Sometimes when I stop the program, drivebase motors keep running forever. I may eventually stop them with a long press on hub's green button, but sometimes really need to remove the batteries so that the program can be stopped.

[TechnicBRICKs]

Have been trying this code, without effect.

# Main
print("Start: BCar")
light.on(brightness = light_intensity)
    
motor_left.control.stall_tolerances(car_speed, stall_time)
motor_right.control.stall_tolerances(car_speed, stall_time)
    
while True:
    drive_base.drive(car_speed, car_fwd_turn_rate)
    
    if motor_left.control.stalled() or motor_right.control.stalled():
        drive_base.stop()
        drive_base.straight(car_reverse)  

The parameters may need fine tuning, but not even if I hold one or the two motors by and, I can trigger the stalled condition.

Not sure if I can access motor_XX.control.stalled() while the drive_base is running.
According to the documentation "You cannot use these motors individually while the DriveBase is active."
Although if not possible this way, how to use it with drive_base?
Apparently can't apply .control.stall_tolerances() directly to the drive_base, hence my approach.

[TechnicBRICKs]

Still regarding the usage of IMU to detect a collision, still wondering if this would be feasible...

Prime and Inventor hubs support tap detection with their native firmware tap detection, which work fine with relative weak taps. Hence it should a matter to find out how this is implemented.
Still need to check if a 300mm/s car collision is enough to trigger a tap detection event, or if the velocity needs to be higher and how much.

On this regard I've also another question. Which are the acceleration units returned by IMU sensor with Pybricks?

• LEGO firmware on SPIKE and Inventor hubs return around 1 on Z-axis at rest, hence I believe these are G units.
• TechnicHub with Pybricks returns around 10 on Z-axis at rest, hence I believe these are SI units (m/s2).

1G ~= 9,81 m/s2

[laurensvalk]

how this is implemented.

Yes, the sensor can be configured with all kinds of filters. For now we only return the raw values (scaled to appropriate units, see below).

It would be good to have some discussion and a list of use cases, so we can decide on the right settings. So one use case is impact detection like yours. Let's try to make a list of all kinds of use cases.

Which are the acceleration units returned by IMU sensor with Pybricks?

[TechnicBRICKs]

My bad!... I read the Pybricks documentation, and later when posting here forgot the units were there. Perhaps suggested by the fact that the same information does not exist regarding LEGO firmware...

As you refer to scale the raw values to appropriate units, you might know which are the IMU raw values units? Gs I guess!?

As for the use cases tap is indeed a useful one, but I can think about a few other:

• Double tap, triple tap
• Directional tap (+/-x, +/-y, +/-z)
• Free fall detection, like in origina SPIKE software
• I've made a model where I use SPIKE display to show X/Y acceleration in very basic polar graph, and compare next to the readings in my car dashboard (might publish a video of it some day...).
  Hence the raw reading are useful. In these case I display in Gs, but that's a conversion everyone can do once the readings units are known.

[laurensvalk]

As you refer to scale the raw values to appropriate units, you might know which are the IMU raw values units? Gs I guess!?

That's configurable also. You can select higher range (higher maximum detection) at the cost of less resolution. Ideally we find the right balance for most applications so we don't have to make commands to set each and every option.

Directional tap would be cool, I'd have to check if the sensor can do that.

[TechnicBRICKs]

Still tinkering around this... 😊

IMU readings are still my preferred option, if viable to make them work reliably or close to that.
Hence I decided to make some additional measurements and graphs to check whether there are readings that standout during a vehicle collision event, or not. My previous analysis were made through a quick inspection at the printouts to the Python console, which is definitely not a reliable way to look at it...

In an attempt to get more insights, I've attached a SPIKE hub to my drivebase, which is powered by the Technic hub, and logged the respective IMU readings through the LEGO SPIKE Prime app (hope you don't mind that I show them here) to compare both logs and eventually take some insights to tweak Pybricks IMU parameters setting.

Some of my initial observations are:

• Car collision at tested speeds were not enough to trigger a 'tap' event, as detected by the SPIKE LEGO app. Although we can often find IMU acceleration spikes in the expected axis (Y), but sometimes also at X, and more often and surprisingly more prominent at Z.
• Sometimes visible spikes do not show up at all.
• And sometimes it would be difficult to identify a possible collision, because there are several reading spikes in one axis or in different axis at different points in time...
• If a collision event needs to be detected in real-time, many existing dataset processing techniques could not be used effectively as we can't wait for future data to arrive for a complete dataset with previous and post spike data points. Hence some expedite algorithm might be considered.

As a preliminary conclusion, the IMU with current Pybricks implementation seems to have some potential to successfully identify collisions, with some false positives and some false negatives along the way.
If complemented with a "slippery" wheels slow down detection, I believe it could be a viable solution to reduce the false negatives.

Below some samples and comments. Please note that Technic hub data displayed in Excel charts were measured with Pybricks, and correspondent SPIKE hub data is measured with LEGO firmware and SPIKE app.
Added vertical orange lines to mark the presumable moment of impact, where possible.

1) Well behaved and consistent data.

2) Another well behaved example.

3) Consistent data, although some unexpected peaks at Y axis. Should get confirmation at another axis, within a a defined short time interval.

4) Well behaved example with X, Y and Z peaks. Z peak not evident from SPIKE readings.

5) Good Y and X peaks, but apparently not consistent with SPIKE readings.

6) Z axis gives a good hint about the collision, but not 100% confirmed at Y neither X.

7) Hard to infere when the collision occurred, at least through automation.

8) Loads of noise on the Technic hub... Hard to be sure where the impact did happen! All fine with SPIKE hub data.

Still needing to better look at this data in order to implement some heuristic that fulfills the objectives. And looking forward to a wheel slow down implementation as a complement. 👌

[laurensvalk]

Thanks for the detailed analysis! I’ll have to come back to this later when we are having a closer look at this sensor.

While your memory is still fresh, could you add a few notes below each screenshot to say when (approximately) the impact happened and/or how many collisions there were?

[TechnicBRICKs]

I've update the post with the charts.

The vehicle was heading towards a wall in each test run, hence there is only one collision per chart.
Both Technic and SPIKE hubs logging were initiated and stopped manually, through Pybricks and SPIKE app, hence these were not synch, neither the exact moment of the collision is registered. I've positioned the collision lines at my best guess for each run.
There were already some notes for each run before the charts, which I've now updated in some cases.

Hope this will turn useful.

[ZPhilo]

I had another idea, which was to monitor current consumption using hub.battery.current(). I got it easily working using unregulated motor but current with regulated motor is much more shaky and would need better filtering to get it working. In the charts attached, yellow is the unfiltered current, pink is shortly damped current, blue is heavily damped one.
Unregulated mode chart

Regulated mode chart

Code used (don't remember how to show a .py code in a readable form, "code" doesn't seem to work! - hence the zip)
currentmonitor-dcmode.zip

[dlech]

don't remember how to show a .py code in a readable form

Like this:

```python
# your code here
```

[dlech]

Extracted from zip.

from pybricks.hubs import InventorHub
from pybricks.pupdevices import Motor, UltrasonicSensor
from pybricks.parameters import Port, Direction
from pybricks.tools import wait, StopWatch

# Initialize motors.
left = Motor(Port.A)
right = Motor(Port.B, Direction.COUNTERCLOCKWISE)

# Initialize hub and color sensor.
# You can also use the TechnicHub and/or the ColorDistanceSensor instead.
hub = InventorHub()
eyes = UltrasonicSensor(Port.D)
eyes.lights.on(100)

watch=StopWatch()

longdamp=10
shortdamp=3

#Go!
left.dc(100)
right.dc(100)
wait(100)

avgcurlong=hub.battery.current()
avgcurshort=hub.battery.current()
while watch.time()<5000:
    avgcurlong=(avgcurlong*longdamp+hub.battery.current())/(longdamp+1)
    avgcurshort=(avgcurshort*shortdamp+hub.battery.current())/(shortdamp+1)
    print(avgcurlong, ";", avgcurshort, ";", hub.battery.current())
    wait (20)

[TechnicBRICKs]

This could be a good idea indeed!

However at least in my case, I want to use a relative lightweight car and small wheels.
The wheels will easily slip on the floor when the car is blocked. Even if the motors will slightly slow down, I've serious doubts there will be any sensible effect in the motors current consumption.

[laurensvalk]

Separately, we've discussed detecting collision by looking at the motors/wheels.

I'm starting to look at exposing the approximation of the load/effort in the user API. In essence, this tells you how much feedback torque the motor delivers. It is kind of similar to stalled, but much more refined than simply True or False.

Video.10-02-2021.11.40.34.mov

It seems to work quite well in this test. (There's nothing particularly special about this vehicle design; it's just the segway I built earlier with some support wheels. But it does not need to be flat.)

Like acceleration, the raw load estimate is a bit noisy, but in this example I've filtered it a bit, giving a tradeoff between accuracy and response time.

Using it would be fairly straight forward:

load = my_motor.control.load()

if load > threshold_value:
    do_something()

I think I'll let this method return values in millinewtonmeters. You could then use @ZPhilo's motor page to get some ideas for thresholds.

Ideas? Thoughts?

[TechnicBRICKs]

Wow, indeed!

[laurensvalk]

This is now documented here and will be useable in the next beta, or sooner if you use the nightly builds.

[TechnicBRICKs]

The latest post in v3 Beta Announcements refers to beta.5, but Pybricks Code reports beta.6.
So you mean beta.7 right!?

[laurensvalk]

You can try the latest build, which has the load method, using these instructions.

There's no beta build yet. We usually do one every couple of weeks. So if you need it now, try the latest build.

[dlech]

and just a warning, Bluetooth has issues currently in the CI builds