Motion Planning and Control for FRC - FIRST Championship Conference Session

[Jared Russell]

Have you been wondering why your robot never seems to drive to exactly the same spot in autonomous mode?

Why can't your robot turn exactly 90 degrees every time?

Does your elevator throw totes or cans when it starts or stops?

Have you heard of this thing called PID, but don't know how to take it to the next level?

Have you come across Chief Delphi threads like this one (http://www.chiefdelphi.com/forums/showthread.php?t=98358), or maybe this one (http://www.chiefdelphi.com/media/photos/40771), or a post like this one (http://www.chiefdelphi.com/media/papers/3047), and thought: "This looks like it might be useful, but I have no idea what they are talking about"?

Does your robot's coordination and control look more like this:
http://i.imgur.com/lj3c9Mk.gif

than this?
http://cdn1.tnwcdn.com/wp-content/blogs.dir/1/files/2015/02/0209_robotdog2.gif

If you answered yes to any of the above questions, you should come to our seminar on Motion Planning and Control for FRC, on Wednesday, April 22 at 4:30PM in Room 240 of the America's Center. All are welcome.

This presentation is being prepared by the control systems teams from Team 254 and Team 971, and it should be fun and informative. I'll see you there!

[kgzak]

I would love to see this but won't be in St. Louis until Thursday morning. I will try to send my dad and fill me in on what happens but will there be a recording of this after the event? I would love to watch it.

[MrNick]

Seconding the request for a recording.

This is the one area where we have both struggled and tried to learn the most. We are mostly in try-and-test modes and having trouble figuring out the underlying guiding principals to show the way.

[Jared Russell]

We will make sure that, one way or another, this talk gets recorded.

[Thad House]

We will make sure that, one way or another, this talk gets recorded.

That will be awesome. I had a lot of fun last year digging through the path generator you guys used, and getting it to work on our bot. I would be great to hear live more information on this and how you guys make your PID loops so good.

[Bpk9p4]

That is great. I will not be able to attend but am looking forward to watching the video. Do you know where this will be posted?

[AWoL]

This looks awesome! It's currently my first year as 225's lead programmer and I feel like I've got a handle on motion, but it could definitely be hundreds of times better. I can't wait to attend if I can between matches, or otherwise watch the recording!

[Tom Bottiglieri]

We will be leaving a portion of the presentation for a Q&A session, so if you can attend in person, please do!

[Spoam]

Is 971 going touch on state-space models (like they mention here (http://www.chiefdelphi.com/forums/showthread.php?t=129574&page=2))? It seems a little beyond the scope of this presentation, but it's something that interests me a lot and of which there's not very much accessible info out there on.

[AdamHeard]

Is 971 going touch on state-space models (like they mention here (http://www.chiefdelphi.com/forums/showthread.php?t=129574&page=2))? It seems a little beyond the scope of this presentation, but it's something that interests me a lot and of which there's not very much accessible info out there on.

If you can understand english with a heavy indian accent there are lots of youtube videos on it.

[cjl2625]

I'll be there.

It'll be nice to figure out how to make my robot go where I want.

[Joe Ross]

What level of math are you planning to get into?

[GreyingJay]

What level of math are you planning to get into?

Seconding this question.

I have two engineering degrees so I'm not TOO worried about math (well... ok, maybe a little :( ) but I know a few ninth-graders from my team (y'all call them freshmen, right?) who I would love to bring to this session as long as the math isn't so far above their heads that it would confuse them more than inspire.

[JohnFogarty]

Glad this will be recorded.

[rcmolloy]

Glad this will be recorded.

I came here upset knowing that I'd miss a killer presentation. However, not so much anymore. :)

[Jared Russell]

What level of math are you planning to get into?

There will be some advanced high school level math, but nothing more than that.

There will be no root locus plots, Laplace transforms, or serious state-space math. Those are certainly useful tools, but there are already textbooks devoted to them. There will be algebra, some calculus, and some light differential equations.

We will be presenting some ideas whose derivations involve all of the above concepts, but we intend to be pragmatic and focus on shedding light on the intuition behind the math. We will also talk about best practices and FRC "tribal knowledge" to help overcome common problems in implementing motion control on our robots.

[NWChen]

We will make sure that, one way or another, this talk gets recorded.

Awesome, thank you guys. We worked on a spline-based path generator after being inspired by 254's, but it never got implemented beyond an interface for creating curves. Would love to see how to take that tool through the next steps.

[AlexD744]

So many good conferences on wednesday and I'm not coming til Thursday :(

Thank you for ensuring it's recorded, will definitely check it out!

[Bryce Paputa]

I wish I didn't have a robot to put back together and drive once I get there...

I guess a recording will suffice.

[kgzak]

I was just wondering where/when/if the video recording would be available.

Thank you

[AlexanderTheOK]

I'm sure it will be up somewhere once these teams have breathing room after champs.

[Poseidon5817]

Seconding the request for the presentation recording. This would be a nice project for the off season, if we have time.

[Jared Russell]

Expect this to be posted some time this week.

[Jared Russell]

Thank you to all who attended our conference session. We were blown away by the number of people in the audience - several hundred FIRSTers were there and it was standing room only! This goes to show that the FRC community has quite an appetite for technical conference sessions, and I hope we can grow this part of the Championship Conferences substantially in the years to come.

As promised, here is the recording of the session (in standard Cheesy Poof 4k of course): https://youtu.be/8319J1BEHwM

And here are the slides (with a couple of small corrections from the version used in the session): https://docs.google.com/presentation/d/1xjtQ5m3Ay4AYxS_SfloF2n_vWZnCU25aXZuu9A59xPY/pub?start=false&loop=false&delayms=3000

We hope that our session was useful! We know there are some things we can improve for future sessions. One request for anyone who attended: please feel free to PM or email me feedback so we can get better at this in the future. A major challenge in presenting on such a deep subject is hitting the right technical level - there were many levels of experience and expertise in the audience, and you don't want to alienate either end. Please tell us how we did!

[Abhishek R]

Thank you to all who attended our conference session. We were blown away by the number of people in the audience - several hundred FIRSTers were there and it was standing room only! This goes to show that the FRC community has quite an appetite for technical conference sessions, and I hope we can grow this part of the Championship Conferences substantially in the years to come.

As promised, here is the recording of the session (in standard Cheesy Poof 4k of course): https://youtu.be/8319J1BEHwM

And here are the slides (with a couple of small corrections from the version used in the session): https://docs.google.com/presentation/d/1xjtQ5m3Ay4AYxS_SfloF2n_vWZnCU25aXZuu9A59xPY/pub?start=false&loop=false&delayms=3000

We hope that our session was useful! We know there are some things we can improve for future sessions. One request for anyone who attended: please feel free to PM or email me feedback so we can get better at this in the future. A major challenge in presenting on such a deep subject is hitting the right technical level - there were many levels of experience and expertise in the audience, and you don't want to alienate either end. Please tell us how we did!

Thank you!

It may just be me, but are the slides private right now?

Edit: Never mind, doesn't like the mobile app for some reason.

[mman1506]

Thank you!

It may just be me, but are the slides private right now?

Just you

[Ether]

Thank you!

It may just be me, but are the slides private right now?

The download options do not work for me either :-(

[Asymons]

I've been avidly waiting for this :) .

Both links work for me perfectly. Perhaps the issue is resolved now or maybe try a different browser(I'm using chrome).

[Jared]

This is an awesome presentation - I'm our team had to miss it because we didn't have badges yet. I think the level of detail was pretty much perfect for an hour long presentation.

As you mentioned, the internet has lots of great papers describing the process:
http://www2.informatik.uni-freiburg.de/~lau/students/Sprunk2008.pdf

In the presentation, you talked about tuning ka and kv, which we found to be a frustrating process. We started with the method you describe with the robot's maximum velocity and maximum acceleration, but we found that these values of ka and kv only worked well when we ran aggressive and fast trajectories, and weren't as accurate with slower, less terrifying trajectories. kP for the feedback controller had to be increased to maintain accuracy, which caused the robot to oscillate if it was bumped off course. With different values of ka and kv, we could let the feedforward controller do most of the work and decrease kp to improve stability.

Our method to find kp and ka was to simply drive the robot in a straight line with varying speed, acceleration, and power, and record this information every 20 ms. I used a 4 sample moving average to get smooth data from the encoders.

I looked at two points in time that each had different velocity, acceleration, and power, and solved these two equations for ka and kv.

kv * v1 + ka * a1 = p1
kv * v2 + ka * a2 = p2

I repeated this process of selecting two points and solving the equations for 10 times, and averaged the values of ka and kv to get the actual gains.

This resulted in a slightly higher value of kv and a much higher value of ka than we got with the full acceleration test.

[GreyingJay]

Thanks again for putting this session on.

I am on the newbie end of the knowledge scale for this topic - I know about PID and I understood the basic concepts of motion profiling but it will take some further reading and research to figure out how to make it work for us. I think it was a bit above the heads of the freshmen students I brought with us but they were interested and following along as best they could. They can't wait to try out the code that has been made available.

[Ether]

Just you

Both links work for me perfectly.

I can view the presentation on-line. Is that what you both are referring to?

I am saying that the PDF and PPTX download options in this popup menu do not work for me:

http://i.imgur.com/jWPsNVP.png


Jared, is that intentional or an oversight?

~

[Jared Russell]

Not intentional. The PDF and PPT links work for me. :ahh:

I've attached the PDF.

[Ether]

I've attached the PDF.

Thank you.

~

[chsahit]

What does the spline paramater represent? When discussing 2d hermite cubic spline fits functions are created in terms of "s". Is this time?

Thank you and great presentation!

[artK]

What does the spline paramater represent? When discussing 2d hermite cubic spline fits functions are created in terms of "s". Is this time?

Thank you and great presentation!

Spline parameters can be whatever you want: time, % of time, % of total arclength. When 254 implemented it in 2014, I believe we used % of total arclength. As long as you're consistent with your parameter usage, the code should be mostly the same.

[Jared Russell]

What does the spline paramater represent? When discussing 2d hermite cubic spline fits functions are created in terms of "s". Is this time?

Thank you and great presentation!

Usually 's' is not time; it is just an arbitrary parameter (nominally varying between 0 and 1 for any given segment). You can reparameterize a spline by creating another function that maps 's' to something else, like time or arc length.

In other words, instead of looking up your position by calling a function x(s), you would call x(s(t)), or x(s(arc_length)).

Note that if you are creative, you can formulate the problem in different ways. In 2014, 254 actually created 2D splines by making a single function y(x). In this case, the 's' parameter was actually the x coordinate (shifted and rotated to the origin for each spline segment). There are some pros and cons to doing it this way as opposed to the more common parametric way (where you create two independent splines for x and y).

[apache8080]

How does 254 generate the trapezoidal curve for the velocity? My assumption is you just set motor speed to max(1.0) and then calculate what the velocity is at specific time intervals until it reaches a cruise velocity. Then I guess you derive the acceleration from that velocity graph. This makes sense but where I get confused is the deceleration because if you just set the drive speed in the code to 0 then since the motor controllers are in brake mode the motors will just stop.

I am a newbie when it comes to motion profiling so these questions may seem a little dumb.
Thanks for all the help.

[AustinSchuh]

How does 254 generate the trapezoidal curve for the velocity? My assumption is you just set motor speed to max(1.0) and then calculate what the velocity is at specific time intervals until it reaches a cruise velocity. Then I guess you derive the acceleration from that velocity graph. This makes sense but where I get confused is the deceleration because if you just set the drive speed in the code to 0 then since the motor controllers are in brake mode the motors will just stop.


You never want to run saturated (full PWM output). The math to handle that with a drivetrain properly is very nasty due to the non-holonomic nature of a skid steer robot and coupled dynamics.

The correct solution is to pick a velocity and acceleration for your profile such that you never run saturated while driving. For 971's 2015 robot, we used 2 m/s as the peak speed and 3 m/s^2 as the acceleration. That results in very clean motion for us which doesn't saturate.

I would suggest verifying this by picking a velocity and acceleration for your profile, driving the profile with a robot, and then plotting the PWM value requested. You should never see a request above 1.0 or below -1.0.

[ranlevinstein]

Hi,
1. during autonomous why did you apply control only to the speed and rotation of the drivetrain wheels? there are other options like DFL or IOSFL and I was wondering why you didn't choose an option with regulation for the robot's absolute position and and heading.
2. Can someone from 971 please explain how did they use integral control and how to tune the gains matrices for LQR? also in your auto, did you use LQR for the trajectory? if so can you explain how you did that?

You can read more about DFL, IOSFL and posture regulation in this slideshow:http://www.dis.uniroma1.it/~deluca/rob1_en/17_MobileRobotsPlanningControl.pdf

Any help will be greatly appreciated :)

[ranlevinstein]

Hi,
1. during autonomous why did you apply control only to the speed and rotation of the drivetrain wheels? there are other options like DFL or IOSFL and I was wondering why you didn't choose an option with regulation for the robot's absolute position and and heading.
2. Can someone from 971 please explain how did they use integral control and how to tune the gains matrices for LQR? also in your auto, did you use LQR for the trajectory? if so can you explain how you did that?

You can read more about DFL, IOSFL and posture regulation in this slideshow:http://www.dis.uniroma1.it/~deluca/rob1_en/17_MobileRobotsPlanningControl.pdf

Any help will be greatly appreciated :)

[kylelanman]

As promised, here is the recording of the session (in standard Cheesy Poof 4k of course): https://youtu.be/8319J1BEHwM

This was a wonderful presentation!

You made mention that the Talon SRX's can do a lot of "this" for you and that it is extensively documented. Are you just referring to the Feedback/Feedforward PID control built in to the Talons or can they execute a full trajectory/path? I have searched through the software manual for the Talon SRX (http://www.ctr-electronics.com/Talon%20SRX%20Software%20Reference%20Manual.pdf
(http://www.ctr-electronics.com/Talon%20SRX%20Software%20Reference%20Manual.pdf)) and didn't find any mention of motion planning/trajectories.

Looking through the source for CANTalon in WPILib (https://github.com/robotpy/allwpilib/blob/931693345469e1b7f4bbf0eac0844a1afad304af/wpilibc/Athena/src/CANTalon.cpp) I do see mention of trajectories and motion profiles. Is there additional documentation somewhere that I am missing that actually talks about using motion profiles?

[Thad House]

This was a wonderful presentation!

You made mention that the Talon SRX's can do a lot of "this" for you and that it is extensively documented. Are you just referring to the Feedback/Feedforward PID control built in to the Talons or can they execute a full trajectory/path? I have searched through the software manual for the Talon SRX (http://www.ctr-electronics.com/Talon%20SRX%20Software%20Reference%20Manual.pdf
(http://www.ctr-electronics.com/Talon%20SRX%20Software%20Reference%20Manual.pdf)) and didn't find any mention of motion planning/trajectories.

Looking through the source for CANTalon in WPILib (https://github.com/robotpy/allwpilib/blob/931693345469e1b7f4bbf0eac0844a1afad304af/wpilibc/Athena/src/CANTalon.cpp) I do see mention of trajectories and motion profiles. Is there additional documentation somewhere that I am missing that actually talks about using motion profiles?

The motion profile stuff is being added this offseason to the CAN Talons. The documentation for it should be released at kickoff.