921 kinematics pose estimation #1089

Ezward · 2023-01-26T05:44:55Z

This PR adds a new capability; the ability to estimate pose using wheel encoders for differential drive robots and car-like robots.

Encoders
Encoders count ticks. They are not a Donkeycar 'part', but rather a Python object, subclassed from class AbstractEncoder, that must be passed to to the Tachometer constructor.

The SerialEncoder talks to an Arduino over a serial port to acquire ticks. There are two Arduino sketches included for this purpose.
- The mono_encoder.ino sketch supports one or two single channel encoders.
- The quadrature_encoder.ino sketch supports one or two quadrature encoders.
- When using the sketches with two encoders, the EncoderChannel part is used to separate out the readings from the two encoders.
The GpioEncoder supports reading a single channel encoder using a GPIO input pin. For a differential drive scheme two GpioEncoder instances are used.
The MockEncoder can be used to simulate an encoder for testing. It takes in the throttle and steering and outputs an appropriate number of ticks per second to simulate the throttle.

Tachometer
The Tachometer part takes an encoder and the encoder's pulses per revolution in it's constructor and outputs revolutions from it's run()/run_threaded() method.

tachometer.py includes a __main__ that can be used is isolation from the full donkey framework to test the encoder setup.

Odometer
The Odometer part takes the output of a Tachometer (revolutions) and turns revolutions into a distance and velocity. Velocity can be optionally smoothed across a given number of readings. Eventually we will use the velocity output for closed-loop speed control, but that will be in another branch and PR.

Kinematics
The kinematics parts take in the distance output of one or two Odometers and output an estimate of the vehicle's pose; (x,y) position and (angle) of orientation, as well as rate of change components for each of those. For car-like vehicles the distance from a single Odometer is combined with the steering angle to produce the estimate using a Bicycle kinematics part. For differential drive vehicles (or a car-like vehicle that uses two encoders in a differential layout) the distance output from two Odometers are used to produce a pose estimate using the Unicycle kinematics part.

Pose
The a fore mentioned parts are combined into a single part that creates a unitified pipeline that takes in ticks and produces pose and can be run in thread for maximum update rate.

BicyclePose wraps a single encoder/tachometer/odometer and the Bicycle kinematics part to produce pose for a car-like vehicle.
UnicyclePose wraps two encoders, two tachometers, two odometers and a Unicycle kinematics part to produce pose for a differential drive vehicle.

This is a rework of the encoder - tachometer - odometer - kinematics pipeline from branch 921-gps-logger. Prior to this change the individual parts were added to the vehicle. This had several problems;

The code was really really messy because we have 4 parts to make this work.
The code did not perform well. We think that is because it was not all running in a thread and so some parts did not update enough or were using data that was as much as a frame old, which can be very bad when incrementally estimating pose.

Templates
The path_follow.py template has been updated to use the kinematics pose estimate to train a path and follow the path in auto-pilot mode.

Currently GPS mode and encoder mode are not compatible (they cannot be fused). We intend to make these two compatible such that we can fuse the two sources of pose together if both are active. This will require specifying an initial pose that the kinematics pipeline start with, likely derived from recent GPS readings. This would create a nice system as the encoder can produce high frequency estimates between the lower frequency positions provided by GPS and the high accuracy GPS reading can be used to rectify the cumulative error inherent in encoders based pose estimation.

@TCIII has tested the path_follow template using a differential drive configuration and the mono_encoder.ino sketch to get ticks from an Arduino compatible board. He can successfully record and follow a path.

Ezward · 2023-01-26T05:52:23Z

Two things worth noting

We are producing a velocity estimate. I have code to do close-loop speed control and to use velocity rather than throttle in the linear model. I intend to add this to the framework at some point in the near future.
This feature need not be confined to the path_follow template. The ability to plan and follow a route around an obstacle would be very useful when racing or otherwise using the deep learning autopilot.

Ezward · 2023-01-26T06:01:26Z

As a heads-up, there is are changes to path_follow.py and complete.py associated with changing some terminology from 'angle' to 'steering'. This is because angle has a specific meaning in the kinematics and pose estimate realm. The values we are calling 'angle' are more properly a steering input.

TCIII · 2023-01-26T11:42:03Z

@Ezward,
During my initial testing of the kinematics pose estimate branch I started off with the default negative P and D values and found that the test vehicle would go straight ahead for a ways and then veer off to the right during the recorded path playback.
I adjusted the default negative P and D values to be positive values and tested again.
The test vehicle began and tracked the recorded path fairly accurately, but fishtailed (rear end wiggle on a tricycle chassis) a little on the straights and was not making the recorded turns quickly enough.
So I upped the positive P value towards 1.0 and the positive D value towards 0.75 and that helped reduce the fishtailing and improved the turning response.
PID tuning is everything.

TCIII

Ezward · 2023-01-27T23:07:12Z

Demo of @TCIII differential drive robot running path_follow autopilot on it's third lap; https://www.youtube.com/watch?v=c8VipGXt4kI It went 5 laps before accumulated error causes a collision.

arduino/mono_encoder/mono_encoder.ino

DocGarbanzo

@Ezward - this looks good to me. Only minor questions / comments.

arduino/quadrature_encoder/quadrature_encoder.ino

donkeycar/parts/kinematics.py

donkeycar/parts/odometer.py

donkeycar/parts/tachometer.py

donkeycar/parts/pose.py

donkeycar/parts/tachometer.py

DocGarbanzo

This looks good to me, I have only a minor comment in the vehicle loop. If you merge, please update the version number in setup.py and please apply the new version format, i.e. it should go from '4.4.dev4' to '4.4.dev5'.

donkeycar/parts/pose.py

DocGarbanzo · 2023-02-13T21:21:45Z

donkeycar/vehicle.py

+
+
+            loop_total_time = time.time() - loop_start_time
+            print(f"Vehicle executed {loop_count} steps in {loop_total_time} seconds.")


Optimally this should be a logger statement as well.

I updated that to a logger statement

updated to version="4.4.dev5"

- tachometer takes an encoder and returns revolutions - odometer coverts revolutions to distance and velocity - kinematics converts odometry to pose estimates - BicyclePose part implements the pose estimation pipeline for a car-like vehicle (fixed back wheels, turnable front wheels) - UnicyclePose part implements the pose estimation pipeline for differential drive vehicles.

- UnicyclePose handles encoder/tachometer/odometer/kinematics for differential drive. - BicyclePose handles encoder/tachometer/odomter/kinematics for car-like vehicle - add a mock encoder that is driven by throttle and steering input. This allows use to simulate the vehicle without a full-on simulator. - Fix the drawing of the path so that it handles the fact that Y increases going north.

- Interrupts mode can handle much higher tick rates, but does a poor job of debouncing. It is appropriate for high resolution optical encoders.

It was at the end of `#define ENCODER_OPTIMIZE_INTERRUPTS` and so could have been causing the #define to be unrecognized.

- there was a syntax error if only one pin was defined and it was being used in interrupt mode; that is fixed. - Add more documentation to the mono_encoder.ino sketch

- The sketch counts ticks on a quadrature encoder without using 3rd party libraries. - This is done 1) to make is simpler to compile and download the sketch; the user does not need to figure out what library to use. 2) the library that was in use only worked on AVR hardware and causes compilation errors on other hardware. - if USE_ENCODER_INTERRUPTS is defined when the sketch is compiled, then the interrupt driven tick counting will be used. This has no debounce logic, so it is not suitable for mechanical encoder, but is appropriate for optical or hall effect encoders. - if USE_ENCODER_INTERRUPTS is NOT defined, then this used polling mode with debouncing, which is suitable for mechanical encoders, but may be to slow for high resolution optical or hall effect encoders.

- it had literal 2 for size of encoders array, so when there was only one encoder we got memory overwrites.

- I used the wrong symbol for adding the #2 isr

- use array rather than pointer in readEncoders() argument.

- the library we used was only for AVR microcontrollers, so the code could not work on RPi Pico for instance. - I reimplemented the sketch to have explicit polling mode logic that is suitable for noisy mechanical encoders. - To that I added an interrupt driven mode that works if there is one interrupt capable pin available for each encoder. This is suitable for optical or hall effect encoders that do not need to be debounced.

- the constructor has a default for pin_scheme based on the GPIO object. The GPIO object will not be defined on a PC. - this change use None as a default and then checks for it and set the GPIO default if it is None. - This fixes a bug in the actuator unit test.

- The vehicle loop now counts frames accurately; prior to this change the counter would be one more than the actual number of executed frames - When the vehicle loop terminates the number of executed iterations and the total time are printed and returned to the caller. - This was used for the kinematics tests because we needed to know how long the vehicle drive loop executed so we could calculate the expected distance that the mock vehicle drove.

- We dramatically changed how a mock drivetrain handles odometry. Now is uses a velocity based on encoder ticks per second scaled by throttle. This is a more realistic mock, but it is harder to test. - The tests setup a vehicle with a mock encoder, then run the vehicle loop for a set number of iterations. The vehicle loop now returns how long the loop ran and this is used along with the configuration for ticks_per_second from the mock encoder to calculate how far the vehicle travelled. Then the kinematics model is applied to see if the resulting ending pose matches the expected pose.

- the code now uses the front wheels as the reference point for the calculations - this fixes the unit test, which were using the front wheels while the code used the back wheels.

- change print statement to a log

version="4.4.dev5"

Ezward requested review from sctse999 and DocGarbanzo January 26, 2023 05:44

Ezward force-pushed the 921-kinematics-pose-estimation branch from ba535c0 to c743ca3 Compare February 4, 2023 18:05

Ezward mentioned this pull request Feb 5, 2023

link is not valid encoder.py #1090

Closed

Ezward changed the title ~~921 kinematics pose estimation~~ DRAFT 921 kinematics pose estimation Feb 5, 2023

Ezward mentioned this pull request Feb 6, 2023

Enable odometry/kinematics pose estimation in the path_follow.py template #948

Closed

DocGarbanzo reviewed Feb 7, 2023

View reviewed changes

arduino/mono_encoder/mono_encoder.ino Show resolved Hide resolved

DocGarbanzo reviewed Feb 7, 2023

View reviewed changes

Ezward changed the title ~~DRAFT 921 kinematics pose estimation~~ 921 kinematics pose estimation Feb 9, 2023

Ezward requested a review from DocGarbanzo February 10, 2023 02:31

DocGarbanzo reviewed Feb 11, 2023

View reviewed changes

donkeycar/parts/tachometer.py Outdated Show resolved Hide resolved

donkeycar/parts/pose.py Outdated Show resolved Hide resolved

Ezward commented Feb 12, 2023

View reviewed changes

donkeycar/parts/tachometer.py Show resolved Hide resolved

Ezward requested a review from DocGarbanzo February 12, 2023 20:56

DocGarbanzo approved these changes Feb 13, 2023

View reviewed changes

Ezward and others added 12 commits February 13, 2023 18:24

comment out logging of pose to quiet the console

d31b163

add arduino encoder sketches

9710c16

remove unicode characters that prevent compilation of arduino sketch

6d66d2c

clean up comments a little

ff95fdf

Add more documentation to the quadrature_encoder.ino sketch

e2a9a54

Add interrupt mode to the mono encoder

325e89b

- Interrupts mode can handle much higher tick rates, but does a poor job of debouncing. It is appropriate for high resolution optical encoders.

Remove spurious unicode beta character

a5c6a44

It was at the end of `#define ENCODER_OPTIMIZE_INTERRUPTS` and so could have been causing the #define to be unrecognized.

removed digital write high in mono encoder

ae4ab8f

Fix syntax error if only using one channel in int mode

8f2f859

- there was a syntax error if only one pin was defined and it was being used in interrupt mode; that is fixed. - Add more documentation to the mono_encoder.ino sketch

Ezward and others added 17 commits February 13, 2023 18:24

Fix bug in mono encoder sketch

162ff35

- it had literal 2 for size of encoders array, so when there was only one encoder we got memory overwrites.

Fix bug in quadrature nolib sketch

5d05b11

- I used the wrong symbol for adding the #2 isr

minor change to quadrature nolib

a4ede0a

- use array rather than pointer in readEncoders() argument.

fix merge error in path_follow.py

1cf569d

added non-threaded run() to UnicyclePose and BicyclePose for testing

92af322

Improved accuracy of MockEncoder by propagating fractional ticks

4e6c371

Updated add_odometry() so we could add as non-threaded for testing.

6d4f506

Adjust bicycle kinematics to use front wheel reference

0cda9b0

- the code now uses the front wheels as the reference point for the calculations - this fixes the unit test, which were using the front wheels while the code used the back wheels.

relax orientation test to 3% accuracy

a1d4f78

updated based on PR feedback

024fc0f

removed hard-coded logging level

0ae8db9

Update vehicle.py

ee22a1b

- change print statement to a log

Update setup.py version="4.4.dev5"

78711a7

version="4.4.dev5"

Ezward force-pushed the 921-kinematics-pose-estimation branch from 0b9362d to 78711a7 Compare February 14, 2023 02:34

DocGarbanzo merged commit c4d9091 into main Feb 16, 2023

This was referenced Feb 18, 2023

In both the DC master and dev branches there is no "test_encoder.py" in the tests folder #924

Closed

Adding velocity to the model #921

Open

Ezward mentioned this pull request Mar 6, 2023

921 tachometer odometer kinematics parts #1003

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

921 kinematics pose estimation #1089

921 kinematics pose estimation #1089

Ezward commented Jan 26, 2023 •

edited

Loading

Ezward commented Jan 26, 2023

Ezward commented Jan 26, 2023

TCIII commented Jan 26, 2023 •

edited

Loading

Ezward commented Jan 27, 2023

DocGarbanzo left a comment

DocGarbanzo left a comment

DocGarbanzo Feb 13, 2023

Ezward Feb 13, 2023

Ezward Feb 13, 2023



		loop_total_time = time.time() - loop_start_time
		print(f"Vehicle executed {loop_count} steps in {loop_total_time} seconds.")

921 kinematics pose estimation #1089

921 kinematics pose estimation #1089

Conversation

Ezward commented Jan 26, 2023 • edited Loading

Ezward commented Jan 26, 2023

Ezward commented Jan 26, 2023

TCIII commented Jan 26, 2023 • edited Loading

Ezward commented Jan 27, 2023

DocGarbanzo left a comment

Choose a reason for hiding this comment

DocGarbanzo left a comment

Choose a reason for hiding this comment

DocGarbanzo Feb 13, 2023

Choose a reason for hiding this comment

Ezward Feb 13, 2023

Choose a reason for hiding this comment

Ezward Feb 13, 2023

Choose a reason for hiding this comment

Ezward commented Jan 26, 2023 •

edited

Loading

TCIII commented Jan 26, 2023 •

edited

Loading