Trajectory Tracking with EV3

[Wesley]

Our team hasn’t been having off-season meetings this summer. As such, I haven’t has any programming projects to work on, so I was starting to get a little bored. I had, however, been thinking about trying to port the WPILib trajectory tracking to ev3dev2 Python. Ev3dev (and the modern ev3dev2) is an operating system that can be run on an EV3 via an sd card, allowing the user access to what is essentially a very slow ARM Linux box. This means that you can run just about any language you like, rather than the terrible EV3-G; the Python libraries seem to be the best-maintained and fully featured. I had already done some work with ev3dev Python, creating a menu system and some simple driving functions (it’s on GitHub), so I decided to add trajectory stuff to that.

Anyway, it works! It is possible to run trajectory tracking on an EV3. I don’t know about generation.
Here’s a simple test path:

EV3 Simple Trajectory Tracking

It should return to exactly where it started, but it’s an EV3, and I didn’t spend too much time tuning it either.

Also, because I’m using the same trajectories as WPILib, it works with Pathweaver. I created a custom game image, and made a fairly simple mission:

And here’s the actual run:

EV3 Trajectory Tracking Bridge Mission

Again, it’s not perfect, but it’s definitely close. With actual models on the field to align to and reset odometry with, it might even be useful!

[marshall]

I love this.

More of this please.

[Wesley]

I was also thinking of trying to port a command-based robot framework, as a potential way to make the transition from FLL to FRC easier, so there might be more in the future. Another possibility would be trying to “complete” the City Shaper game, i.e. make a set of auto routines that can achieve all the missions.

This project also somewhat achieved a secondary goal— understanding how trajectory tracking works.
At one point, I had mistyped the ramsete zeta parameter as 7.0 instead of 0.7— this provided quite the learning experience. Here’s some graphs of the controller-commanded wheel speed (Set) and the actual measured wheel speed (Measured) from when that was happening:

(The output was clamped to -1 to 1, representing -100% to 100% motor power)

I also ended up graphing the x, y, and theta components of the desired and actual pose, giving some even more wild graphs:

Here, red is the current pose of the trajectory, while blue is the actual pose of the robot as reported by odometry.
Eventually, I noticed the error, and everything started working again. As part of determining what was going on, I at one point had the ramsete controller disabled, which yielded this:

Close, but it could definitely use better feedforwards and/or an actual feedback controller.
Once zeta was fixed, I reenabled the controller, and got this:

X and Y are much better, and there’s not much that could be done to improve the angular overshoot— it’s within the slop of the drivetrain ( (not-precise motors * large wheels) / small trackwidth)
All graphs are from the simple test trajectory (first video).

[Wesley]

As you might guess, I was very excited when, still in FLL, I read this line of the game manual:

Sadly, this was my last year in FLL, and otherwise stuff like this might have appeared at competition.

[anon37142155]

This looks really cool, and is probably would be really good to implement for FLL teams with older students to help them transition in to FTC or FRC. Do you have the code for this published somewhere?

[JamesCH95]

@Wesley

it’s on GitHub

Specifically: Branches · WesleyJ-128/FLL_Python_Codebase · GitHub

[Dan_Karol]

This is a really neat project. Good work!

[gerthworm]

Yup.

[Wesley]

Our team has an interesting approach to RDES. Each member would say something about the robot, and then I would talk to the judges for the rest of the available time. We won the RDES award almost every year.

[cgmv123]

Too bad that’s the old rubric.

[gerthworm]

<insert rant about how they reduced from five possible levels to really only three, reducing the possible differentiation between teams and making the judging process harder>

[marshall]

Tell us more stories about the old days granddad!

Edit: Ohh yeah, this thread was about Wesley and how awesome he is. I’ve already offered James my right arm and he told me my right arm was more useless than my left one… and we all know how useless that one is.

[JN25]

Wow this is awesome! I love all of the detailed analysis. The data logging is especially nice to analyze the EV3 behavior given the lack of specs on the hardware. That’s super smart to generate the trajectories on the computer to bypass the PotatoV3 . I’m curious how you implemented the trajectory follower? It is always a bit tricky to make the movements reliable, but it looks like you’ve been able to get everything working super smoothly!

One thing I might suggest is looking into EV3RT which allows for real-time control loops onboard the processor in c++ (much faster than python which could be helpful given the computational constraints).

A few years ago we did something similar, but definitely a lot more simple than what you’ve come up with. It used arcs + lines similar to the 254 generator in 2017 and worked pretty well FLL Share and Learn Virtual Open Invitational Day 2 - First Updates Now - YouTube

[Wesley]

The trajectories are created on the computer for a few different reasons, actually:

• I wanted to use PathWeaver
• When I looked at the code for TrajectoryGenerator, it would have about doubled the amount of stuff I’d need to port. Then I realized there was no reason to do so, as I was going to generate trajectories of-robot anyway.

The trajectory follower is basically just exactly what WPILib provides, ported to Python. The only real change I had to make was to RamseteCommand, since I’m not using a command-based system (really just needed to put execute() into a loop).

Characterization was an interesting experience without the SysId tool or a very clear understanding of how it works. The direct measurements, wheel circumference and trackwidth, were easy enough; I actually already had them for the driving functions I had made previously (Follow a heading for a distance, or until colliding with something, or until reaching a line, drive an arc, turn in place, follow a line). After re-reading the Trajectory Tutorial section about characterization, ks and kv were easy to find, with the knowledge that they should be in Volts and Volts * Seconds / Meters, respectively. Although, instead of working with volts, I’m using motor percentages, so the range is -100 to 100. This doesn’t actually change anything significant; you just make everything that cares think it’s dealing with a 100V motor. Anyway, mimicking/reverse engineering the quasistatic test was easy (measure the speed at various different power levels), and gave me this graph:

X is applied power (%), and Y is measured m/s.
This graph also provides another useful value, max speed. It’s about 0.6 m/s.
Because the forward and reverse datasets line up so well, ks must be effectively zero— which makes sense, because the wheels are direct drive off the motors, and this test was done on a nice smooth floor. Separating the datasets confirms this, as ks should be the x-intercept of the regression line:

ks definitely isn’t negative.
Because of this, the entire dataset (minus the bits at the end where the robot was at max speed) can be used to calculate kv. It’s the inverse slope of the regression:

Now that we have wheel circumference, trackwidth, max speed, ks, and kv, what’s next?
The two constants left are the annoying ones, ka, kp, and max accel.
I tried to imitate the dynamic test by setting the power to 100, then rapidly measuring the actual speed. I got data, then has no idea what to do with it. This graph, with time on the x-axis and speed on the y-axis, gave me the value I ended up using for max accel:

I used the two points on either end of the steep bit of the graph, and found the slope of the line between them, giving a max accel of 20m/s/s. High, but believable, considering how light the robot is.
I eventually gave up on determining ka from measurements, remembering that a paper I found a while ago mentioned how to calculate ka theoretically. Great! I couldn’t find the paper again— until I realized it was a pdf, and thus downloaded. It’s this paper. From said paper, ka = max voltage / max accel, so 100 / 20 = 5.
Fortunetly, I can use the SysId tool to calculate kp; in theoretical analysis mode, it needs ks, kv, ka, and max voltage, and it gives out a kp value. The response initially wasn’t great; here’s the wheel speed graphs again, with kp ~ 0.6 and no ramsete:

It’s close, but could be better. That’s when I realized that SysId has a maxVelocityError parameter, which was set to 1.5 m/s. Twice the max speed of the robot. Setting it to 0.1 gave a kp ~ 1500. I decided to try it anyway, with predictable results:

I started slowly increasing the velocity error parameter, which decreased kp (seemed like exponential decay), until I got these graphs with kp ~ 15:

It’s at least a little better.

EV3RT sounds like a really cool reason to learn C++ …unfortunately I have too much school too soon to start a large project like that.

[BolivarFernandes]

I thought the idea was amazing, I’m a mentor for an FLL team and we’re going to try to implement the system you’ve been developing this season. The boys are prototyping a Spike robot that already has native python.
We are excited to try something totally new.
@Wesley Would you like to work with our team as a programming mentor on this Trajectory Tracking section?